Moved, renamed, and deleted files

The original directory structure was scattered and unorganized.
Changes are basically to make it look like kernel structure.

30 files changed, 69003 insertions, 0 deletions

diff --git a/drivers/net/wireless/rtl8188E_8192E/core/efuse/rtw_efuse.c b/drivers/net/wireless/rtl8188E_8192E/core/efuse/rtw_efuse.c
new file mode 100755
index 00000000..d7307b48
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/efuse/rtw_efuse.c
@@ -0,0 +1,1283 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_EFUSE_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+
+#include <rtw_efuse.h>
+
+
+
+/*------------------------Define local variable------------------------------*/
+u8	fakeEfuseBank=0;
+u32	fakeEfuseUsedBytes=0;
+u8	fakeEfuseContent[EFUSE_MAX_HW_SIZE]={0};
+u8	fakeEfuseInitMap[EFUSE_MAX_MAP_LEN]={0};
+u8	fakeEfuseModifiedMap[EFUSE_MAX_MAP_LEN]={0};
+
+u32	BTEfuseUsedBytes=0;
+u8	BTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+u8	BTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN]={0};
+u8	BTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN]={0};
+
+u32	fakeBTEfuseUsedBytes=0;
+u8	fakeBTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+u8	fakeBTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN]={0};
+u8	fakeBTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN]={0};
+/*------------------------Define local variable------------------------------*/
+
+//------------------------------------------------------------------------------
+#define REG_EFUSE_CTRL		0x0030
+#define EFUSE_CTRL			REG_EFUSE_CTRL		// E-Fuse Control.
+//------------------------------------------------------------------------------
+
+BOOLEAN
+Efuse_Read1ByteFromFakeContent(
+	IN		PADAPTER	pAdapter,
+	IN		u16		Offset,
+	IN OUT	u8		*Value	);
+BOOLEAN
+Efuse_Read1ByteFromFakeContent(
+	IN		PADAPTER	pAdapter,
+	IN		u16		Offset,
+	IN OUT	u8		*Value	)
+{
+	if(Offset >= EFUSE_MAX_HW_SIZE)
+	{
+		return _FALSE;
+	}
+	//DbgPrint("Read fake content, offset = %d\n", Offset);
+	if(fakeEfuseBank == 0)
+		*Value = fakeEfuseContent[Offset];
+	else
+		*Value = fakeBTEfuseContent[fakeEfuseBank-1][Offset];
+	return _TRUE;
+}
+
+BOOLEAN
+Efuse_Write1ByteToFakeContent(
+	IN		PADAPTER	pAdapter,
+	IN		u16		Offset,
+	IN 		u8		Value	);
+BOOLEAN
+Efuse_Write1ByteToFakeContent(
+	IN		PADAPTER	pAdapter,
+	IN		u16		Offset,
+	IN 		u8		Value	)
+{
+	if(Offset >= EFUSE_MAX_HW_SIZE)
+	{
+		return _FALSE;
+	}
+	if(fakeEfuseBank == 0)
+		fakeEfuseContent[Offset] = Value;
+	else
+	{
+		fakeBTEfuseContent[fakeEfuseBank-1][Offset] = Value;
+	}
+	return _TRUE;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	Efuse_PowerSwitch
+ *
+ * Overview:	When we want to enable write operation, we should change to 
+ *				pwr on state. When we stop write, we should switch to 500k mode
+ *				and disable LDO 2.5V.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/17/2008 	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+VOID
+Efuse_PowerSwitch(
+	IN	PADAPTER	pAdapter,
+	IN	u8		bWrite,
+	IN	u8		PwrState)
+{
+	pAdapter->HalFunc.EfusePowerSwitch(pAdapter, bWrite, PwrState);
+}	
+
+/*-----------------------------------------------------------------------------
+ * Function:	efuse_GetCurrentSize
+ *
+ * Overview:	Get current efuse size!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/16/2008 	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+u16
+Efuse_GetCurrentSize(
+	IN PADAPTER		pAdapter,
+	IN u8			efuseType,
+	IN BOOLEAN		bPseudoTest)
+{
+	u16 ret=0;
+
+	ret = pAdapter->HalFunc.EfuseGetCurrentSize(pAdapter, efuseType, bPseudoTest);
+
+	return ret;
+}
+
+/*  11/16/2008 MH Add description. Get current efuse area enabled word!!. */
+u8
+Efuse_CalculateWordCnts(IN u8	word_en)
+{
+	u8 word_cnts = 0;
+	if(!(word_en & BIT(0)))	word_cnts++; // 0 : write enable
+	if(!(word_en & BIT(1)))	word_cnts++;
+	if(!(word_en & BIT(2)))	word_cnts++;
+	if(!(word_en & BIT(3)))	word_cnts++;
+	return word_cnts;
+}
+
+//
+//	Description:
+//		Execute E-Fuse read byte operation.
+//		Refered from SD1 Richard.
+//
+//	Assumption:
+//		1. Boot from E-Fuse and successfully auto-load.
+//		2. PASSIVE_LEVEL (USB interface)
+//
+//	Created by Roger, 2008.10.21.
+//
+VOID
+ReadEFuseByte(
+		PADAPTER	Adapter,
+		u16 			_offset, 
+		u8 			*pbuf, 
+		IN BOOLEAN	bPseudoTest) 
+{
+	u32	value32;
+	u8	readbyte;
+	u16	retry;
+	//u32 start=rtw_get_current_time();
+
+	if(bPseudoTest)
+	{
+		Efuse_Read1ByteFromFakeContent(Adapter, _offset, pbuf);
+		return;
+	}
+
+	//Write Address
+	rtw_write8(Adapter, EFUSE_CTRL+1, (_offset & 0xff));  		
+	readbyte = rtw_read8(Adapter, EFUSE_CTRL+2);
+	rtw_write8(Adapter, EFUSE_CTRL+2, ((_offset >> 8) & 0x03) | (readbyte & 0xfc));  		
+
+	//Write bit 32 0
+	readbyte = rtw_read8(Adapter, EFUSE_CTRL+3);		
+	rtw_write8(Adapter, EFUSE_CTRL+3, (readbyte & 0x7f));  	
+	
+	//Check bit 32 read-ready
+	retry = 0;
+	value32 = rtw_read32(Adapter, EFUSE_CTRL);
+	//while(!(((value32 >> 24) & 0xff) & 0x80)  && (retry<10))
+	while(!(((value32 >> 24) & 0xff) & 0x80)  && (retry<10000))
+	{
+		value32 = rtw_read32(Adapter, EFUSE_CTRL);
+		retry++;
+	}
+
+	// 20100205 Joseph: Add delay suggested by SD1 Victor.
+	// This fix the problem that Efuse read error in high temperature condition.
+	// Designer says that there shall be some delay after ready bit is set, or the
+	// result will always stay on last data we read.
+	rtw_udelay_os(50);
+	value32 = rtw_read32(Adapter, EFUSE_CTRL);
+	
+	*pbuf = (u8)(value32 & 0xff);
+	//DBG_871X("ReadEFuseByte _offset:%08u, in %d ms\n",_offset ,rtw_get_passing_time_ms(start));
+	
+}
+
+
+//
+//	Description:
+//		1. Execute E-Fuse read byte operation according as map offset and 
+//		    save to E-Fuse table.
+//		2. Refered from SD1 Richard.
+//
+//	Assumption:
+//		1. Boot from E-Fuse and successfully auto-load.
+//		2. PASSIVE_LEVEL (USB interface)
+//
+//	Created by Roger, 2008.10.21.
+//
+//	2008/12/12 MH 	1. Reorganize code flow and reserve bytes. and add description.
+//					2. Add efuse utilization collect.
+//	2008/12/22 MH	Read Efuse must check if we write section 1 data again!!! Sec1
+//					write addr must be after sec5.
+//
+
+VOID
+efuse_ReadEFuse(
+	PADAPTER	Adapter,
+	u8		efuseType,
+	u16		_offset,
+	u16 		_size_byte,
+	u8      	*pbuf,
+	IN	BOOLEAN	bPseudoTest
+	);
+VOID
+efuse_ReadEFuse(
+	PADAPTER	Adapter,
+	u8		efuseType,
+	u16		_offset,
+	u16 		_size_byte,
+	u8      	*pbuf,
+	IN	BOOLEAN	bPseudoTest
+	)
+{
+	Adapter->HalFunc.ReadEFuse(Adapter, efuseType, _offset, _size_byte, pbuf, bPseudoTest);
+}
+
+VOID
+EFUSE_GetEfuseDefinition(
+	IN		PADAPTER	pAdapter,
+	IN		u8		efuseType,
+	IN		u8		type,
+	OUT		void		*pOut,
+	IN		BOOLEAN		bPseudoTest
+	)
+{
+	pAdapter->HalFunc.EFUSEGetEfuseDefinition(pAdapter, efuseType, type, pOut, bPseudoTest);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_Read1Byte
+ *
+ * Overview:	Copy from WMAC fot EFUSE read 1 byte.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 09/23/2008 	MHC		Copy from WMAC.
+ *
+ *---------------------------------------------------------------------------*/
+u8
+EFUSE_Read1Byte(	
+	IN	PADAPTER	Adapter, 
+	IN	u16		Address)
+{
+	u8	data;
+	u8	Bytetemp = {0x00};
+	u8	temp = {0x00};
+	u32	k=0;
+	u16	contentLen=0;
+
+	EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI , TYPE_EFUSE_REAL_CONTENT_LEN, (PVOID)&contentLen, _FALSE);
+
+	if (Address < contentLen)	//E-fuse 512Byte
+	{
+		//Write E-fuse Register address bit0~7
+		temp = Address & 0xFF;	
+		rtw_write8(Adapter, EFUSE_CTRL+1, temp);	
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+2);	
+		//Write E-fuse Register address bit8~9
+		temp = ((Address >> 8) & 0x03) | (Bytetemp & 0xFC);	
+		rtw_write8(Adapter, EFUSE_CTRL+2, temp);	
+
+		//Write 0x30[31]=0
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		temp = Bytetemp & 0x7F;
+		rtw_write8(Adapter, EFUSE_CTRL+3, temp);
+
+		//Wait Write-ready (0x30[31]=1)
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		while(!(Bytetemp & 0x80))
+		{				
+			Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+			k++;
+			if(k==1000)
+			{
+				k=0;
+				break;
+			}
+		}
+		data=rtw_read8(Adapter, EFUSE_CTRL);
+		return data;
+	}
+	else
+		return 0xFF;
+	
+}/* EFUSE_Read1Byte */
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_Write1Byte
+ *
+ * Overview:	Copy from WMAC fot EFUSE write 1 byte.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 09/23/2008 	MHC		Copy from WMAC.
+ *
+ *---------------------------------------------------------------------------*/
+
+void	
+EFUSE_Write1Byte(	
+	IN	PADAPTER	Adapter, 
+	IN	u16		Address,
+	IN	u8		Value);
+void	
+EFUSE_Write1Byte(	
+	IN	PADAPTER	Adapter, 
+	IN	u16		Address,
+	IN	u8		Value)
+{
+	u8	Bytetemp = {0x00};
+	u8	temp = {0x00};
+	u32	k=0;
+	u16	contentLen=0;
+
+	//RT_TRACE(COMP_EFUSE, DBG_LOUD, ("Addr=%x Data =%x\n", Address, Value));
+	EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI , TYPE_EFUSE_REAL_CONTENT_LEN, (PVOID)&contentLen, _FALSE);
+
+	if( Address < contentLen)	//E-fuse 512Byte
+	{
+		rtw_write8(Adapter, EFUSE_CTRL, Value);
+
+		//Write E-fuse Register address bit0~7
+		temp = Address & 0xFF;	
+		rtw_write8(Adapter, EFUSE_CTRL+1, temp);	
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+2);	
+		
+		//Write E-fuse Register address bit8~9
+		temp = ((Address >> 8) & 0x03) | (Bytetemp & 0xFC);	
+		rtw_write8(Adapter, EFUSE_CTRL+2, temp);	
+
+		//Write 0x30[31]=1
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		temp = Bytetemp | 0x80;
+		rtw_write8(Adapter, EFUSE_CTRL+3, temp);
+
+		//Wait Write-ready (0x30[31]=0)
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		while(Bytetemp & 0x80)
+		{
+			Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);			
+			k++;
+			if(k==100)
+			{
+				k=0;
+				break;
+			}
+		}
+	}
+}/* EFUSE_Write1Byte */
+
+/*  11/16/2008 MH Read one byte from real Efuse. */
+u8
+efuse_OneByteRead(
+	IN	PADAPTER	pAdapter, 
+	IN	u16			addr,
+	IN	u8			*data,
+	IN	BOOLEAN		bPseudoTest)
+{
+	u8	tmpidx = 0;
+	u8	bResult;
+
+	if(bPseudoTest)
+	{
+		bResult = Efuse_Read1ByteFromFakeContent(pAdapter, addr, data);
+		return bResult;
+	}
+	// -----------------e-fuse reg ctrl ---------------------------------
+	//address			
+	rtw_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr&0xff));		
+	rtw_write8(pAdapter, EFUSE_CTRL+2, ((u8)((addr>>8) &0x03) ) | 
+	(rtw_read8(pAdapter, EFUSE_CTRL+2)&0xFC ));	
+
+	rtw_write8(pAdapter, EFUSE_CTRL+3,  0x72);//read cmd	
+
+	while(!(0x80 &rtw_read8(pAdapter, EFUSE_CTRL+3))&&(tmpidx<100))
+	{
+		tmpidx++;
+	}
+	if(tmpidx<100)
+	{			
+		*data=rtw_read8(pAdapter, EFUSE_CTRL);		
+		bResult = _TRUE;
+	}
+	else
+	{
+		*data = 0xff;	
+		bResult = _FALSE;
+	}
+	return bResult;
+}
+		
+/*  11/16/2008 MH Write one byte to reald Efuse. */
+u8
+efuse_OneByteWrite(
+	IN	PADAPTER	pAdapter,  
+	IN	u16			addr, 
+	IN	u8			data,
+	IN	BOOLEAN		bPseudoTest)
+{
+	u8	tmpidx = 0;
+	u8	bResult;
+
+	if(bPseudoTest)
+	{
+		bResult = Efuse_Write1ByteToFakeContent(pAdapter, addr, data);
+		return bResult;
+	}
+	//RT_TRACE(COMP_EFUSE, DBG_LOUD, ("Addr = %x Data=%x\n", addr, data));
+
+	//return	0;
+
+	// -----------------e-fuse reg ctrl ---------------------------------	
+	//address			
+	rtw_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr&0xff));
+	rtw_write8(pAdapter, EFUSE_CTRL+2, 
+	(rtw_read8(pAdapter, EFUSE_CTRL+2)&0xFC )|(u8)((addr>>8)&0x03) );	
+	rtw_write8(pAdapter, EFUSE_CTRL, data);//data		
+
+	rtw_write8(pAdapter, EFUSE_CTRL+3, 0xF2);//write cmd
+		
+	while((0x80 &  rtw_read8(pAdapter, EFUSE_CTRL+3)) && (tmpidx<100) ){
+		tmpidx++;
+	}
+	
+	if(tmpidx<100)
+	{					
+		bResult = _TRUE;
+	}
+	else
+	{			
+		bResult = _FALSE;
+	}		
+	
+	return bResult;	
+}
+
+int
+Efuse_PgPacketRead(	IN	PADAPTER	pAdapter,
+					IN	u8			offset,
+					IN	u8			*data,
+					IN	BOOLEAN		bPseudoTest)
+{
+	int	ret=0;
+
+	ret =  pAdapter->HalFunc.Efuse_PgPacketRead(pAdapter, offset, data, bPseudoTest);
+
+	return ret;
+}
+
+int 
+Efuse_PgPacketWrite(IN	PADAPTER	pAdapter, 
+					IN	u8 			offset,
+					IN	u8			word_en,
+					IN	u8			*data,
+					IN	BOOLEAN		bPseudoTest)
+{
+	int ret;
+
+	ret =  pAdapter->HalFunc.Efuse_PgPacketWrite(pAdapter, offset, word_en, data, bPseudoTest);
+
+	return ret;
+}
+
+
+int 
+Efuse_PgPacketWrite_BT(IN	PADAPTER	pAdapter, 
+					IN	u8 			offset,
+					IN	u8			word_en,
+					IN	u8			*data,
+					IN	BOOLEAN		bPseudoTest)
+{
+	int ret;
+
+	ret =  pAdapter->HalFunc.Efuse_PgPacketWrite_BT(pAdapter, offset, word_en, data, bPseudoTest);
+
+	return ret;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	efuse_WordEnableDataRead
+ *
+ * Overview:	Read allowed word in current efuse section data.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/16/2008 	MHC		Create Version 0.
+ * 11/21/2008 	MHC		Fix Write bug when we only enable late word.
+ *
+ *---------------------------------------------------------------------------*/
+void
+efuse_WordEnableDataRead(IN	u8	word_en,
+							IN	u8	*sourdata,
+							IN	u8	*targetdata)
+{	
+	if (!(word_en&BIT(0)))
+	{
+		targetdata[0] = sourdata[0];
+		targetdata[1] = sourdata[1];
+	}
+	if (!(word_en&BIT(1)))
+	{
+		targetdata[2] = sourdata[2];
+		targetdata[3] = sourdata[3];
+	}
+	if (!(word_en&BIT(2)))
+	{
+		targetdata[4] = sourdata[4];
+		targetdata[5] = sourdata[5];
+	}
+	if (!(word_en&BIT(3)))
+	{
+		targetdata[6] = sourdata[6];
+		targetdata[7] = sourdata[7];
+	}
+}
+
+
+u8
+Efuse_WordEnableDataWrite(	IN	PADAPTER	pAdapter,
+							IN	u16		efuse_addr,
+							IN	u8		word_en, 
+							IN	u8		*data,
+							IN	BOOLEAN		bPseudoTest)
+{
+	u8	ret=0;
+
+	ret =  pAdapter->HalFunc.Efuse_WordEnableDataWrite(pAdapter, efuse_addr, word_en, data, bPseudoTest);
+	
+	return ret;
+}
+
+static u8 efuse_read8(PADAPTER padapter, u16 address, u8 *value)
+{
+	return efuse_OneByteRead(padapter,address, value, _FALSE);
+}
+
+static u8 efuse_write8(PADAPTER padapter, u16 address, u8 *value)
+{
+	return efuse_OneByteWrite(padapter,address, *value, _FALSE);
+}
+
+/*
+ * read/wirte raw efuse data
+ */
+u8 rtw_efuse_access(PADAPTER padapter, u8 bWrite, u16 start_addr, u16 cnts, u8 *data)
+{
+	int i = 0;
+	u16	real_content_len = 0, max_available_size = 0;
+	u8 res = _FAIL ;
+	u8 (*rw8)(PADAPTER, u16, u8*);
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_REAL_CONTENT_LEN, (PVOID)&real_content_len, _FALSE);
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (PVOID)&max_available_size, _FALSE);
+
+	if (start_addr > real_content_len)
+		return _FAIL;
+
+	if (_TRUE == bWrite) {
+		if ((start_addr + cnts) > max_available_size)
+			return _FAIL;
+		rw8 = &efuse_write8;
+	} else
+		rw8 = &efuse_read8;
+
+	Efuse_PowerSwitch(padapter, bWrite, _TRUE);
+
+	// e-fuse one byte read / write
+	for (i = 0; i < cnts; i++) {
+		if (start_addr >= real_content_len) {
+			res = _FAIL;
+			break;
+		}
+
+		res = rw8(padapter, start_addr++, data++);
+		if (_FAIL == res) break;
+	}
+
+	Efuse_PowerSwitch(padapter, bWrite, _FALSE);
+
+	return res;
+}
+//------------------------------------------------------------------------------
+u16 efuse_GetMaxSize(PADAPTER padapter)
+{
+	u16	max_size;
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI , TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (PVOID)&max_size, _FALSE);
+	return max_size;
+}
+//------------------------------------------------------------------------------
+u8 efuse_GetCurrentSize(PADAPTER padapter, u16 *size)
+{
+	Efuse_PowerSwitch(padapter, _FALSE, _TRUE);
+	*size = Efuse_GetCurrentSize(padapter, EFUSE_WIFI, _FALSE);
+	Efuse_PowerSwitch(padapter, _FALSE, _FALSE);
+
+	return _SUCCESS;
+}
+//------------------------------------------------------------------------------
+u8 rtw_efuse_map_read(PADAPTER padapter, u16 addr, u16 cnts, u8 *data)
+{
+	u16	mapLen=0;
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (PVOID)&mapLen, _FALSE);
+
+	if ((addr + cnts) > mapLen)
+		return _FAIL;
+
+	Efuse_PowerSwitch(padapter, _FALSE, _TRUE);
+
+	efuse_ReadEFuse(padapter, EFUSE_WIFI, addr, cnts, data, _FALSE);
+
+	Efuse_PowerSwitch(padapter, _FALSE, _FALSE);
+
+	return _SUCCESS;
+}
+
+u8 rtw_BT_efuse_map_read(PADAPTER padapter, u16 addr, u16 cnts, u8 *data)
+{
+	u16	mapLen=0;
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_BT, TYPE_EFUSE_MAP_LEN, (PVOID)&mapLen, _FALSE);
+
+	if ((addr + cnts) > mapLen)
+		return _FAIL;
+
+	Efuse_PowerSwitch(padapter, _FALSE, _TRUE);
+
+	efuse_ReadEFuse(padapter, EFUSE_BT, addr, cnts, data, _FALSE);
+
+	Efuse_PowerSwitch(padapter, _FALSE, _FALSE);
+
+	return _SUCCESS;
+}
+//------------------------------------------------------------------------------
+u8 rtw_efuse_map_write(PADAPTER padapter, u16 addr, u16 cnts, u8 *data)
+{
+	u8	offset, word_en;
+	u8	*map;
+	u8	newdata[PGPKT_DATA_SIZE];
+	s32	i, j, idx;
+	u8	ret = _SUCCESS;
+	u16	mapLen=0;
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (PVOID)&mapLen, _FALSE);
+
+	if ((addr + cnts) > mapLen)
+		return _FAIL;
+
+	map = rtw_zmalloc(mapLen);
+	if(map == NULL){
+		return _FAIL;
+	}
+
+	ret = rtw_efuse_map_read(padapter, 0, mapLen, map);
+	if (ret == _FAIL) goto exit;
+
+	Efuse_PowerSwitch(padapter, _TRUE, _TRUE);
+
+	offset = (addr >> 3);
+	word_en = 0xF;
+	_rtw_memset(newdata, 0xFF, PGPKT_DATA_SIZE);
+	i = addr & 0x7;	// index of one package
+	j = 0;		// index of new package
+	idx = 0;	// data index
+
+	if (i & 0x1) {
+		// odd start
+		if (data[idx] != map[addr+idx]) {
+			word_en &= ~BIT(i >> 1);
+			newdata[i-1] = map[addr+idx-1];
+			newdata[i] = data[idx];
+		}
+		i++;
+		idx++;
+	}
+	do {
+		for (; i < PGPKT_DATA_SIZE; i += 2)
+		{
+			if (cnts == idx) break;
+			if ((cnts - idx) == 1) {
+				if (data[idx] != map[addr+idx]) {
+					word_en &= ~BIT(i >> 1);
+					newdata[i] = data[idx];
+					newdata[i+1] = map[addr+idx+1];
+				}
+				idx++;
+				break;
+			} else {
+				if ((data[idx] != map[addr+idx]) ||
+				    (data[idx+1] != map[addr+idx+1]))
+				{
+					word_en &= ~BIT(i >> 1);
+					newdata[i] = data[idx];
+					newdata[i+1] = data[idx + 1];
+				}
+				idx += 2;
+			}
+			if (idx == cnts) break;
+		}
+
+		if (word_en != 0xF) {
+			ret = Efuse_PgPacketWrite(padapter, offset, word_en, newdata, _FALSE);
+			DBG_871X("offset=%x \n",offset);
+			DBG_871X("word_en=%x \n",word_en);
+
+			for(i=0;i<PGPKT_DATA_SIZE;i++)
+			{
+				DBG_871X("data=%x \t",newdata[i]);
+			}
+			if (ret == _FAIL) break;
+		}
+
+		if (idx == cnts) break;
+
+		offset++;
+		i = 0;
+		j = 0;
+		word_en = 0xF;
+		_rtw_memset(newdata, 0xFF, PGPKT_DATA_SIZE);
+	} while (1);
+
+	Efuse_PowerSwitch(padapter, _TRUE, _FALSE);
+
+exit:
+
+	rtw_mfree(map, mapLen);
+
+	return ret;
+}
+
+
+//------------------------------------------------------------------------------
+u8 rtw_BT_efuse_map_write(PADAPTER padapter, u16 addr, u16 cnts, u8 *data)
+{
+	u8	offset, word_en;
+	u8	*map;
+	u8	newdata[PGPKT_DATA_SIZE];
+	s32	i, j, idx;
+	u8	ret = _SUCCESS;
+	u16	mapLen=0;
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_BT, TYPE_EFUSE_MAP_LEN, (PVOID)&mapLen, _FALSE);
+
+	if ((addr + cnts) > mapLen)
+		return _FAIL;
+
+	map = rtw_zmalloc(mapLen);
+	if(map == NULL){
+		return _FAIL;
+	}
+
+	ret = rtw_BT_efuse_map_read(padapter, 0, mapLen, map);
+	if (ret == _FAIL) goto exit;
+
+	Efuse_PowerSwitch(padapter, _TRUE, _TRUE);
+
+	offset = (addr >> 3);
+	word_en = 0xF;
+	_rtw_memset(newdata, 0xFF, PGPKT_DATA_SIZE);
+	i = addr & 0x7;	// index of one package
+	j = 0;		// index of new package
+	idx = 0;	// data index
+
+	if (i & 0x1) {
+		// odd start
+		if (data[idx] != map[addr+idx]) {
+			word_en &= ~BIT(i >> 1);
+			newdata[i-1] = map[addr+idx-1];
+			newdata[i] = data[idx];
+		}
+		i++;
+		idx++;
+	}
+	do {
+		for (; i < PGPKT_DATA_SIZE; i += 2)
+		{
+			if (cnts == idx) break;
+			if ((cnts - idx) == 1) {
+				if (data[idx] != map[addr+idx]) {
+					word_en &= ~BIT(i >> 1);
+					newdata[i] = data[idx];
+					newdata[i+1] = map[addr+idx+1];
+				}
+				idx++;
+				break;
+			} else {
+				if ((data[idx] != map[addr+idx]) ||
+				    (data[idx+1] != map[addr+idx+1]))
+				{
+					word_en &= ~BIT(i >> 1);
+					newdata[i] = data[idx];
+					newdata[i+1] = data[idx + 1];
+				}
+				idx += 2;
+			}
+			if (idx == cnts) break;
+		}
+
+		if (word_en != 0xF)
+		{
+			DBG_871X("%s: offset=%#X\n", __FUNCTION__, offset);
+			DBG_871X("%s: word_en=%#X\n", __FUNCTION__, word_en);
+			DBG_871X("%s: data=", __FUNCTION__);
+			for (i=0; i<PGPKT_DATA_SIZE; i++)
+			{
+				DBG_871X("0x%02X ", newdata[i]);
+			}
+			DBG_871X("\n");
+
+			ret = Efuse_PgPacketWrite_BT(padapter, offset, word_en, newdata, _FALSE);
+			if (ret == _FAIL) break;
+		}
+
+		if (idx == cnts) break;
+
+		offset++;
+		i = 0;
+		j = 0;
+		word_en = 0xF;
+		_rtw_memset(newdata, 0xFF, PGPKT_DATA_SIZE);
+	} while (1);
+
+	Efuse_PowerSwitch(padapter, _TRUE, _FALSE);
+
+exit:
+
+	rtw_mfree(map, mapLen);
+
+	return ret;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	Efuse_ReadAllMap
+ *
+ * Overview:	Read All Efuse content
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/11/2008 	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+VOID 
+Efuse_ReadAllMap(
+	IN		PADAPTER	pAdapter, 
+	IN		u8		efuseType,
+	IN OUT	u8		*Efuse,
+	IN		BOOLEAN		bPseudoTest);
+VOID 
+Efuse_ReadAllMap(
+	IN		PADAPTER	pAdapter, 
+	IN		u8		efuseType,
+	IN OUT	u8		*Efuse,
+	IN		BOOLEAN		bPseudoTest)
+{
+	u16	mapLen=0;
+
+	Efuse_PowerSwitch(pAdapter,_FALSE, _TRUE);
+
+	EFUSE_GetEfuseDefinition(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN, (PVOID)&mapLen, bPseudoTest);
+
+	efuse_ReadEFuse(pAdapter, efuseType, 0, mapLen, Efuse, bPseudoTest);
+
+	Efuse_PowerSwitch(pAdapter,_FALSE, _FALSE);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	efuse_ShadowRead1Byte
+ *			efuse_ShadowRead2Byte
+ *			efuse_ShadowRead4Byte
+ *
+ * Overview:	Read from efuse init map by one/two/four bytes !!!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/12/2008 	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+static VOID
+efuse_ShadowRead1Byte(
+	IN	PADAPTER	pAdapter,
+	IN	u16		Offset,
+	IN OUT	u8		*Value)
+{
+	EEPROM_EFUSE_PRIV *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter);
+
+	*Value = pEEPROM->efuse_eeprom_data[Offset];
+
+}	// EFUSE_ShadowRead1Byte
+
+//---------------Read Two Bytes
+static VOID
+efuse_ShadowRead2Byte(
+	IN	PADAPTER	pAdapter,
+	IN	u16		Offset,
+	IN OUT	u16		*Value)
+{
+	EEPROM_EFUSE_PRIV *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter);
+
+	*Value = pEEPROM->efuse_eeprom_data[Offset];
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+1]<<8;
+
+}	// EFUSE_ShadowRead2Byte
+
+//---------------Read Four Bytes
+static VOID
+efuse_ShadowRead4Byte(
+	IN	PADAPTER	pAdapter,
+	IN	u16		Offset,
+	IN OUT	u32		*Value)
+{
+	EEPROM_EFUSE_PRIV *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter);
+
+	*Value = pEEPROM->efuse_eeprom_data[Offset];
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+1]<<8;
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+2]<<16;
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+3]<<24;
+
+}	// efuse_ShadowRead4Byte
+
+
+/*-----------------------------------------------------------------------------
+ * Function:	efuse_ShadowWrite1Byte
+ *			efuse_ShadowWrite2Byte
+ *			efuse_ShadowWrite4Byte
+ *
+ * Overview:	Write efuse modify map by one/two/four byte.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/12/2008 	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+#ifdef PLATFORM
+static VOID
+efuse_ShadowWrite1Byte(
+	IN	PADAPTER	pAdapter,
+	IN	u16		Offset,
+	IN 	u8		Value);
+#endif //PLATFORM
+static VOID
+efuse_ShadowWrite1Byte(
+	IN	PADAPTER	pAdapter,
+	IN	u16		Offset,
+	IN 	u8		Value)
+{
+	EEPROM_EFUSE_PRIV *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter);
+
+	pEEPROM->efuse_eeprom_data[Offset] = Value;
+
+}	// efuse_ShadowWrite1Byte
+
+//---------------Write Two Bytes
+static VOID
+efuse_ShadowWrite2Byte(
+	IN	PADAPTER	pAdapter,
+	IN	u16		Offset,
+	IN 	u16		Value)
+{
+	EEPROM_EFUSE_PRIV *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter);
+
+	pEEPROM->efuse_eeprom_data[Offset] = Value&0x00FF;
+	pEEPROM->efuse_eeprom_data[Offset+1] = Value>>8;
+
+}	// efuse_ShadowWrite1Byte
+
+//---------------Write Four Bytes
+static VOID
+efuse_ShadowWrite4Byte(
+	IN	PADAPTER	pAdapter,
+	IN	u16		Offset,
+	IN	u32		Value)
+{
+	EEPROM_EFUSE_PRIV *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter);
+
+	pEEPROM->efuse_eeprom_data[Offset] = (u8)(Value&0x000000FF);
+	pEEPROM->efuse_eeprom_data[Offset+1] = (u8)((Value>>8)&0x0000FF);
+	pEEPROM->efuse_eeprom_data[Offset+2] = (u8)((Value>>16)&0x00FF);
+	pEEPROM->efuse_eeprom_data[Offset+3] = (u8)((Value>>24)&0xFF);
+
+}	// efuse_ShadowWrite1Byte
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_ShadowMapUpdate
+ *
+ * Overview:	Transfer current EFUSE content to shadow init and modify map.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/13/2008 	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void EFUSE_ShadowMapUpdate(
+	IN PADAPTER	pAdapter,
+	IN u8		efuseType,
+	IN BOOLEAN	bPseudoTest)
+{
+	EEPROM_EFUSE_PRIV *pEEPROM = GET_EEPROM_EFUSE_PRIV(pAdapter);
+	u16	mapLen=0;
+
+	EFUSE_GetEfuseDefinition(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN, (PVOID)&mapLen, bPseudoTest);
+
+	if (pEEPROM->bautoload_fail_flag == _TRUE)
+	{
+		_rtw_memset(pEEPROM->efuse_eeprom_data, 0xFF, mapLen);
+	}
+	else
+	{
+		#ifdef CONFIG_ADAPTOR_INFO_CACHING_FILE			
+		if(_SUCCESS != retriveAdaptorInfoFile(pAdapter->registrypriv.adaptor_info_caching_file_path, pEEPROM)) {
+		#endif
+		
+		Efuse_ReadAllMap(pAdapter, efuseType, pEEPROM->efuse_eeprom_data, bPseudoTest);
+		
+		#ifdef CONFIG_ADAPTOR_INFO_CACHING_FILE
+			storeAdaptorInfoFile(pAdapter->registrypriv.adaptor_info_caching_file_path, pEEPROM);
+		}
+		#endif
+	}
+
+	//PlatformMoveMemory((PVOID)&pHalData->EfuseMap[EFUSE_MODIFY_MAP][0], 
+	//(PVOID)&pHalData->EfuseMap[EFUSE_INIT_MAP][0], mapLen);
+}// EFUSE_ShadowMapUpdate
+
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_ShadowRead
+ *
+ * Overview:	Read from efuse init map !!!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/12/2008 	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void
+EFUSE_ShadowRead(
+	IN		PADAPTER	pAdapter,
+	IN		u8		Type,
+	IN		u16		Offset,
+	IN OUT	u32		*Value	)
+{
+	if (Type == 1)
+		efuse_ShadowRead1Byte(pAdapter, Offset, (u8 *)Value);
+	else if (Type == 2)
+		efuse_ShadowRead2Byte(pAdapter, Offset, (u16 *)Value);
+	else if (Type == 4)
+		efuse_ShadowRead4Byte(pAdapter, Offset, (u32 *)Value);
+	
+}	// EFUSE_ShadowRead
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_ShadowWrite
+ *
+ * Overview:	Write efuse modify map for later update operation to use!!!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/12/2008 	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+VOID
+EFUSE_ShadowWrite(
+	IN	PADAPTER	pAdapter,
+	IN	u8		Type,
+	IN	u16		Offset,
+	IN OUT	u32		Value);
+VOID
+EFUSE_ShadowWrite(
+	IN	PADAPTER	pAdapter,
+	IN	u8		Type,
+	IN	u16		Offset,
+	IN OUT	u32		Value)
+{
+#if (MP_DRIVER == 0)
+	return;
+#endif
+	if ( pAdapter->registrypriv.mp_mode == 0)
+		return;
+
+
+	if (Type == 1)
+		efuse_ShadowWrite1Byte(pAdapter, Offset, (u8)Value);
+	else if (Type == 2)
+		efuse_ShadowWrite2Byte(pAdapter, Offset, (u16)Value);
+	else if (Type == 4)
+		efuse_ShadowWrite4Byte(pAdapter, Offset, (u32)Value);
+
+}	// EFUSE_ShadowWrite
+
+VOID
+Efuse_InitSomeVar(
+	IN		PADAPTER	pAdapter
+	);
+VOID
+Efuse_InitSomeVar(
+	IN		PADAPTER	pAdapter
+	)
+{
+	u8 i;
+	
+	_rtw_memset((PVOID)&fakeEfuseContent[0], 0xff, EFUSE_MAX_HW_SIZE);
+	_rtw_memset((PVOID)&fakeEfuseInitMap[0], 0xff, EFUSE_MAX_MAP_LEN);
+	_rtw_memset((PVOID)&fakeEfuseModifiedMap[0], 0xff, EFUSE_MAX_MAP_LEN);
+
+	for(i=0; i<EFUSE_MAX_BT_BANK; i++)
+	{
+		_rtw_memset((PVOID)&BTEfuseContent[i][0], EFUSE_MAX_HW_SIZE, 0xff);
+	}
+	_rtw_memset((PVOID)&BTEfuseInitMap[0], 0xff, EFUSE_BT_MAX_MAP_LEN);
+	_rtw_memset((PVOID)&BTEfuseModifiedMap[0], 0xff, EFUSE_BT_MAX_MAP_LEN);
+
+	for(i=0; i<EFUSE_MAX_BT_BANK; i++)
+	{
+		_rtw_memset((PVOID)&fakeBTEfuseContent[i][0], 0xff, EFUSE_MAX_HW_SIZE);
+	}
+	_rtw_memset((PVOID)&fakeBTEfuseInitMap[0], 0xff, EFUSE_BT_MAX_MAP_LEN);
+	_rtw_memset((PVOID)&fakeBTEfuseModifiedMap[0], 0xff, EFUSE_BT_MAX_MAP_LEN);
+}
+
+#ifdef PLATFORM_LINUX
+#ifdef CONFIG_ADAPTOR_INFO_CACHING_FILE
+//#include <rtw_eeprom.h>
+
+ int isAdaptorInfoFileValid(void)
+{
+	return _TRUE;
+}
+
+int storeAdaptorInfoFile(char *path, struct eeprom_priv * eeprom_priv)
+{
+	int ret =_SUCCESS;
+
+	if(path && eeprom_priv) {
+		ret = rtw_store_to_file(path, eeprom_priv->efuse_eeprom_data, EEPROM_MAX_SIZE_512);
+		if(ret == EEPROM_MAX_SIZE)
+			ret = _SUCCESS;
+		else
+			ret = _FAIL;
+	} else {
+		DBG_871X("%s NULL pointer\n",__FUNCTION__);
+		ret =  _FAIL;
+	}
+	return ret;
+}
+
+int retriveAdaptorInfoFile(char *path, struct eeprom_priv * eeprom_priv)
+{
+	int ret = _SUCCESS;
+	mm_segment_t oldfs;
+	struct file *fp;
+	
+	if(path && eeprom_priv) {
+
+		ret = rtw_retrive_from_file(path, eeprom_priv->efuse_eeprom_data, EEPROM_MAX_SIZE);
+		
+		if(ret == EEPROM_MAX_SIZE)
+			ret = _SUCCESS;
+		else
+			ret = _FAIL;
+
+		#if 0
+		if(isAdaptorInfoFileValid()) {	
+			return 0;
+		} else {
+			return _FAIL;
+		}
+		#endif
+		
+	} else {
+		DBG_871X("%s NULL pointer\n",__FUNCTION__);
+		ret = _FAIL;
+	}
+	return ret;
+}
+#endif //CONFIG_ADAPTOR_INFO_CACHING_FILE
+#endif //PLATFORM_LINUX
+
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_ap.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_ap.c
new file mode 100755
index 00000000..c12ffd0c
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_ap.c
@@ -0,0 +1,2936 @@
+/******************************************************************************

+ *

+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.

+ *                                        

+ * This program is free software; you can redistribute it and/or modify it

+ * under the terms of version 2 of the GNU General Public License as

+ * published by the Free Software Foundation.

+ *

+ * 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.,

+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA

+ *

+ *

+ ******************************************************************************/

+#define _RTW_AP_C_

+

+#include <drv_conf.h>

+#include <osdep_service.h>

+#include <drv_types.h>

+#include <wifi.h>

+

+

+#ifdef CONFIG_AP_MODE

+

+extern unsigned char	RTW_WPA_OUI[];

+extern unsigned char 	WMM_OUI[];

+extern unsigned char	WPS_OUI[];

+extern unsigned char	P2P_OUI[];

+extern unsigned char	WFD_OUI[];

+

+void init_mlme_ap_info(_adapter *padapter)

+{

+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct sta_priv *pstapriv = &padapter->stapriv;	

+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;

+	

+

+	_rtw_spinlock_init(&pmlmepriv->bcn_update_lock);	

+

+	//for ACL 

+	_rtw_init_queue(&pacl_list->acl_node_q);

+

+	//pmlmeext->bstart_bss = _FALSE;

+

+	start_ap_mode(padapter);

+}

+

+void free_mlme_ap_info(_adapter *padapter)

+{

+	_irqL irqL;

+	struct sta_info *psta=NULL;

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+

+	//stop_ap_mode(padapter);

+

+	pmlmepriv->update_bcn = _FALSE;

+	pmlmeext->bstart_bss = _FALSE;	

+	

+	rtw_sta_flush(padapter);

+

+	pmlmeinfo->state = _HW_STATE_NOLINK_;

+

+	//free_assoc_sta_resources

+	rtw_free_all_stainfo(padapter);

+

+	//free bc/mc sta_info

+	psta = rtw_get_bcmc_stainfo(padapter);	

+	_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		

+	rtw_free_stainfo(padapter, psta);

+	_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);

+	

+

+	_rtw_spinlock_free(&pmlmepriv->bcn_update_lock);

+	

+}

+

+static void update_BCNTIM(_adapter *padapter)

+{

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);

+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);

+	WLAN_BSSID_EX *pnetwork_mlmeext = &(pmlmeinfo->network);

+	unsigned char *pie = pnetwork_mlmeext->IEs;

+

+	//DBG_871X("%s\n", __FUNCTION__);

+	

+	//update TIM IE

+	//if(pstapriv->tim_bitmap)

+	if(_TRUE)

+	{

+		u8 *p, *dst_ie, *premainder_ie=NULL, *pbackup_remainder_ie=NULL;

+		u16 tim_bitmap_le;

+		uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen;	

+	

+		tim_bitmap_le = cpu_to_le16(pstapriv->tim_bitmap);

+

+		p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, _TIM_IE_, &tim_ielen, pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_);

+		if (p != NULL && tim_ielen>0)

+		{

+			tim_ielen += 2;

+			

+			premainder_ie = p+tim_ielen;

+

+			tim_ie_offset = (sint)(p -pie);

+			

+			remainder_ielen = pnetwork_mlmeext->IELength - tim_ie_offset - tim_ielen;

+

+			//append TIM IE from dst_ie offset

+			dst_ie = p;

+		}

+		else

+		{

+			tim_ielen = 0;

+

+			//calucate head_len		

+			offset = _FIXED_IE_LENGTH_;

+

+			/* get ssid_ie len */

+			p = rtw_get_ie(pie + _BEACON_IE_OFFSET_, _SSID_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_));

+			if (p != NULL)

+				offset += tmp_len+2;

+

+			// get supported rates len

+			p = rtw_get_ie(pie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_));	

+			if (p !=  NULL) 

+			{			

+				offset += tmp_len+2;

+			}

+

+			//DS Parameter Set IE, len=3	

+			offset += 3;

+

+			premainder_ie = pie + offset;

+

+			remainder_ielen = pnetwork_mlmeext->IELength - offset - tim_ielen;	

+

+			//append TIM IE from offset

+			dst_ie = pie + offset;

+			

+		}

+

+		

+		if(remainder_ielen>0)

+		{

+			pbackup_remainder_ie = rtw_malloc(remainder_ielen);

+			if(pbackup_remainder_ie && premainder_ie)

+				_rtw_memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);

+		}

+		

+		*dst_ie++=_TIM_IE_;

+

+		if((pstapriv->tim_bitmap&0xff00) && (pstapriv->tim_bitmap&0x00fc))			

+			tim_ielen = 5;

+		else

+			tim_ielen = 4;

+

+		*dst_ie++= tim_ielen;

+		

+		*dst_ie++=0;//DTIM count

+		*dst_ie++=1;//DTIM peroid

+		

+		if(pstapriv->tim_bitmap&BIT(0))//for bc/mc frames

+			*dst_ie++ = BIT(0);//bitmap ctrl 

+		else

+			*dst_ie++ = 0;

+

+		if(tim_ielen==4)

+		{

+			*dst_ie++ = *(u8*)&tim_bitmap_le;

+		}	

+		else if(tim_ielen==5)

+		{

+			_rtw_memcpy(dst_ie, &tim_bitmap_le, 2);

+			dst_ie+=2;				

+		}	

+		

+		//copy remainder IE

+		if(pbackup_remainder_ie)

+		{

+			_rtw_memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen);

+

+			rtw_mfree(pbackup_remainder_ie, remainder_ielen);

+		}	

+

+		offset =  (uint)(dst_ie - pie);

+		pnetwork_mlmeext->IELength = offset + remainder_ielen;

+	

+	}

+

+#ifndef CONFIG_INTERRUPT_BASED_TXBCN 

+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)

+	set_tx_beacon_cmd(padapter);

+#endif

+#endif //!CONFIG_INTERRUPT_BASED_TXBCN

+

+

+}

+

+void rtw_add_bcn_ie(_adapter *padapter, WLAN_BSSID_EX *pnetwork, u8 index, u8 *data, u8 len)

+{

+	PNDIS_802_11_VARIABLE_IEs	pIE;

+	u8	bmatch = _FALSE;

+	u8	*pie = pnetwork->IEs;

+	u8	*p, *dst_ie, *premainder_ie=NULL, *pbackup_remainder_ie=NULL;

+	u32	i, offset, ielen, ie_offset, remainder_ielen = 0;

+

+	for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pnetwork->IELength;)

+	{

+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pnetwork->IEs + i);

+

+		if (pIE->ElementID > index)

+		{

+			break;

+		}

+		else if(pIE->ElementID == index) // already exist the same IE

+		{

+			p = (u8 *)pIE;

+			ielen = pIE->Length;

+			bmatch = _TRUE;

+			break;

+		}

+

+		p = (u8 *)pIE;

+		ielen = pIE->Length;

+		i += (pIE->Length + 2);

+	}

+

+	if (p != NULL && ielen>0)

+	{

+		ielen += 2;

+		

+		premainder_ie = p+ielen;

+

+		ie_offset = (sint)(p -pie);

+		

+		remainder_ielen = pnetwork->IELength - ie_offset - ielen;

+

+		if(bmatch)

+			dst_ie = p;

+		else

+			dst_ie = (p+ielen);

+	}

+

+	if(remainder_ielen>0)

+	{

+		pbackup_remainder_ie = rtw_malloc(remainder_ielen);

+		if(pbackup_remainder_ie && premainder_ie)

+			_rtw_memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);

+	}

+

+	*dst_ie++=index;

+	*dst_ie++=len;

+

+	_rtw_memcpy(dst_ie, data, len);

+	dst_ie+=len;

+

+	//copy remainder IE

+	if(pbackup_remainder_ie)

+	{

+		_rtw_memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen);

+

+		rtw_mfree(pbackup_remainder_ie, remainder_ielen);

+	}

+

+	offset =  (uint)(dst_ie - pie);

+	pnetwork->IELength = offset + remainder_ielen;

+}

+

+void rtw_remove_bcn_ie(_adapter *padapter, WLAN_BSSID_EX *pnetwork, u8 index)

+{

+	u8 *p, *dst_ie, *premainder_ie=NULL, *pbackup_remainder_ie=NULL;

+	uint offset, ielen, ie_offset, remainder_ielen = 0;

+	u8	*pie = pnetwork->IEs;

+

+	p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, index, &ielen, pnetwork->IELength - _FIXED_IE_LENGTH_);

+	if (p != NULL && ielen>0)

+	{

+		ielen += 2;

+		

+		premainder_ie = p+ielen;

+

+		ie_offset = (sint)(p -pie);

+		

+		remainder_ielen = pnetwork->IELength - ie_offset - ielen;

+

+		dst_ie = p;

+	}

+

+	if(remainder_ielen>0)

+	{

+		pbackup_remainder_ie = rtw_malloc(remainder_ielen);

+		if(pbackup_remainder_ie && premainder_ie)

+			_rtw_memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);

+	}

+

+	//copy remainder IE

+	if(pbackup_remainder_ie)

+	{

+		_rtw_memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen);

+

+		rtw_mfree(pbackup_remainder_ie, remainder_ielen);

+	}

+

+	offset =  (uint)(dst_ie - pie);

+	pnetwork->IELength = offset + remainder_ielen;

+}

+

+

+u8 chk_sta_is_alive(struct sta_info *psta);

+u8 chk_sta_is_alive(struct sta_info *psta)

+{

+	u8 ret = _FALSE;

+	#ifdef DBG_EXPIRATION_CHK

+	DBG_871X("sta:"MAC_FMT", rssi:%d, rx:"STA_PKTS_FMT", expire_to:%u, %s%ssq_len:%u\n"

+		, MAC_ARG(psta->hwaddr)

+		, psta->rssi_stat.UndecoratedSmoothedPWDB

+		//, STA_RX_PKTS_ARG(psta)

+		, STA_RX_PKTS_DIFF_ARG(psta)

+		, psta->expire_to

+		, psta->state&WIFI_SLEEP_STATE?"PS, ":""

+		, psta->state&WIFI_STA_ALIVE_CHK_STATE?"SAC, ":""

+		, psta->sleepq_len

+	);

+	#endif

+

+	//if(sta_last_rx_pkts(psta) == sta_rx_pkts(psta))

+	if((psta->sta_stats.last_rx_data_pkts + psta->sta_stats.last_rx_ctrl_pkts) == (psta->sta_stats.rx_data_pkts + psta->sta_stats.rx_ctrl_pkts))

+	{

+		#if 0

+		if(psta->state&WIFI_SLEEP_STATE)

+			ret = _TRUE;

+		#endif

+	}

+	else

+	{

+		ret = _TRUE;

+	}

+

+	sta_update_last_rx_pkts(psta);

+

+	return ret;

+}

+

+void	expire_timeout_chk(_adapter *padapter)

+{

+	_irqL irqL;

+	_list	*phead, *plist;

+	u8 updated;

+	struct sta_info *psta=NULL;	

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	u8 chk_alive_num = 0;

+	char chk_alive_list[NUM_STA];

+	int i;

+

+	_enter_critical_bh(&pstapriv->auth_list_lock, &irqL);

+	

+	phead = &pstapriv->auth_list;

+	plist = get_next(phead);

+

+	//check auth_queue

+	#ifdef DBG_EXPIRATION_CHK

+	if (rtw_end_of_queue_search(phead, plist) == _FALSE) {

+		DBG_871X(FUNC_NDEV_FMT" auth_list, cnt:%u\n"

+			, FUNC_NDEV_ARG(padapter->pnetdev), pstapriv->auth_list_cnt);

+	}

+	#endif

+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)	

+	{

+		psta = LIST_CONTAINOR(plist, struct sta_info, auth_list);

+		plist = get_next(plist);

+	

+		if(psta->expire_to>0)

+		{

+			psta->expire_to--;

+			if (psta->expire_to == 0)

+			{				

+				rtw_list_delete(&psta->auth_list);

+				pstapriv->auth_list_cnt--;

+				

+				DBG_871X("auth expire %02X%02X%02X%02X%02X%02X\n",

+					psta->hwaddr[0],psta->hwaddr[1],psta->hwaddr[2],psta->hwaddr[3],psta->hwaddr[4],psta->hwaddr[5]);

+				

+				_exit_critical_bh(&pstapriv->auth_list_lock, &irqL);

+				

+				_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);	

+				rtw_free_stainfo(padapter, psta);

+				_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);	

+				

+				_enter_critical_bh(&pstapriv->auth_list_lock, &irqL);

+			}	

+		}	

+		

+	}

+

+	_exit_critical_bh(&pstapriv->auth_list_lock, &irqL);

+

+	psta = NULL;

+	

+

+	_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+	

+	phead = &pstapriv->asoc_list;

+	plist = get_next(phead);

+

+	//check asoc_queue

+	#ifdef DBG_EXPIRATION_CHK

+	if (rtw_end_of_queue_search(phead, plist) == _FALSE) {

+		DBG_871X(FUNC_NDEV_FMT" asoc_list, cnt:%u\n"

+			, FUNC_NDEV_ARG(padapter->pnetdev), pstapriv->asoc_list_cnt);

+	}

+	#endif

+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)

+	{

+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);

+		plist = get_next(plist);

+	

+		if (chk_sta_is_alive(psta) || !psta->expire_to) {

+			psta->expire_to = pstapriv->expire_to;

+			psta->keep_alive_trycnt = 0;

+			#ifdef CONFIG_TX_MCAST2UNI

+			psta->under_exist_checking = 0;

+			#endif	// CONFIG_TX_MCAST2UNI

+		} else {

+			psta->expire_to--;

+		}

+

+#ifndef CONFIG_ACTIVE_KEEP_ALIVE_CHECK

+#ifdef CONFIG_TX_MCAST2UNI

+#ifdef CONFIG_80211N_HT

+		if ( (psta->flags & WLAN_STA_HT) && (psta->htpriv.agg_enable_bitmap || psta->under_exist_checking) ) {

+			// check sta by delba(addba) for 11n STA 

+			// ToDo: use CCX report to check for all STAs

+			//DBG_871X("asoc check by DELBA/ADDBA! (pstapriv->expire_to=%d s)(psta->expire_to=%d s), [%02x, %d]\n", pstapriv->expire_to*2, psta->expire_to*2, psta->htpriv.agg_enable_bitmap, psta->under_exist_checking);

+			

+				if ( psta->expire_to <= (pstapriv->expire_to - 50 ) ) {

+				DBG_871X("asoc expire by DELBA/ADDBA! (%d s)\n", (pstapriv->expire_to-psta->expire_to)*2);

+				psta->under_exist_checking = 0;

+				psta->expire_to = 0;

+			} else if ( psta->expire_to <= (pstapriv->expire_to - 3) && (psta->under_exist_checking==0)) {

+				DBG_871X("asoc check by DELBA/ADDBA! (%d s)\n", (pstapriv->expire_to-psta->expire_to)*2);

+				psta->under_exist_checking = 1;

+				//tear down TX AMPDU

+				send_delba(padapter, 1, psta->hwaddr);// // originator

+				psta->htpriv.agg_enable_bitmap = 0x0;//reset

+				psta->htpriv.candidate_tid_bitmap = 0x0;//reset

+			}

+		}

+#endif //CONFIG_80211N_HT

+#endif // CONFIG_TX_MCAST2UNI

+#endif //CONFIG_ACTIVE_KEEP_ALIVE_CHECK

+

+		if (psta->expire_to <= 0)

+		{

+			#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK

+			struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;

+

+			if (padapter->registrypriv.wifi_spec == 1)

+			{

+				psta->expire_to = pstapriv->expire_to;

+				continue;

+			}

+

+			if (psta->state & WIFI_SLEEP_STATE) {

+				if (!(psta->state & WIFI_STA_ALIVE_CHK_STATE)) {

+					//to check if alive by another methods if staion is at ps mode.					

+					psta->expire_to = pstapriv->expire_to;

+					psta->state |= WIFI_STA_ALIVE_CHK_STATE;

+

+					//DBG_871X("alive chk, sta:" MAC_FMT " is at ps mode!\n", MAC_ARG(psta->hwaddr));

+

+					//to update bcn with tim_bitmap for this station

+					pstapriv->tim_bitmap |= BIT(psta->aid);

+					update_beacon(padapter, _TIM_IE_, NULL, _FALSE);

+

+					if(!pmlmeext->active_keep_alive_check)

+						continue;

+				}

+			}

+

+			if (pmlmeext->active_keep_alive_check) {

+				int stainfo_offset;

+

+				stainfo_offset = rtw_stainfo_offset(pstapriv, psta);

+				if (stainfo_offset_valid(stainfo_offset)) {

+					chk_alive_list[chk_alive_num++] = stainfo_offset;

+				}

+

+				continue;

+			}

+			#endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */

+

+			rtw_list_delete(&psta->asoc_list);

+			pstapriv->asoc_list_cnt--;

+

+			DBG_871X("asoc expire "MAC_FMT", state=0x%x\n", MAC_ARG(psta->hwaddr), psta->state);

+			updated = ap_free_sta(padapter, psta, _FALSE, WLAN_REASON_DEAUTH_LEAVING);

+		}	

+		else

+		{

+			/* TODO: Aging mechanism to digest frames in sleep_q to avoid running out of xmitframe */

+			if (psta->sleepq_len > (NR_XMITFRAME/pstapriv->asoc_list_cnt)

+				&& padapter->xmitpriv.free_xmitframe_cnt < ((NR_XMITFRAME/pstapriv->asoc_list_cnt)/2)

+			){

+				DBG_871X("%s sta:"MAC_FMT", sleepq_len:%u, free_xmitframe_cnt:%u, asoc_list_cnt:%u, clear sleep_q\n", __func__

+					, MAC_ARG(psta->hwaddr)

+					, psta->sleepq_len, padapter->xmitpriv.free_xmitframe_cnt, pstapriv->asoc_list_cnt);

+				wakeup_sta_to_xmit(padapter, psta);

+			}

+		}

+	}

+

+	_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+

+#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK

+if (chk_alive_num) {

+

+	u8 backup_oper_channel=0;

+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;

+	/* switch to correct channel of current network  before issue keep-alive frames */

+	if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {

+		backup_oper_channel = rtw_get_oper_ch(padapter);

+		SelectChannel(padapter, pmlmeext->cur_channel);

+	}

+

+	/* issue null data to check sta alive*/

+	for (i = 0; i < chk_alive_num; i++) {

+		

+		int ret = _FAIL;

+

+		psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);

+		if(!(psta->state &_FW_LINKED))

+			continue;		

+	

+		if (psta->state & WIFI_SLEEP_STATE)

+			ret = issue_nulldata(padapter, psta->hwaddr, 0, 1, 50);

+		else

+			ret = issue_nulldata(padapter, psta->hwaddr, 0, 3, 50);

+

+		psta->keep_alive_trycnt++;

+		if (ret == _SUCCESS)

+		{

+			DBG_871X("asoc check, sta(" MAC_FMT ") is alive\n", MAC_ARG(psta->hwaddr));

+			psta->expire_to = pstapriv->expire_to;

+			psta->keep_alive_trycnt = 0;

+			continue;

+		}

+		else if (psta->keep_alive_trycnt <= 3)

+		{

+			DBG_871X("ack check for asoc expire, keep_alive_trycnt=%d\n", psta->keep_alive_trycnt);

+			psta->expire_to = 1;

+			continue;

+		}

+

+		psta->keep_alive_trycnt = 0;

+

+		DBG_871X("asoc expire "MAC_FMT", state=0x%x\n", MAC_ARG(psta->hwaddr), psta->state);

+		_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+		if (rtw_is_list_empty(&psta->asoc_list)==_FALSE) {

+			rtw_list_delete(&psta->asoc_list);

+			pstapriv->asoc_list_cnt--;

+			updated = ap_free_sta(padapter, psta, _FALSE, WLAN_REASON_DEAUTH_LEAVING);

+		}

+		_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+

+	}

+

+	if (backup_oper_channel>0) /* back to the original operation channel */

+		SelectChannel(padapter, backup_oper_channel);

+}

+#endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */

+

+	associated_clients_update(padapter, updated);

+}

+

+void add_RATid(_adapter *padapter, struct sta_info *psta, u8 rssi_level)

+{	

+	int i;

+	u8 rf_type;

+	u32 init_rate=0;

+	unsigned char sta_band = 0, raid, shortGIrate = _FALSE;

+	unsigned char limit;	

+	unsigned int tx_ra_bitmap=0;

+	struct ht_priv	*psta_ht = NULL;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	WLAN_BSSID_EX *pcur_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;	

+

+#ifdef CONFIG_80211N_HT

+	if(psta)

+		psta_ht = &psta->htpriv;

+	else

+		return;

+#endif //CONFIG_80211N_HT

+

+	if(!(psta->state & _FW_LINKED))

+		return;

+	

+	//b/g mode ra_bitmap  

+	for (i=0; i<sizeof(psta->bssrateset); i++)

+	{

+		if (psta->bssrateset[i])

+			tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value(psta->bssrateset[i]&0x7f);

+	}

+#ifdef CONFIG_80211N_HT

+	//n mode ra_bitmap

+	if(psta_ht->ht_option) 

+	{

+		rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));

+		if(rf_type == RF_2T2R)

+			limit=16;// 2R

+		else

+			limit=8;//  1R

+

+		for (i=0; i<limit; i++) {

+			if (psta_ht->ht_cap.supp_mcs_set[i/8] & BIT(i%8))

+				tx_ra_bitmap |= BIT(i+12);

+		}

+

+		//max short GI rate

+		shortGIrate = psta_ht->sgi;

+	}

+#endif //CONFIG_80211N_HT

+

+#if 0//gtest

+	if(get_rf_mimo_mode(padapter) == RTL8712_RF_2T2R)

+	{

+		//is this a 2r STA?

+		if((pstat->tx_ra_bitmap & 0x0ff00000) != 0 && !(priv->pshare->has_2r_sta & BIT(pstat->aid)))

+		{

+			priv->pshare->has_2r_sta |= BIT(pstat->aid);

+			if(rtw_read16(padapter, 0x102501f6) != 0xffff)

+			{

+				rtw_write16(padapter, 0x102501f6, 0xffff);

+				reset_1r_sta_RA(priv, 0xffff);

+				Switch_1SS_Antenna(priv, 3);

+			}

+		}

+		else// bg or 1R STA? 

+		{ 

+			if((priv->pmib->dot11BssType.net_work_type & WIRELESS_11N) && pstat->ht_cap_len && priv->pshare->has_2r_sta == 0)

+			{

+				if(rtw_read16(padapter, 0x102501f6) != 0x7777)

+				{ // MCS7 SGI

+					rtw_write16(padapter, 0x102501f6,0x7777);

+					reset_1r_sta_RA(priv, 0x7777);

+					Switch_1SS_Antenna(priv, 2);

+				}

+			}

+		}

+		

+	}

+

+	if ((pstat->rssi_level < 1) || (pstat->rssi_level > 3)) 

+	{

+		if (pstat->rssi >= priv->pshare->rf_ft_var.raGoDownUpper)

+			pstat->rssi_level = 1;

+		else if ((pstat->rssi >= priv->pshare->rf_ft_var.raGoDown20MLower) ||

+			((priv->pshare->is_40m_bw) && (pstat->ht_cap_len) &&

+			(pstat->rssi >= priv->pshare->rf_ft_var.raGoDown40MLower) &&

+			(pstat->ht_cap_buf.ht_cap_info & cpu_to_le16(_HTCAP_SUPPORT_CH_WDTH_))))

+			pstat->rssi_level = 2;

+		else

+			pstat->rssi_level = 3;

+	}

+

+	// rate adaptive by rssi

+	if ((priv->pmib->dot11BssType.net_work_type & WIRELESS_11N) && pstat->ht_cap_len)

+	{

+		if ((get_rf_mimo_mode(priv) == MIMO_1T2R) || (get_rf_mimo_mode(priv) == MIMO_1T1R))

+		{

+			switch (pstat->rssi_level) {

+				case 1:

+					pstat->tx_ra_bitmap &= 0x100f0000;

+					break;

+				case 2:

+					pstat->tx_ra_bitmap &= 0x100ff000;

+					break;

+				case 3:

+					if (priv->pshare->is_40m_bw)

+						pstat->tx_ra_bitmap &= 0x100ff005;

+					else

+						pstat->tx_ra_bitmap &= 0x100ff001;

+

+					break;

+			}

+		}

+		else 

+		{

+			switch (pstat->rssi_level) {

+				case 1:

+					pstat->tx_ra_bitmap &= 0x1f0f0000;

+					break;

+				case 2:

+					pstat->tx_ra_bitmap &= 0x1f0ff000;

+					break;

+				case 3:

+					if (priv->pshare->is_40m_bw)

+						pstat->tx_ra_bitmap &= 0x000ff005;

+					else

+						pstat->tx_ra_bitmap &= 0x000ff001;

+

+					break;

+			}

+

+			// Don't need to mask high rates due to new rate adaptive parameters

+			//if (pstat->is_broadcom_sta)		// use MCS12 as the highest rate vs. Broadcom sta

+			//	pstat->tx_ra_bitmap &= 0x81ffffff;

+

+			// NIC driver will report not supporting MCS15 and MCS14 in asoc req

+			//if (pstat->is_rtl8190_sta && !pstat->is_2t_mimo_sta)

+			//	pstat->tx_ra_bitmap &= 0x83ffffff;		// if Realtek 1x2 sta, don't use MCS15 and MCS14

+		}

+	}

+	else if ((priv->pmib->dot11BssType.net_work_type & WIRELESS_11G) && isErpSta(pstat))

+	{

+		switch (pstat->rssi_level) {

+			case 1:

+				pstat->tx_ra_bitmap &= 0x00000f00;

+				break;

+			case 2:

+				pstat->tx_ra_bitmap &= 0x00000ff0;

+				break;

+			case 3:

+				pstat->tx_ra_bitmap &= 0x00000ff5;

+				break;

+		}

+	}

+	else 

+	{

+		pstat->tx_ra_bitmap &= 0x0000000d;

+	}

+

+	// disable tx short GI when station cannot rx MCS15(AP is 2T2R)

+	// disable tx short GI when station cannot rx MCS7 (AP is 1T2R or 1T1R)

+	// if there is only 1r STA and we are 2T2R, DO NOT mask SGI rate

+	if ((!(pstat->tx_ra_bitmap & 0x8000000) && (priv->pshare->has_2r_sta > 0) && (get_rf_mimo_mode(padapter) == RTL8712_RF_2T2R)) ||

+		 (!(pstat->tx_ra_bitmap & 0x80000) && (get_rf_mimo_mode(padapter) != RTL8712_RF_2T2R)))

+	{

+		pstat->tx_ra_bitmap &= ~BIT(28);	

+	}

+#endif

+

+	if ( pcur_network->Configuration.DSConfig > 14 ) {

+		// 5G band

+		if (tx_ra_bitmap & 0xffff000)

+			sta_band |= WIRELESS_11_5N | WIRELESS_11A;

+		else

+			sta_band |= WIRELESS_11A;

+	} else {

+		if (tx_ra_bitmap & 0xffff000)

+			sta_band |= WIRELESS_11_24N | WIRELESS_11G | WIRELESS_11B;

+		else if (tx_ra_bitmap & 0xff0)

+			sta_band |= WIRELESS_11G |WIRELESS_11B;

+		else

+			sta_band |= WIRELESS_11B;

+	}

+

+	psta->wireless_mode = sta_band;

+

+	raid = networktype_to_raid(sta_band);	

+	init_rate = get_highest_rate_idx(tx_ra_bitmap&0x0fffffff)&0x3f;

+	

+	if (psta->aid < NUM_STA) 

+	{

+		u8 arg = 0;

+

+		arg = psta->mac_id&0x1f;

+		

+		arg |= BIT(7);//support entry 2~31

+		

+		if (shortGIrate==_TRUE)

+			arg |= BIT(5);

+

+		tx_ra_bitmap |= ((raid<<28)&0xf0000000);

+

+		DBG_871X("%s=> mac_id:%d , raid:%d , bitmap=0x%x, arg=0x%x\n", 

+			__FUNCTION__ , psta->mac_id, raid ,tx_ra_bitmap, arg);

+

+		//bitmap[0:27] = tx_rate_bitmap

+		//bitmap[28:31]= Rate Adaptive id

+		//arg[0:4] = macid

+		//arg[5] = Short GI

+		rtw_hal_add_ra_tid(padapter, tx_ra_bitmap, arg, rssi_level);

+		

+

+		if (shortGIrate==_TRUE)

+			init_rate |= BIT(6);

+		

+		//set ra_id, init_rate

+		psta->raid = raid;

+		psta->init_rate = init_rate;

+		

+	}

+	else 

+	{

+		DBG_871X("station aid %d exceed the max number\n", psta->aid);

+	}

+

+}

+

+static void update_bmc_sta(_adapter *padapter)

+{

+	_irqL	irqL;

+	u32 init_rate=0;

+	unsigned char	network_type, raid;

+	int i, supportRateNum = 0;	

+	unsigned int tx_ra_bitmap=0;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	WLAN_BSSID_EX *pcur_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;	

+	struct sta_info *psta = rtw_get_bcmc_stainfo(padapter);

+

+	if(psta)

+	{

+		psta->aid = 0;//default set to 0

+		//psta->mac_id = psta->aid+4;	

+		psta->mac_id = psta->aid + 1;

+

+		psta->qos_option = 0;

+#ifdef CONFIG_80211N_HT	

+		psta->htpriv.ht_option = _FALSE;

+#endif //CONFIG_80211N_HT

+

+		psta->ieee8021x_blocked = 0;

+

+		_rtw_memset((void*)&psta->sta_stats, 0, sizeof(struct stainfo_stats));

+

+		//psta->dot118021XPrivacy = _NO_PRIVACY_;//!!! remove it, because it has been set before this.

+

+

+

+		//prepare for add_RATid		

+		supportRateNum = rtw_get_rateset_len((u8*)&pcur_network->SupportedRates);

+		network_type = rtw_check_network_type((u8*)&pcur_network->SupportedRates, supportRateNum, 1);

+		

+		_rtw_memcpy(psta->bssrateset, &pcur_network->SupportedRates, supportRateNum);

+		psta->bssratelen = supportRateNum;

+

+		//b/g mode ra_bitmap  

+		for (i=0; i<supportRateNum; i++)

+		{	

+			if (psta->bssrateset[i])

+				tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value(psta->bssrateset[i]&0x7f);

+		}

+

+		if ( pcur_network->Configuration.DSConfig > 14 ) {

+			//force to A mode. 5G doesn't support CCK rates

+			network_type = WIRELESS_11A;

+			tx_ra_bitmap = 0x150; // 6, 12, 24 Mbps		

+		} else {

+			//force to b mode 

+			network_type = WIRELESS_11B;

+			tx_ra_bitmap = 0xf;		

+		}

+

+		//tx_ra_bitmap = update_basic_rate(pcur_network->SupportedRates, supportRateNum);

+

+		raid = networktype_to_raid(network_type);

+		init_rate = get_highest_rate_idx(tx_ra_bitmap&0x0fffffff)&0x3f;

+				

+		//DBG_871X("Add id %d val %08x to ratr for bmc sta\n", psta->aid, tx_ra_bitmap);

+		//ap mode

+		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, _TRUE);

+

+		//if(pHalData->fw_ractrl == _TRUE)

+		{

+			u8 arg = 0;

+

+			arg = psta->mac_id&0x1f;

+		

+			arg |= BIT(7);

+		

+			//if (shortGIrate==_TRUE)

+			//	arg |= BIT(5);

+			

+			tx_ra_bitmap |= ((raid<<28)&0xf0000000);			

+

+			DBG_871X("update_bmc_sta, mask=0x%x, arg=0x%x\n", tx_ra_bitmap, arg);

+

+			//bitmap[0:27] = tx_rate_bitmap

+			//bitmap[28:31]= Rate Adaptive id

+			//arg[0:4] = macid

+			//arg[5] = Short GI

+			rtw_hal_add_ra_tid(padapter, tx_ra_bitmap, arg, 0);			

+		

+		}

+

+		//set ra_id, init_rate

+		psta->raid = raid;

+		psta->init_rate = init_rate;

+

+		rtw_stassoc_hw_rpt(padapter, psta);

+

+		_enter_critical_bh(&psta->lock, &irqL);

+		psta->state = _FW_LINKED;

+		_exit_critical_bh(&psta->lock, &irqL);

+

+	}

+	else

+	{

+		DBG_871X("add_RATid_bmc_sta error!\n");

+	}

+		

+}

+

+//notes:

+//AID: 1~MAX for sta and 0 for bc/mc in ap/adhoc mode 

+//MAC_ID = AID+1 for sta in ap/adhoc mode 

+//MAC_ID = 1 for bc/mc for sta/ap/adhoc

+//MAC_ID = 0 for bssid for sta/ap/adhoc

+//CAM_ID = //0~3 for default key, cmd_id=macid + 3, macid=aid+1;

+

+void update_sta_info_apmode(_adapter *padapter, struct sta_info *psta)

+{	

+	_irqL	irqL;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct security_priv *psecuritypriv = &padapter->securitypriv;

+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);

+#ifdef CONFIG_80211N_HT

+	struct ht_priv	*phtpriv_ap = &pmlmepriv->htpriv;

+	struct ht_priv	*phtpriv_sta = &psta->htpriv;

+#endif //CONFIG_80211N_HT

+	//set intf_tag to if1

+	//psta->intf_tag = 0;

+

+	//psta->mac_id = psta->aid+4;

+	psta->mac_id = psta->aid+1; 

+	DBG_871X("%s\n",__FUNCTION__);	

+

+	//ap mode

+	rtw_hal_set_odm_var(padapter,HAL_ODM_STA_INFO,psta,_TRUE);

+	

+	if(psecuritypriv->dot11AuthAlgrthm==dot11AuthAlgrthm_8021X)

+		psta->ieee8021x_blocked = _TRUE;

+	else

+		psta->ieee8021x_blocked = _FALSE;

+	

+

+	//update sta's cap

+	

+	//ERP

+	VCS_update(padapter, psta);

+#ifdef CONFIG_80211N_HT	

+	//HT related cap

+	if(phtpriv_sta->ht_option)

+	{

+		//check if sta supports rx ampdu

+		phtpriv_sta->ampdu_enable = phtpriv_ap->ampdu_enable;

+

+		//check if sta support s Short GI

+		if((phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_20|IEEE80211_HT_CAP_SGI_40))

+		{

+			phtpriv_sta->sgi = _TRUE;

+		}

+

+		// bwmode

+		if((phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH))

+		{

+			//phtpriv_sta->bwmode = HT_CHANNEL_WIDTH_40;

+			phtpriv_sta->bwmode = pmlmeext->cur_bwmode;

+			phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;

+			

+		}		

+

+		psta->qos_option = _TRUE;

+		

+	}

+	else

+	{

+		phtpriv_sta->ampdu_enable = _FALSE;

+		

+		phtpriv_sta->sgi = _FALSE;

+		phtpriv_sta->bwmode = HT_CHANNEL_WIDTH_20;

+		phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;

+	}

+

+	//Rx AMPDU

+	send_delba(padapter, 0, psta->hwaddr);// recipient

+	

+	//TX AMPDU

+	send_delba(padapter, 1, psta->hwaddr);// // originator

+	phtpriv_sta->agg_enable_bitmap = 0x0;//reset

+	phtpriv_sta->candidate_tid_bitmap = 0x0;//reset

+#endif //CONFIG_80211N_HT

+

+	//todo: init other variables

+	

+	_rtw_memset((void*)&psta->sta_stats, 0, sizeof(struct stainfo_stats));

+

+

+	//add ratid

+	//add_RATid(padapter, psta);//move to ap_sta_info_defer_update()

+

+

+	_enter_critical_bh(&psta->lock, &irqL);

+	psta->state |= _FW_LINKED;

+	_exit_critical_bh(&psta->lock, &irqL);

+	

+

+}

+

+static void update_hw_ht_param(_adapter *padapter)

+{

+	unsigned char		max_AMPDU_len;

+	unsigned char		min_MPDU_spacing;

+	struct registry_priv	 *pregpriv = &padapter->registrypriv;

+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+	

+	DBG_871X("%s\n", __FUNCTION__);

+	

+

+	//handle A-MPDU parameter field

+	/* 	

+		AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k

+		AMPDU_para [4:2]:Min MPDU Start Spacing	

+	*/

+	max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;	

+	

+	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;	

+

+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));

+

+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));

+

+	//

+	// Config SM Power Save setting

+	//

+	pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2;

+	if(pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC)

+	{

+		/*u8 i;

+		//update the MCS rates

+		for (i = 0; i < 16; i++)

+		{

+			pmlmeinfo->HT_caps.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i];

+		}*/

+		DBG_871X("%s(): WLAN_HT_CAP_SM_PS_STATIC\n",__FUNCTION__);

+	}

+

+	//

+	// Config current HT Protection mode.

+	//

+	//pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3;

+

+}

+

+static void start_bss_network(_adapter *padapter, u8 *pbuf)

+{

+	u8 *p;

+	u8 val8, cur_channel, cur_bwmode, cur_ch_offset;

+	u16 bcn_interval;

+	u32	acparm;	

+	int	ie_len;	

+	struct registry_priv	 *pregpriv = &padapter->registrypriv;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct security_priv* psecuritypriv=&(padapter->securitypriv);	

+	WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;

+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+	WLAN_BSSID_EX *pnetwork_mlmeext = &(pmlmeinfo->network);

+	struct HT_info_element *pht_info=NULL;

+#ifdef CONFIG_P2P

+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);

+#endif //CONFIG_P2P

+	

+	//DBG_871X("%s\n", __FUNCTION__);

+

+	bcn_interval = (u16)pnetwork->Configuration.BeaconPeriod;	

+	cur_channel = pnetwork->Configuration.DSConfig;

+	cur_bwmode = HT_CHANNEL_WIDTH_20;;

+	cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;

+	

+

+	//check if there is wps ie, 

+	//if there is wpsie in beacon, the hostapd will update beacon twice when stating hostapd,

+	//and at first time the security ie ( RSN/WPA IE) will not include in beacon.

+	if(NULL == rtw_get_wps_ie(pnetwork->IEs+_FIXED_IE_LENGTH_, pnetwork->IELength-_FIXED_IE_LENGTH_, NULL, NULL))

+	{

+		pmlmeext->bstart_bss = _TRUE;

+	}

+

+	//todo: update wmm, ht cap

+	//pmlmeinfo->WMM_enable;

+	//pmlmeinfo->HT_enable;

+	if(pmlmepriv->qospriv.qos_option)

+		pmlmeinfo->WMM_enable = _TRUE;

+#ifdef CONFIG_80211N_HT

+	if(pmlmepriv->htpriv.ht_option)

+	{

+		pmlmeinfo->WMM_enable = _TRUE;

+		pmlmeinfo->HT_enable = _TRUE;

+		//pmlmeinfo->HT_info_enable = _TRUE;

+		//pmlmeinfo->HT_caps_enable = _TRUE;

+

+		update_hw_ht_param(padapter);

+	}

+#endif //#CONFIG_80211N_HT

+	

+

+	if(pmlmepriv->cur_network.join_res != _TRUE) //setting only at  first time

+	{		

+		//WEP Key will be set before this function, do not clear CAM.

+		if ((psecuritypriv->dot11PrivacyAlgrthm != _WEP40_) && (psecuritypriv->dot11PrivacyAlgrthm != _WEP104_))

+			flush_all_cam_entry(padapter);	//clear CAM

+	}	

+

+	//set MSR to AP_Mode		

+	Set_MSR(padapter, _HW_STATE_AP_);	

+		

+	//Set BSSID REG

+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pnetwork->MacAddress);

+

+	//Set EDCA param reg

+#ifdef CONFIG_CONCURRENT_MODE

+	acparm = 0x005ea42b;

+#else

+	acparm = 0x002F3217; // VO

+#endif

+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));

+	acparm = 0x005E4317; // VI

+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));

+	//acparm = 0x00105320; // BE

+	acparm = 0x005ea42b;

+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));

+	acparm = 0x0000A444; // BK

+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));

+

+	//Set Security

+	val8 = (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf;

+	rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));

+

+	//Beacon Control related register

+	rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&bcn_interval));

+

+	

+	UpdateBrateTbl(padapter, pnetwork->SupportedRates);

+	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);

+

+	if(pmlmepriv->cur_network.join_res != _TRUE) //setting only at  first time

+	{

+		//u32 initialgain;

+

+		//initialgain = 0x1e;

+

+

+		//disable dynamic functions, such as high power, DIG

+		//Save_DM_Func_Flag(padapter);

+		//Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);

+		

+#ifdef CONFIG_CONCURRENT_MODE	

+		if(padapter->adapter_type > PRIMARY_ADAPTER)

+		{

+			if(rtw_buddy_adapter_up(padapter))

+			{

+				_adapter *pbuddy_adapter = padapter->pbuddy_adapter;

+		

+				//turn on all dynamic functions on PRIMARY_ADAPTER, dynamic functions only runs at PRIMARY_ADAPTER

+				Switch_DM_Func(pbuddy_adapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE);

+	

+				//rtw_hal_set_hwreg(pbuddy_adapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));

+			}

+		}

+		else

+#endif

+		{

+			//turn on all dynamic functions	

+			Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE);

+

+			//rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));

+		}	

+	

+	}

+#ifdef CONFIG_80211N_HT

+	//set channel, bwmode	

+	p = rtw_get_ie((pnetwork->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - sizeof(NDIS_802_11_FIXED_IEs)));

+	if( p && ie_len)

+	{

+		pht_info = (struct HT_info_element *)(p+2);

+					

+		if ((pregpriv->cbw40_enable) &&	 (pht_info->infos[0] & BIT(2)))

+		{

+			//switch to the 40M Hz mode

+			//pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;

+			cur_bwmode = HT_CHANNEL_WIDTH_40;

+			switch (pht_info->infos[0] & 0x3)

+			{

+				case 1:

+					//pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;

+					cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;

+					break;

+			

+				case 3:

+					//pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;

+					cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;					

+					break;

+				

+				default:

+					//pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;

+					cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;

+					break;

+			}		

+						

+		}

+					

+	}

+#endif //CONFIG_80211N_HT

+#ifdef CONFIG_DUALMAC_CONCURRENT

+	dc_set_ap_channel_bandwidth(padapter, cur_channel, cur_ch_offset, cur_bwmode);

+#else

+	//TODO: need to judge the phy parameters on concurrent mode for single phy

+	//set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);

+#ifdef CONFIG_CONCURRENT_MODE

+	if(!check_buddy_fwstate(padapter, _FW_LINKED|_FW_UNDER_LINKING|_FW_UNDER_SURVEY))

+	{

+	set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);

+	}

+	else if(check_buddy_fwstate(padapter, _FW_LINKED)==_TRUE)//only second adapter can enter AP Mode

+	{

+		_adapter *pbuddy_adapter = padapter->pbuddy_adapter;		

+		struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;

+	

+		//To sync cur_channel/cur_bwmode/cur_ch_offset with primary adapter

+		DBG_871X("primary iface is at linked state, sync cur_channel/cur_bwmode/cur_ch_offset\n");

+		DBG_871X("primary adapter, CH=%d, BW=%d, offset=%d\n", pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);

+		DBG_871X("second adapter, CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);

+		

+		cur_channel = pbuddy_mlmeext->cur_channel;

+		if(cur_bwmode == HT_CHANNEL_WIDTH_40)

+		{

+			if(pht_info)

+				pht_info->infos[0] &= ~(BIT(0)|BIT(1));

+			

+			if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)

+			{

+				cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;

+

+				//to update cur_ch_offset value in beacon

+				if(pht_info)

+				{				

+					switch(cur_ch_offset)

+					{

+						case HAL_PRIME_CHNL_OFFSET_LOWER:

+							pht_info->infos[0] |= 0x1;

+							break;

+						case HAL_PRIME_CHNL_OFFSET_UPPER:

+							pht_info->infos[0] |= 0x3;

+							break;

+						case HAL_PRIME_CHNL_OFFSET_DONT_CARE:							

+						default:							

+							break;					

+					}

+				}		

+				

+			}

+			else if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_20)

+			{

+				cur_bwmode = HT_CHANNEL_WIDTH_20;

+				cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;

+			

+				if(cur_channel>0 && cur_channel<5)

+				{

+					if(pht_info)

+						pht_info->infos[0] |= 0x1;		

+

+					cur_bwmode = HT_CHANNEL_WIDTH_40;

+					cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;

+				}

+

+				if(cur_channel>7 && cur_channel<(14+1))

+				{

+					if(pht_info)

+						pht_info->infos[0] |= 0x3;

+						

+					cur_bwmode = HT_CHANNEL_WIDTH_40;

+					cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;						

+				}					

+			

+				set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);

+			}

+			

+		}

+

+		// to update channel value in beacon

+		pnetwork->Configuration.DSConfig = cur_channel;		

+		p = rtw_get_ie((pnetwork->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (pnetwork->IELength - sizeof(NDIS_802_11_FIXED_IEs)));

+		if(p && ie_len>0)

+			*(p + 2) = cur_channel;

+		

+		if(pht_info)

+			pht_info->primary_channel = cur_channel;

+	}

+#else

+	set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);

+#endif //CONFIG_CONCURRENT_MODE

+

+	DBG_871X("CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);

+

+	//

+	pmlmeext->cur_channel = cur_channel;	

+	pmlmeext->cur_bwmode = cur_bwmode;

+	pmlmeext->cur_ch_offset = cur_ch_offset;	

+#endif //CONFIG_DUALMAC_CONCURRENT

+	pmlmeext->cur_wireless_mode = pmlmepriv->cur_network.network_type;

+

+	//update cur_wireless_mode

+	update_wireless_mode(padapter);

+

+	//udpate capability after cur_wireless_mode updated

+	update_capinfo(padapter, rtw_get_capability((WLAN_BSSID_EX *)pnetwork));

+	

+	//let pnetwork_mlmeext == pnetwork_mlme.

+	_rtw_memcpy(pnetwork_mlmeext, pnetwork, pnetwork->Length);

+

+#ifdef CONFIG_P2P

+	_rtw_memcpy(pwdinfo->p2p_group_ssid, pnetwork->Ssid.Ssid, pnetwork->Ssid.SsidLength);	

+	pwdinfo->p2p_group_ssid_len = pnetwork->Ssid.SsidLength;

+#endif //CONFIG_P2P

+

+	if(_TRUE == pmlmeext->bstart_bss)

+	{

+		update_beacon(padapter, _TIM_IE_, NULL, _FALSE);

+

+#ifndef CONFIG_INTERRUPT_BASED_TXBCN //other case will  tx beacon when bcn interrupt coming in.

+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)

+		//issue beacon frame

+		if(send_beacon(padapter)==_FAIL)

+		{

+			DBG_871X("issue_beacon, fail!\n");

+		}

+#endif 

+#endif //!CONFIG_INTERRUPT_BASED_TXBCN

+		

+	}

+

+

+	//update bc/mc sta_info

+	update_bmc_sta(padapter);

+	

+	//pmlmeext->bstart_bss = _TRUE;

+	

+}

+

+int rtw_check_beacon_data(_adapter *padapter, u8 *pbuf,  int len)

+{

+	int ret=_SUCCESS;

+	u8 *p;

+	u8 *pHT_caps_ie=NULL;

+	u8 *pHT_info_ie=NULL;

+	struct sta_info *psta = NULL;

+	u16 cap, ht_cap=_FALSE;

+	uint ie_len = 0;

+	int group_cipher, pairwise_cipher;	

+	u8	channel, network_type, supportRate[NDIS_802_11_LENGTH_RATES_EX];

+	int supportRateNum = 0;

+	u8 OUI1[] = {0x00, 0x50, 0xf2,0x01};

+	u8 wps_oui[4]={0x0,0x50,0xf2,0x04};

+	u8 WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};	

+	struct registry_priv *pregistrypriv = &padapter->registrypriv;	

+	struct security_priv *psecuritypriv = &padapter->securitypriv;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	WLAN_BSSID_EX *pbss_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;	

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	u8 *ie = pbss_network->IEs;

+	

+

+	/* SSID */

+	/* Supported rates */

+	/* DS Params */

+	/* WLAN_EID_COUNTRY */

+	/* ERP Information element */

+	/* Extended supported rates */

+	/* WPA/WPA2 */

+	/* Wi-Fi Wireless Multimedia Extensions */

+	/* ht_capab, ht_oper */

+	/* WPS IE */

+

+	DBG_871X("%s, len=%d\n", __FUNCTION__, len);

+

+	if(check_fwstate(pmlmepriv, WIFI_AP_STATE) != _TRUE)

+		return _FAIL;

+

+

+	if(len>MAX_IE_SZ)

+		return _FAIL;

+	

+	pbss_network->IELength = len;

+

+	_rtw_memset(ie, 0, MAX_IE_SZ);

+	

+	_rtw_memcpy(ie, pbuf, pbss_network->IELength);

+

+

+	if(pbss_network->InfrastructureMode!=Ndis802_11APMode)

+		return _FAIL;

+

+	pbss_network->Rssi = 0;

+

+	_rtw_memcpy(pbss_network->MacAddress, myid(&(padapter->eeprompriv)), ETH_ALEN);

+	

+	//beacon interval

+	p = rtw_get_beacon_interval_from_ie(ie);//ie + 8;	// 8: TimeStamp, 2: Beacon Interval 2:Capability

+	//pbss_network->Configuration.BeaconPeriod = le16_to_cpu(*(unsigned short*)p);

+	pbss_network->Configuration.BeaconPeriod = RTW_GET_LE16(p);

+	

+	//capability

+	//cap = *(unsigned short *)rtw_get_capability_from_ie(ie);

+	//cap = le16_to_cpu(cap);

+	cap = RTW_GET_LE16(ie);

+

+	//SSID

+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _SSID_IE_, &ie_len, (pbss_network->IELength -_BEACON_IE_OFFSET_));

+	if(p && ie_len>0)

+	{

+		_rtw_memset(&pbss_network->Ssid, 0, sizeof(NDIS_802_11_SSID));

+		_rtw_memcpy(pbss_network->Ssid.Ssid, (p + 2), ie_len);

+		pbss_network->Ssid.SsidLength = ie_len;

+	}	

+

+	//chnnel

+	channel = 0;

+	pbss_network->Configuration.Length = 0;

+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _DSSET_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));

+	if(p && ie_len>0)

+		channel = *(p + 2);

+

+	pbss_network->Configuration.DSConfig = channel;

+

+	

+	_rtw_memset(supportRate, 0, NDIS_802_11_LENGTH_RATES_EX);

+	// get supported rates

+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));	

+	if (p !=  NULL) 

+	{

+		_rtw_memcpy(supportRate, p+2, ie_len);	

+		supportRateNum = ie_len;

+	}

+	

+	//get ext_supported rates

+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ie_len, pbss_network->IELength - _BEACON_IE_OFFSET_);	

+	if (p !=  NULL)

+	{

+		_rtw_memcpy(supportRate+supportRateNum, p+2, ie_len);

+		supportRateNum += ie_len;

+	

+	}

+

+	network_type = rtw_check_network_type(supportRate, supportRateNum, channel);

+

+	rtw_set_supported_rate(pbss_network->SupportedRates, network_type);

+

+

+	//parsing ERP_IE

+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));

+	if(p && ie_len>0)

+	{

+		ERP_IE_handler(padapter, (PNDIS_802_11_VARIABLE_IEs)p);

+	}

+

+	//update privacy/security

+	if (cap & BIT(4))

+		pbss_network->Privacy = 1;

+	else

+		pbss_network->Privacy = 0;

+

+	psecuritypriv->wpa_psk = 0;

+

+	//wpa2

+	group_cipher = 0; pairwise_cipher = 0;

+	psecuritypriv->wpa2_group_cipher = _NO_PRIVACY_;

+	psecuritypriv->wpa2_pairwise_cipher = _NO_PRIVACY_;	

+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _RSN_IE_2_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));		

+	if(p && ie_len>0)

+	{

+		if(rtw_parse_wpa2_ie(p, ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS)

+		{

+			psecuritypriv->dot11AuthAlgrthm= dot11AuthAlgrthm_8021X;

+			

+			psecuritypriv->dot8021xalg = 1;//psk,  todo:802.1x

+			psecuritypriv->wpa_psk |= BIT(1);

+

+			psecuritypriv->wpa2_group_cipher = group_cipher;

+			psecuritypriv->wpa2_pairwise_cipher = pairwise_cipher;

+#if 0

+			switch(group_cipher)

+			{

+				case WPA_CIPHER_NONE:				

+				psecuritypriv->wpa2_group_cipher = _NO_PRIVACY_;

+				break;

+				case WPA_CIPHER_WEP40:				

+				psecuritypriv->wpa2_group_cipher = _WEP40_;

+				break;

+				case WPA_CIPHER_TKIP:				

+				psecuritypriv->wpa2_group_cipher = _TKIP_;

+				break;

+				case WPA_CIPHER_CCMP:				

+				psecuritypriv->wpa2_group_cipher = _AES_;				

+				break;

+				case WPA_CIPHER_WEP104:					

+				psecuritypriv->wpa2_group_cipher = _WEP104_;

+				break;

+			}

+

+			switch(pairwise_cipher)

+			{

+				case WPA_CIPHER_NONE:			

+				psecuritypriv->wpa2_pairwise_cipher = _NO_PRIVACY_;

+				break;

+				case WPA_CIPHER_WEP40:			

+				psecuritypriv->wpa2_pairwise_cipher = _WEP40_;

+				break;

+				case WPA_CIPHER_TKIP:				

+				psecuritypriv->wpa2_pairwise_cipher = _TKIP_;

+				break;

+				case WPA_CIPHER_CCMP:			

+				psecuritypriv->wpa2_pairwise_cipher = _AES_;

+				break;

+				case WPA_CIPHER_WEP104:					

+				psecuritypriv->wpa2_pairwise_cipher = _WEP104_;

+				break;

+			}

+#endif			

+		}

+		

+	}

+

+	//wpa

+	ie_len = 0;

+	group_cipher = 0; pairwise_cipher = 0;

+	psecuritypriv->wpa_group_cipher = _NO_PRIVACY_;

+	psecuritypriv->wpa_pairwise_cipher = _NO_PRIVACY_;	

+	for (p = ie + _BEACON_IE_OFFSET_; ;p += (ie_len + 2))

+	{

+		p = rtw_get_ie(p, _SSN_IE_1_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));		

+		if ((p) && (_rtw_memcmp(p+2, OUI1, 4)))

+		{

+			if(rtw_parse_wpa_ie(p, ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS)

+			{

+				psecuritypriv->dot11AuthAlgrthm= dot11AuthAlgrthm_8021X;

+				

+				psecuritypriv->dot8021xalg = 1;//psk,  todo:802.1x

+

+				psecuritypriv->wpa_psk |= BIT(0);

+

+				psecuritypriv->wpa_group_cipher = group_cipher;

+				psecuritypriv->wpa_pairwise_cipher = pairwise_cipher;

+

+#if 0

+				switch(group_cipher)

+				{

+					case WPA_CIPHER_NONE:					

+					psecuritypriv->wpa_group_cipher = _NO_PRIVACY_;

+					break;

+					case WPA_CIPHER_WEP40:					

+					psecuritypriv->wpa_group_cipher = _WEP40_;

+					break;

+					case WPA_CIPHER_TKIP:					

+					psecuritypriv->wpa_group_cipher = _TKIP_;

+					break;

+					case WPA_CIPHER_CCMP:					

+					psecuritypriv->wpa_group_cipher = _AES_;				

+					break;

+					case WPA_CIPHER_WEP104:					

+					psecuritypriv->wpa_group_cipher = _WEP104_;

+					break;

+				}

+

+				switch(pairwise_cipher)

+				{

+					case WPA_CIPHER_NONE:					

+					psecuritypriv->wpa_pairwise_cipher = _NO_PRIVACY_;

+					break;

+					case WPA_CIPHER_WEP40:					

+					psecuritypriv->wpa_pairwise_cipher = _WEP40_;

+					break;

+					case WPA_CIPHER_TKIP:					

+					psecuritypriv->wpa_pairwise_cipher = _TKIP_;

+					break;

+					case WPA_CIPHER_CCMP:					

+					psecuritypriv->wpa_pairwise_cipher = _AES_;

+					break;

+					case WPA_CIPHER_WEP104:					

+					psecuritypriv->wpa_pairwise_cipher = _WEP104_;

+					break;

+				}

+#endif				

+			}

+

+			break;

+			

+		}

+			

+		if ((p == NULL) || (ie_len == 0))

+		{

+				break;

+		}

+		

+	}

+

+	//wmm

+	ie_len = 0;

+	pmlmepriv->qospriv.qos_option = 0;

+	if(pregistrypriv->wmm_enable)

+	{

+		for (p = ie + _BEACON_IE_OFFSET_; ;p += (ie_len + 2))

+		{			

+			p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));	

+			if((p) && _rtw_memcmp(p+2, WMM_PARA_IE, 6)) 

+			{

+				pmlmepriv->qospriv.qos_option = 1;	

+

+				*(p+8) |= BIT(7);//QoS Info, support U-APSD

+				

+				/* disable all ACM bits since the WMM admission control is not supported */

+				*(p + 10) &= ~BIT(4); /* BE */

+				*(p + 14) &= ~BIT(4); /* BK */

+				*(p + 18) &= ~BIT(4); /* VI */

+				*(p + 22) &= ~BIT(4); /* VO */

+				

+				break;				

+			}

+			

+			if ((p == NULL) || (ie_len == 0))

+			{

+				break;

+			}			

+		}		

+	}

+#ifdef CONFIG_80211N_HT

+	//parsing HT_CAP_IE

+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));

+	if(p && ie_len>0)

+	{

+		u8 rf_type;

+

+		struct rtw_ieee80211_ht_cap *pht_cap = (struct rtw_ieee80211_ht_cap *)(p+2);

+

+		pHT_caps_ie=p;

+		

+		

+		ht_cap = _TRUE;

+		network_type |= WIRELESS_11_24N;

+

+	

+		rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));

+

+		if((psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_CCMP) ||

+			(psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_CCMP))

+		{

+			pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&(0x07<<2));

+		}	

+		else

+		{

+			pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&0x00);	

+		}	

+

+		pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_FACTOR & 0x03); //set  Max Rx AMPDU size  to 64K

+

+		if(rf_type == RF_1T1R)

+		{			

+			pht_cap->supp_mcs_set[0] = 0xff;

+			pht_cap->supp_mcs_set[1] = 0x0;				

+		}

+

+		_rtw_memcpy(&pmlmepriv->htpriv.ht_cap, p+2, ie_len);		

+		

+	}

+

+	//parsing HT_INFO_IE

+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));

+	if(p && ie_len>0)

+	{

+		pHT_info_ie=p;

+	}

+#endif //CONFIG_80211N_HT

+	switch(network_type)

+	{

+		case WIRELESS_11B:

+			pbss_network->NetworkTypeInUse = Ndis802_11DS;

+			break;	

+		case WIRELESS_11G:

+		case WIRELESS_11BG:

+             case WIRELESS_11G_24N:

+		case WIRELESS_11BG_24N:

+			pbss_network->NetworkTypeInUse = Ndis802_11OFDM24;

+			break;

+		case WIRELESS_11A:

+			pbss_network->NetworkTypeInUse = Ndis802_11OFDM5;

+			break;

+		default :

+			pbss_network->NetworkTypeInUse = Ndis802_11OFDM24;

+			break;

+	}

+	

+	pmlmepriv->cur_network.network_type = network_type;

+

+#ifdef CONFIG_80211N_HT

+	pmlmepriv->htpriv.ht_option = _FALSE;

+

+	if( (psecuritypriv->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||

+		      (psecuritypriv->wpa_pairwise_cipher&WPA_CIPHER_TKIP))

+	{	

+		//todo:

+		//ht_cap = _FALSE;

+	}

+		      

+	//ht_cap	

+	if(pregistrypriv->ht_enable && ht_cap==_TRUE)

+	{		

+		pmlmepriv->htpriv.ht_option = _TRUE;

+		pmlmepriv->qospriv.qos_option = 1;

+

+		if(pregistrypriv->ampdu_enable==1)

+		{

+			pmlmepriv->htpriv.ampdu_enable = _TRUE;

+		}

+

+		HT_caps_handler(padapter, (PNDIS_802_11_VARIABLE_IEs)pHT_caps_ie);

+		

+		HT_info_handler(padapter, (PNDIS_802_11_VARIABLE_IEs)pHT_info_ie);

+	}

+#endif

+

+

+	pbss_network->Length = get_WLAN_BSSID_EX_sz((WLAN_BSSID_EX  *)pbss_network);

+

+	//issue beacon to start bss network

+	start_bss_network(padapter, (u8*)pbss_network);

+			

+

+	//alloc sta_info for ap itself

+	psta = rtw_get_stainfo(&padapter->stapriv, pbss_network->MacAddress);

+	if(!psta)

+	{

+		psta = rtw_alloc_stainfo(&padapter->stapriv, pbss_network->MacAddress);

+		if (psta == NULL) 

+		{ 

+			return _FAIL;

+		}	

+	}	

+	psta->state |= WIFI_AP_STATE;		//Aries, add,fix bug of flush_cam_entry at STOP AP mode , 0724 	

+	rtw_indicate_connect( padapter);

+

+	pmlmepriv->cur_network.join_res = _TRUE;//for check if already set beacon

+		

+	//update bc/mc sta_info

+	//update_bmc_sta(padapter);

+

+	return ret;

+

+}

+

+void rtw_set_macaddr_acl(_adapter *padapter, int mode)

+{

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;

+

+	DBG_871X("%s, mode=%d\n", __func__, mode);

+

+	pacl_list->mode = mode;

+}

+

+int rtw_acl_add_sta(_adapter *padapter, u8 *addr)

+{

+	_irqL irqL;

+	_list	*plist, *phead;

+	u8 added = _FALSE;

+	int i, ret=0;

+	struct rtw_wlan_acl_node *paclnode;

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;

+	_queue	*pacl_node_q =&pacl_list->acl_node_q;	

+

+	DBG_871X("%s(acl_num=%d)=" MAC_FMT "\n", __func__, pacl_list->num, MAC_ARG(addr));	

+

+	if((NUM_ACL-1) < pacl_list->num)

+		return (-1);	

+

+

+	_enter_critical_bh(&(pacl_node_q->lock), &irqL);

+

+	phead = get_list_head(pacl_node_q);

+	plist = get_next(phead);

+		

+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)

+	{

+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);

+		plist = get_next(plist);

+

+		if(_rtw_memcmp(paclnode->addr, addr, ETH_ALEN))

+		{

+			if(paclnode->valid == _TRUE)

+			{

+				added = _TRUE;

+				DBG_871X("%s, sta has been added\n", __func__);

+				break;

+			}

+		}		

+	}

+	

+	_exit_critical_bh(&(pacl_node_q->lock), &irqL);

+

+

+	if(added == _TRUE)

+		return ret;

+	

+

+	_enter_critical_bh(&(pacl_node_q->lock), &irqL);

+

+	for(i=0; i< NUM_ACL; i++)

+	{

+		paclnode = &pacl_list->aclnode[i];

+

+		if(paclnode->valid == _FALSE)

+		{

+			_rtw_init_listhead(&paclnode->list);

+	

+			_rtw_memcpy(paclnode->addr, addr, ETH_ALEN);

+		

+			paclnode->valid = _TRUE;

+

+			rtw_list_insert_tail(&paclnode->list, get_list_head(pacl_node_q));

+	

+			pacl_list->num++;

+

+			break;

+		}

+	}

+

+	DBG_871X("%s, acl_num=%d\n", __func__, pacl_list->num);

+	

+	_exit_critical_bh(&(pacl_node_q->lock), &irqL);

+

+	return ret;

+}

+

+int rtw_acl_remove_sta(_adapter *padapter, u8 *addr)

+{

+	_irqL irqL;

+	_list	*plist, *phead;

+	int i, ret=0;

+	struct rtw_wlan_acl_node *paclnode;

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;

+	_queue	*pacl_node_q =&pacl_list->acl_node_q;	

+

+	DBG_871X("%s(acl_num=%d)=" MAC_FMT "\n", __func__, pacl_list->num, MAC_ARG(addr));	

+

+	_enter_critical_bh(&(pacl_node_q->lock), &irqL);

+

+	phead = get_list_head(pacl_node_q);

+	plist = get_next(phead);

+		

+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)

+	{

+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);

+		plist = get_next(plist);

+

+		if(_rtw_memcmp(paclnode->addr, addr, ETH_ALEN))

+		{

+			if(paclnode->valid == _TRUE)

+			{

+				paclnode->valid = _FALSE;

+

+				rtw_list_delete(&paclnode->list);

+				

+				pacl_list->num--;

+			}

+		}		

+	}

+	

+	_exit_critical_bh(&(pacl_node_q->lock), &irqL);

+

+	DBG_871X("%s, acl_num=%d\n", __func__, pacl_list->num);

+	

+	return ret;

+

+}

+

+#ifdef CONFIG_NATIVEAP_MLME

+

+static void update_bcn_fixed_ie(_adapter *padapter)

+{

+	DBG_871X("%s\n", __FUNCTION__);

+

+}

+

+static void update_bcn_erpinfo_ie(_adapter *padapter)

+{

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+	WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);

+	unsigned char *p, *ie = pnetwork->IEs;

+	u32 len = 0;

+

+	DBG_871X("%s, ERP_enable=%d\n", __FUNCTION__, pmlmeinfo->ERP_enable);

+

+	if(!pmlmeinfo->ERP_enable)

+		return;

+

+	//parsing ERP_IE

+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &len, (pnetwork->IELength - _BEACON_IE_OFFSET_));

+	if(p && len>0)

+	{

+		PNDIS_802_11_VARIABLE_IEs pIE = (PNDIS_802_11_VARIABLE_IEs)p;

+

+		if (pmlmepriv->num_sta_non_erp == 1)

+			pIE->data[0] |= RTW_ERP_INFO_NON_ERP_PRESENT|RTW_ERP_INFO_USE_PROTECTION;

+		else

+			pIE->data[0] &= ~(RTW_ERP_INFO_NON_ERP_PRESENT|RTW_ERP_INFO_USE_PROTECTION);

+

+		if(pmlmepriv->num_sta_no_short_preamble > 0)

+			pIE->data[0] |= RTW_ERP_INFO_BARKER_PREAMBLE_MODE;

+		else

+			pIE->data[0] &= ~(RTW_ERP_INFO_BARKER_PREAMBLE_MODE);

+	

+		ERP_IE_handler(padapter, pIE);

+	}

+	

+}

+

+static void update_bcn_htcap_ie(_adapter *padapter)

+{

+	DBG_871X("%s\n", __FUNCTION__);

+

+}

+

+static void update_bcn_htinfo_ie(_adapter *padapter)

+{	

+	DBG_871X("%s\n", __FUNCTION__);

+

+}

+

+static void update_bcn_rsn_ie(_adapter *padapter)

+{

+	DBG_871X("%s\n", __FUNCTION__);

+

+}

+

+static void update_bcn_wpa_ie(_adapter *padapter)

+{

+	DBG_871X("%s\n", __FUNCTION__);

+

+}

+

+static void update_bcn_wmm_ie(_adapter *padapter)

+{

+	DBG_871X("%s\n", __FUNCTION__);

+	

+}

+

+static void update_bcn_wps_ie(_adapter *padapter)

+{

+	u8 *pwps_ie=NULL, *pwps_ie_src, *premainder_ie, *pbackup_remainder_ie=NULL;

+	uint wps_ielen=0, wps_offset, remainder_ielen;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+	WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);

+	unsigned char *ie = pnetwork->IEs;

+	u32 ielen = pnetwork->IELength;

+

+

+	DBG_871X("%s\n", __FUNCTION__);

+

+	pwps_ie = rtw_get_wps_ie(ie+_FIXED_IE_LENGTH_, ielen-_FIXED_IE_LENGTH_, NULL, &wps_ielen);

+	

+	if(pwps_ie==NULL || wps_ielen==0)

+		return;

+

+	wps_offset = (uint)(pwps_ie-ie);

+

+	premainder_ie = pwps_ie + wps_ielen;

+

+	remainder_ielen = ielen - wps_offset - wps_ielen;

+

+	if(remainder_ielen>0)

+	{

+		pbackup_remainder_ie = rtw_malloc(remainder_ielen);

+		if(pbackup_remainder_ie)

+			_rtw_memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);

+	}

+

+	

+	pwps_ie_src = pmlmepriv->wps_beacon_ie;

+	if(pwps_ie_src == NULL)

+		return;

+

+

+	wps_ielen = (uint)pwps_ie_src[1];//to get ie data len

+	if((wps_offset+wps_ielen+2+remainder_ielen)<=MAX_IE_SZ)

+	{

+		_rtw_memcpy(pwps_ie, pwps_ie_src, wps_ielen+2);

+		pwps_ie += (wps_ielen+2);

+

+		if(pbackup_remainder_ie)

+			_rtw_memcpy(pwps_ie, pbackup_remainder_ie, remainder_ielen);

+

+		//update IELength

+		pnetwork->IELength = wps_offset + (wps_ielen+2) + remainder_ielen;

+	}

+

+	if(pbackup_remainder_ie)

+		rtw_mfree(pbackup_remainder_ie, remainder_ielen);

+

+}

+

+static void update_bcn_p2p_ie(_adapter *padapter)

+{

+

+}

+

+static void update_bcn_vendor_spec_ie(_adapter *padapter, u8*oui)

+{

+	DBG_871X("%s\n", __FUNCTION__);

+

+	if(_rtw_memcmp(RTW_WPA_OUI, oui, 4))

+	{

+		update_bcn_wpa_ie(padapter);

+	}

+	else if(_rtw_memcmp(WMM_OUI, oui, 4))

+	{

+		update_bcn_wmm_ie(padapter);

+	}

+	else if(_rtw_memcmp(WPS_OUI, oui, 4))

+	{

+		update_bcn_wps_ie(padapter);

+	}

+	else if(_rtw_memcmp(P2P_OUI, oui, 4))

+	{

+		update_bcn_p2p_ie(padapter);

+	}

+	else

+	{

+		DBG_871X("unknown OUI type!\n");

+ 	}

+	

+	

+}

+

+void update_beacon(_adapter *padapter, u8 ie_id, u8 *oui, u8 tx)

+{

+	_irqL irqL;

+	struct mlme_priv *pmlmepriv;

+	struct mlme_ext_priv	*pmlmeext;

+	//struct mlme_ext_info	*pmlmeinfo;

+	

+	//DBG_871X("%s\n", __FUNCTION__);

+

+	if(!padapter)

+		return;

+

+	pmlmepriv = &(padapter->mlmepriv);

+	pmlmeext = &(padapter->mlmeextpriv);

+	//pmlmeinfo = &(pmlmeext->mlmext_info);

+

+	if(_FALSE == pmlmeext->bstart_bss)

+		return;

+

+	_enter_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);

+

+	switch(ie_id)

+	{

+		case 0xFF:

+

+			update_bcn_fixed_ie(padapter);//8: TimeStamp, 2: Beacon Interval 2:Capability

+			

+			break;

+	

+		case _TIM_IE_:

+			

+			update_BCNTIM(padapter);

+			

+			break;

+

+		case _ERPINFO_IE_:

+

+			update_bcn_erpinfo_ie(padapter);

+

+			break;

+

+		case _HT_CAPABILITY_IE_:

+

+			update_bcn_htcap_ie(padapter);

+			

+			break;

+

+		case _RSN_IE_2_:

+

+			update_bcn_rsn_ie(padapter);

+

+			break;

+			

+		case _HT_ADD_INFO_IE_:

+

+			update_bcn_htinfo_ie(padapter);

+			

+			break;

+	

+		case _VENDOR_SPECIFIC_IE_:

+

+			update_bcn_vendor_spec_ie(padapter, oui);

+			

+			break;

+			

+		default:

+			break;

+	}

+

+	pmlmepriv->update_bcn = _TRUE;

+	

+	_exit_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);		

+	

+#ifndef CONFIG_INTERRUPT_BASED_TXBCN 

+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)

+	if(tx)

+	{

+		//send_beacon(padapter);//send_beacon must execute on TSR level

+		set_tx_beacon_cmd(padapter);

+	}

+#else

+	{	

+		//PCI will issue beacon when BCN interrupt occurs.		

+	}

+#endif

+#endif //!CONFIG_INTERRUPT_BASED_TXBCN

+	

+}

+

+#ifdef CONFIG_80211N_HT

+

+/*

+op_mode

+Set to 0 (HT pure) under the followign conditions

+	- all STAs in the BSS are 20/40 MHz HT in 20/40 MHz BSS or

+	- all STAs in the BSS are 20 MHz HT in 20 MHz BSS

+Set to 1 (HT non-member protection) if there may be non-HT STAs

+	in both the primary and the secondary channel

+Set to 2 if only HT STAs are associated in BSS,

+	however and at least one 20 MHz HT STA is associated

+Set to 3 (HT mixed mode) when one or more non-HT STAs are associated

+	(currently non-GF HT station is considered as non-HT STA also)

+*/

+static int rtw_ht_operation_update(_adapter *padapter)

+{

+	u16 cur_op_mode, new_op_mode;

+	int op_mode_changes = 0;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct ht_priv	*phtpriv_ap = &pmlmepriv->htpriv;

+

+	if(pmlmepriv->htpriv.ht_option == _TRUE) 

+		return 0;

+	

+	//if (!iface->conf->ieee80211n || iface->conf->ht_op_mode_fixed)

+	//	return 0;

+

+	DBG_871X("%s current operation mode=0x%X\n",

+		   __FUNCTION__, pmlmepriv->ht_op_mode);

+

+	if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT)

+	    && pmlmepriv->num_sta_ht_no_gf) {

+		pmlmepriv->ht_op_mode |=

+			HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;

+		op_mode_changes++;

+	} else if ((pmlmepriv->ht_op_mode &

+		    HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) &&

+		   pmlmepriv->num_sta_ht_no_gf == 0) {

+		pmlmepriv->ht_op_mode &=

+			~HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;

+		op_mode_changes++;

+	}

+

+	if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&

+	    (pmlmepriv->num_sta_no_ht || pmlmepriv->olbc_ht)) {

+		pmlmepriv->ht_op_mode |= HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;

+		op_mode_changes++;

+	} else if ((pmlmepriv->ht_op_mode &

+		    HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&

+		   (pmlmepriv->num_sta_no_ht == 0 && !pmlmepriv->olbc_ht)) {

+		pmlmepriv->ht_op_mode &=

+			~HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;

+		op_mode_changes++;

+	}

+

+	/* Note: currently we switch to the MIXED op mode if HT non-greenfield

+	 * station is associated. Probably it's a theoretical case, since

+	 * it looks like all known HT STAs support greenfield.

+	 */

+	new_op_mode = 0;

+	if (pmlmepriv->num_sta_no_ht ||

+	    (pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT))

+		new_op_mode = OP_MODE_MIXED;

+	else if ((phtpriv_ap->ht_cap.cap_info & IEEE80211_HT_CAP_SUP_WIDTH)

+		 && pmlmepriv->num_sta_ht_20mhz)

+		new_op_mode = OP_MODE_20MHZ_HT_STA_ASSOCED;

+	else if (pmlmepriv->olbc_ht)

+		new_op_mode = OP_MODE_MAY_BE_LEGACY_STAS;

+	else

+		new_op_mode = OP_MODE_PURE;

+

+	cur_op_mode = pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_OP_MODE_MASK;

+	if (cur_op_mode != new_op_mode) {

+		pmlmepriv->ht_op_mode &= ~HT_INFO_OPERATION_MODE_OP_MODE_MASK;

+		pmlmepriv->ht_op_mode |= new_op_mode;

+		op_mode_changes++;

+	}

+

+	DBG_871X("%s new operation mode=0x%X changes=%d\n",

+		   __FUNCTION__, pmlmepriv->ht_op_mode, op_mode_changes);

+

+	return op_mode_changes;

+	

+}

+

+#endif /* CONFIG_80211N_HT */

+

+void associated_clients_update(_adapter *padapter, u8 updated)

+{

+	//update associcated stations cap.

+	if(updated == _TRUE)

+	{

+		_irqL irqL;

+		_list	*phead, *plist;

+		struct sta_info *psta=NULL;	

+		struct sta_priv *pstapriv = &padapter->stapriv;

+			

+		_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+		

+		phead = &pstapriv->asoc_list;

+		plist = get_next(phead);

+		

+		//check asoc_queue

+		while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)	

+		{

+			psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);

+		

+			plist = get_next(plist);

+

+			VCS_update(padapter, psta);		

+		}

+

+		_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+

+	}		

+

+}

+

+/* called > TSR LEVEL for USB or SDIO Interface*/

+void bss_cap_update_on_sta_join(_adapter *padapter, struct sta_info *psta)

+{

+	u8 beacon_updated = _FALSE;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);

+

+	

+#if 0

+	if (!(psta->capability & WLAN_CAPABILITY_SHORT_PREAMBLE) &&

+	    !psta->no_short_preamble_set) {

+		psta->no_short_preamble_set = 1;

+		pmlmepriv->num_sta_no_short_preamble++;

+		if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && 

+		     (pmlmepriv->num_sta_no_short_preamble == 1))

+			ieee802_11_set_beacons(hapd->iface);

+	}

+#endif

+

+

+	if(!(psta->flags & WLAN_STA_SHORT_PREAMBLE))	

+	{

+		if(!psta->no_short_preamble_set)

+		{

+			psta->no_short_preamble_set = 1;

+			

+			pmlmepriv->num_sta_no_short_preamble++;

+			

+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && 

+		     		(pmlmepriv->num_sta_no_short_preamble == 1))

+			{

+				beacon_updated = _TRUE;

+				update_beacon(padapter, 0xFF, NULL, _TRUE);

+			}	

+			

+		}

+	}

+	else

+	{

+		if(psta->no_short_preamble_set)

+		{

+			psta->no_short_preamble_set = 0;

+			

+			pmlmepriv->num_sta_no_short_preamble--;

+			

+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) && 

+		     		(pmlmepriv->num_sta_no_short_preamble == 0))

+			{

+				beacon_updated = _TRUE;

+				update_beacon(padapter, 0xFF, NULL, _TRUE);

+			}	

+			

+		}

+	}

+

+#if 0

+	if (psta->flags & WLAN_STA_NONERP && !psta->nonerp_set) {

+		psta->nonerp_set = 1;

+		pmlmepriv->num_sta_non_erp++;

+		if (pmlmepriv->num_sta_non_erp == 1)

+			ieee802_11_set_beacons(hapd->iface);

+	}

+#endif

+

+	if(psta->flags & WLAN_STA_NONERP)

+	{

+		if(!psta->nonerp_set)

+		{

+			psta->nonerp_set = 1;

+			

+			pmlmepriv->num_sta_non_erp++;

+			

+			if (pmlmepriv->num_sta_non_erp == 1)

+			{

+				beacon_updated = _TRUE;

+				update_beacon(padapter, _ERPINFO_IE_, NULL, _TRUE);

+			}	

+		}

+		

+	}

+	else

+	{

+		if(psta->nonerp_set)

+		{

+			psta->nonerp_set = 0;

+			

+			pmlmepriv->num_sta_non_erp--;

+			

+			if (pmlmepriv->num_sta_non_erp == 0)

+			{

+				beacon_updated = _TRUE;

+				update_beacon(padapter, _ERPINFO_IE_, NULL, _TRUE);

+			}	

+		}

+		

+	}

+

+

+#if 0

+	if (!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT) &&

+	    !psta->no_short_slot_time_set) {

+		psta->no_short_slot_time_set = 1;

+		pmlmepriv->num_sta_no_short_slot_time++;

+		if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&

+		    (pmlmepriv->num_sta_no_short_slot_time == 1))

+			ieee802_11_set_beacons(hapd->iface);

+	}

+#endif

+

+	if(!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT))

+	{

+		if(!psta->no_short_slot_time_set)

+		{

+			psta->no_short_slot_time_set = 1;

+			

+			pmlmepriv->num_sta_no_short_slot_time++;

+			

+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&

+		   		 (pmlmepriv->num_sta_no_short_slot_time == 1))

+			{

+				beacon_updated = _TRUE;

+				update_beacon(padapter, 0xFF, NULL, _TRUE);

+			}			

+			

+		}

+	}

+	else

+	{

+		if(psta->no_short_slot_time_set)

+		{

+			psta->no_short_slot_time_set = 0;

+			

+			pmlmepriv->num_sta_no_short_slot_time--;

+			

+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&

+		   		 (pmlmepriv->num_sta_no_short_slot_time == 0))

+			{

+				beacon_updated = _TRUE;

+				update_beacon(padapter, 0xFF, NULL, _TRUE);

+			}			

+		}

+	}	

+	

+#ifdef CONFIG_80211N_HT

+

+	if (psta->flags & WLAN_STA_HT) 

+	{

+		u16 ht_capab = le16_to_cpu(psta->htpriv.ht_cap.cap_info);

+			

+		DBG_871X("HT: STA " MAC_FMT " HT Capabilities "

+			   "Info: 0x%04x\n", MAC_ARG(psta->hwaddr), ht_capab);

+

+		if (psta->no_ht_set) {

+			psta->no_ht_set = 0;

+			pmlmepriv->num_sta_no_ht--;

+		}

+		

+		if ((ht_capab & IEEE80211_HT_CAP_GRN_FLD) == 0) {

+			if (!psta->no_ht_gf_set) {

+				psta->no_ht_gf_set = 1;

+				pmlmepriv->num_sta_ht_no_gf++;

+			}

+			DBG_871X("%s STA " MAC_FMT " - no "

+				   "greenfield, num of non-gf stations %d\n",

+				   __FUNCTION__, MAC_ARG(psta->hwaddr),

+				   pmlmepriv->num_sta_ht_no_gf);

+		}

+		

+		if ((ht_capab & IEEE80211_HT_CAP_SUP_WIDTH) == 0) {

+			if (!psta->ht_20mhz_set) {

+				psta->ht_20mhz_set = 1;

+				pmlmepriv->num_sta_ht_20mhz++;

+			}

+			DBG_871X("%s STA " MAC_FMT " - 20 MHz HT, "

+				   "num of 20MHz HT STAs %d\n",

+				   __FUNCTION__, MAC_ARG(psta->hwaddr),

+				   pmlmepriv->num_sta_ht_20mhz);

+		}

+		

+	} 

+	else 

+	{

+		if (!psta->no_ht_set) {

+			psta->no_ht_set = 1;

+			pmlmepriv->num_sta_no_ht++;

+		}

+		if(pmlmepriv->htpriv.ht_option == _TRUE) {		

+			DBG_871X("%s STA " MAC_FMT

+				   " - no HT, num of non-HT stations %d\n",

+				   __FUNCTION__, MAC_ARG(psta->hwaddr),

+				   pmlmepriv->num_sta_no_ht);

+		}

+	}

+

+	if (rtw_ht_operation_update(padapter) > 0)

+	{

+		update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, _FALSE);

+		update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, _TRUE);

+	}	

+	

+#endif /* CONFIG_80211N_HT */

+

+	//update associcated stations cap.

+	associated_clients_update(padapter,  beacon_updated);

+

+	DBG_871X("%s, updated=%d\n", __func__, beacon_updated);

+

+}

+

+u8 bss_cap_update_on_sta_leave(_adapter *padapter, struct sta_info *psta)

+{

+	u8 beacon_updated = _FALSE;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);

+

+	if(!psta)

+		return beacon_updated;

+

+	if (psta->no_short_preamble_set) {

+		psta->no_short_preamble_set = 0;

+		pmlmepriv->num_sta_no_short_preamble--;

+		if (pmlmeext->cur_wireless_mode > WIRELESS_11B

+		    && pmlmepriv->num_sta_no_short_preamble == 0)

+		{

+			beacon_updated = _TRUE;

+			update_beacon(padapter, 0xFF, NULL, _TRUE);

+		}	

+	}	

+

+	if (psta->nonerp_set) {

+		psta->nonerp_set = 0;		

+		pmlmepriv->num_sta_non_erp--;

+		if (pmlmepriv->num_sta_non_erp == 0)

+		{

+			beacon_updated = _TRUE;

+			update_beacon(padapter, _ERPINFO_IE_, NULL, _TRUE);

+		}	

+	}

+

+	if (psta->no_short_slot_time_set) {

+		psta->no_short_slot_time_set = 0;

+		pmlmepriv->num_sta_no_short_slot_time--;

+		if (pmlmeext->cur_wireless_mode > WIRELESS_11B

+		    && pmlmepriv->num_sta_no_short_slot_time == 0)

+		{

+			beacon_updated = _TRUE;

+			update_beacon(padapter, 0xFF, NULL, _TRUE);

+		}	

+	}

+	

+#ifdef CONFIG_80211N_HT

+

+	if (psta->no_ht_gf_set) {

+		psta->no_ht_gf_set = 0;

+		pmlmepriv->num_sta_ht_no_gf--;

+	}

+

+	if (psta->no_ht_set) {

+		psta->no_ht_set = 0;

+		pmlmepriv->num_sta_no_ht--;

+	}

+

+	if (psta->ht_20mhz_set) {

+		psta->ht_20mhz_set = 0;

+		pmlmepriv->num_sta_ht_20mhz--;

+	}

+

+	if (rtw_ht_operation_update(padapter) > 0)

+	{

+		update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, _FALSE);

+		update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, _TRUE);

+	}

+	

+#endif /* CONFIG_80211N_HT */

+

+	//update associcated stations cap.

+	//associated_clients_update(padapter,  beacon_updated); //move it to avoid deadlock

+

+	DBG_871X("%s, updated=%d\n", __func__, beacon_updated);

+

+	return beacon_updated;

+

+}

+

+u8 ap_free_sta(_adapter *padapter, struct sta_info *psta, bool active, u16 reason)

+{

+	_irqL irqL;

+	u8 beacon_updated = _FALSE;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);

+	struct sta_priv *pstapriv = &padapter->stapriv;

+

+	if(!psta)

+		return beacon_updated;

+

+	if (active == _TRUE)

+	{

+#ifdef CONFIG_80211N_HT

+		//tear down Rx AMPDU

+		send_delba(padapter, 0, psta->hwaddr);// recipient

+	

+		//tear down TX AMPDU

+		send_delba(padapter, 1, psta->hwaddr);// // originator

+		

+#endif //CONFIG_80211N_HT

+

+		issue_deauth(padapter, psta->hwaddr, reason);

+	}

+

+	psta->htpriv.agg_enable_bitmap = 0x0;//reset

+	psta->htpriv.candidate_tid_bitmap = 0x0;//reset

+

+

+	//report_del_sta_event(padapter, psta->hwaddr, reason);

+

+	//clear cam entry / key

+	//clear_cam_entry(padapter, (psta->mac_id + 3));

+	rtw_clearstakey_cmd(padapter, (u8*)psta, (u8)(psta->mac_id + 3), _TRUE);

+

+

+	_enter_critical_bh(&psta->lock, &irqL);

+	psta->state &= ~_FW_LINKED;

+	_exit_critical_bh(&psta->lock, &irqL);

+

+	#ifdef CONFIG_IOCTL_CFG80211

+	if (1) {

+		#ifdef COMPAT_KERNEL_RELEASE

+		rtw_cfg80211_indicate_sta_disassoc(padapter, psta->hwaddr, reason);

+		#elif (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)

+		rtw_cfg80211_indicate_sta_disassoc(padapter, psta->hwaddr, reason);

+		#else //(LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)

+		/* will call rtw_cfg80211_indicate_sta_disassoc() in cmd_thread for old API context */

+		#endif //(LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)

+	} else

+	#endif //CONFIG_IOCTL_CFG80211

+	{

+		rtw_indicate_sta_disassoc_event(padapter, psta);

+	}

+

+	report_del_sta_event(padapter, psta->hwaddr, reason);

+

+	beacon_updated = bss_cap_update_on_sta_leave(padapter, psta);

+

+	_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);					

+	rtw_free_stainfo(padapter, psta);

+	_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);

+	

+

+	return beacon_updated;

+

+}

+

+int rtw_ap_inform_ch_switch(_adapter *padapter, u8 new_ch, u8 ch_offset)

+{

+	_irqL irqL;

+	_list	*phead, *plist;

+	int ret=0;	

+	struct sta_info *psta = NULL;	

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+	u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};

+

+	if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)

+		return ret;

+

+	DBG_871X(FUNC_NDEV_FMT" with ch:%u, offset:%u\n",

+		FUNC_NDEV_ARG(padapter->pnetdev), new_ch, ch_offset);

+

+	_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+	phead = &pstapriv->asoc_list;

+	plist = get_next(phead);

+	

+	/* for each sta in asoc_queue */

+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)	

+	{		

+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);

+		plist = get_next(plist);

+

+		issue_action_spct_ch_switch(padapter, psta->hwaddr, new_ch, ch_offset);

+		psta->expire_to = ((pstapriv->expire_to * 2) > 5) ? 5 : (pstapriv->expire_to * 2);

+	}

+	_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+

+	issue_action_spct_ch_switch(padapter, bc_addr, new_ch, ch_offset);

+

+	return ret;

+}

+

+int rtw_sta_flush(_adapter *padapter)

+{

+	_irqL irqL;

+	_list	*phead, *plist;

+	int ret=0;	

+	struct sta_info *psta = NULL;	

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+	u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};

+	u8 chk_alive_num = 0;

+	char chk_alive_list[NUM_STA];

+	int i;

+

+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));

+

+	if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)

+		return ret;

+

+	_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+	phead = &pstapriv->asoc_list;

+	plist = get_next(phead);

+

+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) {

+		int stainfo_offset;

+		

+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);

+		plist = get_next(plist);

+

+		/* Remove sta from asoc_list */

+		rtw_list_delete(&psta->asoc_list);

+		pstapriv->asoc_list_cnt--;

+

+		/* Keep sta for ap_free_sta() beyond this asoc_list loop */

+		stainfo_offset = rtw_stainfo_offset(pstapriv, psta);

+		if (stainfo_offset_valid(stainfo_offset)) {

+			chk_alive_list[chk_alive_num++] = stainfo_offset;

+		}

+	}

+	_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+

+

+	/* For each sta in chk_alive_list, call ap_free_sta */

+	for (i = 0; i < chk_alive_num; i++) {

+		psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);

+		ap_free_sta(padapter, psta, _TRUE, WLAN_REASON_DEAUTH_LEAVING);

+	}

+

+	issue_deauth(padapter, bc_addr, WLAN_REASON_DEAUTH_LEAVING);

+

+	associated_clients_update(padapter, _TRUE);

+

+	return ret;

+

+}

+

+/* called > TSR LEVEL for USB or SDIO Interface*/

+void sta_info_update(_adapter *padapter, struct sta_info *psta)

+{	

+	int flags = psta->flags;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	

+				

+	//update wmm cap.

+	if(WLAN_STA_WME&flags)

+		psta->qos_option = 1;

+	else

+		psta->qos_option = 0;

+

+	if(pmlmepriv->qospriv.qos_option == 0)	

+		psta->qos_option = 0;

+

+		

+#ifdef CONFIG_80211N_HT		

+	//update 802.11n ht cap.

+	if(WLAN_STA_HT&flags)

+	{

+		psta->htpriv.ht_option = _TRUE;

+		psta->qos_option = 1;	

+	}

+	else		

+	{

+		psta->htpriv.ht_option = _FALSE;

+	}

+		

+	if(pmlmepriv->htpriv.ht_option == _FALSE)	

+		psta->htpriv.ht_option = _FALSE;

+#endif		

+

+

+	update_sta_info_apmode(padapter, psta);

+		

+

+}

+

+/* called >= TSR LEVEL for USB or SDIO Interface*/

+void ap_sta_info_defer_update(_adapter *padapter, struct sta_info *psta)

+{

+	if(psta->state & _FW_LINKED)

+	{	

+		//add ratid

+		add_RATid(padapter, psta, 0);//DM_RATR_STA_INIT

+	}	

+}

+

+/* restore hw setting from sw data structures */

+void rtw_ap_restore_network(_adapter *padapter)

+{

+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;

+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+	struct sta_priv * pstapriv = &padapter->stapriv;

+	struct sta_info *psta;

+	struct security_priv* psecuritypriv=&(padapter->securitypriv);

+	_irqL irqL;

+	_list	*phead, *plist;

+	u8 chk_alive_num = 0;

+	char chk_alive_list[NUM_STA];

+	int i;

+

+	rtw_setopmode_cmd(padapter, Ndis802_11APMode);

+

+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);

+

+	start_bss_network(padapter, (u8*)&mlmepriv->cur_network.network);

+

+	if((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||

+		(padapter->securitypriv.dot11PrivacyAlgrthm == _AES_))

+	{

+		/* restore group key, WEP keys is restored in ips_leave() */

+		rtw_set_key(padapter, psecuritypriv, psecuritypriv->dot118021XGrpKeyid, 0);

+	}

+

+	/* per sta pairwise key and settings */

+	if((padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_) &&

+		(padapter->securitypriv.dot11PrivacyAlgrthm != _AES_)) {

+		return;

+	}

+

+	_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+	

+	phead = &pstapriv->asoc_list;

+	plist = get_next(phead);

+

+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) {

+		int stainfo_offset;

+

+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);

+		plist = get_next(plist);

+

+		stainfo_offset = rtw_stainfo_offset(pstapriv, psta);

+		if (stainfo_offset_valid(stainfo_offset)) {

+			chk_alive_list[chk_alive_num++] = stainfo_offset;

+		}

+	}

+

+	_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);

+

+	for (i = 0; i < chk_alive_num; i++) {

+		psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);

+

+		if (psta == NULL) {

+			DBG_871X(FUNC_ADPT_FMT" sta_info is null\n", FUNC_ADPT_ARG(padapter));

+		}

+		else if(psta->state &_FW_LINKED)

+		{

+			Update_RA_Entry(padapter, psta);

+			//pairwise key

+			rtw_setstakey_cmd(padapter, (unsigned char *)psta, _TRUE);

+		}

+	}

+

+}

+

+void start_ap_mode(_adapter *padapter)

+{

+	int i;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;

+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;

+	

+	pmlmepriv->update_bcn = _FALSE;

+	

+	//init_mlme_ap_info(padapter);

+	pmlmeext->bstart_bss = _FALSE;

+

+	pmlmepriv->num_sta_non_erp = 0;

+

+	pmlmepriv->num_sta_no_short_slot_time = 0;

+

+	pmlmepriv->num_sta_no_short_preamble = 0;

+

+	pmlmepriv->num_sta_ht_no_gf = 0;

+#ifdef CONFIG_80211N_HT

+	pmlmepriv->num_sta_no_ht = 0;

+#endif //CONFIG_80211N_HT

+	pmlmepriv->num_sta_ht_20mhz = 0;

+

+	pmlmepriv->olbc = _FALSE;

+

+	pmlmepriv->olbc_ht = _FALSE;

+	

+#ifdef CONFIG_80211N_HT

+	pmlmepriv->ht_op_mode = 0;

+#endif

+

+	for(i=0; i<NUM_STA; i++)

+		pstapriv->sta_aid[i] = NULL;

+

+	pmlmepriv->wps_beacon_ie = NULL;	

+	pmlmepriv->wps_probe_resp_ie = NULL;

+	pmlmepriv->wps_assoc_resp_ie = NULL;

+	

+	pmlmepriv->p2p_beacon_ie = NULL;

+	pmlmepriv->p2p_probe_resp_ie = NULL;

+

+	

+	//for ACL 

+	_rtw_init_listhead(&(pacl_list->acl_node_q.queue));

+	pacl_list->num = 0;

+	pacl_list->mode = 0;

+	for(i = 0; i < NUM_ACL; i++)

+	{		

+		_rtw_init_listhead(&pacl_list->aclnode[i].list);

+		pacl_list->aclnode[i].valid = _FALSE;

+	}

+

+}

+

+void stop_ap_mode(_adapter *padapter)

+{

+	_irqL irqL;

+	_list	*phead, *plist;

+	struct rtw_wlan_acl_node *paclnode;

+	struct sta_info *psta=NULL;

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;	

+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;

+	_queue	*pacl_node_q =&pacl_list->acl_node_q;	

+

+	pmlmepriv->update_bcn = _FALSE;

+	pmlmeext->bstart_bss = _FALSE;

+	//_rtw_spinlock_free(&pmlmepriv->bcn_update_lock);

+	

+	//reset and init security priv , this can refine with rtw_reset_securitypriv

+	_rtw_memset((unsigned char *)&padapter->securitypriv, 0, sizeof (struct security_priv));

+	padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;

+	padapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled;

+

+	//for ACL

+	_enter_critical_bh(&(pacl_node_q->lock), &irqL);

+	phead = get_list_head(pacl_node_q);

+	plist = get_next(phead);		

+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)

+	{

+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);

+		plist = get_next(plist);

+

+		if(paclnode->valid == _TRUE)

+		{

+			paclnode->valid = _FALSE;

+

+			rtw_list_delete(&paclnode->list);

+				

+			pacl_list->num--;		

+		}		

+	}	

+	_exit_critical_bh(&(pacl_node_q->lock), &irqL);

+	

+	DBG_871X("%s, free acl_node_queue, num=%d\n", __func__, pacl_list->num);

+	

+	rtw_sta_flush(padapter);

+

+	//free_assoc_sta_resources	

+	rtw_free_all_stainfo(padapter);

+	

+	psta = rtw_get_bcmc_stainfo(padapter);

+	_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		

+	rtw_free_stainfo(padapter, psta);

+	_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);

+	

+	rtw_init_bcmc_stainfo(padapter);	

+

+	rtw_free_mlme_priv_ie_data(pmlmepriv);

+

+}

+

+#endif //CONFIG_NATIVEAP_MLME

+#endif //CONFIG_AP_MODE

+

diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_br_ext.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_br_ext.c
new file mode 100755
index 00000000..44ff8275
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_br_ext.c
@@ -0,0 +1,1700 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_BR_EXT_C_
+
+#ifdef __KERNEL__
+#include <linux/if_arp.h>
+#include <net/ip.h>
+#include <net/ipx.h>
+#include <linux/atalk.h>
+#include <linux/udp.h>
+#include <linux/if_pppox.h>
+#endif
+
+#if 1	// rtw_wifi_driver
+#include <drv_conf.h>
+#include <drv_types.h>
+#include "rtw_br_ext.h"
+#else	// rtw_wifi_driver
+#include "./8192cd_cfg.h"
+
+#ifndef __KERNEL__
+#include "./sys-support.h"
+#endif
+
+#include "./8192cd.h"
+#include "./8192cd_headers.h"
+#include "./8192cd_br_ext.h"
+#include "./8192cd_debug.h"
+#endif	// rtw_wifi_driver
+
+#ifdef CL_IPV6_PASS
+#ifdef __KERNEL__
+#include <linux/ipv6.h>
+#include <linux/icmpv6.h>
+#include <net/ndisc.h>
+#include <net/checksum.h>
+#endif
+#endif
+
+#ifdef CONFIG_BR_EXT
+
+//#define BR_EXT_DEBUG
+
+#define NAT25_IPV4		01
+#define NAT25_IPV6		02
+#define NAT25_IPX		03
+#define NAT25_APPLE		04
+#define NAT25_PPPOE		05
+
+#define RTL_RELAY_TAG_LEN (ETH_ALEN)
+#define TAG_HDR_LEN		4
+
+#define MAGIC_CODE		0x8186
+#define MAGIC_CODE_LEN	2
+#define WAIT_TIME_PPPOE	5	// waiting time for pppoe server in sec
+
+/*-----------------------------------------------------------------
+  How database records network address:
+           0    1    2    3    4    5    6    7    8    9   10
+        |----|----|----|----|----|----|----|----|----|----|----|
+  IPv4  |type|                             |      IP addr      |
+  IPX   |type|      Net addr     |          Node addr          |
+  IPX   |type|      Net addr     |Sckt addr|
+  Apple |type| Network |node|
+  PPPoE |type|   SID   |           AC MAC            |
+-----------------------------------------------------------------*/
+
+
+//Find a tag in pppoe frame and return the pointer
+static __inline__ unsigned char *__nat25_find_pppoe_tag(struct pppoe_hdr *ph, unsigned short type)
+{
+	unsigned char *cur_ptr, *start_ptr;
+	unsigned short tagLen, tagType;
+
+	start_ptr = cur_ptr = (unsigned char *)ph->tag;
+	while((cur_ptr - start_ptr) < ntohs(ph->length)) {
+		// prevent un-alignment access
+		tagType = (unsigned short)((cur_ptr[0] << 8) + cur_ptr[1]);
+		tagLen  = (unsigned short)((cur_ptr[2] << 8) + cur_ptr[3]);
+		if(tagType == type)
+			return cur_ptr;
+		cur_ptr = cur_ptr + TAG_HDR_LEN + tagLen;
+	}
+	return 0;
+}
+
+
+static __inline__ int __nat25_add_pppoe_tag(struct sk_buff *skb, struct pppoe_tag *tag)
+{
+	struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
+	int data_len;
+
+	data_len = tag->tag_len + TAG_HDR_LEN;
+	if (skb_tailroom(skb) < data_len) {
+		_DEBUG_ERR("skb_tailroom() failed in add SID tag!\n");
+		return -1;
+	}
+
+	skb_put(skb, data_len);
+	// have a room for new tag
+	memmove(((unsigned char *)ph->tag + data_len), (unsigned char *)ph->tag, ntohs(ph->length));
+	ph->length = htons(ntohs(ph->length) + data_len);
+	memcpy((unsigned char *)ph->tag, tag, data_len);
+	return data_len;
+}
+
+static int skb_pull_and_merge(struct sk_buff *skb, unsigned char *src, int len)
+{
+	int tail_len;
+	unsigned long end, tail;
+
+	if ((src+len) > skb_tail_pointer(skb) || skb->len < len)
+		return -1;
+
+	tail = (unsigned long)skb_tail_pointer(skb);
+	end = (unsigned long)src+len;
+	if (tail < end)
+		return -1;
+
+	tail_len = (int)(tail-end);
+	if (tail_len > 0)
+		memmove(src, src+len, tail_len);
+
+	skb_trim(skb, skb->len-len);
+	return 0;
+}
+
+static __inline__ unsigned long __nat25_timeout(_adapter *priv)
+{
+	unsigned long timeout;
+
+	timeout = jiffies - NAT25_AGEING_TIME*HZ;
+
+	return timeout;
+}
+
+
+static __inline__ int  __nat25_has_expired(_adapter *priv,
+				struct nat25_network_db_entry *fdb)
+{
+	if(time_before_eq(fdb->ageing_timer, __nat25_timeout(priv)))
+		return 1;
+
+	return 0;
+}
+
+
+static __inline__ void __nat25_generate_ipv4_network_addr(unsigned char *networkAddr,
+				unsigned int *ipAddr)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_IPV4;
+	memcpy(networkAddr+7, (unsigned char *)ipAddr, 4);
+}
+
+
+static __inline__ void __nat25_generate_ipx_network_addr_with_node(unsigned char *networkAddr,
+				unsigned int *ipxNetAddr, unsigned char *ipxNodeAddr)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_IPX;
+	memcpy(networkAddr+1, (unsigned char *)ipxNetAddr, 4);
+	memcpy(networkAddr+5, ipxNodeAddr, 6);
+}
+
+
+static __inline__ void __nat25_generate_ipx_network_addr_with_socket(unsigned char *networkAddr,
+				unsigned int *ipxNetAddr, unsigned short *ipxSocketAddr)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_IPX;
+	memcpy(networkAddr+1, (unsigned char *)ipxNetAddr, 4);
+	memcpy(networkAddr+5, (unsigned char *)ipxSocketAddr, 2);
+}
+
+
+static __inline__ void __nat25_generate_apple_network_addr(unsigned char *networkAddr,
+				unsigned short *network, unsigned char *node)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_APPLE;
+	memcpy(networkAddr+1, (unsigned char *)network, 2);
+	networkAddr[3] = *node;
+}
+
+
+static __inline__ void __nat25_generate_pppoe_network_addr(unsigned char *networkAddr,
+				unsigned char *ac_mac, unsigned short *sid)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_PPPOE;
+	memcpy(networkAddr+1, (unsigned char *)sid, 2);
+	memcpy(networkAddr+3, (unsigned char *)ac_mac, 6);
+}
+
+
+#ifdef CL_IPV6_PASS
+static  void __nat25_generate_ipv6_network_addr(unsigned char *networkAddr,
+				unsigned int *ipAddr)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_IPV6;
+	memcpy(networkAddr+1, (unsigned char *)ipAddr, 16);
+}
+
+
+static unsigned char *scan_tlv(unsigned char *data, int len, unsigned char tag, unsigned char len8b)
+{
+	while (len > 0) {
+		if (*data == tag && *(data+1) == len8b && len >= len8b*8)
+			return data+2;	
+		
+		len -= (*(data+1))*8;		
+		data += (*(data+1))*8;		
+	}
+	return NULL;
+}
+
+
+static int update_nd_link_layer_addr(unsigned char *data, int len, unsigned char *replace_mac)
+{
+	struct icmp6hdr *icmphdr = (struct icmp6hdr *)data;
+	unsigned char *mac;
+	
+	if (icmphdr->icmp6_type == NDISC_ROUTER_SOLICITATION) { 
+		if (len >= 8) {
+			mac = scan_tlv(&data[8], len-8, 1, 1);
+			if (mac) {
+				_DEBUG_INFO("Router Solicitation, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+					mac[0],mac[1],mac[2],mac[3],mac[4],mac[5],					
+					replace_mac[0],replace_mac[1],replace_mac[2],replace_mac[3],replace_mac[4],replace_mac[5]);
+				memcpy(mac, replace_mac, 6);	
+				return 1;				
+			}
+		}
+	}
+	else if (icmphdr->icmp6_type == NDISC_ROUTER_ADVERTISEMENT) {
+		if (len >= 16) {
+			mac = scan_tlv(&data[16], len-16, 1, 1);
+			if (mac) {
+				_DEBUG_INFO("Router Advertisement, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+					mac[0],mac[1],mac[2],mac[3],mac[4],mac[5],					
+					replace_mac[0],replace_mac[1],replace_mac[2],replace_mac[3],replace_mac[4],replace_mac[5]);
+				memcpy(mac, replace_mac, 6);			
+				return 1;				
+			}
+		}		
+	}
+	else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_SOLICITATION) {
+		if (len >= 24) {
+			mac = scan_tlv(&data[24], len-24, 1, 1);
+			if (mac) {		
+				_DEBUG_INFO("Neighbor Solicitation, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+					mac[0],mac[1],mac[2],mac[3],mac[4],mac[5],					
+					replace_mac[0],replace_mac[1],replace_mac[2],replace_mac[3],replace_mac[4],replace_mac[5]);
+				memcpy(mac, replace_mac, 6);	
+				return 1;								
+			}
+		}		
+	}
+	else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_ADVERTISEMENT) {
+		if (len >= 24) {
+			mac = scan_tlv(&data[24], len-24, 2, 1);
+			if (mac) {
+				_DEBUG_INFO("Neighbor Advertisement, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+					mac[0],mac[1],mac[2],mac[3],mac[4],mac[5],					
+					replace_mac[0],replace_mac[1],replace_mac[2],replace_mac[3],replace_mac[4],replace_mac[5]);
+				memcpy(mac, replace_mac, 6);		
+				return 1;				
+			}
+		}		
+	}
+	else if (icmphdr->icmp6_type == NDISC_REDIRECT) {
+		if (len >= 40) {
+			mac = scan_tlv(&data[40], len-40, 2, 1);
+			if (mac) {				
+				_DEBUG_INFO("Redirect,  replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+					mac[0],mac[1],mac[2],mac[3],mac[4],mac[5],					
+					replace_mac[0],replace_mac[1],replace_mac[2],replace_mac[3],replace_mac[4],replace_mac[5]);
+				memcpy(mac, replace_mac, 6);	
+				return 1;				
+			}
+		}		
+	}	
+	return 0;
+}
+
+
+static void convert_ipv6_mac_to_mc(struct sk_buff *skb)
+{	
+	struct ipv6hdr *iph = (struct ipv6hdr *)(skb->data + ETH_HLEN);
+	unsigned char *dst_mac = skb->data;
+
+	//dst_mac[0] = 0xff;
+	//dst_mac[1] = 0xff;
+	/*modified by qinjunjie,ipv6 multicast address ix 0x33-33-xx-xx-xx-xx*/
+	dst_mac[0] = 0x33;
+	dst_mac[1] = 0x33;
+	memcpy(&dst_mac[2], &iph->daddr.s6_addr32[3], 4);
+	#if defined(__LINUX_2_6__) 
+	/*modified by qinjunjie,warning:should not remove next line*/
+	skb->pkt_type = PACKET_MULTICAST;
+	#endif
+}
+#endif /* CL_IPV6_PASS */
+
+
+static __inline__ int __nat25_network_hash(unsigned char *networkAddr)
+{
+	if(networkAddr[0] == NAT25_IPV4)
+	{
+		unsigned long x;
+
+		x = networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	}
+	else if(networkAddr[0] == NAT25_IPX)
+	{
+		unsigned long x;
+
+		x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^
+			networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	}
+	else if(networkAddr[0] == NAT25_APPLE)
+	{
+		unsigned long x;
+
+		x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	}
+	else if(networkAddr[0] == NAT25_PPPOE)
+	{
+		unsigned long x;
+
+		x = networkAddr[0] ^ networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^ networkAddr[6] ^ networkAddr[7] ^ networkAddr[8];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	}
+#ifdef CL_IPV6_PASS
+	else if(networkAddr[0] == NAT25_IPV6)
+	{
+		unsigned long x;
+
+		x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^
+			networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10] ^
+			networkAddr[11] ^ networkAddr[12] ^ networkAddr[13] ^ networkAddr[14] ^ networkAddr[15] ^
+			networkAddr[16];
+	
+		return x & (NAT25_HASH_SIZE - 1);
+	}
+#endif	
+	else
+	{
+		unsigned long x = 0;
+		int i;
+
+		for (i=0; i<MAX_NETWORK_ADDR_LEN; i++)
+			x ^= networkAddr[i];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	}
+}
+
+
+static __inline__ void __network_hash_link(_adapter *priv,
+				struct nat25_network_db_entry *ent, int hash)
+{
+	// Caller must _enter_critical_bh already!
+	//_irqL irqL;
+	//_enter_critical_bh(&priv->br_ext_lock, &irqL);
+
+	ent->next_hash = priv->nethash[hash];
+	if(ent->next_hash != NULL)
+		ent->next_hash->pprev_hash = &ent->next_hash;
+	priv->nethash[hash] = ent;
+	ent->pprev_hash = &priv->nethash[hash];
+
+	//_exit_critical_bh(&priv->br_ext_lock, &irqL);
+}
+
+
+static __inline__ void __network_hash_unlink(struct nat25_network_db_entry *ent)
+{
+	// Caller must _enter_critical_bh already!
+	//_irqL irqL;
+	//_enter_critical_bh(&priv->br_ext_lock, &irqL);
+
+	*(ent->pprev_hash) = ent->next_hash;
+	if(ent->next_hash != NULL)
+		ent->next_hash->pprev_hash = ent->pprev_hash;
+	ent->next_hash = NULL;
+	ent->pprev_hash = NULL;
+
+	//_exit_critical_bh(&priv->br_ext_lock, &irqL);
+}
+
+
+static int __nat25_db_network_lookup_and_replace(_adapter *priv,
+				struct sk_buff *skb, unsigned char *networkAddr)
+{
+	struct nat25_network_db_entry *db;
+	_irqL irqL;
+	_enter_critical_bh(&priv->br_ext_lock, &irqL);
+
+	db = priv->nethash[__nat25_network_hash(networkAddr)];
+	while (db != NULL)
+	{
+		if(!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN))
+		{
+			if(!__nat25_has_expired(priv, db))
+			{
+				// replace the destination mac address
+				memcpy(skb->data, db->macAddr, ETH_ALEN);
+				atomic_inc(&db->use_count);
+
+#ifdef CL_IPV6_PASS
+				DEBUG_INFO("NAT25: Lookup M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x"	
+							"%02x%02x%02x%02x%02x%02x\n",				
+					db->macAddr[0],
+					db->macAddr[1],
+					db->macAddr[2],
+					db->macAddr[3],
+					db->macAddr[4],
+					db->macAddr[5],
+					db->networkAddr[0],
+					db->networkAddr[1],
+					db->networkAddr[2],
+					db->networkAddr[3],
+					db->networkAddr[4],
+					db->networkAddr[5],
+					db->networkAddr[6],
+					db->networkAddr[7],
+					db->networkAddr[8],
+					db->networkAddr[9],
+					db->networkAddr[10],
+					db->networkAddr[11],
+					db->networkAddr[12],
+					db->networkAddr[13],
+					db->networkAddr[14],
+					db->networkAddr[15],
+					db->networkAddr[16]);
+#else				
+				DEBUG_INFO("NAT25: Lookup M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
+					db->macAddr[0],
+					db->macAddr[1],
+					db->macAddr[2],
+					db->macAddr[3],
+					db->macAddr[4],
+					db->macAddr[5],
+					db->networkAddr[0],
+					db->networkAddr[1],
+					db->networkAddr[2],
+					db->networkAddr[3],
+					db->networkAddr[4],
+					db->networkAddr[5],
+					db->networkAddr[6],
+					db->networkAddr[7],
+					db->networkAddr[8],
+					db->networkAddr[9],
+					db->networkAddr[10]);
+#endif
+			}
+			_exit_critical_bh(&priv->br_ext_lock, &irqL);
+			return 1;
+		}
+
+		db = db->next_hash;
+	}
+
+	_exit_critical_bh(&priv->br_ext_lock, &irqL);
+	return 0;
+}
+
+
+static void __nat25_db_network_insert(_adapter *priv,
+				unsigned char *macAddr, unsigned char *networkAddr)
+{
+	struct nat25_network_db_entry *db;
+	int hash;
+	_irqL irqL;
+	_enter_critical_bh(&priv->br_ext_lock, &irqL);
+
+	hash = __nat25_network_hash(networkAddr);
+	db = priv->nethash[hash];
+	while (db != NULL)
+	{
+		if(!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN))
+		{
+			memcpy(db->macAddr, macAddr, ETH_ALEN);
+			db->ageing_timer = jiffies;
+			_exit_critical_bh(&priv->br_ext_lock, &irqL);
+			return;
+		}
+
+		db = db->next_hash;
+	}
+
+	db = (struct nat25_network_db_entry *) rtw_malloc(sizeof(*db));
+	if(db == NULL) {
+		_exit_critical_bh(&priv->br_ext_lock, &irqL);
+		return;
+	}
+
+	memcpy(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN);
+	memcpy(db->macAddr, macAddr, ETH_ALEN);
+	atomic_set(&db->use_count, 1);
+	db->ageing_timer = jiffies;
+
+	__network_hash_link(priv, db, hash);
+
+	_exit_critical_bh(&priv->br_ext_lock, &irqL);
+}
+
+
+static void __nat25_db_print(_adapter *priv)
+{
+	_irqL irqL;
+	_enter_critical_bh(&priv->br_ext_lock, &irqL);
+
+#ifdef BR_EXT_DEBUG
+	static int counter = 0;
+	int i, j;
+	struct nat25_network_db_entry *db;
+
+	counter++;
+	if((counter % 16) != 0)
+		return;
+
+	for(i=0, j=0; i<NAT25_HASH_SIZE; i++)
+	{
+		db = priv->nethash[i];
+
+		while (db != NULL)
+		{
+#ifdef CL_IPV6_PASS
+			panic_printk("NAT25: DB(%d) H(%02d) C(%d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x"
+					"%02x%02x%02x%02x%02x%02x\n",
+				j,
+				i,
+				atomic_read(&db->use_count),
+				db->macAddr[0],
+				db->macAddr[1],
+				db->macAddr[2],
+				db->macAddr[3],
+				db->macAddr[4],
+				db->macAddr[5],
+				db->networkAddr[0],
+				db->networkAddr[1],
+				db->networkAddr[2],
+				db->networkAddr[3],
+				db->networkAddr[4],
+				db->networkAddr[5],
+				db->networkAddr[6],
+				db->networkAddr[7],
+				db->networkAddr[8],
+				db->networkAddr[9],
+				db->networkAddr[10],
+				db->networkAddr[11],
+				db->networkAddr[12],
+				db->networkAddr[13],
+				db->networkAddr[14],
+				db->networkAddr[15],
+				db->networkAddr[16]);
+#else
+			panic_printk("NAT25: DB(%d) H(%02d) C(%d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
+				j,
+				i,
+				atomic_read(&db->use_count),
+				db->macAddr[0],
+				db->macAddr[1],
+				db->macAddr[2],
+				db->macAddr[3],
+				db->macAddr[4],
+				db->macAddr[5],
+				db->networkAddr[0],
+				db->networkAddr[1],
+				db->networkAddr[2],
+				db->networkAddr[3],
+				db->networkAddr[4],
+				db->networkAddr[5],
+				db->networkAddr[6],
+				db->networkAddr[7],
+				db->networkAddr[8],
+				db->networkAddr[9],
+				db->networkAddr[10]);
+#endif
+			j++;
+
+			db = db->next_hash;
+		}
+	}
+#endif
+
+	_exit_critical_bh(&priv->br_ext_lock, &irqL);
+}
+
+
+
+
+/*
+ *	NAT2.5 interface
+ */
+
+void nat25_db_cleanup(_adapter *priv)
+{
+	int i;
+	_irqL irqL;
+	_enter_critical_bh(&priv->br_ext_lock, &irqL);
+	
+	for(i=0; i<NAT25_HASH_SIZE; i++)
+	{
+		struct nat25_network_db_entry *f;
+		f = priv->nethash[i];
+		while (f != NULL) {
+			struct nat25_network_db_entry *g;
+
+			g = f->next_hash;
+			if(priv->scdb_entry == f)
+			{
+				memset(priv->scdb_mac, 0, ETH_ALEN);
+				memset(priv->scdb_ip, 0, 4);
+				priv->scdb_entry = NULL;
+			}
+			__network_hash_unlink(f);
+			rtw_mfree((u8 *) f, sizeof(struct nat25_network_db_entry));
+
+			f = g;
+		}
+	}
+
+	_exit_critical_bh(&priv->br_ext_lock, &irqL);
+}
+
+
+void nat25_db_expire(_adapter *priv)
+{
+	int i;
+	_irqL irqL;
+	_enter_critical_bh(&priv->br_ext_lock, &irqL);
+	
+	//if(!priv->ethBrExtInfo.nat25_disable)
+	{
+		for (i=0; i<NAT25_HASH_SIZE; i++)
+		{
+			struct nat25_network_db_entry *f;
+			f = priv->nethash[i];
+
+			while (f != NULL)
+			{
+				struct nat25_network_db_entry *g;
+				g = f->next_hash;
+
+				if(__nat25_has_expired(priv, f))
+				{
+					if(atomic_dec_and_test(&f->use_count))
+					{
+#ifdef BR_EXT_DEBUG
+#ifdef CL_IPV6_PASS
+						panic_printk("NAT25 Expire H(%02d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x"
+								"%02x%02x%02x%02x%02x%02x\n",
+							i,
+							f->macAddr[0],
+							f->macAddr[1],
+							f->macAddr[2],
+							f->macAddr[3],
+							f->macAddr[4],
+							f->macAddr[5],
+							f->networkAddr[0],
+							f->networkAddr[1],
+							f->networkAddr[2],
+							f->networkAddr[3],
+							f->networkAddr[4],
+							f->networkAddr[5],
+							f->networkAddr[6],
+							f->networkAddr[7],
+							f->networkAddr[8],
+							f->networkAddr[9],
+							f->networkAddr[10],
+							f->networkAddr[11],
+							f->networkAddr[12],
+							f->networkAddr[13],
+							f->networkAddr[14],
+							f->networkAddr[15],
+							f->networkAddr[16]);
+#else
+
+						panic_printk("NAT25 Expire H(%02d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
+							i,
+							f->macAddr[0],
+							f->macAddr[1],
+							f->macAddr[2],
+							f->macAddr[3],
+							f->macAddr[4],
+							f->macAddr[5],
+							f->networkAddr[0],
+							f->networkAddr[1],
+							f->networkAddr[2],
+							f->networkAddr[3],
+							f->networkAddr[4],
+							f->networkAddr[5],
+							f->networkAddr[6],
+							f->networkAddr[7],
+							f->networkAddr[8],
+							f->networkAddr[9],
+							f->networkAddr[10]);
+#endif
+#endif
+						if(priv->scdb_entry == f)
+						{
+							memset(priv->scdb_mac, 0, ETH_ALEN);
+							memset(priv->scdb_ip, 0, 4);
+							priv->scdb_entry = NULL;
+						}
+						__network_hash_unlink(f);
+						rtw_mfree((u8 *) f, sizeof(struct nat25_network_db_entry));
+					}
+				}
+
+				f = g;
+			}
+		}
+	}
+
+	_exit_critical_bh(&priv->br_ext_lock, &irqL);
+}
+
+
+#ifdef SUPPORT_TX_MCAST2UNI
+static int checkIPMcAndReplace(_adapter *priv, struct sk_buff *skb, unsigned int *dst_ip)
+{
+	struct stat_info	*pstat;
+	struct list_head	*phead, *plist;
+	int i;
+
+	phead = &priv->asoc_list;
+	plist = phead->next;
+
+	while (plist != phead) {
+		pstat = list_entry(plist, struct stat_info, asoc_list);
+		plist = plist->next;
+
+		if (pstat->ipmc_num == 0)
+			continue;
+
+		for (i=0; i<MAX_IP_MC_ENTRY; i++) {
+			if (pstat->ipmc[i].used && !memcmp(&pstat->ipmc[i].mcmac[3], ((unsigned char *)dst_ip)+1, 3)) {
+				memcpy(skb->data, pstat->ipmc[i].mcmac, ETH_ALEN);
+				return 1;
+			}
+		}
+	}
+	return 0;
+}
+#endif
+
+int nat25_db_handle(_adapter *priv, struct sk_buff *skb, int method)
+{
+	unsigned short protocol;
+	unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
+
+	if(skb == NULL)
+		return -1;
+
+	if((method <= NAT25_MIN) || (method >= NAT25_MAX))
+		return -1;
+
+	protocol = *((unsigned short *)(skb->data + 2 * ETH_ALEN));
+
+	/*---------------------------------------------------*/
+	/*                 Handle IP frame                   */
+	/*---------------------------------------------------*/
+	if(protocol == __constant_htons(ETH_P_IP))
+	{
+		struct iphdr* iph = (struct iphdr *)(skb->data + ETH_HLEN);
+
+		if(((unsigned char*)(iph) + (iph->ihl<<2)) >= (skb->data + ETH_HLEN + skb->len))
+		{
+			DEBUG_WARN("NAT25: malformed IP packet !\n");
+			return -1;
+		}
+
+		switch(method)
+		{
+			case NAT25_CHECK:
+				return -1;
+
+			case NAT25_INSERT:
+				{
+					//some muticast with source IP is all zero, maybe other case is illegal
+					//in class A, B, C, host address is all zero or all one is illegal
+					if (iph->saddr == 0)
+						return 0;
+					DEBUG_INFO("NAT25: Insert IP, SA=%08x, DA=%08x\n", iph->saddr, iph->daddr);
+					__nat25_generate_ipv4_network_addr(networkAddr, &iph->saddr);
+					//record source IP address and , source mac address into db
+					__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+
+					__nat25_db_print(priv);
+				}
+				return 0;
+
+			case NAT25_LOOKUP:
+				{
+					DEBUG_INFO("NAT25: Lookup IP, SA=%08x, DA=%08x\n", iph->saddr, iph->daddr);
+#ifdef SUPPORT_TX_MCAST2UNI
+					if (priv->pshare->rf_ft_var.mc2u_disable ||
+							((((OPMODE & (WIFI_STATION_STATE|WIFI_ASOC_STATE))
+							== (WIFI_STATION_STATE|WIFI_ASOC_STATE)) &&
+							!checkIPMcAndReplace(priv, skb, &iph->daddr)) ||
+							(OPMODE & WIFI_ADHOC_STATE)))
+#endif
+					{
+						__nat25_generate_ipv4_network_addr(networkAddr, &iph->daddr);
+
+						if (!__nat25_db_network_lookup_and_replace(priv, skb, networkAddr)) {
+							if (*((unsigned char *)&iph->daddr + 3) == 0xff) {
+								// L2 is unicast but L3 is broadcast, make L2 bacome broadcast
+								DEBUG_INFO("NAT25: Set DA as boardcast\n");
+								memset(skb->data, 0xff, ETH_ALEN);
+							}
+							else {
+								// forward unknow IP packet to upper TCP/IP
+								DEBUG_INFO("NAT25: Replace DA with BR's MAC\n");
+								if ( (*(u32 *)priv->br_mac) == 0 && (*(u16 *)(priv->br_mac+4)) == 0 ) {
+									void netdev_br_init(struct net_device *netdev);
+									printk("Re-init netdev_br_init() due to br_mac==0!\n");
+									netdev_br_init(priv->pnetdev);
+								}
+								memcpy(skb->data, priv->br_mac, ETH_ALEN);
+							}
+						}
+					}
+				}
+				return 0;
+
+			default:
+				return -1;
+		}
+	}
+
+	/*---------------------------------------------------*/
+	/*                 Handle ARP frame                  */
+	/*---------------------------------------------------*/
+	else if(protocol == __constant_htons(ETH_P_ARP))
+	{
+		struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
+		unsigned char *arp_ptr = (unsigned char *)(arp + 1);
+		unsigned int *sender, *target;
+
+		if(arp->ar_pro != __constant_htons(ETH_P_IP))
+		{
+			DEBUG_WARN("NAT25: arp protocol unknown (%4x)!\n", htons(arp->ar_pro));
+			return -1;
+		}
+
+		switch(method)
+		{
+			case NAT25_CHECK:
+				return 0;	// skb_copy for all ARP frame
+
+			case NAT25_INSERT:
+				{
+					DEBUG_INFO("NAT25: Insert ARP, MAC=%02x%02x%02x%02x%02x%02x\n", arp_ptr[0],
+						arp_ptr[1], arp_ptr[2], arp_ptr[3], arp_ptr[4], arp_ptr[5]);
+
+					// change to ARP sender mac address to wlan STA address
+                                        memcpy(arp_ptr, GET_MY_HWADDR(priv), ETH_ALEN);
+
+					arp_ptr += arp->ar_hln;
+					sender = (unsigned int *)arp_ptr;
+
+					__nat25_generate_ipv4_network_addr(networkAddr, sender);
+
+					__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+
+					__nat25_db_print(priv);
+				}
+				return 0;
+
+			case NAT25_LOOKUP:
+				{
+					DEBUG_INFO("NAT25: Lookup ARP\n");
+
+					arp_ptr += arp->ar_hln;
+					sender = (unsigned int *)arp_ptr;
+					arp_ptr += (arp->ar_hln + arp->ar_pln);
+					target = (unsigned int *)arp_ptr;
+
+					__nat25_generate_ipv4_network_addr(networkAddr, target);
+
+					__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+
+					// change to ARP target mac address to Lookup result
+					arp_ptr = (unsigned char *)(arp + 1);
+					arp_ptr += (arp->ar_hln + arp->ar_pln);
+					memcpy(arp_ptr, skb->data, ETH_ALEN);
+				}
+				return 0;
+
+			default:
+				return -1;
+		}
+	}
+
+	/*---------------------------------------------------*/
+	/*         Handle IPX and Apple Talk frame           */
+	/*---------------------------------------------------*/
+	else if((protocol == __constant_htons(ETH_P_IPX)) ||
+		(protocol <= __constant_htons(ETH_FRAME_LEN)))
+	{
+		unsigned char ipx_header[2] = {0xFF, 0xFF};
+		struct ipxhdr	*ipx = NULL;
+		struct elapaarp	*ea = NULL;
+		struct ddpehdr	*ddp = NULL;
+		unsigned char *framePtr = skb->data + ETH_HLEN;
+
+		if(protocol == __constant_htons(ETH_P_IPX))
+		{
+			DEBUG_INFO("NAT25: Protocol=IPX (Ethernet II)\n");
+			ipx = (struct ipxhdr *)framePtr;
+		}
+		else if(protocol <= __constant_htons(ETH_FRAME_LEN))
+		{
+			if(!memcmp(ipx_header, framePtr, 2))
+			{
+				DEBUG_INFO("NAT25: Protocol=IPX (Ethernet 802.3)\n");
+				ipx = (struct ipxhdr *)framePtr;
+			}
+			else
+			{
+				unsigned char ipx_8022_type =  0xE0;
+				unsigned char snap_8022_type = 0xAA;
+
+				if(*framePtr == snap_8022_type)
+				{
+					unsigned char ipx_snap_id[5] = {0x0, 0x0, 0x0, 0x81, 0x37};		// IPX SNAP ID
+					unsigned char aarp_snap_id[5] = {0x00, 0x00, 0x00, 0x80, 0xF3};	// Apple Talk AARP SNAP ID
+					unsigned char ddp_snap_id[5] = {0x08, 0x00, 0x07, 0x80, 0x9B};	// Apple Talk DDP SNAP ID
+
+					framePtr += 3;	// eliminate the 802.2 header
+
+					if(!memcmp(ipx_snap_id, framePtr, 5))
+					{
+						framePtr += 5;	// eliminate the SNAP header
+
+						DEBUG_INFO("NAT25: Protocol=IPX (Ethernet SNAP)\n");
+						ipx = (struct ipxhdr *)framePtr;
+					}
+					else if(!memcmp(aarp_snap_id, framePtr, 5))
+					{
+						framePtr += 5;	// eliminate the SNAP header
+
+						ea = (struct elapaarp *)framePtr;
+					}
+					else if(!memcmp(ddp_snap_id, framePtr, 5))
+					{
+						framePtr += 5;	// eliminate the SNAP header
+
+						ddp = (struct ddpehdr *)framePtr;
+					}
+					else
+					{
+						DEBUG_WARN("NAT25: Protocol=Ethernet SNAP %02x%02x%02x%02x%02x\n", framePtr[0],
+							framePtr[1], framePtr[2], framePtr[3], framePtr[4]);
+						return -1;
+					}
+				}
+				else if(*framePtr == ipx_8022_type)
+				{
+					framePtr += 3;	// eliminate the 802.2 header
+
+					if(!memcmp(ipx_header, framePtr, 2))
+					{
+						DEBUG_INFO("NAT25: Protocol=IPX (Ethernet 802.2)\n");
+						ipx = (struct ipxhdr *)framePtr;
+					}
+					else
+						return -1;
+				}
+				else
+					return -1;
+			}
+		}
+		else
+			return -1;
+
+		/*   IPX   */
+		if(ipx != NULL)
+		{
+			switch(method)
+			{
+				case NAT25_CHECK:
+					if(!memcmp(skb->data+ETH_ALEN, ipx->ipx_source.node, ETH_ALEN))
+					{
+						DEBUG_INFO("NAT25: Check IPX skb_copy\n");
+						return 0;
+					}
+					return -1;
+
+				case NAT25_INSERT:
+					{
+						DEBUG_INFO("NAT25: Insert IPX, Dest=%08x,%02x%02x%02x%02x%02x%02x,%04x Source=%08x,%02x%02x%02x%02x%02x%02x,%04x\n",
+							ipx->ipx_dest.net,
+							ipx->ipx_dest.node[0],
+							ipx->ipx_dest.node[1],
+							ipx->ipx_dest.node[2],
+							ipx->ipx_dest.node[3],
+							ipx->ipx_dest.node[4],
+							ipx->ipx_dest.node[5],
+							ipx->ipx_dest.sock,
+							ipx->ipx_source.net,
+							ipx->ipx_source.node[0],
+							ipx->ipx_source.node[1],
+							ipx->ipx_source.node[2],
+							ipx->ipx_source.node[3],
+							ipx->ipx_source.node[4],
+							ipx->ipx_source.node[5],
+							ipx->ipx_source.sock);
+
+						if(!memcmp(skb->data+ETH_ALEN, ipx->ipx_source.node, ETH_ALEN))
+						{
+							DEBUG_INFO("NAT25: Use IPX Net, and Socket as network addr\n");
+
+							__nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_source.net, &ipx->ipx_source.sock);
+
+							// change IPX source node addr to wlan STA address
+                                                        memcpy(ipx->ipx_source.node, GET_MY_HWADDR(priv), ETH_ALEN);
+						}
+						else
+						{
+							__nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_source.net, ipx->ipx_source.node);
+						}
+
+						__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+
+						__nat25_db_print(priv);
+					}
+					return 0;
+
+				case NAT25_LOOKUP:
+					{
+                                                if(!memcmp(GET_MY_HWADDR(priv), ipx->ipx_dest.node, ETH_ALEN))
+						{
+							DEBUG_INFO("NAT25: Lookup IPX, Modify Destination IPX Node addr\n");
+
+							__nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_dest.net, &ipx->ipx_dest.sock);
+
+							__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+
+							// replace IPX destination node addr with Lookup destination MAC addr
+							memcpy(ipx->ipx_dest.node, skb->data, ETH_ALEN);
+						}
+						else
+						{
+							__nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_dest.net, ipx->ipx_dest.node);
+
+							__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+						}
+					}
+					return 0;
+
+				default:
+					return -1;
+			}
+		}
+
+		/*   AARP   */
+		else if(ea != NULL)
+		{
+			/* Sanity check fields. */
+			if(ea->hw_len != ETH_ALEN || ea->pa_len != AARP_PA_ALEN)
+			{
+				DEBUG_WARN("NAT25: Appletalk AARP Sanity check fail!\n");
+				return -1;
+			}
+
+			switch(method)
+			{
+				case NAT25_CHECK:
+					return 0;
+
+				case NAT25_INSERT:
+					{
+						// change to AARP source mac address to wlan STA address
+                                                memcpy(ea->hw_src, GET_MY_HWADDR(priv), ETH_ALEN);
+
+						DEBUG_INFO("NAT25: Insert AARP, Source=%d,%d Destination=%d,%d\n",
+							ea->pa_src_net,
+							ea->pa_src_node,
+							ea->pa_dst_net,
+							ea->pa_dst_node);
+
+						__nat25_generate_apple_network_addr(networkAddr, &ea->pa_src_net, &ea->pa_src_node);
+
+						__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+
+						__nat25_db_print(priv);
+					}
+					return 0;
+
+				case NAT25_LOOKUP:
+					{
+						DEBUG_INFO("NAT25: Lookup AARP, Source=%d,%d Destination=%d,%d\n",
+							ea->pa_src_net,
+							ea->pa_src_node,
+							ea->pa_dst_net,
+							ea->pa_dst_node);
+
+						__nat25_generate_apple_network_addr(networkAddr, &ea->pa_dst_net, &ea->pa_dst_node);
+
+						__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+
+						// change to AARP destination mac address to Lookup result
+						memcpy(ea->hw_dst, skb->data, ETH_ALEN);
+					}
+					return 0;
+
+				default:
+					return -1;
+			}
+		}
+
+		/*   DDP   */
+		else if(ddp != NULL)
+		{
+			switch(method)
+			{
+				case NAT25_CHECK:
+					return -1;
+
+				case NAT25_INSERT:
+					{
+						DEBUG_INFO("NAT25: Insert DDP, Source=%d,%d Destination=%d,%d\n",
+							ddp->deh_snet,
+							ddp->deh_snode,
+							ddp->deh_dnet,
+							ddp->deh_dnode);
+
+						__nat25_generate_apple_network_addr(networkAddr, &ddp->deh_snet, &ddp->deh_snode);
+
+						__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+
+						__nat25_db_print(priv);
+					}
+					return 0;
+
+				case NAT25_LOOKUP:
+					{
+						DEBUG_INFO("NAT25: Lookup DDP, Source=%d,%d Destination=%d,%d\n",
+							ddp->deh_snet,
+							ddp->deh_snode,
+							ddp->deh_dnet,
+							ddp->deh_dnode);
+
+						__nat25_generate_apple_network_addr(networkAddr, &ddp->deh_dnet, &ddp->deh_dnode);
+
+						__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+					}
+					return 0;
+
+				default:
+					return -1;
+			}
+		}
+
+		return -1;
+	}
+
+	/*---------------------------------------------------*/
+	/*                Handle PPPoE frame                 */
+	/*---------------------------------------------------*/
+	else if((protocol == __constant_htons(ETH_P_PPP_DISC)) ||
+		(protocol == __constant_htons(ETH_P_PPP_SES)))
+	{
+		struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
+		unsigned short *pMagic;
+
+		switch(method)
+		{
+			case NAT25_CHECK:
+				if (ph->sid == 0)
+					return 0;
+				return 1;
+
+			case NAT25_INSERT:
+				if(ph->sid == 0)	// Discovery phase according to tag
+				{
+					if(ph->code == PADI_CODE || ph->code == PADR_CODE)
+					{
+						if (priv->ethBrExtInfo.addPPPoETag) {
+							struct pppoe_tag *tag, *pOldTag;
+							unsigned char tag_buf[40];
+							int old_tag_len=0;
+
+							tag = (struct pppoe_tag *)tag_buf;
+							pOldTag = (struct pppoe_tag *)__nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID));
+							if (pOldTag) { // if SID existed, copy old value and delete it
+								old_tag_len = ntohs(pOldTag->tag_len);
+								if (old_tag_len+TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN > sizeof(tag_buf)) {
+									DEBUG_ERR("SID tag length too long!\n");
+									return -1;
+								}
+
+								memcpy(tag->tag_data+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN,
+									pOldTag->tag_data, old_tag_len);
+
+								if (skb_pull_and_merge(skb, (unsigned char *)pOldTag, TAG_HDR_LEN+old_tag_len) < 0) {
+									DEBUG_ERR("call skb_pull_and_merge() failed in PADI/R packet!\n");
+									return -1;
+								}
+								ph->length = htons(ntohs(ph->length)-TAG_HDR_LEN-old_tag_len);
+							}
+
+							tag->tag_type = PTT_RELAY_SID;
+							tag->tag_len = htons(MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN+old_tag_len);
+
+							// insert the magic_code+client mac in relay tag
+							pMagic = (unsigned short *)tag->tag_data;
+							*pMagic = htons(MAGIC_CODE);
+							memcpy(tag->tag_data+MAGIC_CODE_LEN, skb->data+ETH_ALEN, ETH_ALEN);
+
+							//Add relay tag
+							if(__nat25_add_pppoe_tag(skb, tag) < 0)
+								return -1;
+
+							DEBUG_INFO("NAT25: Insert PPPoE, forward %s packet\n",
+											(ph->code == PADI_CODE ? "PADI" : "PADR"));
+						}
+						else { // not add relay tag
+							if (priv->pppoe_connection_in_progress &&
+									memcmp(skb->data+ETH_ALEN, priv->pppoe_addr, ETH_ALEN))	 {
+								DEBUG_ERR("Discard PPPoE packet due to another PPPoE connection is in progress!\n");
+								return -2;
+							}
+
+							if (priv->pppoe_connection_in_progress == 0)
+								memcpy(priv->pppoe_addr, skb->data+ETH_ALEN, ETH_ALEN);
+
+							priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
+						}
+					}
+					else
+						return -1;
+				}
+				else	// session phase
+				{
+						DEBUG_INFO("NAT25: Insert PPPoE, insert session packet to %s\n", skb->dev->name);
+
+						__nat25_generate_pppoe_network_addr(networkAddr, skb->data, &(ph->sid));
+
+						__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+
+						__nat25_db_print(priv);
+
+						if (!priv->ethBrExtInfo.addPPPoETag &&
+								priv->pppoe_connection_in_progress &&
+									!memcmp(skb->data+ETH_ALEN, priv->pppoe_addr, ETH_ALEN))
+							priv->pppoe_connection_in_progress = 0;
+				}
+				return 0;
+
+			case NAT25_LOOKUP:
+				if(ph->code == PADO_CODE || ph->code == PADS_CODE)
+				{
+					if (priv->ethBrExtInfo.addPPPoETag) {
+						struct pppoe_tag *tag;
+						unsigned char *ptr;
+						unsigned short tagType, tagLen;
+						int offset=0;
+
+						if((ptr = __nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID))) == 0) {
+							DEBUG_ERR("Fail to find PTT_RELAY_SID in FADO!\n");
+							return -1;
+						}
+
+						tag = (struct pppoe_tag *)ptr;
+						tagType = (unsigned short)((ptr[0] << 8) + ptr[1]);
+						tagLen = (unsigned short)((ptr[2] << 8) + ptr[3]);
+
+						if((tagType != ntohs(PTT_RELAY_SID)) || (tagLen < (MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN))) {
+							DEBUG_ERR("Invalid PTT_RELAY_SID tag length [%d]!\n", tagLen);
+							return -1;
+						}
+
+						pMagic = (unsigned short *)tag->tag_data;
+						if (ntohs(*pMagic) != MAGIC_CODE) {
+							DEBUG_ERR("Can't find MAGIC_CODE in %s packet!\n",
+								(ph->code == PADO_CODE ? "PADO" : "PADS"));
+							return -1;
+						}
+
+						memcpy(skb->data, tag->tag_data+MAGIC_CODE_LEN, ETH_ALEN);
+
+						if (tagLen > MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN)
+							offset = TAG_HDR_LEN;
+
+						if (skb_pull_and_merge(skb, ptr+offset, TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN-offset) < 0) {
+							DEBUG_ERR("call skb_pull_and_merge() failed in PADO packet!\n");
+							return -1;
+						}
+						ph->length = htons(ntohs(ph->length)-(TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN-offset));
+						if (offset > 0)
+							tag->tag_len = htons(tagLen-MAGIC_CODE_LEN-RTL_RELAY_TAG_LEN);
+
+						DEBUG_INFO("NAT25: Lookup PPPoE, forward %s Packet from %s\n",
+							(ph->code == PADO_CODE ? "PADO" : "PADS"),	skb->dev->name);
+					}
+					else { // not add relay tag
+						if (!priv->pppoe_connection_in_progress) {
+							DEBUG_ERR("Discard PPPoE packet due to no connection in progresss!\n");
+							return -1;
+						}
+						memcpy(skb->data, priv->pppoe_addr, ETH_ALEN);
+						priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
+					}
+				}
+				else {
+					if(ph->sid != 0)
+					{
+						DEBUG_INFO("NAT25: Lookup PPPoE, lookup session packet from %s\n", skb->dev->name);
+						__nat25_generate_pppoe_network_addr(networkAddr, skb->data+ETH_ALEN, &(ph->sid));
+
+						__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+
+						__nat25_db_print(priv);
+					}
+					else
+						return -1;
+
+				}
+				return 0;
+
+			default:
+				return -1;
+		}
+	}
+
+	/*---------------------------------------------------*/
+	/*                 Handle EAP frame                  */
+	/*---------------------------------------------------*/
+	else if(protocol == __constant_htons(0x888e))
+	{
+		switch(method)
+		{
+			case NAT25_CHECK:
+				return -1;
+
+			case NAT25_INSERT:
+				return 0;
+
+			case NAT25_LOOKUP:
+				return 0;
+
+			default:
+				return -1;
+		}
+	}
+
+	/*---------------------------------------------------*/
+	/*         Handle C-Media proprietary frame          */
+	/*---------------------------------------------------*/
+	else if((protocol == __constant_htons(0xe2ae)) ||
+		(protocol == __constant_htons(0xe2af)))
+	{
+		switch(method)
+		{
+			case NAT25_CHECK:
+				return -1;
+
+			case NAT25_INSERT:
+				return 0;
+
+			case NAT25_LOOKUP:
+				return 0;
+
+			default:
+				return -1;
+		}
+	}
+
+	/*---------------------------------------------------*/
+	/*         Handle IPV6 frame      							  */
+	/*---------------------------------------------------*/
+#ifdef CL_IPV6_PASS
+	else if(protocol == __constant_htons(ETH_P_IPV6))
+	{
+		struct ipv6hdr *iph = (struct ipv6hdr *)(skb->data + ETH_HLEN);
+
+		if (sizeof(*iph) >= (skb->len - ETH_HLEN))
+		{
+			DEBUG_WARN("NAT25: malformed IPv6 packet !\n");
+			return -1;
+		}
+
+		switch(method)
+		{
+			case NAT25_CHECK:
+				if (skb->data[0] & 1)
+					return 0;				
+				return -1;
+
+			case NAT25_INSERT:
+				{
+					DEBUG_INFO("NAT25: Insert IP, SA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x,"
+									" DA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n", 
+						iph->saddr.s6_addr16[0],iph->saddr.s6_addr16[1],iph->saddr.s6_addr16[2],iph->saddr.s6_addr16[3],
+						iph->saddr.s6_addr16[4],iph->saddr.s6_addr16[5],iph->saddr.s6_addr16[6],iph->saddr.s6_addr16[7],
+						iph->daddr.s6_addr16[0],iph->daddr.s6_addr16[1],iph->daddr.s6_addr16[2],iph->daddr.s6_addr16[3],
+						iph->daddr.s6_addr16[4],iph->daddr.s6_addr16[5],iph->daddr.s6_addr16[6],iph->daddr.s6_addr16[7]);
+
+					if (memcmp(&iph->saddr, "\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0", 16)) {
+						__nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->saddr);
+						__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
+						__nat25_db_print(priv);
+
+						if (iph->nexthdr == IPPROTO_ICMPV6 && 
+								skb->len > (ETH_HLEN +  sizeof(*iph) + 4)) {
+							if (update_nd_link_layer_addr(skb->data + ETH_HLEN + sizeof(*iph), 
+                                                                skb->len - ETH_HLEN - sizeof(*iph), GET_MY_HWADDR(priv))) {                                                   
+								struct icmp6hdr  *hdr = (struct icmp6hdr *)(skb->data + ETH_HLEN + sizeof(*iph));
+								hdr->icmp6_cksum = 0;
+								hdr->icmp6_cksum = csum_ipv6_magic(&iph->saddr, &iph->daddr,
+												iph->payload_len,
+												IPPROTO_ICMPV6,
+												csum_partial((__u8 *)hdr, iph->payload_len, 0));
+							}
+						}						
+					}
+				}
+				return 0;
+
+			case NAT25_LOOKUP:
+				DEBUG_INFO("NAT25: Lookup IP, SA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x,"
+								" DA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n", 
+						iph->saddr.s6_addr16[0],iph->saddr.s6_addr16[1],iph->saddr.s6_addr16[2],iph->saddr.s6_addr16[3],
+						iph->saddr.s6_addr16[4],iph->saddr.s6_addr16[5],iph->saddr.s6_addr16[6],iph->saddr.s6_addr16[7],
+						iph->daddr.s6_addr16[0],iph->daddr.s6_addr16[1],iph->daddr.s6_addr16[2],iph->daddr.s6_addr16[3],
+						iph->daddr.s6_addr16[4],iph->daddr.s6_addr16[5],iph->daddr.s6_addr16[6],iph->daddr.s6_addr16[7]);
+			
+
+				__nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->daddr);
+				if (!__nat25_db_network_lookup_and_replace(priv, skb, networkAddr)) {
+#ifdef SUPPORT_RX_UNI2MCAST							
+					if (iph->daddr.s6_addr[0] == 0xff)
+						convert_ipv6_mac_to_mc(skb);	
+#endif											
+				}
+				return 0;
+
+			default:
+				return -1;
+		}
+	}
+#endif	// CL_IPV6_PASS
+
+	return -1;
+}
+
+
+int nat25_handle_frame(_adapter *priv, struct sk_buff *skb)
+{
+#ifdef BR_EXT_DEBUG
+	if((!priv->ethBrExtInfo.nat25_disable) && (!(skb->data[0] & 1)))
+	{
+		panic_printk("NAT25: Input Frame: DA=%02x%02x%02x%02x%02x%02x SA=%02x%02x%02x%02x%02x%02x\n",
+			skb->data[0],
+			skb->data[1],
+			skb->data[2],
+			skb->data[3],
+			skb->data[4],
+			skb->data[5],
+			skb->data[6],
+			skb->data[7],
+			skb->data[8],
+			skb->data[9],
+			skb->data[10],
+			skb->data[11]);
+	}
+#endif
+
+	if(!(skb->data[0] & 1))
+	{
+		int is_vlan_tag=0, i, retval=0;
+		unsigned short vlan_hdr=0;
+
+		if (*((unsigned short *)(skb->data+ETH_ALEN*2)) == __constant_htons(ETH_P_8021Q)) {
+			is_vlan_tag = 1;
+			vlan_hdr = *((unsigned short *)(skb->data+ETH_ALEN*2+2));
+			for (i=0; i<6; i++)
+				*((unsigned short *)(skb->data+ETH_ALEN*2+2-i*2)) = *((unsigned short *)(skb->data+ETH_ALEN*2-2-i*2));
+			skb_pull(skb, 4);
+		}
+
+		if (!priv->ethBrExtInfo.nat25_disable)
+		{
+			_irqL irqL;
+			_enter_critical_bh(&priv->br_ext_lock, &irqL);
+			/*
+			 *	This function look up the destination network address from
+			 *	the NAT2.5 database. Return value = -1 means that the
+			 *	corresponding network protocol is NOT support.
+			 */
+			if (!priv->ethBrExtInfo.nat25sc_disable &&
+				(*((unsigned short *)(skb->data+ETH_ALEN*2)) == __constant_htons(ETH_P_IP)) &&
+				!memcmp(priv->scdb_ip, skb->data+ETH_HLEN+16, 4)) {
+				memcpy(skb->data, priv->scdb_mac, ETH_ALEN);
+				
+				_exit_critical_bh(&priv->br_ext_lock, &irqL);
+			}
+			else {
+				_exit_critical_bh(&priv->br_ext_lock, &irqL);
+				
+				retval = nat25_db_handle(priv, skb, NAT25_LOOKUP);
+			}
+		}
+		else {
+			if (((*((unsigned short *)(skb->data+ETH_ALEN*2)) == __constant_htons(ETH_P_IP)) &&
+					!memcmp(priv->br_ip, skb->data+ETH_HLEN+16, 4)) ||
+				((*((unsigned short *)(skb->data+ETH_ALEN*2)) == __constant_htons(ETH_P_ARP)) &&
+					!memcmp(priv->br_ip, skb->data+ETH_HLEN+24, 4))) {
+				// for traffic to upper TCP/IP
+				retval = nat25_db_handle(priv, skb, NAT25_LOOKUP);
+			}
+		}
+
+		if (is_vlan_tag) {
+			skb_push(skb, 4);
+			for (i=0; i<6; i++)
+				*((unsigned short *)(skb->data+i*2)) = *((unsigned short *)(skb->data+4+i*2));
+			*((unsigned short *)(skb->data+ETH_ALEN*2)) = __constant_htons(ETH_P_8021Q);
+			*((unsigned short *)(skb->data+ETH_ALEN*2+2)) = vlan_hdr;
+		}
+
+		if(retval == -1) {
+			//DEBUG_ERR("NAT25: Lookup fail!\n");
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+#if 0
+void mac_clone(_adapter *priv, unsigned char *addr)
+{
+	struct sockaddr sa;
+
+	memcpy(sa.sa_data, addr, ETH_ALEN);
+	DEBUG_INFO("MAC Clone: Addr=%02x%02x%02x%02x%02x%02x\n",
+		addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+	rtl8192cd_set_hwaddr(priv->dev, &sa);
+}
+
+
+int mac_clone_handle_frame(_adapter *priv, struct sk_buff *skb)
+{
+	if(priv->ethBrExtInfo.macclone_enable && !priv->macclone_completed)
+	{
+		if(!(skb->data[ETH_ALEN] & 1))	//// check any other particular MAC add
+		{
+                        if(memcmp(skb->data+ETH_ALEN, GET_MY_HWADDR(priv), ETH_ALEN) &&
+				((priv->dev->br_port) &&
+				 memcmp(skb->data+ETH_ALEN, priv->br_mac, ETH_ALEN)))
+			{
+				mac_clone(priv, skb->data+ETH_ALEN);
+				priv->macclone_completed = 1;
+			}
+		}
+	}
+
+	return 0;
+}
+#endif // 0
+
+#define SERVER_PORT			67
+#define CLIENT_PORT			68
+#define DHCP_MAGIC			0x63825363
+#define BROADCAST_FLAG		0x8000
+
+struct dhcpMessage {
+	u_int8_t op;
+	u_int8_t htype;
+	u_int8_t hlen;
+	u_int8_t hops;
+	u_int32_t xid;
+	u_int16_t secs;
+	u_int16_t flags;
+	u_int32_t ciaddr;
+	u_int32_t yiaddr;
+	u_int32_t siaddr;
+	u_int32_t giaddr;
+	u_int8_t chaddr[16];
+	u_int8_t sname[64];
+	u_int8_t file[128];
+	u_int32_t cookie;
+	u_int8_t options[308]; /* 312 - cookie */
+};
+
+void dhcp_flag_bcast(_adapter *priv, struct sk_buff *skb)
+{
+	if(skb == NULL)
+		return;
+
+	if(!priv->ethBrExtInfo.dhcp_bcst_disable)
+	{
+		unsigned short protocol = *((unsigned short *)(skb->data + 2 * ETH_ALEN));
+
+		if(protocol == __constant_htons(ETH_P_IP)) // IP
+		{
+			struct iphdr* iph = (struct iphdr *)(skb->data + ETH_HLEN);
+
+			if(iph->protocol == IPPROTO_UDP) // UDP
+			{
+				struct udphdr *udph = (struct udphdr *)((SIZE_PTR)iph + (iph->ihl << 2));
+
+				if((udph->source == __constant_htons(CLIENT_PORT))
+					&& (udph->dest == __constant_htons(SERVER_PORT))) // DHCP request
+				{
+					struct dhcpMessage *dhcph =
+						(struct dhcpMessage *)((SIZE_PTR)udph + sizeof(struct udphdr));
+
+					if(dhcph->cookie == __constant_htonl(DHCP_MAGIC)) // match magic word
+					{
+						if(!(dhcph->flags & htons(BROADCAST_FLAG))) // if not broadcast
+						{
+							register int sum = 0;
+
+							DEBUG_INFO("DHCP: change flag of DHCP request to broadcast.\n");
+							// or BROADCAST flag
+							dhcph->flags |= htons(BROADCAST_FLAG);
+							// recalculate checksum
+							sum = ~(udph->check) & 0xffff;
+							sum += dhcph->flags;
+							while(sum >> 16)
+								sum = (sum & 0xffff) + (sum >> 16);
+							udph->check = ~sum;
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
+
+void *scdb_findEntry(_adapter *priv, unsigned char *macAddr,
+				unsigned char *ipAddr)
+{
+	unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
+	struct nat25_network_db_entry *db;
+	int hash;
+	//_irqL irqL;
+	//_enter_critical_bh(&priv->br_ext_lock, &irqL);
+
+	__nat25_generate_ipv4_network_addr(networkAddr, (unsigned int *)ipAddr);
+	hash = __nat25_network_hash(networkAddr);
+	db = priv->nethash[hash];
+	while (db != NULL)
+	{
+		if(!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
+			//_exit_critical_bh(&priv->br_ext_lock, &irqL);
+			return (void *)db;
+		}
+
+		db = db->next_hash;
+	}
+
+	//_exit_critical_bh(&priv->br_ext_lock, &irqL);
+	return NULL;
+}
+
+#endif	// CONFIG_BR_EXT
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_bt_mp.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_bt_mp.c
new file mode 100755
index 00000000..90e9ff54
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_bt_mp.c
@@ -0,0 +1,1734 @@
+/******************************************************************************

+ *

+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.

+ *										  

+ * This program is free software; you can redistribute it and/or modify it

+ * under the terms of version 2 of the GNU General Public License as

+ * published by the Free Software Foundation.

+ *

+ * 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.,

+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA

+ *

+ *

+ ******************************************************************************/

+

+

+#include "rtw_bt_mp.h"

+#include <drv_types.h>

+#include <rtl8723a_hal.h>

+

+#ifdef CONFIG_RTL8723A

+

+void MPh2c_timeout_handle(void *FunctionContext)

+{

+	_adapter *pAdapter = (_adapter *)FunctionContext;

+	PMPT_CONTEXT		pMptCtx=&pAdapter->mppriv.MptCtx;

+

+	DBG_8192C("[MPT], MPh2c_timeout_handle \n");

+

+	pMptCtx->bMPh2c_timeout=_TRUE;

+	

+	_rtw_up_sema(&pMptCtx->MPh2c_Sema);

+	

+	//_cancel_timer_ex( &pMptCtx->MPh2c_timeout_timer);

+	

+	return;

+}

+u32 WaitC2Hevent( PADAPTER pAdapter,BOOLEAN *C2H_event ,u32 delay_time)

+{

+	PMPT_CONTEXT		pMptCtx=&(pAdapter->mppriv.MptCtx);

+	pMptCtx->bMPh2c_timeout=_FALSE;

+	

+	_set_timer( &pMptCtx->MPh2c_timeout_timer, delay_time );

+	

+	_rtw_down_sema(&pMptCtx->MPh2c_Sema);

+

+	if( pMptCtx->bMPh2c_timeout == _TRUE )

+	{

+		C2H_event =_FALSE;

+		

+		return _FALSE;

+	}

+	

+	return _TRUE;

+	

+}

+

+BT_CTRL_STATUS

+mptbt_CheckC2hFrame(

+	PADAPTER		Adapter,

+	PBT_H2C			pH2c,

+	PBT_EXT_C2H		pExtC2h

+	)

+{

+	BT_CTRL_STATUS	c2hStatus = BT_STATUS_C2H_SUCCESS;

+		

+	//DBG_8192C("[MPT], MPT rsp C2H hex: %x %x %x  %x %x %x \n"), pExtC2h , pExtC2h+1 ,pExtC2h+2 ,pExtC2h+3 ,pExtC2h+4 ,pExtC2h+5);

+

+	DBG_8192C("[MPT], statusCode = 0x%x\n", pExtC2h->statusCode);

+	DBG_8192C("[MPT], retLen = %d\n", pExtC2h->retLen);

+	DBG_8192C("[MPT], opCodeVer : req/rsp=%d/%d\n", pH2c->opCodeVer, pExtC2h->opCodeVer);

+	DBG_8192C("[MPT], reqNum : req/rsp=%d/%d\n", pH2c->reqNum, pExtC2h->reqNum);

+	if(pExtC2h->reqNum != pH2c->reqNum)

+	{

+		c2hStatus = BT_STATUS_C2H_REQNUM_MISMATCH;

+		DBG_8192C("[MPT], Error!! C2H reqNum Mismatch!!\n");

+	}

+	else if(pExtC2h->opCodeVer != pH2c->opCodeVer)

+	{

+		c2hStatus = BT_STATUS_OPCODE_L_VERSION_MISMATCH;

+		DBG_8192C("[MPT], Error!! OPCode version L mismatch!!\n");

+	}

+

+	return c2hStatus;

+}

+

+extern s32 FillH2CCmd(PADAPTER padapter, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer);

+

+BT_CTRL_STATUS

+mptbt_SendH2c(

+	PADAPTER	Adapter,

+	PBT_H2C	pH2c,

+	u2Byte		h2cCmdLen

+	)

+{

+	//KIRQL				OldIrql = KeGetCurrentIrql();

+	BT_CTRL_STATUS	h2cStatus=BT_STATUS_H2C_SUCCESS;

+	PMPT_CONTEXT		pMptCtx=&(Adapter->mppriv.MptCtx);

+	u1Byte				i;

+

+	DBG_8192C("[MPT], mptbt_SendH2c()=========>\n");

+

+	//PlatformResetEvent(&pMptCtx->MptH2cRspEvent);

+	//PlatformResetEvent(&pMptCtx->MptBtC2hEvent);

+	

+//	if(OldIrql == PASSIVE_LEVEL)

+//	{

+		//RTPRINT_DATA(FMPBT, FMPBT_H2C_CONTENT, ("[MPT], MPT H2C hex: \n"), pH2c, h2cCmdLen);

+

+		for(i=0; i<BT_H2C_MAX_RETRY; i++)

+		{

+			DBG_8192C("[MPT], Send H2C command to wifi!!!\n");

+			FillH2CCmd(Adapter, 70, h2cCmdLen, (pu1Byte)pH2c);

+			pMptCtx->h2cReqNum++;

+			pMptCtx->h2cReqNum %= 16;

+

+			if(WaitC2Hevent(Adapter, &pMptCtx->MptH2cRspEvent, 100))

+			{

+				DBG_8192C("[MPT], Received WiFi MptH2cRspEvent!!!\n");

+				if(WaitC2Hevent(Adapter, &pMptCtx->MptBtC2hEvent, 400))

+				{

+					DBG_8192C("[MPT], Received MptBtC2hEvent!!!\n");

+					break;

+				}

+				else

+				{

+					DBG_8192C("[MPT], Error!!BT MptBtC2hEvent timeout!!\n");

+					h2cStatus = BT_STATUS_H2C_BT_NO_RSP;

+				}

+			}

+			else

+			{

+				DBG_8192C("[MPT], Error!!WiFi  MptH2cRspEvent timeout!!\n");

+				h2cStatus = BT_STATUS_H2C_TIMTOUT;

+			}

+		}

+//	}

+//	else

+//	{

+//		RT_ASSERT(FALSE, ("[MPT],  mptbt_SendH2c() can only run under PASSIVE_LEVEL!!\n"));

+//		h2cStatus = BT_STATUS_WRONG_LEVEL;

+//	}

+

+	DBG_8192C("[MPT], mptbt_SendH2c()<=========\n");

+	return h2cStatus;

+}

+

+

+

+BT_CTRL_STATUS

+mptbt_CheckBtRspStatus(

+	PADAPTER			Adapter,

+	PBT_EXT_C2H			pExtC2h

+	)

+{

+	BT_CTRL_STATUS	retStatus=BT_OP_STATUS_SUCCESS;

+

+	switch(pExtC2h->statusCode)

+	{

+		case BT_OP_STATUS_SUCCESS:

+			retStatus = BT_STATUS_BT_OP_SUCCESS;

+			DBG_8192C("[MPT], BT status : BT_STATUS_SUCCESS\n");

+			break;

+		case BT_OP_STATUS_VERSION_MISMATCH:

+			retStatus = BT_STATUS_OPCODE_L_VERSION_MISMATCH;

+			DBG_8192C("[MPT], BT status : BT_STATUS_OPCODE_L_VERSION_MISMATCH\n");

+			break;

+		case BT_OP_STATUS_UNKNOWN_OPCODE:

+			retStatus = BT_STATUS_UNKNOWN_OPCODE_L;

+			DBG_8192C("[MPT], BT status : BT_STATUS_UNKNOWN_OPCODE_L\n");

+			break;

+		case BT_OP_STATUS_ERROR_PARAMETER:

+			retStatus = BT_STATUS_PARAMETER_FORMAT_ERROR_L;

+			DBG_8192C("[MPT], BT status : BT_STATUS_PARAMETER_FORMAT_ERROR_L\n");

+			break;

+		default:

+			retStatus = BT_STATUS_UNKNOWN_STATUS_L;

+			DBG_8192C("[MPT], BT status : BT_STATUS_UNKNOWN_STATUS_L\n");

+			break;

+	}

+	

+	return retStatus;

+}	

+

+

+

+BT_CTRL_STATUS

+mptbt_BtFwOpCodeProcess(

+	PADAPTER		Adapter,

+	u1Byte			btFwOpCode,

+	u1Byte			opCodeVer,

+	pu1Byte			pH2cPar,

+	u1Byte			h2cParaLen

+	)

+{

+	u1Byte				H2C_Parameter[6] ={0};

+	PBT_H2C				pH2c=(PBT_H2C)&H2C_Parameter[0];

+	PMPT_CONTEXT		pMptCtx=&(Adapter->mppriv.MptCtx);

+	PBT_EXT_C2H			pExtC2h=(PBT_EXT_C2H)&pMptCtx->c2hBuf[0];

+	u2Byte				paraLen=0,i;

+	BT_CTRL_STATUS	h2cStatus=BT_STATUS_H2C_SUCCESS, c2hStatus=BT_STATUS_C2H_SUCCESS;

+	BT_CTRL_STATUS	retStatus=BT_STATUS_H2C_BT_NO_RSP;

+

+	pH2c->opCode = btFwOpCode;

+	pH2c->opCodeVer = opCodeVer;

+	pH2c->reqNum = pMptCtx->h2cReqNum;

+	//PlatformMoveMemory(&pH2c->buf[0], pH2cPar, h2cParaLen);

+	//_rtw_memcpy(&pH2c->buf[0], pH2cPar, h2cParaLen);

+	_rtw_memcpy(pH2c->buf, pH2cPar, h2cParaLen);

+

+	DBG_8192C("[MPT], pH2c->opCode=%d\n", pH2c->opCode);

+	DBG_8192C("[MPT], pH2c->opCodeVer=%d\n", pH2c->opCodeVer);

+	DBG_8192C("[MPT], pH2c->reqNum=%d\n", pH2c->reqNum);

+	DBG_8192C("[MPT], h2c parameter length=%d\n", h2cParaLen);

+	if(h2cParaLen)

+	{

+		DBG_8192C("[MPT], parameters(hex): \n");

+		for(i=0;i<h2cParaLen;i++)

+		{

+			DBG_8192C(" 0x%x \n", pH2c->buf[i]);

+		}

+	}

+

+	h2cStatus = mptbt_SendH2c(Adapter, pH2c, h2cParaLen+2);

+	if(BT_STATUS_H2C_SUCCESS == h2cStatus)

+	{

+		// if reach here, it means H2C get the correct c2h response, 

+		c2hStatus = mptbt_CheckC2hFrame(Adapter, pH2c, pExtC2h);

+		if(BT_STATUS_C2H_SUCCESS == c2hStatus)

+		{

+			retStatus = mptbt_CheckBtRspStatus(Adapter, pExtC2h);

+		}

+		else

+		{

+			DBG_8192C("[MPT], Error!! C2H failed for pH2c->opCode=%d\n", pH2c->opCode);

+			// check c2h status error, return error status code to upper layer.

+			retStatus = c2hStatus;

+		}

+	}

+	else

+	{

+		DBG_8192C("[MPT], Error!! H2C failed for pH2c->opCode=%d\n", pH2c->opCode);

+		// check h2c status error, return error status code to upper layer.

+		retStatus = h2cStatus;

+	}

+

+	return retStatus;

+}

+

+

+

+

+u2Byte

+mptbt_BtReady(

+	PADAPTER		Adapter,

+	PBT_REQ_CMD 	pBtReq,

+	PBT_RSP_CMD 	pBtRsp

+	)

+{

+	u1Byte				h2cParaBuf[6] ={0};

+	u1Byte				h2cParaLen=0;

+	u2Byte				paraLen=0;

+	u1Byte				retStatus=BT_STATUS_BT_OP_SUCCESS;

+	u1Byte				btOpcode;

+	u1Byte				btOpcodeVer=0;

+	PMPT_CONTEXT		pMptCtx=&(Adapter->mppriv.MptCtx);

+	PBT_EXT_C2H			pExtC2h=(PBT_EXT_C2H)&pMptCtx->c2hBuf[0];

+	u1Byte				i;

+	u1Byte				btFwVer=0, bdAddr[6]={0};

+	u2Byte				btRealFwVer=0;

+	pu2Byte 			pu2Tmp=NULL;

+

+	//

+	// check upper layer parameters

+	//

+

+	// 1. check upper layer opcode version

+	if(pBtReq->opCodeVer != 1)

+	{

+		DBG_8192C("[MPT], Error!! Upper OP code version not match!!!\n");

+		pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH;

+		return paraLen;

+	}

+

+	pBtRsp->pParamStart[0] = MP_BT_NOT_READY;

+	paraLen = 10;

+	//

+	// execute lower layer opcodes

+	//

+

+	// Get BT FW version

+	// fill h2c parameters

+	btOpcode = BT_LO_OP_GET_BT_VERSION;

+	// execute h2c and check respond c2h from bt fw is correct or not

+	retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	// ckeck bt return status.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}

+	else

+	{

+		pu2Tmp = (pu2Byte)&pExtC2h->buf[0];

+		btRealFwVer = *pu2Tmp;

+		btFwVer = pExtC2h->buf[1];

+		DBG_8192C("[MPT], btRealFwVer=0x%x, btFwVer=0x%x\n", btRealFwVer, btFwVer);

+	}

+

+	// Get BD Address

+	// fill h2c parameters

+	btOpcode = BT_LO_OP_GET_BD_ADDR_L;

+	// execute h2c and check respond c2h from bt fw is correct or not

+	retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	// ckeck bt return status.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}

+	else

+	{

+		bdAddr[5] = pExtC2h->buf[0];

+		bdAddr[4] = pExtC2h->buf[1];

+		bdAddr[3] = pExtC2h->buf[2];

+	}

+

+	// fill h2c parameters

+	btOpcode = BT_LO_OP_GET_BD_ADDR_H;

+	// execute h2c and check respond c2h from bt fw is correct or not

+	retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	// ckeck bt return status.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}

+	else

+	{

+		bdAddr[2] = pExtC2h->buf[0];

+		bdAddr[1] = pExtC2h->buf[1];

+		bdAddr[0] = pExtC2h->buf[2];

+	}

+	DBG_8192C("[MPT], Local BDAddr:");

+	for(i=0; i<6; i++)

+	{

+		DBG_8192C(" 0x%x ", bdAddr[i]);

+	}

+	pBtRsp->status = BT_STATUS_SUCCESS;

+	pBtRsp->pParamStart[0] = MP_BT_READY;

+	pu2Tmp = (pu2Byte)&pBtRsp->pParamStart[1];

+	*pu2Tmp = btRealFwVer;

+	pBtRsp->pParamStart[3] = btFwVer;

+	for(i=0; i<6; i++)

+	{

+		pBtRsp->pParamStart[4+i] = bdAddr[5-i];

+	}

+

+	return paraLen;

+}

+

+void mptbt_close_WiFiRF(PADAPTER Adapter)

+{

+	PHY_SetBBReg(Adapter, 0x824, 0xF, 0x0);

+	PHY_SetBBReg(Adapter, 0x824, 0x700000, 0x0);

+	PHY_SetRFReg(Adapter, RF90_PATH_A, 0x0, 0xF0000, 0x0);

+}

+

+void mptbt_open_WiFiRF(PADAPTER	Adapter)

+{

+	PHY_SetBBReg(Adapter, 0x824, 0x700000, 0x3);

+	PHY_SetBBReg(Adapter, 0x824, 0xF, 0x2);

+	PHY_SetRFReg(Adapter, RF90_PATH_A, 0x0, 0xF0000, 0x3);

+}

+

+u4Byte mptbt_switch_RF(PADAPTER	Adapter, u1Byte	Enter)

+{

+	u2Byte	tmp_2byte = 0;

+

+	//Enter test mode

+	if (Enter) {

+		////1>. close WiFi RF

+		mptbt_close_WiFiRF(Adapter);

+		

+		////2>. change ant switch to BT

+		tmp_2byte = rtw_read16(Adapter, 0x860);

+		tmp_2byte = tmp_2byte | BIT(9);

+		tmp_2byte = tmp_2byte & (~BIT(8));

+		rtw_write16(Adapter, 0x860, tmp_2byte);

+		rtw_write16(Adapter, 0x870, 0x300);

+	} else {

+		////1>. Open WiFi RF

+		mptbt_open_WiFiRF(Adapter);

+		

+		////2>. change ant switch back

+		tmp_2byte = rtw_read16(Adapter, 0x860);

+		tmp_2byte = tmp_2byte | BIT(8);

+		tmp_2byte = tmp_2byte & (~BIT(9));

+		rtw_write16(Adapter, 0x860, tmp_2byte);

+		rtw_write16(Adapter, 0x870, 0x300);

+	}

+

+	return 0;

+}

+

+u2Byte

+mptbt_BtSetMode(

+	PADAPTER		Adapter,

+	PBT_REQ_CMD 	pBtReq,

+	PBT_RSP_CMD 	pBtRsp

+	)

+{

+	u1Byte				h2cParaBuf[6] ={0};

+	u1Byte				h2cParaLen=0;

+	u2Byte				paraLen=0;

+	u1Byte				retStatus=BT_STATUS_BT_OP_SUCCESS;

+	u1Byte				btOpcode;

+	u1Byte				btOpcodeVer=0;

+	u1Byte				btModeToSet=0;

+

+	//

+	// check upper layer parameters

+	//

+	// 1. check upper layer opcode version

+	if(pBtReq->opCodeVer != 1)

+	{

+		DBG_8192C("[MPT], Error!! Upper OP code version not match!!!\n");

+		pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH;

+		return paraLen;

+	}

+	// 2. check upper layer parameter length

+	if(1 == pBtReq->paraLength)

+	{

+		btModeToSet = pBtReq->pParamStart[0];

+		DBG_8192C("[MPT], BtTestMode=%d \n", btModeToSet);

+	}

+	else

+	{

+		DBG_8192C("[MPT], Error!! wrong parameter length=%d (should be 1)\n", pBtReq->paraLength);

+		pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U;

+		return paraLen;

+	}

+	

+	//

+	// execute lower layer opcodes

+	//

+	

+	// 1. fill h2c parameters	

+	// check bt mode

+	btOpcode = BT_LO_OP_SET_BT_MODE;

+	if(btModeToSet >= MP_BT_MODE_MAX)

+	{

+		pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+		return paraLen;

+	}

+	else

+	{

+		mptbt_switch_RF(Adapter, 1);

+

+		h2cParaBuf[0] = btModeToSet;

+		h2cParaLen = 1;

+		// 2. execute h2c and check respond c2h from bt fw is correct or not

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	}

+	

+	// 3. construct respond status code and data.

+	if(BT_STATUS_BT_OP_SUCCESS == retStatus)

+	{

+		pBtRsp->status = BT_STATUS_SUCCESS;

+	}

+	else

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+	}

+	

+	return paraLen;

+}

+

+

+VOID

+MPTBT_FwC2hBtMpCtrl(

+	PADAPTER	Adapter,

+	pu1Byte 	tmpBuf,

+	u1Byte		length

+	)

+{

+	u32 i;

+	PMPT_CONTEXT	pMptCtx=&(Adapter->mppriv.MptCtx);

+	PBT_EXT_C2H pExtC2h=(PBT_EXT_C2H)tmpBuf;

+	

+	if(Adapter->bBTFWReady == _FALSE || Adapter->registrypriv.mp_mode == 0 )

+	{	

+		DBG_8192C("[MPT], %s,bBTFWReady == _FALSE\n",__func__);

+		return;

+	}

+	//cancel_timeout for h2c handle

+	_cancel_timer_ex( &pMptCtx->MPh2c_timeout_timer);

+

+	DBG_8192C("[MPT], MPTBT_FwC2hBtMpCtrl(), hex: \n");

+	for(i=0;i<=length;i++)

+	{

+		//DBG_8192C("[MPT], MPTBT_FwC2hBtMpCtrl(), hex: \n",tmpBuf[i], length);

+		DBG_8192C(" 0x%x ",tmpBuf[i]);

+	}

+	DBG_8192C("\n [MPT], pExtC2h->extendId=0x%x\n", pExtC2h->extendId);

+	

+	switch(pExtC2h->extendId)

+	{

+		case EXT_C2H_WIFI_FW_ACTIVE_RSP:

+			DBG_8192C("[MPT], EXT_C2H_WIFI_FW_ACTIVE_RSP\n");

+			DBG_8192C("[MPT], pExtC2h->buf hex: \n");

+			if( length > 32 || length < 3 )

+					break ;

+			for(i=0;i<=(length-3);i++)

+				DBG_8192C(" 0x%x ",pExtC2h->buf[i]);

+				//PlatformSetEvent(&pMptCtx->MptH2cRspEvent);

+				pMptCtx->MptH2cRspEvent=_TRUE;

+				_rtw_up_sema(&pMptCtx->MPh2c_Sema);

+			break;

+		case EXT_C2H_TRIG_BY_BT_FW:

+			DBG_8192C("[MPT], EXT_C2H_TRIG_BY_BT_FW\n");

+				//PlatformMoveMemory(&pMptCtx->c2hBuf[0], tmpBuf, length);

+			_rtw_memcpy(&pMptCtx->c2hBuf[0], tmpBuf, length);

+			DBG_8192C("[MPT], pExtC2h->statusCode=0x%x\n", pExtC2h->statusCode);

+			DBG_8192C("[MPT], pExtC2h->retLen=0x%x\n", pExtC2h->retLen);

+			DBG_8192C("[MPT], pExtC2h->opCodeVer=0x%x\n", pExtC2h->opCodeVer);

+			DBG_8192C("[MPT], pExtC2h->reqNum=0x%x\n", pExtC2h->reqNum);

+			DBG_8192C("[MPT], pExtC2h->buf hex: \n");

+			for(i=0;i<=(length-3);i++)

+				DBG_8192C(" 0x%x ",pExtC2h->buf[0]);

+				//PlatformSetEvent(&pMptCtx->MptBtC2hEvent);

+				pMptCtx->MptBtC2hEvent=_TRUE;

+				_rtw_up_sema(&pMptCtx->MPh2c_Sema);

+			break;

+		default:

+			break;

+	}

+	

+

+	

+}

+

+

+u2Byte

+mptbt_BtGetGeneral(

+	IN	PADAPTER		Adapter,

+	IN	PBT_REQ_CMD 	pBtReq,

+	IN	PBT_RSP_CMD 	pBtRsp

+	)

+{

+	PMPT_CONTEXT		pMptCtx=&(Adapter->mppriv.MptCtx);

+	PBT_EXT_C2H 		pExtC2h=(PBT_EXT_C2H)&pMptCtx->c2hBuf[0];

+	u1Byte				h2cParaBuf[6] ={0};

+	u1Byte				h2cParaLen=0;

+	u2Byte				paraLen=0;

+	u1Byte				retStatus=BT_STATUS_BT_OP_SUCCESS;

+	u1Byte				btOpcode, bdAddr[6]={0};

+	u1Byte				btOpcodeVer=0;

+	u1Byte				getType=0, i;

+	u2Byte				getParaLen=0, validParaLen=0;

+	u1Byte				regType=0, reportType=0;

+	u4Byte				regAddr=0, regValue=0;

+	pu4Byte 			pu4Tmp;

+	pu2Byte 			pu2Tmp;

+	pu1Byte 			pu1Tmp;

+

+	//

+	// check upper layer parameters

+	//

+	

+	// check upper layer opcode version

+	if(pBtReq->opCodeVer != 1)

+	{

+		DBG_8192C("[MPT], Error!! Upper OP code version not match!!!\n");

+		pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH;

+		return paraLen;

+	}

+	// check upper layer parameter length

+	if(pBtReq->paraLength < 1)

+	{

+		DBG_8192C("[MPT], Error!! wrong parameter length=%d (should larger than 1)\n", pBtReq->paraLength);

+		pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U;

+		return paraLen;

+	}

+	getParaLen = pBtReq->paraLength - 1;

+	getType = pBtReq->pParamStart[0];

+	

+	DBG_8192C("[MPT], getType=%d, getParaLen=%d\n", getType, getParaLen);

+

+	// check parameter first

+	switch(getType)

+	{

+		case BT_GGET_REG:

+			DBG_8192C("[MPT], [BT_GGET_REG]\n");

+			validParaLen = 5;

+			if(getParaLen == validParaLen)

+			{

+				btOpcode = BT_LO_OP_READ_REG;

+				regType = pBtReq->pParamStart[1];

+				pu4Tmp = (pu4Byte)&pBtReq->pParamStart[2];

+				regAddr = *pu4Tmp;

+				DBG_8192C("[MPT], BT_GGET_REG regType=0x%x, regAddr=0x%x!!\n", 

+					regType, regAddr);

+				if(regType >= BT_REG_MAX)

+				{

+					pBtRsp->status = (btOpcode<<8)| BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+					return paraLen;

+				}

+				else

+				{

+					if( ((BT_REG_RF==regType)&&(regAddr>0x7f)) ||

+						((BT_REG_MODEM==regType)&&(regAddr>0x1ff)) ||

+						((BT_REG_BLUEWIZE==regType)&&(regAddr>0xfff)) ||

+						((BT_REG_VENDOR==regType)&&(regAddr>0xfff)) ||

+						((BT_REG_LE==regType)&&(regAddr>0xfff)) )

+					{				

+						pBtRsp->status = (btOpcode<<8)| BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+						return paraLen;

+					}

+				}

+			}

+			break;

+		case BT_GGET_STATUS:

+			DBG_8192C("[MPT], [BT_GGET_STATUS]\n");

+			validParaLen = 0;

+			break;

+		case BT_GGET_REPORT:

+			DBG_8192C("[MPT], [BT_GGET_REPORT]\n");

+			validParaLen = 1;

+			if(getParaLen == validParaLen)

+			{

+				reportType = pBtReq->pParamStart[1];

+				DBG_8192C("[MPT], BT_GGET_REPORT reportType=0x%x!!\n", reportType);

+				if(reportType >= BT_REPORT_MAX)

+				{

+					pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+					return paraLen;

+				}

+			}

+			break;

+		default:

+			{

+				DBG_8192C("[MPT], Error!! getType=%d, out of range\n", getType);

+				pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+				return paraLen;

+			}

+			break;

+	}

+	if(getParaLen != validParaLen)

+	{

+		DBG_8192C("[MPT], Error!! wrong parameter length=%d for BT_GET_GEN_CMD cmd id=0x%x, paraLen should=0x%x\n",

+			getParaLen, getType, validParaLen);

+		pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U;

+		return paraLen;

+	}

+	

+	//

+	// execute lower layer opcodes

+	//

+	if(BT_GGET_REG == getType)

+	{

+		// fill h2c parameters

+		// here we should write reg value first then write the address, adviced by Austin

+		btOpcode = BT_LO_OP_READ_REG;

+		h2cParaBuf[0] = regType;

+		h2cParaBuf[1] = pBtReq->pParamStart[2];

+		h2cParaBuf[2] = pBtReq->pParamStart[3];

+		h2cParaLen = 3;

+		// execute h2c and check respond c2h from bt fw is correct or not

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+		// construct respond status code and data.

+		if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+		{

+			pBtRsp->status = ((btOpcode<<8)|retStatus);

+			DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+			return paraLen;

+		}

+

+		pu2Tmp = (pu2Byte)&pExtC2h->buf[0];

+		regValue = *pu2Tmp;

+		DBG_8192C("[MPT], read reg regType=0x%x, regAddr=0x%x, regValue=0x%x\n", 

+			regType, regAddr, regValue);

+		

+		pu4Tmp = (pu4Byte)&pBtRsp->pParamStart[0];

+		*pu4Tmp = regValue;

+		paraLen = 4;

+	}

+	else if(BT_GGET_STATUS == getType)

+	{

+		btOpcode = BT_LO_OP_GET_BT_STATUS;

+		h2cParaLen = 0;

+		// execute h2c and check respond c2h from bt fw is correct or not

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+		// construct respond status code and data.

+		if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+		{

+			pBtRsp->status = ((btOpcode<<8)|retStatus);

+			DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+			return paraLen;

+		}

+

+		pBtRsp->pParamStart[0] = pExtC2h->buf[0];

+		pBtRsp->pParamStart[1] = pExtC2h->buf[1];

+		DBG_8192C("[MPT], read bt status, testMode=0x%x, testStatus=0x%x\n", 

+			pBtRsp->pParamStart[0], pBtRsp->pParamStart[1]);		

+		paraLen = 2;

+	}

+	else if(BT_GGET_REPORT == getType)

+	{

+		switch(reportType)

+		{

+			case BT_REPORT_RX_PACKET_CNT:

+				{

+					DBG_8192C("[MPT], [Rx Packet Counts]\n");

+					btOpcode = BT_LO_OP_GET_RX_PKT_CNT_L;

+					h2cParaLen = 0;

+					// execute h2c and check respond c2h from bt fw is correct or not

+					retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+					// construct respond status code and data.

+					if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+					{

+						pBtRsp->status = ((btOpcode<<8)|retStatus);

+						DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+						return paraLen;

+					}

+					pBtRsp->pParamStart[0] = pExtC2h->buf[0];

+					pBtRsp->pParamStart[1] = pExtC2h->buf[1];

+

+					btOpcode = BT_LO_OP_GET_RX_PKT_CNT_H;

+					h2cParaLen = 0;

+					// execute h2c and check respond c2h from bt fw is correct or not

+					retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+					// construct respond status code and data.

+					if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+					{

+						pBtRsp->status = ((btOpcode<<8)|retStatus);

+						DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+						return paraLen;

+					}

+					pBtRsp->pParamStart[2] = pExtC2h->buf[0];

+					pBtRsp->pParamStart[3] = pExtC2h->buf[1];

+					paraLen = 4;

+				}

+				break;

+			case BT_REPORT_RX_ERROR_BITS:

+				{

+					DBG_8192C("[MPT], [Rx Error Bits]\n");

+					btOpcode = BT_LO_OP_GET_RX_ERROR_BITS_L;

+					h2cParaLen = 0;

+					// execute h2c and check respond c2h from bt fw is correct or not

+					retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+					// construct respond status code and data.

+					if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+					{

+						pBtRsp->status = ((btOpcode<<8)|retStatus);

+						DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+						return paraLen;

+					}

+					pBtRsp->pParamStart[0] = pExtC2h->buf[0];

+					pBtRsp->pParamStart[1] = pExtC2h->buf[1];

+

+					btOpcode = BT_LO_OP_GET_RX_ERROR_BITS_H;

+					h2cParaLen = 0;

+					// execute h2c and check respond c2h from bt fw is correct or not

+					retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+					// construct respond status code and data.

+					if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+					{

+						pBtRsp->status = ((btOpcode<<8)|retStatus);

+						DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+						return paraLen;

+					}

+					pBtRsp->pParamStart[2] = pExtC2h->buf[0];

+					pBtRsp->pParamStart[3] = pExtC2h->buf[1];

+					paraLen = 4;

+				}

+				break;

+			case BT_REPORT_RSSI:

+				{

+					DBG_8192C("[MPT], [RSSI]\n");

+					btOpcode = BT_LO_OP_GET_RSSI;

+					h2cParaLen = 0;

+					// execute h2c and check respond c2h from bt fw is correct or not

+					retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+					// construct respond status code and data.

+					if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+					{

+						pBtRsp->status = ((btOpcode<<8)|retStatus);

+						DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+						return paraLen;

+					}

+					pBtRsp->pParamStart[0] = pExtC2h->buf[0];

+					pBtRsp->pParamStart[1] = pExtC2h->buf[1];

+					paraLen = 2;

+				}

+				break;

+			case BT_REPORT_CFO_HDR_QUALITY:

+				{

+					DBG_8192C("[MPT], [CFO & Header Quality]\n");

+					btOpcode = BT_LO_OP_GET_CFO_HDR_QUALITY_L;

+					h2cParaLen = 0;

+					// execute h2c and check respond c2h from bt fw is correct or not

+					retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+					// construct respond status code and data.

+					if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+					{

+						pBtRsp->status = ((btOpcode<<8)|retStatus);

+						DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+						return paraLen;

+					}

+					pBtRsp->pParamStart[0] = pExtC2h->buf[0];

+					pBtRsp->pParamStart[1] = pExtC2h->buf[1];

+

+					btOpcode = BT_LO_OP_GET_CFO_HDR_QUALITY_H;

+					h2cParaLen = 0;

+					// execute h2c and check respond c2h from bt fw is correct or not

+					retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+					// construct respond status code and data.

+					if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+					{

+						pBtRsp->status = ((btOpcode<<8)|retStatus);

+						DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+						return paraLen;

+					}

+					pBtRsp->pParamStart[2] = pExtC2h->buf[0];

+					pBtRsp->pParamStart[3] = pExtC2h->buf[1];

+					paraLen = 4;

+				}

+				break;

+			case BT_REPORT_CONNECT_TARGET_BD_ADDR:

+				{

+					DBG_8192C("[MPT], [Connected Target BD ADDR]\n");

+					btOpcode = BT_LO_OP_GET_TARGET_BD_ADDR_L;

+					h2cParaLen = 0;

+					// execute h2c and check respond c2h from bt fw is correct or not

+					retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+					// construct respond status code and data.

+					if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+					{

+						pBtRsp->status = ((btOpcode<<8)|retStatus);

+						DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+						return paraLen;

+					}

+					bdAddr[5] = pExtC2h->buf[0];

+					bdAddr[4] = pExtC2h->buf[1];

+					bdAddr[3] = pExtC2h->buf[2];

+

+					btOpcode = BT_LO_OP_GET_TARGET_BD_ADDR_H;

+					h2cParaLen = 0;

+					// execute h2c and check respond c2h from bt fw is correct or not

+					retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+					// construct respond status code and data.

+					if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+					{

+						pBtRsp->status = ((btOpcode<<8)|retStatus);

+						DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+						return paraLen;

+					}

+					bdAddr[2] = pExtC2h->buf[0];

+					bdAddr[1] = pExtC2h->buf[1];

+					bdAddr[0] = pExtC2h->buf[2];

+						

+					DBG_8192C("[MPT], Connected Target BDAddr:%s", bdAddr);

+					for(i=0; i<6; i++)

+					{

+						pBtRsp->pParamStart[i] = bdAddr[5-i];

+					}

+					paraLen = 6;

+				}

+				break;

+			default:

+				pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+				return paraLen;

+				break;

+		}

+	}

+

+	pBtRsp->status = BT_STATUS_SUCCESS;

+	return paraLen;

+}

+

+

+

+u2Byte

+mptbt_BtSetGeneral(

+	IN	PADAPTER		Adapter,

+	IN	PBT_REQ_CMD 	pBtReq,

+	IN	PBT_RSP_CMD 	pBtRsp

+	)

+{

+	u1Byte				h2cParaBuf[6] ={0};

+	u1Byte				h2cParaLen=0;

+	u2Byte				paraLen=0;

+	u1Byte				retStatus=BT_STATUS_BT_OP_SUCCESS;

+	u1Byte				btOpcode;

+	u1Byte				btOpcodeVer=0;

+	u1Byte				setType=0;

+	u2Byte				setParaLen=0, validParaLen=0;

+	u1Byte				regType=0, bdAddr[6]={0}, calVal=0;

+	u4Byte				regAddr=0, regValue=0;

+	pu4Byte 			pu4Tmp;

+	pu2Byte 			pu2Tmp;

+	pu1Byte 			pu1Tmp;

+

+	//

+	// check upper layer parameters

+	//

+	

+	// check upper layer opcode version

+	if(pBtReq->opCodeVer != 1)

+	{

+		DBG_8192C("[MPT], Error!! Upper OP code version not match!!!\n");

+		pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH;

+		return paraLen;

+	}

+	// check upper layer parameter length

+	if(pBtReq->paraLength < 1)

+	{

+		DBG_8192C("[MPT], Error!! wrong parameter length=%d (should larger than 1)\n", pBtReq->paraLength);

+		pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U;

+		return paraLen;

+	}

+	setParaLen = pBtReq->paraLength - 1;

+	setType = pBtReq->pParamStart[0];

+	

+	DBG_8192C("[MPT], setType=%d, setParaLen=%d\n", setType, setParaLen);

+

+	// check parameter first

+	switch(setType)

+	{

+		case BT_GSET_REG:

+			DBG_8192C ("[MPT], [BT_GSET_REG]\n");

+			validParaLen = 9;

+			if(setParaLen == validParaLen)

+			{

+				btOpcode = BT_LO_OP_WRITE_REG_VALUE;

+				regType = pBtReq->pParamStart[1];

+				pu4Tmp = (pu4Byte)&pBtReq->pParamStart[2];

+				regAddr = *pu4Tmp;

+				pu4Tmp = (pu4Byte)&pBtReq->pParamStart[6];

+				regValue = *pu4Tmp;

+				DBG_8192C("[MPT], BT_GSET_REG regType=0x%x, regAddr=0x%x, regValue=0x%x!!\n", 

+					regType, regAddr, regValue);

+				if(regType >= BT_REG_MAX)

+				{

+					pBtRsp->status = (btOpcode<<8)| BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+					return paraLen;

+				}

+				else

+				{

+					if( ((BT_REG_RF==regType)&&(regAddr>0x7f)) ||

+						((BT_REG_MODEM==regType)&&(regAddr>0x1ff)) ||

+						((BT_REG_BLUEWIZE==regType)&&(regAddr>0xfff)) ||

+						((BT_REG_VENDOR==regType)&&(regAddr>0xfff)) ||

+						((BT_REG_LE==regType)&&(regAddr>0xfff)) )

+					{				

+						pBtRsp->status = (btOpcode<<8)| BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+						return paraLen;

+					}

+				}

+			}

+			break;

+		case BT_GSET_RESET:

+			DBG_8192C("[MPT], [BT_GSET_RESET]\n");

+			validParaLen = 0;

+			break;

+		case BT_GSET_TARGET_BD_ADDR:

+			DBG_8192C("[MPT], [BT_GSET_TARGET_BD_ADDR]\n");

+			validParaLen = 6;

+			if(setParaLen == validParaLen)

+			{

+				btOpcode = BT_LO_OP_SET_TARGET_BD_ADDR_H;

+				if( (pBtReq->pParamStart[1]==0) &&

+					(pBtReq->pParamStart[2]==0) &&

+					(pBtReq->pParamStart[3]==0) &&

+					(pBtReq->pParamStart[4]==0) &&

+					(pBtReq->pParamStart[5]==0) &&

+					(pBtReq->pParamStart[6]==0) )

+				{

+					DBG_8192C("[MPT], Error!! targetBDAddr=all zero\n");

+					pBtRsp->status = (btOpcode<<8)|BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+					return paraLen;

+				}

+				if( (pBtReq->pParamStart[1]==0xff) &&

+					(pBtReq->pParamStart[2]==0xff) &&

+					(pBtReq->pParamStart[3]==0xff) &&

+					(pBtReq->pParamStart[4]==0xff) &&

+					(pBtReq->pParamStart[5]==0xff) &&

+					(pBtReq->pParamStart[6]==0xff) )

+				{

+					DBG_8192C("[MPT], Error!! targetBDAddr=all 0xf\n");

+					pBtRsp->status = (btOpcode<<8)|BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+					return paraLen;

+				}

+				bdAddr[0] = pBtReq->pParamStart[6];

+				bdAddr[1] = pBtReq->pParamStart[5];

+				bdAddr[2] = pBtReq->pParamStart[4];

+				bdAddr[3] = pBtReq->pParamStart[3];

+				bdAddr[4] = pBtReq->pParamStart[2];

+				bdAddr[5] = pBtReq->pParamStart[1];

+				DBG_8192C ("[MPT], target BDAddr:%x,%x,%x,%x,%x,%x\n", 

+							bdAddr[0],bdAddr[1],bdAddr[2],bdAddr[3],bdAddr[4],bdAddr[5]);

+			}

+			break;

+		case BT_GSET_TX_PWR_FINETUNE:

+			DBG_8192C("[MPT], [BT_GSET_TX_PWR_FINETUNE]\n");

+			validParaLen = 1;

+			if(setParaLen == validParaLen)

+			{

+				btOpcode = BT_LO_OP_SET_TX_POWER_CALIBRATION;

+				calVal = pBtReq->pParamStart[1];

+				if( (calVal<1) || (calVal>9) )

+				{

+					pBtRsp->status = (btOpcode<<8)|BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+					return paraLen;

+				}

+				DBG_8192C ("[MPT], calVal=%d\n", calVal);

+			}

+			break;

+		case BT_SET_TRACKING_INTERVAL:

+			DBG_871X("[MPT], [BT_SET_TRACKING_INTERVAL] setParaLen =%d \n",setParaLen);

+			

+			validParaLen = 1;	

+			if(setParaLen == validParaLen)	

+				calVal = pBtReq->pParamStart[1];

+			break;

+		case BT_SET_THERMAL_METER:

+			DBG_871X("[MPT], [BT_SET_THERMAL_METER] setParaLen =%d \n",setParaLen);

+			validParaLen = 1;	

+			if(setParaLen == validParaLen)	

+				calVal = pBtReq->pParamStart[1];

+			break;

+		case BT_ENABLE_CFO_TRACKING:

+			DBG_871X("[MPT], [BT_ENABLE_CFO_TRACKING] setParaLen =%d \n",setParaLen);

+			validParaLen = 1;	

+			if(setParaLen == validParaLen)	

+				calVal = pBtReq->pParamStart[1];

+			break;

+		case BT_GSET_UPDATE_BT_PATCH:

+			if(IS_HARDWARE_TYPE_8723AE(Adapter) && Adapter->bFWReady)

+			{

+				u1Byte i;

+				DBG_8192C ("[MPT], write regs for load patch\n");

+				//BTFwPatch8723A(Adapter);

+				PlatformEFIOWrite1Byte(Adapter, 0xCC, 0x2d);

+				rtw_msleep_os(50);

+				PlatformEFIOWrite4Byte(Adapter, 0x68, 0xa005000c);

+				rtw_msleep_os(50);

+				PlatformEFIOWrite4Byte(Adapter, 0x68, 0xb005000c);

+				rtw_msleep_os(50);

+				PlatformEFIOWrite1Byte(Adapter, 0xCC, 0x29);

+				for(i=0; i<12; i++)

+				rtw_msleep_os(100);

+//#if (DEV_BUS_TYPE == RT_PCI_INTERFACE)

+//				BTFwPatch8723A(Adapter);

+//#endif

+				DBG_8192C("[MPT], load BT FW Patch finished!!!\n");

+			}

+			break;

+		default:

+			{

+				DBG_8192C ("[MPT], Error!! setType=%d, out of range\n", setType);

+				pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+				return paraLen;

+			}

+			break;

+	}

+	if(setParaLen != validParaLen)

+	{

+		DBG_8192C("[MPT], Error!! wrong parameter length=%d for BT_SET_GEN_CMD cmd id=0x%x, paraLen should=0x%x\n",

+			setParaLen, setType, validParaLen);

+		pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U;

+		return paraLen;

+	}

+	

+	//

+	// execute lower layer opcodes

+	//

+	if(BT_GSET_REG == setType)

+	{

+		// fill h2c parameters

+		// here we should write reg value first then write the address, adviced by Austin

+		btOpcode = BT_LO_OP_WRITE_REG_VALUE;

+		h2cParaBuf[0] = pBtReq->pParamStart[6];

+		h2cParaBuf[1] = pBtReq->pParamStart[7];

+		h2cParaBuf[2] = pBtReq->pParamStart[8];

+		h2cParaLen = 3;

+		// execute h2c and check respond c2h from bt fw is correct or not

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+		// construct respond status code and data.

+		if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+		{

+			pBtRsp->status = ((btOpcode<<8)|retStatus);

+			DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+			return paraLen;

+		}

+		

+		// write reg address

+		btOpcode = BT_LO_OP_WRITE_REG_ADDR;

+		h2cParaBuf[0] = regType;

+		h2cParaBuf[1] = pBtReq->pParamStart[2];

+		h2cParaBuf[2] = pBtReq->pParamStart[3];

+		h2cParaLen = 3;

+		// execute h2c and check respond c2h from bt fw is correct or not

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+		// construct respond status code and data.

+		if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+		{

+			pBtRsp->status = ((btOpcode<<8)|retStatus);

+			DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+			return paraLen;

+		}		

+	}

+	else if(BT_GSET_RESET == setType)

+	{

+		btOpcode = BT_LO_OP_RESET;

+		h2cParaLen = 0;

+		// execute h2c and check respond c2h from bt fw is correct or not

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+		// construct respond status code and data.

+		if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+		{

+			pBtRsp->status = ((btOpcode<<8)|retStatus);

+			DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+			return paraLen;

+		}

+	}

+	else if(BT_GSET_TARGET_BD_ADDR == setType)

+	{

+		// fill h2c parameters

+		btOpcode = BT_LO_OP_SET_TARGET_BD_ADDR_L;

+		h2cParaBuf[0] = pBtReq->pParamStart[1];

+		h2cParaBuf[1] = pBtReq->pParamStart[2];

+		h2cParaBuf[2] = pBtReq->pParamStart[3];

+		h2cParaLen = 3;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);		

+		// ckeck bt return status.

+		if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+		{

+			pBtRsp->status = ((btOpcode<<8)|retStatus);

+			DBG_8192C ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+			return paraLen;

+		}

+

+		btOpcode = BT_LO_OP_SET_TARGET_BD_ADDR_H;

+		h2cParaBuf[0] = pBtReq->pParamStart[4];

+		h2cParaBuf[1] = pBtReq->pParamStart[5];

+		h2cParaBuf[2] = pBtReq->pParamStart[6];

+		h2cParaLen = 3;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);		

+		// ckeck bt return status.

+		if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+		{

+			pBtRsp->status = ((btOpcode<<8)|retStatus);

+			DBG_8192C ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+			return paraLen;

+		}

+	}

+	else if(BT_GSET_TX_PWR_FINETUNE == setType)

+	{

+		// fill h2c parameters

+		btOpcode = BT_LO_OP_SET_TX_POWER_CALIBRATION;

+		h2cParaBuf[0] = calVal;

+		h2cParaLen = 1;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+		// ckeck bt return status.

+		if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+		{

+			pBtRsp->status = ((btOpcode<<8)|retStatus);

+			DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+			return paraLen;

+		}

+	}

+	else if(BT_SET_TRACKING_INTERVAL == setType)

+	{

+		//	BT_LO_OP_SET_TRACKING_INTERVAL								= 0x22,

+		//	BT_LO_OP_SET_THERMAL_METER									= 0x23,

+		//	BT_LO_OP_ENABLE_CFO_TRACKING									= 0x24,

+				btOpcode = BT_LO_OP_SET_TRACKING_INTERVAL;

+				h2cParaBuf[0] = calVal;

+				h2cParaLen = 1;

+				retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);		

+				// ckeck bt return status.

+				if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+				{

+					pBtRsp->status = ((btOpcode<<8)|retStatus);

+					DBG_8192C ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+					return paraLen;

+				}

+	}

+	else if(BT_SET_THERMAL_METER == setType)

+	{

+				btOpcode = BT_LO_OP_SET_THERMAL_METER;

+				h2cParaBuf[0] = calVal;

+				h2cParaLen = 1;

+				retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);		

+				// ckeck bt return status.

+				if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+				{

+					pBtRsp->status = ((btOpcode<<8)|retStatus);

+					DBG_8192C ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+					return paraLen;

+				}

+	}

+	else if(BT_ENABLE_CFO_TRACKING == setType)

+	{

+				btOpcode = BT_LO_OP_ENABLE_CFO_TRACKING;

+				h2cParaBuf[0] = calVal;

+				h2cParaLen = 1;

+				retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);		

+				// ckeck bt return status.

+				if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+				{

+					pBtRsp->status = ((btOpcode<<8)|retStatus);

+					DBG_8192C ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+			return paraLen;

+		}

+	}

+	

+	pBtRsp->status = BT_STATUS_SUCCESS;

+	return paraLen;

+}

+

+

+

+u2Byte

+mptbt_BtSetTxRxPars(

+	IN	PADAPTER		Adapter,

+	IN	PBT_REQ_CMD 	pBtReq,

+	IN	PBT_RSP_CMD 	pBtRsp

+	)

+{

+	u1Byte				h2cParaBuf[6] ={0};

+	u1Byte				h2cParaLen=0;

+	u2Byte				paraLen=0;

+	u1Byte				retStatus=BT_STATUS_BT_OP_SUCCESS;

+	u1Byte				btOpcode;

+	u1Byte				btOpcodeVer=0;

+	PBT_TXRX_PARAMETERS pTxRxPars=(PBT_TXRX_PARAMETERS)&pBtReq->pParamStart[0];

+	u2Byte				lenTxRx=sizeof(BT_TXRX_PARAMETERS);

+	u1Byte				i;

+	u1Byte				bdAddr[6]={0};

+

+	//

+	// check upper layer parameters

+	//

+	

+	// 1. check upper layer opcode version

+	if(pBtReq->opCodeVer != 1)

+	{

+		DBG_8192C("[MPT], Error!! Upper OP code version not match!!!\n");

+		pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH;

+		return paraLen;

+	}

+	// 2. check upper layer parameter length

+	if(pBtReq->paraLength == sizeof(BT_TXRX_PARAMETERS))

+	{	

+		DBG_8192C ("[MPT], pTxRxPars->txrxChannel=0x%x \n", pTxRxPars->txrxChannel);

+		DBG_8192C ("[MPT], pTxRxPars->txrxTxPktCnt=0x%8x \n", pTxRxPars->txrxTxPktCnt);

+		DBG_8192C  ("[MPT], pTxRxPars->txrxTxPktInterval=0x%x \n", pTxRxPars->txrxTxPktInterval);

+		DBG_8192C  ("[MPT], pTxRxPars->txrxPayloadType=0x%x \n", pTxRxPars->txrxPayloadType);

+		DBG_8192C  ("[MPT], pTxRxPars->txrxPktType=0x%x \n", pTxRxPars->txrxPktType);

+		DBG_8192C  ("[MPT], pTxRxPars->txrxPayloadLen=0x%x \n", pTxRxPars->txrxPayloadLen);

+		DBG_8192C  ("[MPT], pTxRxPars->txrxPktHeader=0x%x \n", pTxRxPars->txrxPktHeader);

+		DBG_8192C  ("[MPT], pTxRxPars->txrxWhitenCoeff=0x%x \n", pTxRxPars->txrxWhitenCoeff); 	

+		bdAddr[0] = pTxRxPars->txrxBdaddr[5];

+		bdAddr[1] = pTxRxPars->txrxBdaddr[4];

+		bdAddr[2] = pTxRxPars->txrxBdaddr[3];

+		bdAddr[3] = pTxRxPars->txrxBdaddr[2];

+		bdAddr[4] = pTxRxPars->txrxBdaddr[1];

+		bdAddr[5] = pTxRxPars->txrxBdaddr[0];

+		DBG_8192C  ("[MPT], pTxRxPars->txrxBdaddr: %s", &bdAddr[0]);

+		DBG_8192C ("[MPT], pTxRxPars->txrxTxGainIndex=0x%x \n", pTxRxPars->txrxTxGainIndex);

+	}

+	else

+	{

+		DBG_8192C  ("[MPT], Error!! pBtReq->paraLength=%d, correct Len=%d\n", pBtReq->paraLength, lenTxRx);

+		pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U;

+		return paraLen;

+	}

+

+	//

+	// execute lower layer opcodes

+	//

+	

+	// fill h2c parameters

+	btOpcode = BT_LO_OP_SET_PKT_HEADER;

+	if(pTxRxPars->txrxPktHeader > 0x3ffff)

+	{

+		DBG_8192C  ("[MPT], Error!! pTxRxPars->txrxPktHeader=0x%x is out of range, (should be between 0x0~0x3ffff)\n", pTxRxPars->txrxPktHeader);

+		pBtRsp->status = (btOpcode<<8)|BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+		return paraLen;

+	}

+	else

+	{

+		h2cParaBuf[0] = (u1Byte)(pTxRxPars->txrxPktHeader&0xff);

+		h2cParaBuf[1] = (u1Byte)((pTxRxPars->txrxPktHeader&0xff00)>>8);

+		h2cParaBuf[2] = (u1Byte)((pTxRxPars->txrxPktHeader&0xff0000)>>16);

+		h2cParaLen = 3;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	}

+	

+	// ckeck bt return status.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C  ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}	

+

+	// fill h2c parameters

+	btOpcode = BT_LO_OP_SET_PKT_TYPE_LEN;

+	{

+		u2Byte	payloadLenLimit=0;

+		switch(pTxRxPars->txrxPktType)

+		{

+			case MP_BT_PKT_DH1:

+				payloadLenLimit = 27*8;

+				break;

+			case MP_BT_PKT_DH3:

+				payloadLenLimit = 183*8;

+				break;

+			case MP_BT_PKT_DH5:

+				payloadLenLimit = 339*8;

+				break;

+			case MP_BT_PKT_2DH1:

+				payloadLenLimit = 54*8;

+				break;

+			case MP_BT_PKT_2DH3:

+				payloadLenLimit = 367*8;

+				break;

+			case MP_BT_PKT_2DH5:

+				payloadLenLimit = 679*8;

+				break;

+			case MP_BT_PKT_3DH1:

+				payloadLenLimit = 83*8;

+				break;

+			case MP_BT_PKT_3DH3:

+				payloadLenLimit = 552*8;

+				break;

+			case MP_BT_PKT_3DH5:

+				payloadLenLimit = 1021*8;

+				break;

+			case MP_BT_PKT_LE:

+				payloadLenLimit = 39*8;

+				break;

+			default:

+				{

+					DBG_8192C  ("[MPT], Error!! Unknown pTxRxPars->txrxPktType=0x%x\n", pTxRxPars->txrxPktType);

+					pBtRsp->status = (btOpcode<<8)|BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+					return paraLen;

+				}

+				break;

+		}

+

+		if(pTxRxPars->txrxPayloadLen > payloadLenLimit)

+		{

+			DBG_8192C ("[MPT], Error!! pTxRxPars->txrxPayloadLen=0x%x, (should smaller than %d)\n", 

+				pTxRxPars->txrxPayloadLen, payloadLenLimit);

+			pBtRsp->status = (btOpcode<<8)|BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+			return paraLen;

+		}

+

+		h2cParaBuf[0] = pTxRxPars->txrxPktType;

+		h2cParaBuf[1] = (u1Byte)((pTxRxPars->txrxPayloadLen&0xff));

+		h2cParaBuf[2] = (u1Byte)((pTxRxPars->txrxPayloadLen&0xff00)>>8);

+		h2cParaLen = 3;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	}

+

+	// ckeck bt return status.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}

+

+	// fill h2c parameters

+	btOpcode = BT_LO_OP_SET_PKT_CNT_L_PL_TYPE;

+	if(pTxRxPars->txrxPayloadType > MP_BT_PAYLOAD_MAX)

+	{

+		DBG_8192C  ("[MPT], Error!! pTxRxPars->txrxPayloadType=0x%x, (should be between 0~4)\n", pTxRxPars->txrxPayloadType);

+		pBtRsp->status = (btOpcode<<8)|BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+		return paraLen;

+	}

+	else

+	{

+		h2cParaBuf[0] = (u1Byte)((pTxRxPars->txrxTxPktCnt&0xff));

+		h2cParaBuf[1] = (u1Byte)((pTxRxPars->txrxTxPktCnt&0xff00)>>8);

+		h2cParaBuf[2] = pTxRxPars->txrxPayloadType;

+		h2cParaLen = 3;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	}

+	

+	// ckeck bt return status.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}	

+

+	// fill h2c parameters

+	btOpcode = BT_LO_OP_SET_PKT_CNT_H_PKT_INTV;

+	if(pTxRxPars->txrxTxPktInterval > 15)

+	{

+	DBG_8192C  ("[MPT], Error!! pTxRxPars->txrxTxPktInterval=0x%x, (should be between 0~15)\n", pTxRxPars->txrxTxPktInterval);

+	pBtRsp->status = (btOpcode<<8)|BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+		return paraLen;

+	}

+	else

+	{

+		h2cParaBuf[0] = (u1Byte)((pTxRxPars->txrxTxPktCnt&0xff0000)>>16);

+		h2cParaBuf[1] = (u1Byte)((pTxRxPars->txrxTxPktCnt&0xff000000)>>24);

+		h2cParaBuf[2] = pTxRxPars->txrxTxPktInterval;

+		h2cParaLen = 3;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	}

+	

+	// ckeck bt return status.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}	

+

+	// fill h2c parameters

+	btOpcode = BT_LO_OP_SET_WHITENCOEFF;

+	{

+		h2cParaBuf[0] = pTxRxPars->txrxWhitenCoeff;

+		h2cParaLen = 1;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	}

+	

+	// ckeck bt return status.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C  ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}

+

+

+	// fill h2c parameters

+	btOpcode = BT_LO_OP_SET_CHNL_TX_GAIN;

+	if( (pTxRxPars->txrxChannel > 78) ||

+		(pTxRxPars->txrxTxGainIndex > 7) )

+	{

+		DBG_8192C ("[MPT], Error!! pTxRxPars->txrxChannel=0x%x, (should be between 0~78)\n", pTxRxPars->txrxChannel);

+		DBG_8192C ("[MPT], Error!! pTxRxPars->txrxTxGainIndex=0x%x, (should be between 0~7)\n", pTxRxPars->txrxTxGainIndex);

+		pBtRsp->status = (btOpcode<<8)|BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+		return paraLen;

+	}

+	else

+	{

+		h2cParaBuf[0] = pTxRxPars->txrxChannel;

+		h2cParaBuf[1] = pTxRxPars->txrxTxGainIndex;

+		h2cParaLen = 2;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	}

+	

+	// ckeck bt return status.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}

+

+	// fill h2c parameters

+	btOpcode = BT_LO_OP_SET_BD_ADDR_L;

+	if( (pTxRxPars->txrxBdaddr[0]==0) &&

+		(pTxRxPars->txrxBdaddr[1]==0) &&

+		(pTxRxPars->txrxBdaddr[2]==0) &&

+		(pTxRxPars->txrxBdaddr[3]==0) &&

+		(pTxRxPars->txrxBdaddr[4]==0) &&

+		(pTxRxPars->txrxBdaddr[5]==0) )

+	{

+		DBG_8192C ("[MPT], Error!! pTxRxPars->txrxBdaddr=all zero\n");

+		pBtRsp->status = (btOpcode<<8)|BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+		return paraLen;

+	}

+	if( (pTxRxPars->txrxBdaddr[0]==0xff) &&

+		(pTxRxPars->txrxBdaddr[1]==0xff) &&

+		(pTxRxPars->txrxBdaddr[2]==0xff) &&

+		(pTxRxPars->txrxBdaddr[3]==0xff) &&

+		(pTxRxPars->txrxBdaddr[4]==0xff) &&

+		(pTxRxPars->txrxBdaddr[5]==0xff) )

+	{

+		DBG_8192C ("[MPT], Error!! pTxRxPars->txrxBdaddr=all 0xf\n");

+		pBtRsp->status = (btOpcode<<8)|BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+		return paraLen;

+	}

+	

+	{

+		h2cParaBuf[0] = pTxRxPars->txrxBdaddr[0];

+		h2cParaBuf[1] = pTxRxPars->txrxBdaddr[1];

+		h2cParaBuf[2] = pTxRxPars->txrxBdaddr[2];

+		h2cParaLen = 3;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	}

+	// ckeck bt return status.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}	

+

+	btOpcode = BT_LO_OP_SET_BD_ADDR_H;

+	{

+		h2cParaBuf[0] = pTxRxPars->txrxBdaddr[3];

+		h2cParaBuf[1] = pTxRxPars->txrxBdaddr[4];

+		h2cParaBuf[2] = pTxRxPars->txrxBdaddr[5];

+		h2cParaLen = 3;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	}

+	// ckeck bt return status.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C  ("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}

+

+	pBtRsp->status = BT_STATUS_SUCCESS;

+	return paraLen;

+}

+

+

+

+u2Byte

+mptbt_BtTestCtrl(

+	IN	PADAPTER		Adapter,

+	IN	PBT_REQ_CMD 	pBtReq,

+	IN	PBT_RSP_CMD 	pBtRsp

+	)

+{

+	u1Byte				h2cParaBuf[6] ={0};

+	u1Byte				h2cParaLen=0;

+	u2Byte				paraLen=0;

+	u1Byte				retStatus=BT_STATUS_BT_OP_SUCCESS;

+	u1Byte				btOpcode;

+	u1Byte				btOpcodeVer=0;

+	u1Byte				testCtrl=0;

+

+	//

+	// check upper layer parameters

+	//

+	

+	// 1. check upper layer opcode version

+	if(pBtReq->opCodeVer != 1)

+	{

+		DBG_8192C("[MPT], Error!! Upper OP code version not match!!!\n");

+		pBtRsp->status = BT_STATUS_OPCODE_U_VERSION_MISMATCH;

+		return paraLen;

+	}

+	// 2. check upper layer parameter length

+	if(1 == pBtReq->paraLength)

+	{

+		testCtrl = pBtReq->pParamStart[0];

+		DBG_8192C("[MPT], testCtrl=%d \n", testCtrl);

+	}

+	else

+	{

+		DBG_8192C("[MPT], Error!! wrong parameter length=%d (should be 1)\n", pBtReq->paraLength);

+		pBtRsp->status = BT_STATUS_PARAMETER_FORMAT_ERROR_U;

+		return paraLen;

+	}

+	

+	//

+	// execute lower layer opcodes

+	//

+	

+	// 1. fill h2c parameters	

+	// check bt mode

+	btOpcode = BT_LO_OP_TEST_CTRL;

+	if(testCtrl >= MP_BT_TEST_MAX)

+	{

+		DBG_8192C("[MPT], Error!! testCtrl=0x%x, (should be between smaller or equal to 0x%x)\n", 

+			testCtrl, MP_BT_TEST_MAX-1);

+		pBtRsp->status = BT_STATUS_PARAMETER_OUT_OF_RANGE_U;

+		return paraLen;

+	}

+	else

+	{

+		h2cParaBuf[0] = testCtrl;

+		h2cParaLen = 1;

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+	}

+	

+	// 3. construct respond status code and data.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}

+

+	pBtRsp->status = BT_STATUS_SUCCESS;

+	return paraLen;

+}

+

+

+u2Byte

+mptbt_TestBT(

+	IN	PADAPTER		Adapter,

+	IN	PBT_REQ_CMD 	pBtReq,

+	IN	PBT_RSP_CMD 	pBtRsp

+	)

+{

+

+	u1Byte				h2cParaBuf[6] ={0};

+	u1Byte				h2cParaLen=0;

+	u2Byte				paraLen=0;

+	u1Byte				retStatus=BT_STATUS_BT_OP_SUCCESS;

+	u1Byte				btOpcode;

+	u1Byte				btOpcodeVer=0;

+	u1Byte				testCtrl=0;

+

+	// 1. fill h2c parameters	

+		btOpcode =  0x11;

+		h2cParaBuf[0] = 0x11;

+		h2cParaBuf[1] = 0x0;

+		h2cParaBuf[2] = 0x0;

+		h2cParaBuf[3] = 0x0;

+		h2cParaBuf[4] = 0x0;

+		h2cParaLen = 1;

+	//	retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, &h2cParaBuf[0], h2cParaLen);

+		retStatus = mptbt_BtFwOpCodeProcess(Adapter, btOpcode, btOpcodeVer, h2cParaBuf, h2cParaLen);

+	

+	

+	// 3. construct respond status code and data.

+	if(BT_STATUS_BT_OP_SUCCESS != retStatus)

+	{

+		pBtRsp->status = ((btOpcode<<8)|retStatus);

+		DBG_8192C("[MPT], Error!! status code=0x%x \n", pBtRsp->status);

+		return paraLen;

+	}

+

+	pBtRsp->status = BT_STATUS_SUCCESS;

+	return paraLen;

+}

+

+VOID

+mptbt_BtControlProcess(

+	PADAPTER	Adapter,

+	PVOID		pInBuf

+	)

+{

+	u1Byte			H2C_Parameter[6] ={0};

+	PBT_H2C 		pH2c=(PBT_H2C)&H2C_Parameter[0];

+	PMPT_CONTEXT	pMptCtx=&(Adapter->mppriv.MptCtx);

+	PBT_REQ_CMD 	pBtReq=(PBT_REQ_CMD)pInBuf;

+	PBT_RSP_CMD 	pBtRsp=(PBT_RSP_CMD)&pMptCtx->mptOutBuf[0];

+	u1Byte			i;

+

+	DBG_8192C("[MPT], mptbt_BtControlProcess()=========>\n");

+

+	DBG_8192C("[MPT], input opCodeVer=%d\n", pBtReq->opCodeVer);

+	DBG_8192C("[MPT], input OpCode=%d\n", pBtReq->OpCode);

+	DBG_8192C("[MPT], paraLength=%d \n", pBtReq->paraLength);

+	if(pBtReq->paraLength)

+	{

+		//DBG_8192C("[MPT], parameters(hex):0x%x %d \n",&pBtReq->pParamStart[0], pBtReq->paraLength);

+	}

+

+	// The following we should maintain the User OP codes sent by upper layer

+

+	pBtRsp->status = BT_STATUS_SUCCESS;

+	pMptCtx->mptOutLen = 4; //length of (BT_RSP_CMD.status+BT_RSP_CMD.paraLength)

+	pBtRsp->paraLength = 0x0;

+	

+	_rtw_memset((PVOID)&pMptCtx->mptOutBuf[0], '\0',100);

+	

+	switch(pBtReq->OpCode)

+	{

+		case BT_UP_OP_BT_READY:

+			DBG_8192C("[MPT], OPcode : [BT_READY]\n");

+			pBtRsp->paraLength = mptbt_BtReady(Adapter, pBtReq, pBtRsp);

+			break;

+		case BT_UP_OP_BT_SET_MODE:

+			DBG_8192C("[MPT], OPcode : [BT_SET_MODE]\n");

+			pBtRsp->paraLength = mptbt_BtSetMode(Adapter, pBtReq, pBtRsp);

+			break;

+		case BT_UP_OP_BT_SET_TX_RX_PARAMETER:

+			DBG_8192C("[MPT], OPcode : [BT_SET_TXRX_PARAMETER]\n");

+			pBtRsp->paraLength = mptbt_BtSetTxRxPars(Adapter, pBtReq, pBtRsp);

+			break;

+		case BT_UP_OP_BT_SET_GENERAL:

+			DBG_8192C("[MPT], OPcode : [BT_SET_GENERAL]\n");

+			pBtRsp->paraLength = mptbt_BtSetGeneral(Adapter, pBtReq, pBtRsp);

+			break;

+		case BT_UP_OP_BT_GET_GENERAL:

+			DBG_8192C("[MPT], OPcode : [BT_GET_GENERAL]\n");

+			pBtRsp->paraLength = mptbt_BtGetGeneral(Adapter, pBtReq, pBtRsp);

+			break;

+		case BT_UP_OP_BT_TEST_CTRL:

+			DBG_8192C("[MPT], OPcode : [BT_TEST_CTRL]\n");

+			pBtRsp->paraLength = mptbt_BtTestCtrl(Adapter, pBtReq, pBtRsp);

+			break;

+		case BT_UP_OP_TEST_BT:

+			DBG_8192C("[MPT], OPcode : [TEST_BT]\n");

+			pBtRsp->paraLength = mptbt_TestBT(Adapter, pBtReq, pBtRsp);

+			break;

+		default:

+			DBG_8192C("[MPT], Error!! OPcode : UNDEFINED!!!!\n");

+			pBtRsp->status = BT_STATUS_UNKNOWN_OPCODE_U;

+			pBtRsp->paraLength = 0x0;

+			break;

+	}

+

+	DBG_8192C("pBtRsp->paraLength =%d \n",pBtRsp->paraLength);

+

+	pMptCtx->mptOutLen += pBtRsp->paraLength;

+

+	DBG_8192C("\n [MPT], OUT to DLL pMptCtx->mptOutLen=%d ,pBtRsp->paraLength =%d ",pMptCtx->mptOutLen,pBtRsp->paraLength);

+		

+	DBG_8192C("\n [MPT], mptbt_BtControlProcess()<=========\n");

+}

+

+#endif

+

diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_cmd.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_cmd.c
new file mode 100755
index 00000000..0e48393a
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_cmd.c
@@ -0,0 +1,3034 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_CMD_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <recv_osdep.h>
+#include <cmd_osdep.h>
+#include <mlme_osdep.h>
+#include <rtw_byteorder.h>
+#ifdef CONFIG_BR_EXT
+#include <rtw_br_ext.h>
+#endif //CONFIG_BR_EXT
+
+#ifdef CONFIG_BT_COEXIST
+#include <rtl8723a_hal.h>
+#endif // CONFIG_BT_COEXIST
+
+/*
+Caller and the rtw_cmd_thread can protect cmd_q by spin_lock.
+No irqsave is necessary.
+*/
+
+sint	_rtw_init_cmd_priv (struct	cmd_priv *pcmdpriv)
+{
+	sint res=_SUCCESS;
+	
+_func_enter_;	
+
+	_rtw_init_sema(&(pcmdpriv->cmd_queue_sema), 0);
+	//_rtw_init_sema(&(pcmdpriv->cmd_done_sema), 0);
+	_rtw_init_sema(&(pcmdpriv->terminate_cmdthread_sema), 0);
+	
+	
+	_rtw_init_queue(&(pcmdpriv->cmd_queue));
+	
+	//allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf
+	
+	pcmdpriv->cmd_seq = 1;
+	
+	pcmdpriv->cmd_allocated_buf = rtw_zmalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ);
+	
+	if (pcmdpriv->cmd_allocated_buf == NULL){
+		res= _FAIL;
+		goto exit;
+	}
+	
+	pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf  +  CMDBUFF_ALIGN_SZ - ( (SIZE_PTR)(pcmdpriv->cmd_allocated_buf) & (CMDBUFF_ALIGN_SZ-1));
+		
+	pcmdpriv->rsp_allocated_buf = rtw_zmalloc(MAX_RSPSZ + 4);
+	
+	if (pcmdpriv->rsp_allocated_buf == NULL){
+		res= _FAIL;
+		goto exit;
+	}
+	
+	pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf  +  4 - ( (SIZE_PTR)(pcmdpriv->rsp_allocated_buf) & 3);
+
+	pcmdpriv->cmd_issued_cnt = pcmdpriv->cmd_done_cnt = pcmdpriv->rsp_cnt = 0;
+
+exit:
+	
+_func_exit_;	  
+
+	return res;
+	
+}	
+
+#ifdef CONFIG_C2H_WK
+static void c2h_wk_callback(_workitem *work);
+#endif
+sint _rtw_init_evt_priv(struct evt_priv *pevtpriv)
+{
+	sint res=_SUCCESS;
+
+_func_enter_;	
+
+#ifdef CONFIG_H2CLBK
+	_rtw_init_sema(&(pevtpriv->lbkevt_done), 0);
+	pevtpriv->lbkevt_limit = 0;
+	pevtpriv->lbkevt_num = 0;
+	pevtpriv->cmdevt_parm = NULL;		
+#endif		
+	
+	//allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf
+	ATOMIC_SET(&pevtpriv->event_seq, 0);
+	pevtpriv->evt_done_cnt = 0;
+
+#ifdef CONFIG_EVENT_THREAD_MODE
+
+	_rtw_init_sema(&(pevtpriv->evt_notify), 0);
+	_rtw_init_sema(&(pevtpriv->terminate_evtthread_sema), 0);
+
+	pevtpriv->evt_allocated_buf = rtw_zmalloc(MAX_EVTSZ + 4);	
+	if (pevtpriv->evt_allocated_buf == NULL){
+		res= _FAIL;
+		goto exit;
+		}
+	pevtpriv->evt_buf = pevtpriv->evt_allocated_buf  +  4 - ((unsigned int)(pevtpriv->evt_allocated_buf) & 3);
+	
+		
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+	pevtpriv->allocated_c2h_mem = rtw_zmalloc(C2H_MEM_SZ +4); 
+	
+	if (pevtpriv->allocated_c2h_mem == NULL){
+		res= _FAIL;
+		goto exit;
+	}
+
+	pevtpriv->c2h_mem = pevtpriv->allocated_c2h_mem +  4\
+	- ( (u32)(pevtpriv->allocated_c2h_mem) & 3);
+#ifdef PLATFORM_OS_XP
+	pevtpriv->pc2h_mdl= IoAllocateMdl((u8 *)pevtpriv->c2h_mem, C2H_MEM_SZ , FALSE, FALSE, NULL);
+	
+	if(pevtpriv->pc2h_mdl == NULL){
+		res= _FAIL;
+		goto exit;
+	}
+	MmBuildMdlForNonPagedPool(pevtpriv->pc2h_mdl);
+#endif
+#endif //end of CONFIG_SDIO_HCI
+
+	_rtw_init_queue(&(pevtpriv->evt_queue));
+
+exit:	
+
+#endif //end of CONFIG_EVENT_THREAD_MODE
+
+#ifdef CONFIG_C2H_WK
+	_init_workitem(&pevtpriv->c2h_wk, c2h_wk_callback, NULL);
+	pevtpriv->c2h_wk_alive = _FALSE;
+	pevtpriv->c2h_queue = rtw_cbuf_alloc(C2H_QUEUE_MAX_LEN+1);
+#endif
+
+_func_exit_;		 
+
+	return res;
+}
+
+void _rtw_free_evt_priv (struct	evt_priv *pevtpriv)
+{
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("+_rtw_free_evt_priv \n"));
+
+#ifdef CONFIG_EVENT_THREAD_MODE
+	_rtw_free_sema(&(pevtpriv->evt_notify));
+	_rtw_free_sema(&(pevtpriv->terminate_evtthread_sema));
+
+
+	if (pevtpriv->evt_allocated_buf)
+		rtw_mfree(pevtpriv->evt_allocated_buf, MAX_EVTSZ + 4);
+#endif
+
+#ifdef CONFIG_C2H_WK
+	_cancel_workitem_sync(&pevtpriv->c2h_wk);
+	while(pevtpriv->c2h_wk_alive)
+		rtw_msleep_os(10);
+
+	while (!rtw_cbuf_empty(pevtpriv->c2h_queue)) {
+		void *c2h;
+		if ((c2h = rtw_cbuf_pop(pevtpriv->c2h_queue)) != NULL
+			&& c2h != (void *)pevtpriv) {
+			rtw_mfree(c2h, 16);
+		}
+	}
+#endif
+
+	RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("-_rtw_free_evt_priv \n"));
+
+_func_exit_;	  	
+
+}
+
+void _rtw_free_cmd_priv (struct	cmd_priv *pcmdpriv)
+{
+_func_enter_;
+
+	if(pcmdpriv){
+		_rtw_spinlock_free(&(pcmdpriv->cmd_queue.lock));
+		_rtw_free_sema(&(pcmdpriv->cmd_queue_sema));
+		//_rtw_free_sema(&(pcmdpriv->cmd_done_sema));
+		_rtw_free_sema(&(pcmdpriv->terminate_cmdthread_sema));
+
+		if (pcmdpriv->cmd_allocated_buf)
+			rtw_mfree(pcmdpriv->cmd_allocated_buf, MAX_CMDSZ + CMDBUFF_ALIGN_SZ);
+		
+		if (pcmdpriv->rsp_allocated_buf)
+			rtw_mfree(pcmdpriv->rsp_allocated_buf, MAX_RSPSZ + 4);
+	}
+_func_exit_;		
+}
+
+/*
+Calling Context:
+
+rtw_enqueue_cmd can only be called between kernel thread, 
+since only spin_lock is used.
+
+ISR/Call-Back functions can't call this sub-function.
+
+*/
+
+sint	_rtw_enqueue_cmd(_queue *queue, struct cmd_obj *obj)
+{
+	_irqL irqL;
+
+_func_enter_;
+
+	if (obj == NULL)
+		goto exit;
+
+	//_enter_critical_bh(&queue->lock, &irqL);
+	_enter_critical(&queue->lock, &irqL);	
+
+	rtw_list_insert_tail(&obj->list, &queue->queue);
+
+	//_exit_critical_bh(&queue->lock, &irqL);	
+	_exit_critical(&queue->lock, &irqL);
+
+exit:	
+
+_func_exit_;
+
+	return _SUCCESS;
+}
+
+struct	cmd_obj	*_rtw_dequeue_cmd(_queue *queue)
+{
+	_irqL irqL;
+	struct cmd_obj *obj;
+
+_func_enter_;
+
+	//_enter_critical_bh(&(queue->lock), &irqL);
+	_enter_critical(&queue->lock, &irqL);
+	if (rtw_is_list_empty(&(queue->queue)))
+		obj = NULL;
+	else
+	{
+		obj = LIST_CONTAINOR(get_next(&(queue->queue)), struct cmd_obj, list);
+		rtw_list_delete(&obj->list);
+	}
+
+	//_exit_critical_bh(&(queue->lock), &irqL);
+	_exit_critical(&queue->lock, &irqL);
+
+_func_exit_;	
+
+	return obj;
+}
+
+u32	rtw_init_cmd_priv(struct cmd_priv *pcmdpriv)
+{
+	u32	res;
+_func_enter_;	
+	res = _rtw_init_cmd_priv (pcmdpriv);
+_func_exit_;	
+	return res;	
+}
+
+u32	rtw_init_evt_priv (struct	evt_priv *pevtpriv)
+{
+	int	res;
+_func_enter_;		
+	res = _rtw_init_evt_priv(pevtpriv);
+_func_exit_;		
+	return res;
+}
+
+void rtw_free_evt_priv (struct	evt_priv *pevtpriv)
+{
+_func_enter_;
+	RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("rtw_free_evt_priv\n"));
+	_rtw_free_evt_priv(pevtpriv);
+_func_exit_;		
+}	
+
+void rtw_free_cmd_priv (struct	cmd_priv *pcmdpriv)
+{
+_func_enter_;
+	RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("rtw_free_cmd_priv\n"));
+	_rtw_free_cmd_priv(pcmdpriv);
+_func_exit_;	
+}	
+
+int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj);
+int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+	u8 bAllow = _FALSE; //set to _TRUE to allow enqueuing cmd when hw_init_completed is _FALSE
+	
+	#ifdef SUPPORT_HW_RFOFF_DETECTED
+	//To decide allow or not
+	if(	(pcmdpriv->padapter->pwrctrlpriv.bHWPwrPindetect)
+		&&(!pcmdpriv->padapter->registrypriv.usbss_enable)
+	)		
+	{
+		if(cmd_obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra) ) 
+		{
+			struct drvextra_cmd_parm	*pdrvextra_cmd_parm = (struct drvextra_cmd_parm	*)cmd_obj->parmbuf;	
+			if(pdrvextra_cmd_parm->ec_id == POWER_SAVING_CTRL_WK_CID)
+			{	
+				//DBG_871X("==>enqueue POWER_SAVING_CTRL_WK_CID\n");
+				bAllow = _TRUE;	
+			}
+		}
+	}
+	#endif
+
+	if(cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan))
+		bAllow = _TRUE;
+
+	if( (pcmdpriv->padapter->hw_init_completed ==_FALSE && bAllow == _FALSE)
+		|| pcmdpriv->cmdthd_running== _FALSE	//com_thread not running
+	)
+	{
+		//DBG_871X("%s:%s: drop cmdcode:%u, hw_init_completed:%u, cmdthd_running:%u\n", caller_func, __FUNCTION__,
+		//	cmd_obj->cmdcode,
+		//	pcmdpriv->padapter->hw_init_completed,
+		//	pcmdpriv->cmdthd_running
+		//);
+
+		return _FAIL;
+	}	
+	return _SUCCESS;
+}
+
+
+
+u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+	int res = _FAIL;
+	PADAPTER padapter = pcmdpriv->padapter;
+	
+_func_enter_;	
+	
+	if (cmd_obj == NULL) {
+		goto exit;
+	}
+
+	cmd_obj->padapter = padapter;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	//change pcmdpriv to primary's pcmdpriv
+	if (padapter->adapter_type != PRIMARY_ADAPTER && padapter->pbuddy_adapter)
+		pcmdpriv = &(padapter->pbuddy_adapter->cmdpriv);
+#endif	
+
+	if( _FAIL == (res=rtw_cmd_filter(pcmdpriv, cmd_obj)) ) {
+		rtw_free_cmd_obj(cmd_obj);
+		goto exit;
+	}
+
+	res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj);
+
+	if(res == _SUCCESS)
+		_rtw_up_sema(&pcmdpriv->cmd_queue_sema);
+	
+exit:	
+	
+_func_exit_;
+
+	return res;
+}
+
+struct	cmd_obj	*rtw_dequeue_cmd(struct cmd_priv *pcmdpriv)
+{
+	struct cmd_obj *cmd_obj;
+	
+_func_enter_;		
+
+	cmd_obj = _rtw_dequeue_cmd(&pcmdpriv->cmd_queue);
+		
+_func_exit_;			
+	return cmd_obj;
+}
+
+void rtw_cmd_clr_isr(struct	cmd_priv *pcmdpriv)
+{
+_func_enter_;
+	pcmdpriv->cmd_done_cnt++;
+	//_rtw_up_sema(&(pcmdpriv->cmd_done_sema));
+_func_exit_;		
+}
+
+void rtw_free_cmd_obj(struct cmd_obj *pcmd)
+{
+_func_enter_;
+
+	if((pcmd->cmdcode!=_JoinBss_CMD_) &&(pcmd->cmdcode!= _CreateBss_CMD_))
+	{
+		//free parmbuf in cmd_obj
+		rtw_mfree((unsigned char*)pcmd->parmbuf, pcmd->cmdsz);
+	}	
+	
+	if(pcmd->rsp!=NULL)
+	{
+		if(pcmd->rspsz!= 0)
+		{
+			//free rsp in cmd_obj
+			rtw_mfree((unsigned char*)pcmd->rsp, pcmd->rspsz);
+		}	
+	}	
+
+	//free cmd_obj
+	rtw_mfree((unsigned char*)pcmd, sizeof(struct cmd_obj));
+	
+_func_exit_;		
+}
+
+thread_return rtw_cmd_thread(thread_context context)
+{
+	u8 ret;
+	struct cmd_obj *pcmd;
+	u8 *pcmdbuf, *prspbuf;
+	u8 (*cmd_hdl)(_adapter *padapter, u8* pbuf);
+	void (*pcmd_callback)(_adapter *dev, struct cmd_obj *pcmd);
+	PADAPTER padapter = (PADAPTER)context;
+	struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
+	
+_func_enter_;
+
+	thread_enter("RTW_CMD_THREAD");
+
+	pcmdbuf = pcmdpriv->cmd_buf;
+	prspbuf = pcmdpriv->rsp_buf;
+
+	pcmdpriv->cmdthd_running=_TRUE;
+	_rtw_up_sema(&pcmdpriv->terminate_cmdthread_sema);
+
+	RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("start r871x rtw_cmd_thread !!!!\n"));
+
+	while(1)
+	{
+		if (_rtw_down_sema(&pcmdpriv->cmd_queue_sema) == _FAIL)
+			break;
+
+		if ((padapter->bDriverStopped == _TRUE)||(padapter->bSurpriseRemoved == _TRUE))
+		{
+			DBG_871X("%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
+				__FUNCTION__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
+			break;
+		}
+
+#ifdef CONFIG_LPS_LCLK
+		if (rtw_register_cmd_alive(padapter) != _SUCCESS)
+		{
+			RT_TRACE(_module_hal_xmit_c_, _drv_notice_,
+					 ("%s: wait to leave LPS_LCLK\n", __FUNCTION__));
+			continue;
+		}
+#endif
+
+_next:
+		if ((padapter->bDriverStopped == _TRUE)||(padapter->bSurpriseRemoved== _TRUE))
+		{
+			DBG_871X("%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
+				__FUNCTION__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
+			break;
+		}
+
+		if(!(pcmd = rtw_dequeue_cmd(pcmdpriv))) {
+#ifdef CONFIG_LPS_LCLK
+			rtw_unregister_cmd_alive(padapter);
+#endif
+			continue;
+		}
+
+		if( _FAIL == rtw_cmd_filter(pcmdpriv, pcmd) )
+		{
+			pcmd->res = H2C_DROPPED;
+			goto post_process;
+		}
+
+		pcmdpriv->cmd_issued_cnt++;
+
+		pcmd->cmdsz = _RND4((pcmd->cmdsz));//_RND4
+
+		_rtw_memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
+
+		if(pcmd->cmdcode <= (sizeof(wlancmds) /sizeof(struct cmd_hdl)))
+		{
+			cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
+
+			if (cmd_hdl)
+			{
+				ret = cmd_hdl(pcmd->padapter, pcmdbuf);
+				pcmd->res = ret;
+			}
+
+			pcmdpriv->cmd_seq++;
+		}
+		else
+		{
+			pcmd->res = H2C_PARAMETERS_ERROR;
+		}
+
+		cmd_hdl = NULL;
+
+post_process:
+
+		//call callback function for post-processed
+		if(pcmd->cmdcode <= (sizeof(rtw_cmd_callback) /sizeof(struct _cmd_callback)))
+		{
+			pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback;
+			if(pcmd_callback == NULL)
+			{
+				RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("mlme_cmd_hdl(): pcmd_callback=0x%p, cmdcode=0x%x\n", pcmd_callback, pcmd->cmdcode));
+				rtw_free_cmd_obj(pcmd);
+			}
+			else
+			{
+				//todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!=NULL)
+				pcmd_callback(pcmd->padapter, pcmd);//need conider that free cmd_obj in rtw_cmd_callback
+			}
+		}
+		else
+		{
+			RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("%s: cmdcode=0x%x callback not defined!\n", __FUNCTION__, pcmd->cmdcode));
+			rtw_free_cmd_obj(pcmd);
+		}
+
+		flush_signals_thread();
+
+		goto _next;
+
+	}
+	pcmdpriv->cmdthd_running=_FALSE;
+
+
+	// free all cmd_obj resources
+	do{
+		pcmd = rtw_dequeue_cmd(pcmdpriv);
+		if(pcmd==NULL) {
+#ifdef CONFIG_LPS_LCLK
+			rtw_unregister_cmd_alive(padapter);
+#endif
+			break;
+		}
+
+		//DBG_871X("%s: leaving... drop cmdcode:%u\n", __FUNCTION__, pcmd->cmdcode);
+
+		rtw_free_cmd_obj(pcmd);	
+	}while(1);
+
+	_rtw_up_sema(&pcmdpriv->terminate_cmdthread_sema);
+
+_func_exit_;
+
+	thread_exit();
+
+}
+
+
+#ifdef CONFIG_EVENT_THREAD_MODE
+u32 rtw_enqueue_evt(struct evt_priv *pevtpriv, struct evt_obj *obj)
+{
+	_irqL irqL;
+	int	res;
+	_queue *queue = &pevtpriv->evt_queue;
+	
+_func_enter_;	
+
+	res = _SUCCESS; 		
+
+	if (obj == NULL) {
+		res = _FAIL;
+		goto exit;
+	}	
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	rtw_list_insert_tail(&obj->list, &queue->queue);
+	
+	_exit_critical_bh(&queue->lock, &irqL);
+
+	//rtw_evt_notify_isr(pevtpriv);
+
+exit:
+	
+_func_exit_;		
+
+	return res;	
+}
+
+struct evt_obj *rtw_dequeue_evt(_queue *queue)
+{
+	_irqL irqL;
+	struct	evt_obj	*pevtobj;
+	
+_func_enter_;		
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	if (rtw_is_list_empty(&(queue->queue)))
+		pevtobj = NULL;
+	else
+	{
+		pevtobj = LIST_CONTAINOR(get_next(&(queue->queue)), struct evt_obj, list);
+		rtw_list_delete(&pevtobj->list);
+	}
+
+	_exit_critical_bh(&queue->lock, &irqL);
+	
+_func_exit_;			
+
+	return pevtobj;	
+}
+
+void rtw_free_evt_obj(struct evt_obj *pevtobj)
+{
+_func_enter_;
+
+	if(pevtobj->parmbuf)
+		rtw_mfree((unsigned char*)pevtobj->parmbuf, pevtobj->evtsz);
+	
+	rtw_mfree((unsigned char*)pevtobj, sizeof(struct evt_obj));
+	
+_func_exit_;		
+}
+
+void rtw_evt_notify_isr(struct evt_priv *pevtpriv)
+{
+_func_enter_;
+	pevtpriv->evt_done_cnt++;
+	_rtw_up_sema(&(pevtpriv->evt_notify));
+_func_exit_;	
+}
+#endif
+
+
+/*
+u8 rtw_setstandby_cmd(unsigned char  *adapter) 
+*/
+u8 rtw_setstandby_cmd(_adapter *padapter, uint action)
+{
+	struct cmd_obj*			ph2c;
+	struct usb_suspend_parm*	psetusbsuspend;
+	struct cmd_priv 			*pcmdpriv=&padapter->cmdpriv;
+
+	u8 ret = _SUCCESS;
+	
+_func_enter_;	
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		ret = _FAIL;
+		goto exit;
+	}
+	
+	psetusbsuspend = (struct usb_suspend_parm*)rtw_zmalloc(sizeof(struct usb_suspend_parm)); 
+	if (psetusbsuspend == NULL) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		ret = _FAIL;
+		goto exit;
+	}
+
+	psetusbsuspend->action = action;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetusbsuspend, GEN_CMD_CODE(_SetUsbSuspend));
+
+	ret = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+	
+exit:	
+	
+_func_exit_;		
+
+	return ret;
+}
+
+/*
+rtw_sitesurvey_cmd(~)
+	### NOTE:#### (!!!!)
+	MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
+*/
+u8 rtw_sitesurvey_cmd(_adapter  *padapter, NDIS_802_11_SSID *ssid, int ssid_num,
+	struct rtw_ieee80211_channel *ch, int ch_num)
+{
+	u8 res = _FAIL;
+	struct cmd_obj		*ph2c;
+	struct sitesurvey_parm	*psurveyPara;
+	struct cmd_priv 	*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
+#endif //CONFIG_P2P
+
+_func_enter_;
+
+#ifdef CONFIG_LPS
+	if(check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE){
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 1);
+	}
+#endif
+
+#ifdef CONFIG_P2P_PS
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE) {
+		p2p_ps_wk_cmd(padapter, P2P_PS_SCAN, 1);
+	}
+#endif //CONFIG_P2P_PS
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL)
+		return _FAIL;
+
+	psurveyPara = (struct sitesurvey_parm*)rtw_zmalloc(sizeof(struct sitesurvey_parm)); 
+	if (psurveyPara == NULL) {
+		rtw_mfree((unsigned char*) ph2c, sizeof(struct cmd_obj));
+		return _FAIL;
+	}
+
+	rtw_free_network_queue(padapter, _FALSE);
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("%s: flush network queue\n", __FUNCTION__));
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+
+	/* psurveyPara->bsslimit = 48; */
+	psurveyPara->scan_mode = pmlmepriv->scan_mode;
+
+	/* prepare ssid list */
+	if (ssid) {
+		int i;
+		for (i=0; i<ssid_num && i< RTW_SSID_SCAN_AMOUNT; i++) {
+			if (ssid[i].SsidLength) {
+				_rtw_memcpy(&psurveyPara->ssid[i], &ssid[i], sizeof(NDIS_802_11_SSID));
+				psurveyPara->ssid_num++;
+				if (0)
+				DBG_871X(FUNC_ADPT_FMT" ssid:(%s, %d)\n", FUNC_ADPT_ARG(padapter),
+					psurveyPara->ssid[i].Ssid, psurveyPara->ssid[i].SsidLength);
+			}
+		}
+	}
+
+	/* prepare channel list */
+	if (ch) {
+		int i;
+		for (i=0; i<ch_num && i< RTW_CHANNEL_SCAN_AMOUNT; i++) {
+			if (ch[i].hw_value && !(ch[i].flags & RTW_IEEE80211_CHAN_DISABLED)) {
+				_rtw_memcpy(&psurveyPara->ch[i], &ch[i], sizeof(struct rtw_ieee80211_channel));
+				psurveyPara->ch_num++;
+				if (0)
+				DBG_871X(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter),
+					psurveyPara->ch[i].hw_value);
+			}
+		}
+	}
+
+	set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+	if(res == _SUCCESS) {
+
+		pmlmepriv->scan_start_time = rtw_get_current_time();
+
+#ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+		if((padapter->pbuddy_adapter->mlmeextpriv.mlmext_info.state&0x03) == WIFI_FW_AP_STATE)
+			_set_timer(&pmlmepriv->scan_to_timer, SURVEY_TO * ( 38 + ( 38 / RTW_SCAN_NUM_OF_CH ) * RTW_STAY_AP_CH_MILLISECOND ) + 1000 );
+		else
+#endif //CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+			_set_timer(&pmlmepriv->scan_to_timer, SCANNING_TIMEOUT);
+
+		rtw_led_control(padapter, LED_CTL_SITE_SURVEY);
+
+		pmlmepriv->scan_interval = SCAN_INTERVAL;// 30*2 sec = 60sec
+	} else {
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	}
+
+_func_exit_;		
+
+	return res;
+}
+
+u8 rtw_setdatarate_cmd(_adapter *padapter, u8 *rateset)
+{
+	struct cmd_obj*			ph2c;
+	struct setdatarate_parm*	pbsetdataratepara;
+	struct cmd_priv*		pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+_func_enter_;	
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pbsetdataratepara = (struct setdatarate_parm*)rtw_zmalloc(sizeof(struct setdatarate_parm)); 
+	if (pbsetdataratepara == NULL) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pbsetdataratepara, GEN_CMD_CODE(_SetDataRate));
+#ifdef MP_FIRMWARE_OFFLOAD
+	pbsetdataratepara->curr_rateidx = *(u32*)rateset;
+//	_rtw_memcpy(pbsetdataratepara, rateset, sizeof(u32));
+#else
+	pbsetdataratepara->mac_id = 5;
+	_rtw_memcpy(pbsetdataratepara->datarates, rateset, NumRates);
+#endif
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+_func_exit_;
+
+	return res;
+}
+
+u8 rtw_setbasicrate_cmd(_adapter *padapter, u8 *rateset)
+{
+	struct cmd_obj*			ph2c;
+	struct setbasicrate_parm*	pssetbasicratepara;
+	struct cmd_priv*		pcmdpriv=&padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+_func_enter_;
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res= _FAIL;
+		goto exit;
+	}
+	pssetbasicratepara = (struct setbasicrate_parm*)rtw_zmalloc(sizeof(struct setbasicrate_parm)); 
+
+	if (pssetbasicratepara == NULL) {
+		rtw_mfree((u8*) ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pssetbasicratepara, _SetBasicRate_CMD_);
+
+	_rtw_memcpy(pssetbasicratepara->basicrates, rateset, NumRates);	   
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+exit:	
+
+_func_exit_;		
+
+	return res;
+}
+
+
+/*
+unsigned char rtw_setphy_cmd(unsigned char  *adapter) 
+
+1.  be called only after rtw_update_registrypriv_dev_network( ~) or mp testing program
+2.  for AdHoc/Ap mode or mp mode?
+
+*/
+u8 rtw_setphy_cmd(_adapter *padapter, u8 modem, u8 ch)
+{
+	struct cmd_obj*			ph2c;
+	struct setphy_parm*		psetphypara;
+	struct cmd_priv 			*pcmdpriv=&padapter->cmdpriv;
+//	struct mlme_priv			*pmlmepriv = &padapter->mlmepriv;
+//	struct registry_priv*		pregistry_priv = &padapter->registrypriv;
+	u8	res=_SUCCESS;
+
+_func_enter_;	
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if(ph2c==NULL){
+		res= _FAIL;
+		goto exit;
+		}
+	psetphypara = (struct setphy_parm*)rtw_zmalloc(sizeof(struct setphy_parm)); 
+
+	if(psetphypara==NULL){
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetphypara, _SetPhy_CMD_);
+
+	RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("CH=%d, modem=%d", ch, modem));
+
+	psetphypara->modem = modem;
+	psetphypara->rfchannel = ch;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+exit:	
+_func_exit_;		
+	return res;
+}
+
+u8 rtw_setbbreg_cmd(_adapter*padapter, u8 offset, u8 val)
+{	
+	struct cmd_obj*			ph2c;
+	struct writeBB_parm*		pwritebbparm;
+	struct cmd_priv 			*pcmdpriv=&padapter->cmdpriv;	
+	u8	res=_SUCCESS;
+_func_enter_;
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if(ph2c==NULL){
+		res= _FAIL;
+		goto exit;
+		}
+	pwritebbparm = (struct writeBB_parm*)rtw_zmalloc(sizeof(struct writeBB_parm)); 
+
+	if(pwritebbparm==NULL){
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pwritebbparm, GEN_CMD_CODE(_SetBBReg));	
+
+	pwritebbparm->offset = offset;
+	pwritebbparm->value = val;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+exit:	
+_func_exit_;	
+	return res;
+}
+
+u8 rtw_getbbreg_cmd(_adapter  *padapter, u8 offset, u8 *pval)
+{	
+	struct cmd_obj*			ph2c;
+	struct readBB_parm*		prdbbparm;
+	struct cmd_priv 			*pcmdpriv=&padapter->cmdpriv;
+	u8	res=_SUCCESS;
+	
+_func_enter_;
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if(ph2c==NULL){
+		res=_FAIL;
+		goto exit;
+		}
+	prdbbparm = (struct readBB_parm*)rtw_zmalloc(sizeof(struct readBB_parm)); 
+
+	if(prdbbparm ==NULL){
+		rtw_mfree((unsigned char *) ph2c, sizeof(struct	cmd_obj));
+		return _FAIL;
+	}
+
+	_rtw_init_listhead(&ph2c->list);
+	ph2c->cmdcode =GEN_CMD_CODE(_GetBBReg);
+	ph2c->parmbuf = (unsigned char *)prdbbparm;
+	ph2c->cmdsz =  sizeof(struct readBB_parm);
+	ph2c->rsp = pval;
+	ph2c->rspsz = sizeof(struct readBB_rsp);
+	
+	prdbbparm ->offset = offset;
+	
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+exit:
+_func_exit_;	
+	return res;
+}
+
+u8 rtw_setrfreg_cmd(_adapter  *padapter, u8 offset, u32 val)
+{	
+	struct cmd_obj*			ph2c;
+	struct writeRF_parm*		pwriterfparm;
+	struct cmd_priv 			*pcmdpriv=&padapter->cmdpriv;	
+	u8	res=_SUCCESS;
+_func_enter_;
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if(ph2c==NULL){
+		res= _FAIL;	
+		goto exit;
+	}
+	pwriterfparm = (struct writeRF_parm*)rtw_zmalloc(sizeof(struct writeRF_parm)); 
+
+	if(pwriterfparm==NULL){
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pwriterfparm, GEN_CMD_CODE(_SetRFReg));	
+
+	pwriterfparm->offset = offset;
+	pwriterfparm->value = val;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+exit:
+_func_exit_;	
+	return res;
+}
+
+u8 rtw_getrfreg_cmd(_adapter  *padapter, u8 offset, u8 *pval)
+{	
+	struct cmd_obj*			ph2c;
+	struct readRF_parm*		prdrfparm;
+	struct cmd_priv 			*pcmdpriv=&padapter->cmdpriv;	
+	u8	res=_SUCCESS;
+
+_func_enter_;
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if(ph2c==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+
+	prdrfparm = (struct readRF_parm*)rtw_zmalloc(sizeof(struct readRF_parm)); 
+	if(prdrfparm ==NULL){
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	_rtw_init_listhead(&ph2c->list);
+	ph2c->cmdcode =GEN_CMD_CODE(_GetRFReg);
+	ph2c->parmbuf = (unsigned char *)prdrfparm;
+	ph2c->cmdsz =  sizeof(struct readRF_parm);
+	ph2c->rsp = pval;
+	ph2c->rspsz = sizeof(struct readRF_rsp);
+	
+	prdrfparm ->offset = offset;
+	
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+
+exit:
+
+_func_exit_;	
+
+	return res;
+}
+
+void rtw_getbbrfreg_cmdrsp_callback(_adapter*	padapter,  struct cmd_obj *pcmd)
+{       
+ _func_enter_;  
+		
+	//rtw_free_cmd_obj(pcmd);
+	rtw_mfree((unsigned char*) pcmd->parmbuf, pcmd->cmdsz);
+	rtw_mfree((unsigned char*) pcmd, sizeof(struct cmd_obj));
+	
+#ifdef CONFIG_MP_INCLUDED
+	if (padapter->registrypriv.mp_mode == 1)
+		padapter->mppriv.workparam.bcompleted= _TRUE;
+#endif	
+_func_exit_;		
+}
+
+void rtw_readtssi_cmdrsp_callback(_adapter*	padapter,  struct cmd_obj *pcmd)
+{
+ _func_enter_;  
+
+	rtw_mfree((unsigned char*) pcmd->parmbuf, pcmd->cmdsz);
+	rtw_mfree((unsigned char*) pcmd, sizeof(struct cmd_obj));
+	
+#ifdef CONFIG_MP_INCLUDED
+	if (padapter->registrypriv.mp_mode == 1)
+		padapter->mppriv.workparam.bcompleted= _TRUE;
+#endif
+
+_func_exit_;
+}
+
+u8 rtw_createbss_cmd(_adapter  *padapter)
+{
+	struct cmd_obj*			pcmd;
+	struct cmd_priv 			*pcmdpriv=&padapter->cmdpriv;
+	struct mlme_priv			*pmlmepriv = &padapter->mlmepriv;
+	WLAN_BSSID_EX		*pdev_network = &padapter->registrypriv.dev_network;
+	u8	res=_SUCCESS;
+
+_func_enter_;
+
+	rtw_led_control(padapter, LED_CTL_START_TO_LINK);
+
+	if (pmlmepriv->assoc_ssid.SsidLength == 0){
+		RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,(" createbss for Any SSid:%s\n",pmlmepriv->assoc_ssid.Ssid));		
+	} else {
+		RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,(" createbss for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
+	}
+		
+	pcmd = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if(pcmd==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+
+	_rtw_init_listhead(&pcmd->list);
+	pcmd->cmdcode = _CreateBss_CMD_;
+	pcmd->parmbuf = (unsigned char *)pdev_network;
+	pcmd->cmdsz = get_WLAN_BSSID_EX_sz((WLAN_BSSID_EX*)pdev_network);
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;	
+	
+	pdev_network->Length = pcmd->cmdsz;	
+
+#ifdef CONFIG_RTL8712
+	//notes: translate IELength & Length after assign the Length to cmdsz;
+	pdev_network->Length = cpu_to_le32(pcmd->cmdsz);
+	pdev_network->IELength = cpu_to_le32(pdev_network->IELength);
+	pdev_network->Ssid.SsidLength = cpu_to_le32(pdev_network->Ssid.SsidLength);
+#endif
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);	
+
+exit:
+
+_func_exit_;	
+
+	return res;
+}
+
+u8 rtw_createbss_cmd_ex(_adapter  *padapter, unsigned char *pbss, unsigned int sz)
+{
+	struct cmd_obj*	pcmd;
+	struct cmd_priv 	*pcmdpriv=&padapter->cmdpriv;
+	u8	res=_SUCCESS;
+	
+_func_enter_;
+			
+	pcmd = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if(pcmd==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+
+	_rtw_init_listhead(&pcmd->list);
+	pcmd->cmdcode = GEN_CMD_CODE(_CreateBss);
+	pcmd->parmbuf = pbss;
+	pcmd->cmdsz =  sz;
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+	
+_func_exit_;	
+
+	return res;	
+}
+
+u8 rtw_joinbss_cmd(_adapter  *padapter, struct wlan_network* pnetwork)
+{
+	u8	*auth, res = _SUCCESS;
+	uint	t_len = 0;
+	WLAN_BSSID_EX		*psecnetwork;
+	struct cmd_obj		*pcmd;
+	struct cmd_priv		*pcmdpriv=&padapter->cmdpriv;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv		*pqospriv= &pmlmepriv->qospriv;
+	struct security_priv	*psecuritypriv=&padapter->securitypriv;
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+#ifdef CONFIG_80211N_HT
+	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+#endif //CONFIG_80211N_HT
+	NDIS_802_11_NETWORK_INFRASTRUCTURE ndis_network_mode = pnetwork->network.InfrastructureMode;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+_func_enter_;
+
+	rtw_led_control(padapter, LED_CTL_START_TO_LINK);
+
+	if (pmlmepriv->assoc_ssid.SsidLength == 0){
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+Join cmd: Any SSid\n"));
+	} else {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+Join cmd: SSid=[%s]\n", pmlmepriv->assoc_ssid.Ssid));
+	}
+
+	pcmd = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if(pcmd==NULL){
+		res=_FAIL;
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd: memory allocate for cmd_obj fail!!!\n"));
+		goto exit;
+	}
+	/* // for IEs is pointer 
+	t_len = sizeof (ULONG) + sizeof (NDIS_802_11_MAC_ADDRESS) + 2 + 
+			sizeof (NDIS_802_11_SSID) + sizeof (ULONG) + 
+			sizeof (NDIS_802_11_RSSI) + sizeof (NDIS_802_11_NETWORK_TYPE) + 
+			sizeof (NDIS_802_11_CONFIGURATION) +	
+			sizeof (NDIS_802_11_NETWORK_INFRASTRUCTURE) +   
+			sizeof (NDIS_802_11_RATES_EX)+ sizeof(WLAN_PHY_INFO)+ sizeof (ULONG) + MAX_IE_SZ;
+	*/
+	//for IEs is fix buf size
+	t_len = sizeof(WLAN_BSSID_EX);
+
+
+	//for hidden ap to set fw_state here
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) != _TRUE)
+	{
+		switch(ndis_network_mode)
+		{
+			case Ndis802_11IBSS:
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+				break;
+
+			case Ndis802_11Infrastructure:
+				set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+				break;
+
+			case Ndis802_11APMode:
+			case Ndis802_11AutoUnknown:
+			case Ndis802_11InfrastructureMax:
+				break;
+
+		}
+	}
+
+	psecnetwork=(WLAN_BSSID_EX *)&psecuritypriv->sec_bss;
+	if(psecnetwork==NULL)
+	{
+		if(pcmd !=NULL)
+			rtw_mfree((unsigned char *)pcmd, sizeof(struct	cmd_obj));
+		
+		res=_FAIL;
+		
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd :psecnetwork==NULL!!!\n"));
+		
+		goto exit;
+	}
+
+	_rtw_memset(psecnetwork, 0, t_len);
+
+	_rtw_memcpy(psecnetwork, &pnetwork->network, get_WLAN_BSSID_EX_sz(&pnetwork->network));
+	
+	auth=&psecuritypriv->authenticator_ie[0];
+	psecuritypriv->authenticator_ie[0]=(unsigned char)psecnetwork->IELength;
+
+	if((psecnetwork->IELength-12) < (256-1)) {
+		_rtw_memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], psecnetwork->IELength-12);
+	} else {
+		_rtw_memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], (256-1));
+	}
+	  
+	psecnetwork->IELength = 0;
+	// Added by Albert 2009/02/18
+	// If the the driver wants to use the bssid to create the connection.
+	// If not,  we have to copy the connecting AP's MAC address to it so that
+	// the driver just has the bssid information for PMKIDList searching.
+        
+	if ( pmlmepriv->assoc_by_bssid == _FALSE )
+	{
+		_rtw_memcpy( &pmlmepriv->assoc_bssid[ 0 ], &pnetwork->network.MacAddress[ 0 ], ETH_ALEN );
+	}
+
+	psecnetwork->IELength = rtw_restruct_sec_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength);
+
+
+	pqospriv->qos_option = 0;
+	
+	if(pregistrypriv->wmm_enable)	
+	{	
+		u32 tmp_len;
+		
+		tmp_len = rtw_restruct_wmm_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength, psecnetwork->IELength);	
+
+		if (psecnetwork->IELength != tmp_len)		
+		{
+			psecnetwork->IELength = tmp_len;
+			pqospriv->qos_option = 1; //There is WMM IE in this corresp. beacon
+		}
+		else 
+		{
+			pqospriv->qos_option = 0;//There is no WMM IE in this corresp. beacon
+		}		
+	}	
+
+#ifdef CONFIG_80211N_HT
+	phtpriv->ht_option = _FALSE;
+	if(pregistrypriv->ht_enable)
+	{
+		//	Added by Albert 2010/06/23
+		//	For the WEP mode, we will use the bg mode to do the connection to avoid some IOT issue.
+		//	Especially for Realtek 8192u SoftAP.
+		if (	( padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_ ) &&
+			( padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_ ) &&
+			( padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_ ))
+		{
+			//rtw_restructure_ht_ie
+			rtw_restructure_ht_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], 
+									pnetwork->network.IELength, &psecnetwork->IELength);			
+		}
+	}
+
+#endif
+
+	pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.IEs, pnetwork->network.IELength);
+
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_TENDA)
+		padapter->pwrctrlpriv.smart_ps = 0;
+	else
+		padapter->pwrctrlpriv.smart_ps = padapter->registrypriv.smart_ps;
+		
+	DBG_871X("%s: smart_ps=%d\n", __func__, padapter->pwrctrlpriv.smart_ps);
+
+	#if 0
+	psecuritypriv->supplicant_ie[0]=(u8)psecnetwork->IELength;
+
+	if(psecnetwork->IELength < (256-1))
+	{
+		_rtw_memcpy(&psecuritypriv->supplicant_ie[1], &psecnetwork->IEs[0], psecnetwork->IELength);
+	}
+	else
+	{
+		_rtw_memcpy(&psecuritypriv->supplicant_ie[1], &psecnetwork->IEs[0], (256-1));
+	}
+	#endif
+	
+	pcmd->cmdsz = get_WLAN_BSSID_EX_sz(psecnetwork);//get cmdsz before endian conversion
+
+#ifdef CONFIG_RTL8712
+	//wlan_network endian conversion	
+	psecnetwork->Length = cpu_to_le32(psecnetwork->Length);
+	psecnetwork->Ssid.SsidLength= cpu_to_le32(psecnetwork->Ssid.SsidLength);
+	psecnetwork->Privacy = cpu_to_le32(psecnetwork->Privacy);
+	psecnetwork->Rssi = cpu_to_le32(psecnetwork->Rssi);
+	psecnetwork->NetworkTypeInUse = cpu_to_le32(psecnetwork->NetworkTypeInUse);
+	psecnetwork->Configuration.ATIMWindow = cpu_to_le32(psecnetwork->Configuration.ATIMWindow);
+	psecnetwork->Configuration.BeaconPeriod = cpu_to_le32(psecnetwork->Configuration.BeaconPeriod);
+	psecnetwork->Configuration.DSConfig = cpu_to_le32(psecnetwork->Configuration.DSConfig);
+	psecnetwork->Configuration.FHConfig.DwellTime=cpu_to_le32(psecnetwork->Configuration.FHConfig.DwellTime);
+	psecnetwork->Configuration.FHConfig.HopPattern=cpu_to_le32(psecnetwork->Configuration.FHConfig.HopPattern);
+	psecnetwork->Configuration.FHConfig.HopSet=cpu_to_le32(psecnetwork->Configuration.FHConfig.HopSet);
+	psecnetwork->Configuration.FHConfig.Length=cpu_to_le32(psecnetwork->Configuration.FHConfig.Length);	
+	psecnetwork->Configuration.Length = cpu_to_le32(psecnetwork->Configuration.Length);
+	psecnetwork->InfrastructureMode = cpu_to_le32(psecnetwork->InfrastructureMode);
+	psecnetwork->IELength = cpu_to_le32(psecnetwork->IELength);      
+#endif
+
+	_rtw_init_listhead(&pcmd->list);
+	pcmd->cmdcode = _JoinBss_CMD_;//GEN_CMD_CODE(_JoinBss)
+	pcmd->parmbuf = (unsigned char *)psecnetwork;
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+	
+_func_exit_;
+
+	return res;
+}
+
+u8 rtw_disassoc_cmd(_adapter*padapter, u32 deauth_timeout_ms, bool enqueue) /* for sta_mode */
+{
+	struct cmd_obj *cmdobj = NULL;
+	struct disconnect_parm *param = NULL;
+	struct cmd_priv *cmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_disassoc_cmd\n"));
+
+	/* prepare cmd parameter */
+	param = (struct disconnect_parm *)rtw_zmalloc(sizeof(*param));
+	if (param == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	param->deauth_timeout_ms = deauth_timeout_ms;
+
+	if (enqueue) {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj));
+		if (cmdobj == NULL) {
+			res = _FAIL;
+			rtw_mfree((u8 *)param, sizeof(*param));
+			goto exit;
+		}
+		init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_);
+		res = rtw_enqueue_cmd(cmdpriv, cmdobj);
+	} else {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (H2C_SUCCESS != disconnect_hdl(padapter, (u8 *)param))
+			res = _FAIL;
+		rtw_mfree((u8 *)param, sizeof(*param));
+	}
+
+exit:
+
+_func_exit_;	
+
+	return res;
+}
+
+u8 rtw_setopmode_cmd(_adapter  *padapter, NDIS_802_11_NETWORK_INFRASTRUCTURE networktype)
+{
+	struct	cmd_obj*	ph2c;
+	struct	setopmode_parm* psetop;
+
+	struct	cmd_priv   *pcmdpriv= &padapter->cmdpriv;
+	u8	res=_SUCCESS;
+
+_func_enter_;
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));			
+	if(ph2c==NULL){
+		res= _FALSE;
+		goto exit;
+	}
+	psetop = (struct setopmode_parm*)rtw_zmalloc(sizeof(struct setopmode_parm)); 
+
+	if(psetop==NULL){
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res=_FALSE;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
+	psetop->mode = (u8)networktype;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+_func_exit_;	
+
+	return res;
+}
+
+u8 rtw_setstakey_cmd(_adapter *padapter, u8 *psta, u8 unicast_key)
+{
+	struct cmd_obj*			ph2c;
+	struct set_stakey_parm	*psetstakey_para;
+	struct cmd_priv 			*pcmdpriv=&padapter->cmdpriv;
+	struct set_stakey_rsp		*psetstakey_rsp = NULL;
+	
+	struct mlme_priv			*pmlmepriv = &padapter->mlmepriv;
+	struct security_priv 		*psecuritypriv = &padapter->securitypriv;
+	struct sta_info* 			sta = (struct sta_info* )psta;
+	u8	res=_SUCCESS;
+
+_func_enter_;
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if ( ph2c == NULL){
+		res= _FAIL;
+		goto exit;
+	}
+
+	psetstakey_para = (struct set_stakey_parm*)rtw_zmalloc(sizeof(struct set_stakey_parm));
+	if(psetstakey_para==NULL){
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res=_FAIL;
+		goto exit;
+	}
+
+	psetstakey_rsp = (struct set_stakey_rsp*)rtw_zmalloc(sizeof(struct set_stakey_rsp)); 
+	if(psetstakey_rsp == NULL){
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm));
+		res=_FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+	ph2c->rsp = (u8 *) psetstakey_rsp;
+	ph2c->rspsz = sizeof(struct set_stakey_rsp);
+
+	_rtw_memcpy(psetstakey_para->addr, sta->hwaddr,ETH_ALEN);
+	
+	if(check_fwstate(pmlmepriv, WIFI_STATION_STATE)){
+#ifdef CONFIG_TDLS		
+		if(sta->tdls_sta_state&TDLS_LINKED_STATE)
+			psetstakey_para->algorithm=(u8)sta->dot118021XPrivacy;
+		else
+#endif //CONFIG_TDLS
+		psetstakey_para->algorithm =(unsigned char) psecuritypriv->dot11PrivacyAlgrthm;
+	}else{
+		GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm, _FALSE);
+	}
+
+	if (unicast_key == _TRUE) {
+#ifdef CONFIG_TDLS
+		if((sta->tdls_sta_state&TDLS_LINKED_STATE)==TDLS_LINKED_STATE)
+			_rtw_memcpy(&psetstakey_para->key, sta->tpk.tk, 16);
+		else
+#endif //CONFIG_TDLS
+			_rtw_memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
+        } else {
+		_rtw_memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, 16);
+        }
+
+	//jeff: set this becasue at least sw key is ready
+	padapter->securitypriv.busetkipkey=_TRUE;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+
+exit:
+
+_func_exit_;	
+
+	return res;
+}
+
+u8 rtw_clearstakey_cmd(_adapter *padapter, u8 *psta, u8 entry, u8 enqueue)
+{
+	struct cmd_obj*			ph2c;
+	struct set_stakey_parm	*psetstakey_para;
+	struct cmd_priv 			*pcmdpriv=&padapter->cmdpriv;
+	struct set_stakey_rsp		*psetstakey_rsp = NULL;	
+	struct mlme_priv			*pmlmepriv = &padapter->mlmepriv;
+	struct security_priv 		*psecuritypriv = &padapter->securitypriv;
+	struct sta_info* 			sta = (struct sta_info* )psta;
+	u8	res=_SUCCESS;
+
+_func_enter_;
+
+	if(!enqueue)
+	{
+		clear_cam_entry(padapter, entry);
+	}
+	else
+	{
+		ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+		if ( ph2c == NULL){
+			res= _FAIL;
+			goto exit;
+		}
+
+		psetstakey_para = (struct set_stakey_parm*)rtw_zmalloc(sizeof(struct set_stakey_parm));
+		if(psetstakey_para==NULL){
+			rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+			res=_FAIL;
+			goto exit;
+		}
+
+		psetstakey_rsp = (struct set_stakey_rsp*)rtw_zmalloc(sizeof(struct set_stakey_rsp)); 
+		if(psetstakey_rsp == NULL){
+			rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+			rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm));
+			res=_FAIL;
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+		ph2c->rsp = (u8 *) psetstakey_rsp;
+		ph2c->rspsz = sizeof(struct set_stakey_rsp);
+
+		_rtw_memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
+
+		psetstakey_para->algorithm = _NO_PRIVACY_;
+
+		psetstakey_para->id = entry;
+	
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+		
+	}
+	
+exit:
+
+_func_exit_;	
+
+	return res;
+}
+
+u8 rtw_setrttbl_cmd(_adapter  *padapter, struct setratable_parm *prate_table)
+{
+	struct cmd_obj*			ph2c;
+	struct setratable_parm *	psetrttblparm;	
+	struct cmd_priv 			*pcmdpriv=&padapter->cmdpriv;
+	u8	res=_SUCCESS;
+_func_enter_;	
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if(ph2c==NULL){
+		res= _FAIL;
+		goto exit;
+		}
+	psetrttblparm = (struct setratable_parm*)rtw_zmalloc(sizeof(struct setratable_parm)); 
+
+	if(psetrttblparm==NULL){
+		rtw_mfree((unsigned char *) ph2c, sizeof(struct	cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetrttblparm, GEN_CMD_CODE(_SetRaTable));
+
+	_rtw_memcpy(psetrttblparm,prate_table,sizeof(struct setratable_parm));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+exit:
+_func_exit_;	
+	return res;
+
+}
+
+u8 rtw_getrttbl_cmd(_adapter  *padapter, struct getratable_rsp *pval)
+{
+	struct cmd_obj*			ph2c;
+	struct getratable_parm *	pgetrttblparm;	
+	struct cmd_priv 			*pcmdpriv=&padapter->cmdpriv;
+	u8	res=_SUCCESS;
+_func_enter_;	
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if(ph2c==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+	pgetrttblparm = (struct getratable_parm*)rtw_zmalloc(sizeof(struct getratable_parm)); 
+
+	if(pgetrttblparm==NULL){
+		rtw_mfree((unsigned char *) ph2c, sizeof(struct	cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+//	init_h2fwcmd_w_parm_no_rsp(ph2c, psetrttblparm, GEN_CMD_CODE(_SetRaTable));
+
+	_rtw_init_listhead(&ph2c->list);
+	ph2c->cmdcode =GEN_CMD_CODE(_GetRaTable);
+	ph2c->parmbuf = (unsigned char *)pgetrttblparm;
+	ph2c->cmdsz =  sizeof(struct getratable_parm);
+	ph2c->rsp = (u8*)pval;
+	ph2c->rspsz = sizeof(struct getratable_rsp);
+	
+	pgetrttblparm ->rsvd = 0x0;
+	
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+exit:
+_func_exit_;	
+	return res;
+
+}
+
+u8 rtw_setassocsta_cmd(_adapter  *padapter, u8 *mac_addr)
+{
+	struct cmd_priv 		*pcmdpriv = &padapter->cmdpriv;
+	struct cmd_obj*			ph2c;
+	struct set_assocsta_parm	*psetassocsta_para;	
+	struct set_stakey_rsp		*psetassocsta_rsp = NULL;
+
+	u8	res=_SUCCESS;
+
+_func_enter_;	
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if(ph2c==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+
+	psetassocsta_para = (struct set_assocsta_parm*)rtw_zmalloc(sizeof(struct set_assocsta_parm));
+	if(psetassocsta_para==NULL){
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res=_FAIL;
+		goto exit;
+	}
+
+	psetassocsta_rsp = (struct set_stakey_rsp*)rtw_zmalloc(sizeof(struct set_assocsta_rsp)); 
+	if(psetassocsta_rsp==NULL){
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		rtw_mfree((u8 *) psetassocsta_para, sizeof(struct set_assocsta_parm));
+		return _FAIL;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetassocsta_para, _SetAssocSta_CMD_);
+	ph2c->rsp = (u8 *) psetassocsta_rsp;
+	ph2c->rspsz = sizeof(struct set_assocsta_rsp);
+
+	_rtw_memcpy(psetassocsta_para->addr, mac_addr,ETH_ALEN);
+	
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);	
+
+exit:
+
+_func_exit_;	
+
+	return res;
+ }
+
+u8 rtw_addbareq_cmd(_adapter*padapter, u8 tid, u8 *addr)
+{
+	struct cmd_priv		*pcmdpriv = &padapter->cmdpriv;
+	struct cmd_obj*		ph2c;
+	struct addBaReq_parm	*paddbareq_parm;
+
+	u8	res=_SUCCESS;
+	
+_func_enter_;	
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));	
+	if(ph2c==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+	
+	paddbareq_parm = (struct addBaReq_parm*)rtw_zmalloc(sizeof(struct addBaReq_parm)); 
+	if(paddbareq_parm==NULL){
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct	cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	paddbareq_parm->tid = tid;
+	_rtw_memcpy(paddbareq_parm->addr, addr, ETH_ALEN);
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm, GEN_CMD_CODE(_AddBAReq));
+
+	//DBG_871X("rtw_addbareq_cmd, tid=%d\n", tid);
+
+	//rtw_enqueue_cmd(pcmdpriv, ph2c);	
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	
+exit:
+	
+_func_exit_;
+
+	return res;
+}
+
+u8 rtw_dynamic_chk_wk_cmd(_adapter*padapter)
+{
+	struct cmd_obj*		ph2c;
+	struct drvextra_cmd_parm  *pdrvextra_cmd_parm;	
+	struct cmd_priv	*pcmdpriv=&padapter->cmdpriv;
+	u8	res=_SUCCESS;
+	
+_func_enter_;	
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (padapter->adapter_type != PRIMARY_ADAPTER && padapter->pbuddy_adapter)
+		pcmdpriv = &(padapter->pbuddy_adapter->cmdpriv);
+#endif
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));	
+	if(ph2c==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+	
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); 
+	if(pdrvextra_cmd_parm==NULL){
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = DYNAMIC_CHK_WK_CID;
+	pdrvextra_cmd_parm->type_size = 0;
+	pdrvextra_cmd_parm->pbuf = (u8 *)padapter;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	
+	//rtw_enqueue_cmd(pcmdpriv, ph2c);	
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	
+exit:
+	
+_func_exit_;
+
+	return res;
+
+}
+
+u8 rtw_set_ch_cmd(_adapter*padapter, u8 ch, u8 bw, u8 ch_offset, u8 enqueue)
+{
+	struct cmd_obj *pcmdobj;
+	struct set_ch_parm *set_ch_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	u8 res=_SUCCESS;
+
+_func_enter_;
+
+	DBG_871X(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
+		FUNC_NDEV_ARG(padapter->pnetdev), ch, bw, ch_offset);
+
+	/* check input parameter */
+
+	/* prepare cmd parameter */
+	set_ch_parm = (struct set_ch_parm *)rtw_zmalloc(sizeof(*set_ch_parm));
+	if (set_ch_parm == NULL) {
+		res= _FAIL;
+		goto exit;
+	}
+	set_ch_parm->ch = ch;
+	set_ch_parm->bw = bw;
+	set_ch_parm->ch_offset = ch_offset;
+
+	if (enqueue) {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		pcmdobj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct	cmd_obj));
+		if(pcmdobj == NULL){
+			rtw_mfree((u8 *)set_ch_parm, sizeof(*set_ch_parm));
+			res=_FAIL;
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(pcmdobj, set_ch_parm, GEN_CMD_CODE(_SetChannel));
+		res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+	} else {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if( H2C_SUCCESS !=set_ch_hdl(padapter, (u8 *)set_ch_parm) )
+			res = _FAIL;
+		
+		rtw_mfree((u8 *)set_ch_parm, sizeof(*set_ch_parm));
+	}
+
+	/* do something based on res... */
+
+exit:
+
+	DBG_871X(FUNC_NDEV_FMT" res:%u\n", FUNC_NDEV_ARG(padapter->pnetdev), res);
+
+_func_exit_;	
+
+	return res;
+}
+
+u8 rtw_set_chplan_cmd(_adapter*padapter, u8 chplan, u8 enqueue)
+{
+	struct	cmd_obj*	pcmdobj;
+	struct	SetChannelPlan_param *setChannelPlan_param;
+	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
+
+	u8	res=_SUCCESS;
+
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_chplan_cmd\n"));
+
+	//check input parameter
+	if(!rtw_is_channel_plan_valid(chplan)) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	//prepare cmd parameter
+	setChannelPlan_param = (struct	SetChannelPlan_param *)rtw_zmalloc(sizeof(struct SetChannelPlan_param));
+	if(setChannelPlan_param == NULL) {
+		res= _FAIL;
+		goto exit;
+	}
+	setChannelPlan_param->channel_plan=chplan;
+
+	if(enqueue)
+	{
+		//need enqueue, prepare cmd_obj and enqueue
+		pcmdobj = (struct	cmd_obj*)rtw_zmalloc(sizeof(struct	cmd_obj));
+		if(pcmdobj == NULL){
+			rtw_mfree((u8 *)setChannelPlan_param, sizeof(struct SetChannelPlan_param));
+			res=_FAIL;
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelPlan_param, GEN_CMD_CODE(_SetChannelPlan));
+		res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+	}
+	else
+	{
+		//no need to enqueue, do the cmd hdl directly and free cmd parameter
+		if( H2C_SUCCESS !=set_chplan_hdl(padapter, (unsigned char *)setChannelPlan_param) )
+			res = _FAIL;
+		
+		rtw_mfree((u8 *)setChannelPlan_param, sizeof(struct SetChannelPlan_param));
+	}
+
+	//do something based on res...
+	if(res == _SUCCESS)
+		padapter->mlmepriv.ChannelPlan = chplan;
+	
+exit:
+
+_func_exit_;	
+
+	return res;
+}
+
+u8 rtw_led_blink_cmd(_adapter*padapter, PLED_871x pLed)
+{
+	struct	cmd_obj*	pcmdobj;
+	struct	LedBlink_param *ledBlink_param;
+	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
+
+	u8	res=_SUCCESS;
+
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_led_blink_cmd\n"));
+	
+	pcmdobj = (struct	cmd_obj*)rtw_zmalloc(sizeof(struct	cmd_obj));
+	if(pcmdobj == NULL){
+		res=_FAIL;
+		goto exit;
+	}
+
+	ledBlink_param = (struct	LedBlink_param *)rtw_zmalloc(sizeof(struct	LedBlink_param));
+	if(ledBlink_param == NULL) {
+		rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	ledBlink_param->pLed=pLed;
+	
+	init_h2fwcmd_w_parm_no_rsp(pcmdobj, ledBlink_param, GEN_CMD_CODE(_LedBlink));
+	res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+	
+exit:
+
+_func_exit_;	
+
+	return res;
+}
+
+u8 rtw_set_csa_cmd(_adapter*padapter, u8 new_ch_no)
+{
+	struct	cmd_obj*	pcmdobj;
+	struct	SetChannelSwitch_param*setChannelSwitch_param;
+	struct 	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
+
+	u8	res=_SUCCESS;
+
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_csa_cmd\n"));
+	
+	pcmdobj = (struct	cmd_obj*)rtw_zmalloc(sizeof(struct	cmd_obj));
+	if(pcmdobj == NULL){
+		res=_FAIL;
+		goto exit;
+	}
+
+	setChannelSwitch_param = (struct SetChannelSwitch_param *)rtw_zmalloc(sizeof(struct	SetChannelSwitch_param));
+	if(setChannelSwitch_param == NULL) {
+		rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	setChannelSwitch_param->new_ch_no=new_ch_no;
+	
+	init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelSwitch_param, GEN_CMD_CODE(_SetChannelSwitch));
+	res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+	
+exit:
+
+_func_exit_;	
+
+	return res;
+}
+
+u8 rtw_tdls_cmd(_adapter *padapter, u8 *addr, u8 option)
+{
+	struct	cmd_obj*	pcmdobj;
+	struct	TDLSoption_param	*TDLSoption;
+	struct 	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
+
+	u8	res=_SUCCESS;
+
+_func_enter_;
+
+#ifdef CONFIG_TDLS
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_tdls_cmd\n"));
+
+	pcmdobj = (struct	cmd_obj*)rtw_zmalloc(sizeof(struct	cmd_obj));
+	if(pcmdobj == NULL){
+		res=_FAIL;
+		goto exit;
+	}
+
+	TDLSoption= (struct TDLSoption_param *)rtw_zmalloc(sizeof(struct TDLSoption_param));
+	if(TDLSoption == NULL) {
+		rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	_rtw_spinlock(&(padapter->tdlsinfo.cmd_lock));
+	_rtw_memcpy(TDLSoption->addr, addr, 6);	
+	TDLSoption->option = option;
+	_rtw_spinunlock(&(padapter->tdlsinfo.cmd_lock));
+	init_h2fwcmd_w_parm_no_rsp(pcmdobj, TDLSoption, GEN_CMD_CODE(_TDLS));
+	res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+
+#endif	//CONFIG_TDLS
+	
+exit:
+
+
+_func_exit_;	
+
+	return res;
+}
+
+static void traffic_status_watchdog(_adapter *padapter)
+{
+#ifdef CONFIG_LPS
+	u8	bEnterPS;
+#endif
+	u16	BusyThreshold = 100;
+	u8	bBusyTraffic = _FALSE, bTxBusyTraffic = _FALSE, bRxBusyTraffic = _FALSE;
+	u8	bHigherBusyTraffic = _FALSE, bHigherBusyRxTraffic = _FALSE, bHigherBusyTxTraffic = _FALSE;
+#ifdef CONFIG_FTP_PROTECT
+	u16	bPktCount = 0;
+#endif
+	struct mlme_priv		*pmlmepriv = &(padapter->mlmepriv);
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &(padapter->tdlsinfo);
+#endif //CONFIG_TDLS
+
+	//
+	// Determine if our traffic is busy now
+	//
+	if((check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE) 
+		/*&& !MgntInitAdapterInProgress(pMgntInfo)*/)
+	{
+
+#ifdef CONFIG_BT_COEXIST
+		if( pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 50 ||
+			pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 50 )
+#else // !CONFIG_BT_COEXIST
+		// if we raise bBusyTraffic in last watchdog, using lower threshold.
+		if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
+			BusyThreshold = 75;
+		if( pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > BusyThreshold ||
+			pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold )
+#endif // !CONFIG_BT_COEXIST
+		{
+			bBusyTraffic = _TRUE;
+
+			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+				bRxBusyTraffic = _TRUE;
+			else
+				bTxBusyTraffic = _TRUE;
+		}
+
+		// Higher Tx/Rx data.
+		if( pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 4000 ||
+			pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 4000 )
+		{
+			bHigherBusyTraffic = _TRUE;
+
+			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+				bHigherBusyRxTraffic = _TRUE;
+			else
+				bHigherBusyTxTraffic = _TRUE;
+		}
+
+#ifdef CONFIG_FTP_PROTECT
+		DBG_871X("RX in period:%d, TX in period:%d, ftp_lock_flag:%d\n", 
+			pmlmepriv->LinkDetectInfo.NumRxOkInPeriod,
+			pmlmepriv->LinkDetectInfo.NumTxOkInPeriod,
+			pmlmepriv->ftp_lock_flag);
+		
+		bPktCount = pmlmepriv->LinkDetectInfo.NumRxOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod;
+		if (bPktCount > 20 && !pmlmepriv->ftp_lock_flag) {
+			pmlmepriv->ftp_lock_flag = 1;
+			rtw_lock_suspend();
+		} else if(bPktCount == 0 && pmlmepriv->ftp_lock_flag) {
+			pmlmepriv->ftp_lock_flag = 0;
+			rtw_unlock_suspend();
+		}
+#endif //CONFIG_KEEP_FTP_TRANSMIT
+		
+#ifdef CONFIG_TDLS
+#ifdef CONFIG_TDLS_AUTOSETUP
+		if( ( ptdlsinfo->watchdog_count % TDLS_WATCHDOG_PERIOD ) == 0 )	//10 * 2sec, periodically sending
+			issue_tdls_dis_req( padapter, NULL );
+		ptdlsinfo->watchdog_count++;
+#endif //CONFIG_TDLS_AUTOSETUP
+#endif //CONFIG_TDLS
+
+#ifdef CONFIG_LPS
+#ifdef CONFIG_BT_COEXIST
+		if (BT_1Ant(padapter) == _FALSE)
+#endif
+		{
+		// check traffic for  powersaving.
+		if( ((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8 ) ||
+			(pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2) )
+		{
+			//DBG_871X("Tx = %d, Rx = %d \n",pmlmepriv->LinkDetectInfo.NumTxOkInPeriod,pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod);
+			bEnterPS= _FALSE;
+		}
+		else
+		{
+			bEnterPS= _TRUE;
+		}
+
+		// LeisurePS only work in infra mode.
+		if(bEnterPS)
+		{
+			LPS_Enter(padapter);
+		}
+		else
+		{
+			LPS_Leave(padapter);
+		}
+		}
+#endif // CONFIG_LPS
+	}
+	else
+	{
+#ifdef CONFIG_LPS
+		LPS_Leave(padapter);
+#endif
+	}
+
+	pmlmepriv->LinkDetectInfo.NumRxOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.NumTxOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = bBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bTxBusyTraffic = bTxBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bRxBusyTraffic = bRxBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyTraffic = bHigherBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyRxTraffic = bHigherBusyRxTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic;
+}
+
+void dynamic_chk_wk_hdl(_adapter *padapter, u8 *pbuf, int sz);
+void dynamic_chk_wk_hdl(_adapter *padapter, u8 *pbuf, int sz)
+{
+	struct mlme_priv *pmlmepriv;
+	
+	padapter = (_adapter *)pbuf;
+	pmlmepriv = &(padapter->mlmepriv);
+
+#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+#ifdef CONFIG_AP_MODE
+	if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+	{			
+		expire_timeout_chk(padapter);
+	}
+#endif
+#endif //CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+
+	#ifdef DBG_CONFIG_ERROR_DETECT	
+	rtw_hal_sreset_xmit_status_check(padapter);		
+	#endif	
+
+	//if(check_fwstate(pmlmepriv, _FW_UNDER_LINKING|_FW_UNDER_SURVEY)==_FALSE)
+	{
+		linked_status_chk(padapter);	
+		traffic_status_watchdog(padapter);
+	}
+
+	rtw_hal_dm_watchdog(padapter);
+
+	//check_hw_pbc(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type_size);	
+
+#ifdef CONFIG_BT_COEXIST
+	//
+	// BT-Coexist
+	//
+	BT_CoexistMechanism(padapter);
+#endif
+}
+
+#ifdef CONFIG_LPS
+
+void lps_ctrl_wk_hdl(_adapter *padapter, u8 lps_ctrl_type);
+void lps_ctrl_wk_hdl(_adapter *padapter, u8 lps_ctrl_type)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	u8	mstatus;
+	
+_func_enter_;
+
+	if((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE)
+		|| (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE))
+	{
+		return;
+	}
+
+	switch(lps_ctrl_type)
+	{
+		case LPS_CTRL_SCAN:
+			//DBG_871X("LPS_CTRL_SCAN \n");
+#ifdef CONFIG_BT_COEXIST
+			BT_WifiScanNotify(padapter, _TRUE);
+			if (BT_1Ant(padapter) == _FALSE)
+#endif
+			{
+				if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+				{ //connect
+					LPS_Leave(padapter);
+				}
+			}
+			break;
+		case LPS_CTRL_JOINBSS:
+			//DBG_871X("LPS_CTRL_JOINBSS \n");
+			LPS_Leave(padapter);
+			break;
+		case LPS_CTRL_CONNECT:
+			//DBG_871X("LPS_CTRL_CONNECT \n");
+			mstatus = 1;//connect
+			// Reset LPS Setting
+			padapter->pwrctrlpriv.LpsIdleCount = 0;
+			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
+#ifdef CONFIG_BT_COEXIST
+			BT_WifiMediaStatusNotify(padapter, mstatus);
+#endif
+			break;
+		case LPS_CTRL_DISCONNECT:
+			//DBG_871X("LPS_CTRL_DISCONNECT \n");
+			mstatus = 0;//disconnect
+#ifdef CONFIG_BT_COEXIST
+			BT_WifiMediaStatusNotify(padapter, mstatus);
+			if (BT_1Ant(padapter) == _FALSE)
+#endif
+			{
+				LPS_Leave(padapter);
+			}
+			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
+			break;
+		case LPS_CTRL_SPECIAL_PACKET:
+			//DBG_871X("LPS_CTRL_SPECIAL_PACKET \n");
+			pwrpriv->DelayLPSLastTimeStamp = rtw_get_current_time();
+#ifdef CONFIG_BT_COEXIST
+			BT_SpecialPacketNotify(padapter);
+			if (BT_1Ant(padapter) == _FALSE)
+#endif
+			{
+				LPS_Leave(padapter);
+			}
+			break;
+		case LPS_CTRL_LEAVE:
+			//DBG_871X("LPS_CTRL_LEAVE \n");
+#ifdef CONFIG_BT_COEXIST
+			BT_LpsLeave(padapter);
+			if (BT_1Ant(padapter) == _FALSE)
+#endif
+			{
+				LPS_Leave(padapter);
+			}
+			break;
+
+		default:
+			break;
+	}
+
+_func_exit_;
+}
+
+u8 rtw_lps_ctrl_wk_cmd(_adapter*padapter, u8 lps_ctrl_type, u8 enqueue)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	//struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+	u8	res = _SUCCESS;
+	
+_func_enter_;
+
+	//if(!pwrctrlpriv->bLeisurePs)
+	//	return res;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (padapter->iface_type != IFACE_PORT0)
+		return res;
+#endif
+
+	if(enqueue)
+	{
+		ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));	
+		if(ph2c==NULL){
+			res= _FAIL;
+			goto exit;
+		}
+		
+		pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); 
+		if(pdrvextra_cmd_parm==NULL){
+			rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+			res= _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm->ec_id = LPS_CTRL_WK_CID;
+		pdrvextra_cmd_parm->type_size = lps_ctrl_type;
+		pdrvextra_cmd_parm->pbuf = NULL;
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	}
+	else
+	{
+		lps_ctrl_wk_hdl(padapter, lps_ctrl_type);
+	}
+	
+exit:
+	
+_func_exit_;
+
+	return res;
+
+}
+
+#endif
+
+#if (RATE_ADAPTIVE_SUPPORT==1)
+void rpt_timer_setting_wk_hdl(_adapter *padapter, u16 minRptTime)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_RPT_TIMER_SETTING, (u8 *)(&minRptTime));
+}
+
+u8 rtw_rpt_timer_cfg_cmd(_adapter*padapter, u16 minRptTime)
+{
+	struct cmd_obj		*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;	
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+
+	u8	res = _SUCCESS;
+
+_func_enter_;
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));	
+	if(ph2c==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if(pdrvextra_cmd_parm==NULL){
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = RTP_TIMER_CFG_WK_CID;
+	pdrvextra_cmd_parm->type_size = minRptTime;
+	pdrvextra_cmd_parm->pbuf = NULL;
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+_func_exit_;
+
+	return res;
+
+}
+
+#endif
+
+#ifdef CONFIG_ANTENNA_DIVERSITY
+void antenna_select_wk_hdl(_adapter *padapter, u8 antenna)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_ANTENNA_DIVERSITY_SELECT, (u8 *)(&antenna));
+}
+
+u8 rtw_antenna_select_cmd(_adapter*padapter, u8 antenna,u8 enqueue)
+{
+	struct cmd_obj		*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;	
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8 	bSupportAntDiv = _FALSE;
+	u8	res = _SUCCESS;
+
+_func_enter_;
+	rtw_hal_get_def_var(padapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &(bSupportAntDiv));
+	if(_FALSE == bSupportAntDiv )	return res;
+
+	if(_TRUE == enqueue)
+	{
+		ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));	
+		if(ph2c==NULL){
+			res= _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+		if(pdrvextra_cmd_parm==NULL){
+			rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+			res= _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm->ec_id = ANT_SELECT_WK_CID;
+		pdrvextra_cmd_parm->type_size = antenna;
+		pdrvextra_cmd_parm->pbuf = NULL;
+		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	}
+	else{
+		antenna_select_wk_hdl(padapter,antenna );
+	}
+exit:
+
+_func_exit_;
+
+	return res;
+
+}
+#endif
+
+void power_saving_wk_hdl(_adapter *padapter, u8 *pbuf, int sz);
+void power_saving_wk_hdl(_adapter *padapter, u8 *pbuf, int sz)
+{
+	 rtw_ps_processor(padapter);
+}
+
+#ifdef CONFIG_P2P
+u8 p2p_protocol_wk_cmd(_adapter*padapter, int intCmdType )
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct wifidirect_info	*pwdinfo= &(padapter->wdinfo);
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+	
+_func_enter_;
+
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+	{
+		return res;
+	}
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));	
+	if(ph2c==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+			
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); 
+	if(pdrvextra_cmd_parm==NULL){
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = P2P_PROTO_WK_CID;
+	pdrvextra_cmd_parm->type_size = intCmdType;	//	As the command tppe.
+	pdrvextra_cmd_parm->pbuf = NULL;		//	Must be NULL here
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	
+exit:
+	
+_func_exit_;
+
+	return res;
+
+}
+#endif //CONFIG_P2P
+
+u8 rtw_ps_cmd(_adapter*padapter)
+{
+	struct cmd_obj		*ppscmd;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;	
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	
+	u8	res = _SUCCESS;
+_func_enter_;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (padapter->adapter_type != PRIMARY_ADAPTER)
+		goto exit;
+#endif
+	
+	ppscmd = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));	
+	if(ppscmd==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+		
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); 
+	if(pdrvextra_cmd_parm==NULL){
+		rtw_mfree((unsigned char *)ppscmd, sizeof(struct cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID;	
+	pdrvextra_cmd_parm->pbuf = NULL;
+	init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ppscmd);
+	
+exit:
+	
+_func_exit_;
+
+	return res;
+
+}
+
+#ifdef CONFIG_AP_MODE
+
+static void rtw_chk_hi_queue_hdl(_adapter *padapter)
+{
+	int cnt=0;
+	struct sta_info *psta_bmc;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+	if(!psta_bmc)
+		return;
+
+	if(psta_bmc->sleepq_len==0)
+	{		
+		u8 val = 0;
+
+		//while((rtw_read32(padapter, 0x414)&0x00ffff00)!=0)
+		//while((rtw_read32(padapter, 0x414)&0x0000ff00)!=0)
+		
+		rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &val);		
+		
+		while(_FALSE == val)
+		{
+			rtw_msleep_os(100);
+
+			cnt++;
+			
+			if(cnt>10)
+				break;
+
+			rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &val);
+		}
+
+		if(cnt<=10)
+		{
+			pstapriv->tim_bitmap &= ~BIT(0);
+			pstapriv->sta_dz_bitmap &= ~BIT(0);
+			
+			update_beacon(padapter, _TIM_IE_, NULL, _FALSE);
+		}
+		else //re check again
+		{
+			rtw_chk_hi_queue_cmd(padapter);
+		}
+		
+	}	
+	
+}
+
+u8 rtw_chk_hi_queue_cmd(_adapter*padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;	
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+	
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));	
+	if(ph2c==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+			
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); 
+	if(pdrvextra_cmd_parm==NULL){
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = CHECK_HIQ_WK_CID;
+	pdrvextra_cmd_parm->type_size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	
+exit:
+	
+	return res;
+
+}
+#endif
+
+u8 rtw_c2h_wk_cmd(PADAPTER padapter, u8 *c2h_evt)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((u8*)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
+	pdrvextra_cmd_parm->type_size = c2h_evt?16:0;
+	pdrvextra_cmd_parm->pbuf = c2h_evt;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	
+exit:
+	
+	return res;
+}
+
+s32 c2h_evt_hdl(_adapter *adapter, struct c2h_evt_hdr *c2h_evt, c2h_id_filter filter)
+{
+	s32 ret = _FAIL;
+	u8 buf[16];
+
+	if (!c2h_evt) {
+		/* No c2h event in cmd_obj, read c2h event before handling*/
+		if (c2h_evt_read(adapter, buf) == _SUCCESS) {
+			c2h_evt = (struct c2h_evt_hdr *)buf;
+			
+			if (filter && filter(c2h_evt->id) == _FALSE)
+				goto exit;
+
+			ret = rtw_hal_c2h_handler(adapter, c2h_evt);
+		}
+	} else {
+
+		if (filter && filter(c2h_evt->id) == _FALSE)
+			goto exit;
+
+		ret = rtw_hal_c2h_handler(adapter, c2h_evt);
+	}
+exit:
+	return ret;
+}
+
+#ifdef CONFIG_C2H_WK
+static void c2h_wk_callback(_workitem *work)
+{
+	struct evt_priv *evtpriv = container_of(work, struct evt_priv, c2h_wk);
+	_adapter *adapter = container_of(evtpriv, _adapter, evtpriv);
+	struct c2h_evt_hdr *c2h_evt;
+	c2h_id_filter ccx_id_filter = rtw_hal_c2h_id_filter_ccx(adapter);
+
+	evtpriv->c2h_wk_alive = _TRUE;
+
+	while (!rtw_cbuf_empty(evtpriv->c2h_queue)) {
+		if ((c2h_evt = (struct c2h_evt_hdr *)rtw_cbuf_pop(evtpriv->c2h_queue)) != NULL) {
+			/* This C2H event is read, clear it */
+			c2h_evt_clear(adapter);
+		} else if ((c2h_evt = (struct c2h_evt_hdr *)rtw_malloc(16)) != NULL) {
+			/* This C2H event is not read, read & clear now */
+			if (c2h_evt_read(adapter, (u8*)c2h_evt) != _SUCCESS)
+				continue;
+		}
+
+		/* Special pointer to trigger c2h_evt_clear only */
+		if ((void *)c2h_evt == (void *)evtpriv)
+			continue;
+
+		if (!c2h_evt_exist(c2h_evt)) {
+			rtw_mfree((u8*)c2h_evt, 16);
+			continue;
+		}
+		
+		if (ccx_id_filter(c2h_evt->id) == _TRUE) {
+			/* Handle CCX report here */
+			rtw_hal_c2h_handler(adapter, c2h_evt);
+			rtw_mfree((u8*)c2h_evt, 16);
+		} else {
+			/* Enqueue into cmd_thread for others */
+			rtw_c2h_wk_cmd(adapter, (u8 *)c2h_evt);
+		}
+	}
+
+	evtpriv->c2h_wk_alive = _FALSE;
+}
+#endif
+
+u8 rtw_drvextra_cmd_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct drvextra_cmd_parm *pdrvextra_cmd;
+
+	if(!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	pdrvextra_cmd = (struct drvextra_cmd_parm*)pbuf;
+	
+	switch(pdrvextra_cmd->ec_id)
+	{
+		case DYNAMIC_CHK_WK_CID:
+			dynamic_chk_wk_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type_size);
+			break;
+		case POWER_SAVING_CTRL_WK_CID:
+			power_saving_wk_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type_size);	
+			break;
+#ifdef CONFIG_LPS
+		case LPS_CTRL_WK_CID:
+			lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type_size);
+			break;
+#endif
+#if (RATE_ADAPTIVE_SUPPORT==1)
+		case RTP_TIMER_CFG_WK_CID:
+			rpt_timer_setting_wk_hdl(padapter, pdrvextra_cmd->type_size);
+			break;
+#endif
+#ifdef CONFIG_ANTENNA_DIVERSITY
+		case ANT_SELECT_WK_CID:
+			antenna_select_wk_hdl(padapter, pdrvextra_cmd->type_size);
+			break;
+#endif
+#ifdef CONFIG_P2P_PS
+		case P2P_PS_WK_CID:
+			p2p_ps_wk_hdl(padapter, pdrvextra_cmd->type_size);
+			break;
+#endif // CONFIG_P2P_PS
+		case P2P_PROTO_WK_CID:
+			//	Commented by Albert 2011/07/01
+			//	I used the type_size as the type command
+			p2p_protocol_wk_hdl( padapter, pdrvextra_cmd->type_size );
+			break;
+#ifdef CONFIG_AP_MODE
+		case CHECK_HIQ_WK_CID:
+			rtw_chk_hi_queue_hdl(padapter);
+			break;
+#endif //CONFIG_AP_MODE
+#ifdef CONFIG_INTEL_WIDI
+		case INTEl_WIDI_WK_CID:
+			intel_widi_wk_hdl(padapter, pdrvextra_cmd->type_size, pdrvextra_cmd->pbuf);
+			break;
+#endif //CONFIG_INTEL_WIDI
+
+		case C2H_WK_CID:
+			c2h_evt_hdl(padapter, (struct c2h_evt_hdr *)pdrvextra_cmd->pbuf, NULL);
+			break;
+
+		default:
+			break;
+	}
+
+	if (pdrvextra_cmd->pbuf && pdrvextra_cmd->type_size>0)
+	{
+		rtw_mfree(pdrvextra_cmd->pbuf, pdrvextra_cmd->type_size);
+	}
+
+	return H2C_SUCCESS;
+}
+
+void rtw_survey_cmd_callback(_adapter*	padapter ,  struct cmd_obj *pcmd)
+{
+	struct 	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+_func_enter_;
+
+	if(pcmd->res == H2C_DROPPED)
+	{
+		//TODO: cancel timer and do timeout handler directly...
+		//need to make timeout handlerOS independent
+		_set_timer(&pmlmepriv->scan_to_timer, 1);
+	}
+	else if (pcmd->res != H2C_SUCCESS) {
+		_set_timer(&pmlmepriv->scan_to_timer, 1);
+		RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\n ********Error: MgntActrtw_set_802_11_bssid_LIST_SCAN Fail ************\n\n."));
+	} 
+
+	// free cmd
+	rtw_free_cmd_obj(pcmd);
+
+_func_exit_;	
+}
+void rtw_disassoc_cmd_callback(_adapter*	padapter,  struct cmd_obj *pcmd)
+{
+	_irqL	irqL;
+	struct 	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	
+_func_enter_;	
+
+	if (pcmd->res != H2C_SUCCESS)
+	{
+		_enter_critical_bh(&pmlmepriv->lock, &irqL);
+		set_fwstate(pmlmepriv, _FW_LINKED);
+		_exit_critical_bh(&pmlmepriv->lock, &irqL);
+				
+		RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\n ***Error: disconnect_cmd_callback Fail ***\n."));
+
+		goto exit;
+	}
+#ifdef CONFIG_BR_EXT
+	else //clear bridge database
+		nat25_db_cleanup(padapter);
+#endif //CONFIG_BR_EXT
+
+	// free cmd
+	rtw_free_cmd_obj(pcmd);
+	
+exit:
+	
+_func_exit_;	
+}
+
+
+void rtw_joinbss_cmd_callback(_adapter*	padapter,  struct cmd_obj *pcmd)
+{
+	struct 	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+_func_enter_;	
+
+	if(pcmd->res == H2C_DROPPED)
+	{
+		//TODO: cancel timer and do timeout handler directly...
+		//need to make timeout handlerOS independent
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+	}
+	else if(pcmd->res != H2C_SUCCESS)
+	{				
+		RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("********Error:rtw_select_and_join_from_scanned_queue Wait Sema  Fail ************\n"));
+		_set_timer(&pmlmepriv->assoc_timer, 1);	
+	}
+	
+	rtw_free_cmd_obj(pcmd);
+	
+_func_exit_;	
+}
+
+void rtw_createbss_cmd_callback(_adapter *padapter, struct cmd_obj *pcmd)
+{	
+	_irqL irqL;
+	u8 timer_cancelled;
+	struct sta_info *psta = NULL;
+	struct wlan_network *pwlan = NULL;		
+	struct 	mlme_priv *pmlmepriv = &padapter->mlmepriv;	
+	WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)pcmd->parmbuf;
+	struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
+
+_func_enter_;	
+
+	if((pcmd->res != H2C_SUCCESS))
+	{	
+		RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\n ********Error: rtw_createbss_cmd_callback  Fail ************\n\n."));
+		_set_timer(&pmlmepriv->assoc_timer, 1 );		
+	}
+	
+	_cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
+
+#ifdef CONFIG_FW_MLMLE
+       //endian_convert
+	pnetwork->Length = le32_to_cpu(pnetwork->Length);
+  	pnetwork->Ssid.SsidLength = le32_to_cpu(pnetwork->Ssid.SsidLength);
+	pnetwork->Privacy =le32_to_cpu(pnetwork->Privacy);
+	pnetwork->Rssi = le32_to_cpu(pnetwork->Rssi);
+	pnetwork->NetworkTypeInUse =le32_to_cpu(pnetwork->NetworkTypeInUse);	
+	pnetwork->Configuration.ATIMWindow = le32_to_cpu(pnetwork->Configuration.ATIMWindow);
+	//pnetwork->Configuration.BeaconPeriod = le32_to_cpu(pnetwork->Configuration.BeaconPeriod);
+	pnetwork->Configuration.DSConfig =le32_to_cpu(pnetwork->Configuration.DSConfig);
+	pnetwork->Configuration.FHConfig.DwellTime=le32_to_cpu(pnetwork->Configuration.FHConfig.DwellTime);
+	pnetwork->Configuration.FHConfig.HopPattern=le32_to_cpu(pnetwork->Configuration.FHConfig.HopPattern);
+	pnetwork->Configuration.FHConfig.HopSet=le32_to_cpu(pnetwork->Configuration.FHConfig.HopSet);
+	pnetwork->Configuration.FHConfig.Length=le32_to_cpu(pnetwork->Configuration.FHConfig.Length);	
+	pnetwork->Configuration.Length = le32_to_cpu(pnetwork->Configuration.Length);
+	pnetwork->InfrastructureMode = le32_to_cpu(pnetwork->InfrastructureMode);
+	pnetwork->IELength = le32_to_cpu(pnetwork->IELength);
+#endif
+	
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+	
+	
+	if(check_fwstate(pmlmepriv, WIFI_AP_STATE) )
+	{
+		psta = rtw_get_stainfo(&padapter->stapriv, pnetwork->MacAddress);
+		if(!psta)
+		{
+		psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->MacAddress);
+		if (psta == NULL) 
+		{ 
+			RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nCan't alloc sta_info when createbss_cmd_callback\n"));
+			goto createbss_cmd_fail ;
+		}
+		}	
+			
+		rtw_indicate_connect( padapter);
+	}
+	else
+	{	
+		_irqL	irqL;
+
+		pwlan = _rtw_alloc_network(pmlmepriv);
+		_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+		if ( pwlan == NULL)
+		{
+			pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue);
+			if( pwlan == NULL)
+			{
+				RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\n Error:  can't get pwlan in rtw_joinbss_event_callback \n"));
+				_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+				goto createbss_cmd_fail;
+			}
+			pwlan->last_scanned = rtw_get_current_time();			
+		}	
+		else
+		{
+			rtw_list_insert_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue);
+		}
+				
+		pnetwork->Length = get_WLAN_BSSID_EX_sz(pnetwork);
+		_rtw_memcpy(&(pwlan->network), pnetwork, pnetwork->Length);
+		//pwlan->fixed = _TRUE;
+
+		//rtw_list_insert_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue);
+
+		// copy pdev_network information to 	pmlmepriv->cur_network
+		_rtw_memcpy(&tgt_network->network, pnetwork, (get_WLAN_BSSID_EX_sz(pnetwork)));
+
+		// reset DSConfig
+		//tgt_network->network.Configuration.DSConfig = (u32)rtw_ch2freq(pnetwork->Configuration.DSConfig);
+
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+#if 0		
+		if((pmlmepriv->fw_state) & WIFI_AP_STATE)
+		{
+			psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->MacAddress);
+
+			if (psta == NULL) { // for AP Mode & Adhoc Master Mode
+				RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nCan't alloc sta_info when createbss_cmd_callback\n"));
+				goto createbss_cmd_fail ;
+			}
+			
+			rtw_indicate_connect( padapter);
+		}
+		else {
+
+			//rtw_indicate_disconnect(dev);
+		}		
+#endif
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+		// we will set _FW_LINKED when there is one more sat to join us (rtw_stassoc_event_callback)
+			
+	}
+
+createbss_cmd_fail:
+	
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+	rtw_free_cmd_obj(pcmd);
+	
+_func_exit_;	
+
+}
+
+
+
+void rtw_setstaKey_cmdrsp_callback(_adapter*	padapter ,  struct cmd_obj *pcmd)
+{
+	
+	struct sta_priv * pstapriv = &padapter->stapriv;
+	struct set_stakey_rsp* psetstakey_rsp = (struct set_stakey_rsp*) (pcmd->rsp);
+	struct sta_info*	psta = rtw_get_stainfo(pstapriv, psetstakey_rsp->addr);
+
+_func_enter_;	
+
+	if(psta==NULL)
+	{
+		RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nERROR: rtw_setstaKey_cmdrsp_callback => can't get sta_info \n\n"));
+		goto exit;
+	}
+	
+	//psta->aid = psta->mac_id = psetstakey_rsp->keyid; //CAM_ID(CAM_ENTRY)
+	
+exit:	
+
+	rtw_free_cmd_obj(pcmd);
+	
+_func_exit_;	
+
+}
+void rtw_setassocsta_cmdrsp_callback(_adapter*	padapter,  struct cmd_obj *pcmd)
+{
+	_irqL	irqL;
+	struct sta_priv * pstapriv = &padapter->stapriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;	
+	struct set_assocsta_parm* passocsta_parm = (struct set_assocsta_parm*)(pcmd->parmbuf);
+	struct set_assocsta_rsp* passocsta_rsp = (struct set_assocsta_rsp*) (pcmd->rsp);		
+	struct sta_info*	psta = rtw_get_stainfo(pstapriv, passocsta_parm->addr);
+
+_func_enter_;	
+	
+	if(psta==NULL)
+	{
+		RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nERROR: setassocsta_cmdrsp_callbac => can't get sta_info \n\n"));
+		goto exit;
+	}
+	
+	psta->aid = psta->mac_id = passocsta_rsp->cam_id;
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE) && (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == _TRUE))           	
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+	set_fwstate(pmlmepriv, _FW_LINKED);
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+exit:
+	rtw_free_cmd_obj(pcmd);
+
+_func_exit_;
+}
+
+void rtw_getrttbl_cmd_cmdrsp_callback(_adapter*	padapter,  struct cmd_obj *pcmd);
+void rtw_getrttbl_cmd_cmdrsp_callback(_adapter*	padapter,  struct cmd_obj *pcmd)
+{
+_func_enter_;
+
+	rtw_free_cmd_obj(pcmd);
+#ifdef CONFIG_MP_INCLUDED
+	if (padapter->registrypriv.mp_mode == 1)
+		padapter->mppriv.workparam.bcompleted=_TRUE;
+#endif
+
+_func_exit_;
+
+}
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_debug.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_debug.c
new file mode 100755
index 00000000..509ed810
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_debug.c
@@ -0,0 +1,1218 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_DEBUG_C_
+
+
+#include <rtw_debug.h>
+
+#ifdef CONFIG_DEBUG_RTL871X
+
+	u32 GlobalDebugLevel = _drv_err_;
+
+	u64 GlobalDebugComponents = \
+			_module_rtl871x_xmit_c_ |
+			_module_xmit_osdep_c_ |
+			_module_rtl871x_recv_c_ |
+			_module_recv_osdep_c_ |
+			_module_rtl871x_mlme_c_ |
+			_module_mlme_osdep_c_ |
+			_module_rtl871x_sta_mgt_c_ |
+			_module_rtl871x_cmd_c_ |
+			_module_cmd_osdep_c_ |
+			_module_rtl871x_io_c_ |
+			_module_io_osdep_c_ |
+			_module_os_intfs_c_|
+			_module_rtl871x_security_c_|
+			_module_rtl871x_eeprom_c_|
+			_module_hal_init_c_|
+			_module_hci_hal_init_c_|
+			_module_rtl871x_ioctl_c_|
+			_module_rtl871x_ioctl_set_c_|
+			_module_rtl871x_ioctl_query_c_|
+			_module_rtl871x_pwrctrl_c_|
+			_module_hci_intfs_c_|
+			_module_hci_ops_c_|
+			_module_hci_ops_os_c_|
+			_module_rtl871x_ioctl_os_c|
+			_module_rtl8712_cmd_c_|
+			_module_hal_xmit_c_|
+			_module_rtl8712_recv_c_ |
+			_module_mp_ |
+			_module_efuse_;
+
+#endif
+
+#ifdef CONFIG_PROC_DEBUG
+#include <rtw_version.h>
+
+int proc_get_drv_version(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	
+	int len = 0;
+
+	len += snprintf(page + len, count - len, "%s\n", DRIVERVERSION);
+				
+	*eof = 1;
+	return len;
+}
+
+int proc_get_write_reg(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	*eof = 1;
+	return 0;
+}
+
+int proc_set_write_reg(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 addr, val, len;
+
+	if (count < 3)
+	{
+		DBG_871X("argument size is less than 3\n");
+		return -EFAULT;
+	}	
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {		
+
+		int num = sscanf(tmp, "%x %x %x", &addr, &val, &len);
+
+		if (num !=  3) {
+			DBG_871X("invalid write_reg parameter!\n");
+			return count;
+		}
+
+		switch(len)
+		{
+			case 1:
+				rtw_write8(padapter, addr, (u8)val);				
+				break;
+			case 2:
+				rtw_write16(padapter, addr, (u16)val);				
+				break;
+			case 4:
+				rtw_write32(padapter, addr, val);				
+				break;
+			default:
+				DBG_871X("error write length=%d", len);
+				break;
+		}			
+		
+	}
+	
+	return count;
+	
+}
+
+static u32 proc_get_read_addr=0xeeeeeeee;
+static u32 proc_get_read_len=0x4;
+
+int proc_get_read_reg(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{	
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);	
+	
+	int len = 0;
+
+	if(proc_get_read_addr==0xeeeeeeee)
+	{
+		*eof = 1;
+		return len;
+	}	
+
+	switch(proc_get_read_len)
+	{
+		case 1:			
+			len += snprintf(page + len, count - len, "rtw_read8(0x%x)=0x%x\n", proc_get_read_addr, rtw_read8(padapter, proc_get_read_addr));
+			break;
+		case 2:
+			len += snprintf(page + len, count - len, "rtw_read16(0x%x)=0x%x\n", proc_get_read_addr, rtw_read16(padapter, proc_get_read_addr));
+			break;
+		case 4:
+			len += snprintf(page + len, count - len, "rtw_read32(0x%x)=0x%x\n", proc_get_read_addr, rtw_read32(padapter, proc_get_read_addr));
+			break;
+		default:
+			len += snprintf(page + len, count - len, "error read length=%d\n", proc_get_read_len);
+			break;
+	}
+
+	*eof = 1;
+	return len;
+
+}
+
+int proc_set_read_reg(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+	char tmp[16];
+	u32 addr, len;
+
+	if (count < 2)
+	{
+		DBG_871X("argument size is less than 2\n");
+		return -EFAULT;
+	}	
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {		
+
+		int num = sscanf(tmp, "%x %x", &addr, &len);
+
+		if (num !=  2) {
+			DBG_871X("invalid read_reg parameter!\n");
+			return count;
+		}
+
+		proc_get_read_addr = addr;
+		
+		proc_get_read_len = len;
+	}
+	
+	return count;
+
+}
+
+int proc_get_fwstate(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	
+	int len = 0;
+
+	len += snprintf(page + len, count - len, "fwstate=0x%x\n", get_fwstate(pmlmepriv));
+				
+	*eof = 1;
+	return len;
+}
+
+int proc_get_sec_info(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);	
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	
+	int len = 0;
+
+	len += snprintf(page + len, count - len, "auth_alg=0x%x, enc_alg=0x%x, auth_type=0x%x, enc_type=0x%x\n", 
+						psecuritypriv->dot11AuthAlgrthm, psecuritypriv->dot11PrivacyAlgrthm,
+						psecuritypriv->ndisauthtype, psecuritypriv->ndisencryptstatus);
+				
+	*eof = 1;
+	return len;
+}
+
+int proc_get_mlmext_state(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);	
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	
+	int len = 0;
+
+	len += snprintf(page + len, count - len, "pmlmeinfo->state=0x%x\n", pmlmeinfo->state);
+				
+	*eof = 1;
+	return len;
+}
+
+int proc_get_qos_option(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	
+	int len = 0;
+
+	len += snprintf(page + len, count - len, "qos_option=%d\n", pmlmepriv->qospriv.qos_option);
+				
+	*eof = 1;
+	return len;
+
+}
+
+int proc_get_ht_option(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	
+	int len = 0;
+#ifdef CONFIG_80211N_HT
+	len += snprintf(page + len, count - len, "ht_option=%d\n", pmlmepriv->htpriv.ht_option);
+#endif //CONFIG_80211N_HT
+	*eof = 1;
+	return len;
+}
+
+int proc_get_rf_info(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);	
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;	
+	int len = 0;
+
+	len += snprintf(page + len, count - len, "cur_ch=%d, cur_bw=%d, cur_ch_offet=%d\n", 
+					pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+	
+				
+	*eof = 1;
+	return len;
+
+}
+
+int proc_get_ap_info(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct sta_info *psta;
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct wlan_network *cur_network = &(pmlmepriv->cur_network);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	int len = 0;
+
+	psta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress);
+	if(psta)
+	{
+		int i;
+		struct recv_reorder_ctrl *preorder_ctrl;
+					
+		len += snprintf(page + len, count - len, "SSID=%s\n", cur_network->network.Ssid.Ssid);		
+		len += snprintf(page + len, count - len, "sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->hwaddr));
+		len += snprintf(page + len, count - len, "cur_channel=%d, cur_bwmode=%d, cur_ch_offset=%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);		
+		len += snprintf(page + len, count - len, "rtsen=%d, cts2slef=%d\n", psta->rtsen, psta->cts2self);
+		len += snprintf(page + len, count - len, "state=0x%x, aid=%d, macid=%d, raid=%d\n", psta->state, psta->aid, psta->mac_id, psta->raid);
+#ifdef CONFIG_80211N_HT
+		len += snprintf(page + len, count - len, "qos_en=%d, ht_en=%d, init_rate=%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate);		
+		len += snprintf(page + len, count - len, "bwmode=%d, ch_offset=%d, sgi=%d\n", psta->htpriv.bwmode, psta->htpriv.ch_offset, psta->htpriv.sgi);						
+		len += snprintf(page + len, count - len, "ampdu_enable = %d\n", psta->htpriv.ampdu_enable);	
+		len += snprintf(page + len, count - len, "agg_enable_bitmap=%x, candidate_tid_bitmap=%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap);
+#endif //CONFIG_80211N_HT
+					
+		for(i=0;i<16;i++)
+		{							
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+			if(preorder_ctrl->enable)
+			{
+				len += snprintf(page + len, count - len, "tid=%d, indicate_seq=%d\n", i, preorder_ctrl->indicate_seq);
+			}
+		}	
+							
+	}
+	else
+	{							
+		len += snprintf(page + len, count - len, "can't get sta's macaddr, cur_network's macaddr:" MAC_FMT "\n", MAC_ARG(cur_network->network.MacAddress));
+	}
+
+	*eof = 1;
+	return len;
+
+}
+
+int proc_get_adapter_state(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	int len = 0;
+	
+	len += snprintf(page + len, count - len, "bSurpriseRemoved=%d, bDriverStopped=%d\n", 
+						padapter->bSurpriseRemoved, padapter->bDriverStopped);
+
+	*eof = 1;
+	return len;
+
+}
+	
+int proc_get_trx_info(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	int i;
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct recv_priv  *precvpriv = &padapter->recvpriv;
+	struct hw_xmit *phwxmit;
+	int len = 0;
+	
+	len += snprintf(page + len, count - len, "free_xmitbuf_cnt=%d, free_xmitframe_cnt=%d"
+				", free_ext_xmitbuf_cnt=%d, free_xframe_ext_cnt=%d"
+				", free_recvframe_cnt=%d\n",
+				pxmitpriv->free_xmitbuf_cnt, pxmitpriv->free_xmitframe_cnt,
+				pxmitpriv->free_xmit_extbuf_cnt, pxmitpriv->free_xframe_ext_cnt,
+				precvpriv->free_recvframe_cnt);
+
+	for(i = 0; i < 4; i++) 
+	{
+		phwxmit = pxmitpriv->hwxmits + i;
+		len += snprintf(page + len, count - len, "%d, hwq.accnt=%d\n", i, phwxmit->accnt);
+	}
+
+#ifdef CONFIG_USB_HCI
+	len += snprintf(page + len, count - len, "rx_urb_pending_cn=%d\n", precvpriv->rx_pending_cnt);
+#endif
+
+	*eof = 1;
+	return len;
+
+}
+
+int proc_get_mac_reg_dump1(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	int len = 0;
+	int i,j=1;
+
+	len += snprintf(page + len, count - len, "\n======= MAC REG =======\n");
+
+	for(i=0x0;i<0x300;i+=4)
+	{	
+		if(j%4==1)	len += snprintf(page + len, count - len,"0x%02x",i);
+		len += snprintf(page + len, count - len," 0x%08x ",rtw_read32(padapter,i));		
+		if((j++)%4 == 0)	len += snprintf(page + len, count - len,"\n");	
+	}
+
+	*eof = 1;
+	return len;
+
+}
+
+int proc_get_mac_reg_dump2(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	int len = 0;
+	int i,j=1;
+
+	len += snprintf(page + len, count - len, "\n======= MAC REG =======\n");
+	memset(page, 0, count);
+	for(i=0x300;i<0x600;i+=4)
+	{	
+		if(j%4==1)	len += snprintf(page + len, count - len,"0x%02x",i);
+		len += snprintf(page + len, count - len," 0x%08x ",rtw_read32(padapter,i));		
+		if((j++)%4 == 0)	len += snprintf(page + len, count - len,"\n");	
+	}
+	
+	*eof = 1;
+	return len;
+
+}
+
+int proc_get_mac_reg_dump3(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	int len = 0;
+	int i,j=1;
+
+	len += snprintf(page + len, count - len, "\n======= MAC REG =======\n");
+
+	for(i=0x600;i<0x800;i+=4)
+	{	
+		if(j%4==1)	len += snprintf(page + len, count - len,"0x%02x",i);
+		len += snprintf(page + len, count - len," 0x%08x ",rtw_read32(padapter,i));		
+		if((j++)%4 == 0)	len += snprintf(page + len, count - len,"\n");	
+	}
+
+	*eof = 1;
+	return len;
+
+}
+
+int proc_get_bb_reg_dump1(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);	
+	int len = 0;
+	int i,j=1;		
+
+	len += snprintf(page + len, count - len, "\n======= BB REG =======\n");	
+	for(i=0x800;i<0xB00;i+=4)
+	{
+		if(j%4==1) 	len += snprintf(page + len, count - len,"0x%02x",i);		
+		len += snprintf(page + len, count - len," 0x%08x ",rtw_read32(padapter,i));		
+		if((j++)%4 == 0)	len += snprintf(page + len, count - len,"\n");	
+	}
+	*eof = 1;
+	return len;	
+}
+
+int proc_get_bb_reg_dump2(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);	
+	int len = 0;
+	int i,j=1;		
+
+	len += snprintf(page + len, count - len, "\n======= BB REG =======\n");	
+	for(i=0xB00;i<0xE00;i+=4)
+	{
+		if(j%4==1) 	len += snprintf(page + len, count - len,"0x%02x",i);		
+		len += snprintf(page + len, count - len," 0x%08x ",rtw_read32(padapter,i));		
+		if((j++)%4 == 0)	len += snprintf(page + len, count - len,"\n");	
+	}
+	*eof = 1;
+	return len;	
+}
+
+int proc_get_bb_reg_dump3(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);	
+	int len = 0;
+	int i,j=1;		
+
+	len += snprintf(page + len, count - len, "\n======= BB REG =======\n");	
+	for(i=0xE00;i<0x1000;i+=4)
+	{
+		if(j%4==1) 	len += snprintf(page + len, count - len,"0x%02x",i);		
+		len += snprintf(page + len, count - len," 0x%08x ",rtw_read32(padapter,i));		
+		if((j++)%4 == 0)	len += snprintf(page + len, count - len,"\n");	
+	}
+	*eof = 1;
+	return len;	
+}
+
+int proc_get_rf_reg_dump1(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	int len = 0;
+	int i,j=1,path;
+	u32 value;
+	
+	len += snprintf(page + len, count - len, "\n======= RF REG =======\n");
+	path = 1;
+	len += snprintf(page + len, count - len, "\nRF_Path(%x)\n",path);
+	for(i=0;i<0xC0;i++)
+	{								
+		//value = PHY_QueryRFReg(padapter, (RF90_RADIO_PATH_E)path,i, bMaskDWord);
+		value = rtw_hal_read_rfreg(padapter, path, i, 0xffffffff);
+		if(j%4==1)	len += snprintf(page + len, count - len, "0x%02x ",i);
+		len += snprintf(page + len, count - len, " 0x%08x ",value);
+		if((j++)%4==0)	len += snprintf(page + len, count - len, "\n");	
+	}
+
+	*eof = 1;
+	return len;	
+}
+
+
+int proc_get_rf_reg_dump2(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	int len = 0;
+	int i,j=1,path;
+	u32 value;	
+
+	len += snprintf(page + len, count - len, "\n======= RF REG =======\n");	
+	path = 1;
+	len += snprintf(page + len, count - len, "\nRF_Path(%x)\n",path);
+	for(i=0xC0;i<0x100;i++)
+	{								
+		//value = PHY_QueryRFReg(padapter, (RF90_RADIO_PATH_E)path,i, bMaskDWord);
+		value = rtw_hal_read_rfreg(padapter, path, i, 0xffffffff);
+		if(j%4==1)	len += snprintf(page + len, count - len, "0x%02x ",i);
+		len += snprintf(page + len, count - len, " 0x%08x ",value);
+		if((j++)%4==0)	len += snprintf(page + len, count - len, "\n");	
+	}
+	*eof = 1;
+	return len;	
+}
+
+
+int proc_get_rf_reg_dump3(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	int len = 0;
+	int i,j=1,path;
+	u32 value;	
+
+	len += snprintf(page + len, count - len, "\n======= RF REG =======\n");
+	path = 2;
+	len += snprintf(page + len, count - len, "\nRF_Path(%x)\n",path);
+	for(i=0;i<0xC0;i++)
+	{								
+		//value = PHY_QueryRFReg(padapter, (RF90_RADIO_PATH_E)path,i, bMaskDWord);
+		value = rtw_hal_read_rfreg(padapter, path, i, 0xffffffff);
+		if(j%4==1)	len += snprintf(page + len, count - len, "0x%02x ",i);
+		len += snprintf(page + len, count - len, " 0x%08x ",value);
+		if((j++)%4==0)	len += snprintf(page + len, count - len, "\n");	
+	}
+
+	*eof = 1;
+	return len;	
+}
+
+
+int proc_get_rf_reg_dump4(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	int len = 0;
+	int i,j=1,path;
+	u32 value;
+
+	len += snprintf(page + len, count - len, "\n======= RF REG =======\n");
+	path = 2;
+	len += snprintf(page + len, count - len, "\nRF_Path(%x)\n",path);
+	for(i=0xC0;i<0x100;i++)
+	{								
+		//value = PHY_QueryRFReg(padapter, (RF90_RADIO_PATH_E)path,i, bMaskDWord);
+		value = rtw_hal_read_rfreg(padapter, path, i, 0xffffffff);
+		if(j%4==1)	len += snprintf(page + len, count - len, "0x%02x ",i);
+		len += snprintf(page + len, count - len, " 0x%08x ",value);
+		if((j++)%4==0)	len += snprintf(page + len, count - len, "\n");
+	}
+	*eof = 1;
+	return len;	
+}
+	
+
+		
+int proc_get_rx_signal(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	
+	int len = 0;
+
+	len += snprintf(page + len, count - len,
+		"rssi:%d\n"
+		"rxpwdb:%d\n"
+		"signal_strength:%u\n"
+		"signal_qual:%u\n"
+		"noise:%u\n", 
+		padapter->recvpriv.rssi,
+		padapter->recvpriv.rxpwdb,
+		padapter->recvpriv.signal_strength,
+		padapter->recvpriv.signal_qual,
+		padapter->recvpriv.noise
+		);
+				
+	*eof = 1;
+	return len;
+}
+
+int proc_set_rx_signal(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 is_signal_dbg, signal_strength;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {		
+
+		int num = sscanf(tmp, "%u %u", &is_signal_dbg, &signal_strength);
+
+		is_signal_dbg = is_signal_dbg==0?0:1;
+		
+		if(is_signal_dbg && num!=2)
+			return count;
+			
+		signal_strength = signal_strength>100?100:signal_strength;
+		signal_strength = signal_strength<0?0:signal_strength;
+
+		padapter->recvpriv.is_signal_dbg = is_signal_dbg;
+		padapter->recvpriv.signal_strength_dbg=signal_strength;
+
+		if(is_signal_dbg)
+			DBG_871X("set %s %u\n", "DBG_SIGNAL_STRENGTH", signal_strength);
+		else
+			DBG_871X("set %s\n", "HW_SIGNAL_STRENGTH");
+		
+	}
+	
+	return count;
+	
+}
+#ifdef CONFIG_80211N_HT
+
+int proc_get_ht_enable(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	
+	int len = 0;
+	
+	if(pregpriv)
+		len += snprintf(page + len, count - len,
+			"%d\n",
+			pregpriv->ht_enable
+			);
+
+	*eof = 1;
+	return len;
+}
+
+int proc_set_ht_enable(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {		
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if( pregpriv && mode >= 0 && mode < 2 )
+		{
+			pregpriv->ht_enable= mode;
+			printk("ht_enable=%d\n", pregpriv->ht_enable);
+		}
+	}
+	
+	return count;
+	
+}
+
+int proc_get_cbw40_enable(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	
+	int len = 0;
+
+	if(pregpriv)
+		len += snprintf(page + len, count - len,
+			"%d\n",
+			pregpriv->cbw40_enable
+			);
+
+	*eof = 1;
+	return len;
+}
+
+int proc_set_cbw40_enable(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {		
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if( pregpriv && mode >= 0 && mode < 2 )
+		{
+
+			pregpriv->cbw40_enable= mode;
+			printk("cbw40_enable=%d\n", mode);
+
+		}
+	}
+	
+	return count;
+	
+}
+
+int proc_get_ampdu_enable(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	
+	int len = 0;
+
+	if(pregpriv)
+		len += snprintf(page + len, count - len,
+			"%d\n",
+			pregpriv->ampdu_enable
+			);
+
+	*eof = 1;
+	return len;
+}
+
+int proc_set_ampdu_enable(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {		
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if( pregpriv && mode >= 0 && mode < 3 )
+		{
+			pregpriv->ampdu_enable= mode;
+			printk("ampdu_enable=%d\n", mode);
+		}
+
+	}
+	
+	return count;
+	
+}
+#endif //CONFIG_80211N_HT
+
+int proc_get_two_path_rssi(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	
+	int len = 0;
+	
+	if(padapter)
+		len += snprintf(page + len, count - len,
+			"%d %d\n",
+			padapter->recvpriv.RxRssi[0],
+			padapter->recvpriv.RxRssi[1]
+			);
+
+	*eof = 1;
+	return len;
+}
+#ifdef CONFIG_80211N_HT
+int proc_get_rx_stbc(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	
+	int len = 0;
+
+	if(pregpriv)
+		len += snprintf(page + len, count - len,
+			"%d\n",
+			pregpriv->rx_stbc
+			);
+
+	*eof = 1;
+	return len;
+}
+
+int proc_set_rx_stbc(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {		
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if( pregpriv && (mode == 0 || mode == 1|| mode == 2|| mode == 3))
+		{
+			pregpriv->rx_stbc= mode;
+			printk("rx_stbc=%d\n", mode);
+		}
+	}
+	
+	return count;
+	
+}
+#endif //CONFIG_80211N_HT
+
+
+int proc_get_rssi_disp(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	*eof = 1;
+	return 0;
+}
+
+int proc_set_rssi_disp(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 enable=0;
+
+	if (count < 1)
+	{
+		DBG_8192C("argument size is less than 1\n");
+		return -EFAULT;
+	}	
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {		
+
+		int num = sscanf(tmp, "%x", &enable);
+
+		if (num !=  1) {
+			DBG_8192C("invalid set_rssi_disp parameter!\n");
+			return count;
+		}
+		
+		if(enable)
+		{			
+			DBG_8192C("Turn On Rx RSSI Display Function\n");
+			padapter->bRxRSSIDisplay = enable ;			
+		}
+		else
+		{
+			DBG_8192C("Turn Off Rx RSSI Display Function\n");
+			padapter->bRxRSSIDisplay = 0 ;
+		}
+	
+	}
+	
+	return count;
+	
+}	
+
+		
+#ifdef CONFIG_AP_MODE
+
+int proc_get_all_sta_info(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	_irqL irqL;
+	struct sta_info *psta;
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	int i, j;
+	_list	*plist, *phead;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int len = 0;	
+						
+
+	len += snprintf(page + len, count - len, "sta_dz_bitmap=0x%x, tim_bitmap=0x%x\n", pstapriv->sta_dz_bitmap, pstapriv->tim_bitmap);
+					
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	for(i=0; i< NUM_STA; i++)
+	{
+		phead = &(pstapriv->sta_hash[i]);
+		plist = get_next(phead);
+		
+		while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
+		{
+			psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+			plist = get_next(plist);
+
+			//if(extra_arg == psta->aid)
+			{
+				len += snprintf(page + len, count - len, "sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->hwaddr));
+				len += snprintf(page + len, count - len, "rtsen=%d, cts2slef=%d\n", psta->rtsen, psta->cts2self);
+				len += snprintf(page + len, count - len, "state=0x%x, aid=%d, macid=%d, raid=%d\n", psta->state, psta->aid, psta->mac_id, psta->raid);
+#ifdef CONFIG_80211N_HT
+				len += snprintf(page + len, count - len, "qos_en=%d, ht_en=%d, init_rate=%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate);	
+				len += snprintf(page + len, count - len, "bwmode=%d, ch_offset=%d, sgi=%d\n", psta->htpriv.bwmode, psta->htpriv.ch_offset, psta->htpriv.sgi);						
+				len += snprintf(page + len, count - len, "ampdu_enable = %d\n", psta->htpriv.ampdu_enable);									
+				len += snprintf(page + len, count - len, "agg_enable_bitmap=%x, candidate_tid_bitmap=%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap);
+#endif //CONFIG_80211N_HT
+				len += snprintf(page + len, count - len, "sleepq_len=%d\n", psta->sleepq_len);
+				len += snprintf(page + len, count - len, "capability=0x%x\n", psta->capability);
+				len += snprintf(page + len, count - len, "flags=0x%x\n", psta->flags);
+				len += snprintf(page + len, count - len, "wpa_psk=0x%x\n", psta->wpa_psk);
+				len += snprintf(page + len, count - len, "wpa2_group_cipher=0x%x\n", psta->wpa2_group_cipher);
+				len += snprintf(page + len, count - len, "wpa2_pairwise_cipher=0x%x\n", psta->wpa2_pairwise_cipher);
+				len += snprintf(page + len, count - len, "qos_info=0x%x\n", psta->qos_info);
+				len += snprintf(page + len, count - len, "dot118021XPrivacy=0x%x\n", psta->dot118021XPrivacy);
+								
+				for(j=0;j<16;j++)
+				{							
+					preorder_ctrl = &psta->recvreorder_ctrl[j];
+					if(preorder_ctrl->enable)
+					{
+						len += snprintf(page + len, count - len, "tid=%d, indicate_seq=%d\n", j, preorder_ctrl->indicate_seq);
+					}
+				}		
+									
+			}							
+			
+		}
+		
+	}
+	
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	*eof = 1;
+	return len;
+
+}
+	
+#endif		
+
+#ifdef DBG_MEMORY_LEAK
+#include <asm/atomic.h>
+extern atomic_t _malloc_cnt;;
+extern atomic_t _malloc_size;;
+
+int proc_get_malloc_cnt(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	
+	int len = 0;
+
+	len += snprintf(page + len, count - len, "_malloc_cnt=%d\n", atomic_read(&_malloc_cnt));
+	len += snprintf(page + len, count - len, "_malloc_size=%d\n", atomic_read(&_malloc_size));
+				
+	*eof = 1;
+	return len;
+}
+#endif /* DBG_MEMORY_LEAK */
+
+#ifdef CONFIG_FIND_BEST_CHANNEL
+int proc_get_best_channel(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	int len = 0;
+	u32 i, best_channel_24G = 1, best_channel_5G = 36, index_24G = 0, index_5G = 0;
+
+	for (i=0; pmlmeext->channel_set[i].ChannelNum !=0; i++) {
+		if ( pmlmeext->channel_set[i].ChannelNum == 1)
+			index_24G = i;
+		if ( pmlmeext->channel_set[i].ChannelNum == 36)
+			index_5G = i;
+	}	
+	
+	for (i=0; pmlmeext->channel_set[i].ChannelNum !=0; i++) {
+		// 2.4G
+		if ( pmlmeext->channel_set[i].ChannelNum == 6 ) {
+			if ( pmlmeext->channel_set[i].rx_count < pmlmeext->channel_set[index_24G].rx_count ) {
+				index_24G = i;
+				best_channel_24G = pmlmeext->channel_set[i].ChannelNum;
+			}
+		}
+
+		// 5G
+		if ( pmlmeext->channel_set[i].ChannelNum >= 36
+			&& pmlmeext->channel_set[i].ChannelNum < 140 ) {
+			 // Find primary channel
+			if ( (( pmlmeext->channel_set[i].ChannelNum - 36) % 8 == 0)
+				&& (pmlmeext->channel_set[i].rx_count < pmlmeext->channel_set[index_5G].rx_count) ) {
+				index_5G = i;
+				best_channel_5G = pmlmeext->channel_set[i].ChannelNum;
+			}
+		}
+
+		if ( pmlmeext->channel_set[i].ChannelNum >= 149
+			&& pmlmeext->channel_set[i].ChannelNum < 165) {
+			 // find primary channel
+			if ( (( pmlmeext->channel_set[i].ChannelNum - 149) % 8 == 0)
+				&& (pmlmeext->channel_set[i].rx_count < pmlmeext->channel_set[index_5G].rx_count) ) {
+				index_5G = i;
+				best_channel_5G = pmlmeext->channel_set[i].ChannelNum;
+			}
+		}
+#if 1 // debug
+		len += snprintf(page + len, count - len, "The rx cnt of channel %3d = %d\n", 
+					pmlmeext->channel_set[i].ChannelNum, pmlmeext->channel_set[i].rx_count);
+#endif
+	}
+	
+	len += snprintf(page + len, count - len, "best_channel_5G = %d\n", best_channel_5G);
+	len += snprintf(page + len, count - len, "best_channel_24G = %d\n", best_channel_24G);
+
+	*eof = 1;
+	return len;
+
+}
+#endif /* CONFIG_FIND_BEST_CHANNEL */
+#ifdef CONFIG_BT_COEXIST
+#define _bt_dbg_off_		0
+#define _bt_dbg_on_		1
+
+extern u32 BTCoexDbgLevel;
+int proc_get_btcoex_dbg(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+
+	int len = 0;
+
+	if(pregpriv)
+		len += snprintf(page + len, count - len,
+			"%d\n",
+			BTCoexDbgLevel
+			);
+
+	*eof = 1;
+	return len;
+}
+
+int proc_set_btcoex_dbg(struct file *file, const char *buffer,
+		unsigned long count, void *data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if( pregpriv && (mode == 0 || mode == 1|| mode == 2|| mode == 3))
+		{
+			BTCoexDbgLevel= mode;
+			printk("btcoex_dbg=%d\n", BTCoexDbgLevel);
+		}
+	}
+	
+	return count;
+	
+}
+#endif /* CONFIG_BT_COEXIST */
+
+#if defined(DBG_CONFIG_ERROR_DETECT)
+#include <rtw_sreset.h>
+int proc_get_sreset(char *page, char **start, off_t offset, int count, int *eof, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	
+	int len = 0;
+	
+	*eof = 1;
+	return len;
+}
+
+int proc_set_sreset(struct file *file, const char *buffer, unsigned long count, void *data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	s32 trigger_point;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {		
+
+		int num = sscanf(tmp, "%d", &trigger_point);
+
+		if (trigger_point == SRESET_TGP_NULL)
+			rtw_hal_sreset_reset(padapter);
+		else
+			sreset_set_trigger_point(padapter, trigger_point);
+	}
+	
+	return count;
+	
+}
+#endif /* DBG_CONFIG_ERROR_DETECT */
+
+#endif
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_eeprom.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_eeprom.c
new file mode 100755
index 00000000..fd07d64d
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_eeprom.c
@@ -0,0 +1,423 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_EEPROM_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+
+void up_clk(_adapter*	padapter,	 u16 *x)
+{
+_func_enter_;
+	*x = *x | _EESK;
+	rtw_write8(padapter, EE_9346CR, (u8)*x);
+	rtw_udelay_os(CLOCK_RATE);
+
+_func_exit_;
+	
+}
+
+void down_clk(_adapter *	padapter, u16 *x	)
+{
+_func_enter_;
+	*x = *x & ~_EESK;
+	rtw_write8(padapter, EE_9346CR, (u8)*x);
+	rtw_udelay_os(CLOCK_RATE);
+_func_exit_;	
+}
+
+void shift_out_bits(_adapter * padapter, u16 data, u16 count)
+{
+	u16 x,mask;
+_func_enter_;
+
+	if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+	mask = 0x01 << (count - 1);
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EEDO | _EEDI);
+
+	do
+	{
+		x &= ~_EEDI;
+		if(data & mask)
+			x |= _EEDI;
+		if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+		}
+		rtw_write8(padapter, EE_9346CR, (u8)x);
+		rtw_udelay_os(CLOCK_RATE);
+		up_clk(padapter, &x);
+		down_clk(padapter, &x);
+		mask = mask >> 1;
+	} while(mask);
+	if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+	x &= ~_EEDI;
+	rtw_write8(padapter, EE_9346CR, (u8)x);
+out:	
+_func_exit_;		
+}
+
+u16 shift_in_bits (_adapter * padapter)
+{
+	u16 x,d=0,i;
+_func_enter_;	
+	if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~( _EEDO | _EEDI);
+	d = 0;
+
+	for(i=0; i<16; i++)
+	{
+		d = d << 1;
+		up_clk(padapter, &x);
+	if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+		x = rtw_read8(padapter, EE_9346CR);
+
+		x &= ~(_EEDI);
+		if(x & _EEDO)
+		d |= 1;
+
+		down_clk(padapter, &x);
+	}
+out:	
+_func_exit_;		
+
+	return d;
+}
+
+void standby(_adapter *	padapter	)
+{
+	u8   x;
+_func_enter_;	
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EECS | _EESK);
+	rtw_write8(padapter, EE_9346CR,x);
+
+	rtw_udelay_os(CLOCK_RATE);
+	x |= _EECS;
+	rtw_write8(padapter, EE_9346CR, x);
+	rtw_udelay_os(CLOCK_RATE);
+_func_exit_;		
+}
+
+u16 wait_eeprom_cmd_done(_adapter* padapter)
+{
+	u8 	x;
+	u16	i,res=_FALSE;
+_func_enter_;	
+	standby(padapter );
+	for (i=0; i<200; i++) 
+	{
+		x = rtw_read8(padapter, EE_9346CR);
+		if (x & _EEDO){
+			res=_TRUE;
+			goto exit;
+			}
+		rtw_udelay_os(CLOCK_RATE);
+	}
+exit:	
+_func_exit_;			
+	return res;
+}
+
+void eeprom_clean(_adapter * padapter)
+{
+	u16 x;
+_func_enter_;		
+	if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+	x = rtw_read8(padapter, EE_9346CR);
+	if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+	x &= ~(_EECS | _EEDI);
+	rtw_write8(padapter, EE_9346CR, (u8)x);
+	if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+	up_clk(padapter, &x);
+		if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+	down_clk(padapter, &x);
+out:	
+_func_exit_;			
+}
+
+void eeprom_write16(_adapter * padapter, u16 reg, u16 data)
+{
+	u8 x;
+#ifdef CONFIG_RTL8712
+	u8	tmp8_ori,tmp8_new,tmp8_clk_ori,tmp8_clk_new;
+	tmp8_ori=rtw_read8(padapter, 0x102502f1);
+	tmp8_new=tmp8_ori & 0xf7;
+	if(tmp8_ori != tmp8_new){	
+		rtw_write8(padapter, 0x102502f1, tmp8_new);
+		RT_TRACE(_module_rtl871x_mp_ioctl_c_,_drv_err_,("====write 0x102502f1=====\n"));
+	}
+	tmp8_clk_ori=rtw_read8(padapter,0x10250003);
+	tmp8_clk_new=tmp8_clk_ori|0x20;
+	if(tmp8_clk_new!=tmp8_clk_ori){
+		RT_TRACE(_module_rtl871x_mp_ioctl_c_,_drv_err_,("====write 0x10250003=====\n"));
+		rtw_write8(padapter, 0x10250003, tmp8_clk_new);
+	}	
+#endif
+_func_enter_;		
+	
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_write8(padapter, EE_9346CR, x);
+
+	shift_out_bits(padapter, EEPROM_EWEN_OPCODE, 5);
+	
+	if(padapter->EepromAddressSize==8)	//CF+ and SDIO
+		shift_out_bits(padapter, 0, 6);
+	else									//USB
+		shift_out_bits(padapter, 0, 4);
+	
+	standby( padapter);
+
+// Commented out by rcnjko, 2004.0
+//	// Erase this particular word.  Write the erase opcode and register
+//	// number in that order. The opcode is 3bits in length; reg is 6 bits long.
+//	shift_out_bits(Adapter, EEPROM_ERASE_OPCODE, 3);
+//	shift_out_bits(Adapter, reg, Adapter->EepromAddressSize);
+//
+//	if (wait_eeprom_cmd_done(Adapter ) == FALSE) 
+//	{
+//		return;
+//	}
+
+
+	standby(padapter );
+
+	// write the new word to the EEPROM
+
+	// send the write opcode the EEPORM
+	shift_out_bits(padapter, EEPROM_WRITE_OPCODE, 3);
+
+	// select which word in the EEPROM that we are writing to.
+	shift_out_bits(padapter, reg, padapter->EepromAddressSize);
+
+	// write the data to the selected EEPROM word.
+	shift_out_bits(padapter, data, 16);
+
+	if (wait_eeprom_cmd_done(padapter ) == _FALSE) 
+	{
+
+		goto exit;
+	}
+
+	standby(padapter );
+
+	shift_out_bits(padapter, EEPROM_EWDS_OPCODE, 5);
+	shift_out_bits(padapter, reg, 4);
+
+	eeprom_clean(padapter );
+exit:	
+#ifdef CONFIG_RTL8712
+	if(tmp8_clk_new!=tmp8_clk_ori)
+		rtw_write8(padapter, 0x10250003, tmp8_clk_ori);
+	if(tmp8_new!=tmp8_ori)
+		rtw_write8(padapter, 0x102502f1, tmp8_ori);
+
+#endif
+_func_exit_;	
+	return;
+}
+
+u16 eeprom_read16(_adapter * padapter, u16 reg) //ReadEEprom
+{
+
+	u16 x;
+	u16 data=0;
+#ifdef CONFIG_RTL8712
+	u8	tmp8_ori,tmp8_new,tmp8_clk_ori,tmp8_clk_new;
+	tmp8_ori= rtw_read8(padapter, 0x102502f1);
+	tmp8_new = tmp8_ori & 0xf7;
+	if(tmp8_ori != tmp8_new){	
+		rtw_write8(padapter, 0x102502f1, tmp8_new);
+		RT_TRACE(_module_rtl871x_mp_ioctl_c_,_drv_err_,("====write 0x102502f1=====\n"));
+	}
+	tmp8_clk_ori=rtw_read8(padapter,0x10250003);
+	tmp8_clk_new=tmp8_clk_ori|0x20;
+	if(tmp8_clk_new!=tmp8_clk_ori){
+		RT_TRACE(_module_rtl871x_mp_ioctl_c_,_drv_err_,("====write 0x10250003=====\n"));
+		rtw_write8(padapter, 0x10250003, tmp8_clk_new);
+	}	
+#endif
+_func_enter_;		
+
+	if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+	// select EEPROM, reset bits, set _EECS
+	x = rtw_read8(padapter, EE_9346CR);
+
+	if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_write8(padapter, EE_9346CR, (unsigned char)x);
+
+	// write the read opcode and register number in that order
+	// The opcode is 3bits in length, reg is 6 bits long
+	shift_out_bits(padapter, EEPROM_READ_OPCODE, 3);
+	shift_out_bits(padapter, reg, padapter->EepromAddressSize);
+
+	// Now read the data (16 bits) in from the selected EEPROM word
+	data = shift_in_bits(padapter);
+
+	eeprom_clean(padapter);
+out:	
+#ifdef CONFIG_RTL8712
+	if(tmp8_clk_new!=tmp8_clk_ori)
+		rtw_write8(padapter, 0x10250003, tmp8_clk_ori);
+	if(tmp8_new!=tmp8_ori)
+		rtw_write8(padapter, 0x102502f1, tmp8_ori);
+
+#endif
+_func_exit_;		
+	return data;
+
+
+}
+
+
+
+
+//From even offset
+void eeprom_read_sz(_adapter * padapter, u16 reg, u8* data, u32 sz) 
+{
+
+	u16 x, data16;
+	u32 i;
+_func_enter_;		
+	if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+	// select EEPROM, reset bits, set _EECS
+	x = rtw_read8(padapter, EE_9346CR);
+
+	if(padapter->bSurpriseRemoved==_TRUE){
+		RT_TRACE(_module_rtl871x_eeprom_c_,_drv_err_,("padapter->bSurpriseRemoved==_TRUE"));
+		goto out;
+	}
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_write8(padapter, EE_9346CR, (unsigned char)x);
+
+	// write the read opcode and register number in that order
+	// The opcode is 3bits in length, reg is 6 bits long
+	shift_out_bits(padapter, EEPROM_READ_OPCODE, 3);
+	shift_out_bits(padapter, reg, padapter->EepromAddressSize);
+
+
+	for(i=0; i<sz; i+=2)
+	{
+		data16 = shift_in_bits(padapter);
+		data[i] = data16 & 0xff;
+		data[i+1] = data16 >>8;		
+	}
+
+	eeprom_clean(padapter);
+out:	
+_func_exit_;		
+
+
+
+}
+
+
+//addr_off : address offset of the entry in eeprom (not the tuple number of eeprom (reg); that is addr_off !=reg)
+u8 eeprom_read(_adapter * padapter, u32 addr_off, u8 sz, u8* rbuf)
+{
+	u8 quotient, remainder, addr_2align_odd;
+	u16 reg, stmp , i=0, idx = 0;
+_func_enter_;		
+	reg = (u16)(addr_off >> 1);
+	addr_2align_odd = (u8)(addr_off & 0x1);
+
+	if(addr_2align_odd) //read that start at high part: e.g  1,3,5,7,9,...
+	{
+		stmp = eeprom_read16(padapter, reg);
+		rbuf[idx++] = (u8) ((stmp>>8)&0xff); //return hogh-part of the short
+		reg++; sz--;
+	}
+	
+	quotient = sz >> 1;
+	remainder = sz & 0x1;
+
+	for( i=0 ; i < quotient; i++)
+	{
+		stmp = eeprom_read16(padapter, reg+i);
+		rbuf[idx++] = (u8) (stmp&0xff);
+		rbuf[idx++] = (u8) ((stmp>>8)&0xff);
+	}
+	
+	reg = reg+i;
+	if(remainder){ //end of read at lower part of short : 0,2,4,6,...
+		stmp = eeprom_read16(padapter, reg);
+		rbuf[idx] = (u8)(stmp & 0xff); 
+	}
+_func_exit_;		
+	return _TRUE;
+}
+
+
+
+VOID read_eeprom_content(_adapter *	padapter)
+{
+
+_func_enter_;		
+
+
+_func_exit_;		
+}
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_ieee80211.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_ieee80211.c
new file mode 100755
index 00000000..ea353d4c
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_ieee80211.c
@@ -0,0 +1,2020 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _IEEE80211_C
+
+#include <drv_types.h>
+#include <ieee80211.h>
+#include <wifi.h>
+#include <osdep_service.h>
+#include <wlan_bssdef.h>
+
+u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 };
+u16 RTW_WPA_VERSION = 1;
+u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_CIPHER_SUITE_WEP40[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_CIPHER_SUITE_TKIP[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_WRAP[] = { 0x00, 0x50, 0xf2, 3 };
+u8 WPA_CIPHER_SUITE_CCMP[] = { 0x00, 0x50, 0xf2, 4 };
+u8 WPA_CIPHER_SUITE_WEP104[] = { 0x00, 0x50, 0xf2, 5 };
+
+u16 RSN_VERSION_BSD = 1;
+u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_NONE[] = { 0x00, 0x0f, 0xac, 0 };
+u8 RSN_CIPHER_SUITE_WEP40[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_CIPHER_SUITE_TKIP[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_WRAP[] = { 0x00, 0x0f, 0xac, 3 };
+u8 RSN_CIPHER_SUITE_CCMP[] = { 0x00, 0x0f, 0xac, 4 };
+u8 RSN_CIPHER_SUITE_WEP104[] = { 0x00, 0x0f, 0xac, 5 };
+//-----------------------------------------------------------
+// for adhoc-master to generate ie and provide supported-rate to fw 
+//-----------------------------------------------------------
+
+static u8 	WIFI_CCKRATES[] = 
+{(IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
+ (IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
+ (IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
+ (IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)};
+
+static u8 	WIFI_OFDMRATES[] = 
+{(IEEE80211_OFDM_RATE_6MB),
+ (IEEE80211_OFDM_RATE_9MB),
+ (IEEE80211_OFDM_RATE_12MB),
+ (IEEE80211_OFDM_RATE_18MB),
+ (IEEE80211_OFDM_RATE_24MB),
+ IEEE80211_OFDM_RATE_36MB,
+ IEEE80211_OFDM_RATE_48MB,
+ IEEE80211_OFDM_RATE_54MB};
+
+
+int rtw_get_bit_value_from_ieee_value(u8 val)
+{
+	unsigned char dot11_rate_table[]={2,4,11,22,12,18,24,36,48,72,96,108,0}; // last element must be zero!!
+
+	int i=0;
+	while(dot11_rate_table[i] != 0) {
+		if (dot11_rate_table[i] == val)
+			return BIT(i);
+		i++;
+	}
+	return 0;
+}
+
+uint	rtw_is_cckrates_included(u8 *rate)
+{	
+		u32	i = 0;			
+
+		while(rate[i]!=0)
+		{		
+			if  (  (((rate[i]) & 0x7f) == 2)	|| (((rate[i]) & 0x7f) == 4) ||		
+			(((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22) )		
+			return _TRUE;	
+			i++;
+		}
+		
+		return _FALSE;
+}
+
+uint	rtw_is_cckratesonly_included(u8 *rate)
+{
+	u32 i = 0;
+
+
+	while(rate[i]!=0)
+	{
+			if  (  (((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
+				(((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22) )
+
+			return _FALSE;		
+
+			i++;
+	}
+	
+	return _TRUE;
+
+}
+
+int rtw_check_network_type(unsigned char *rate, int ratelen, int channel)
+{
+	if (channel > 14)
+	{
+		if ((rtw_is_cckrates_included(rate)) == _TRUE)
+			return WIRELESS_INVALID;
+		else
+			return WIRELESS_11A;
+	}	
+	else  // could be pure B, pure G, or B/G
+	{
+		if ((rtw_is_cckratesonly_included(rate)) == _TRUE)	
+			return WIRELESS_11B;
+		else if((rtw_is_cckrates_included(rate)) == _TRUE)
+			return 	WIRELESS_11BG;
+		else
+			return WIRELESS_11G;
+	}
+	
+}
+
+u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source,
+				unsigned int *frlen)
+{
+	_rtw_memcpy((void *)pbuf, (void *)source, len);
+	*frlen = *frlen + len;
+	return (pbuf + len);
+}
+
+// rtw_set_ie will update frame length
+u8 *rtw_set_ie
+(
+	u8 *pbuf, 
+	sint index, 
+	uint len,
+	u8 *source, 
+	uint *frlen //frame length
+)
+{
+_func_enter_;
+	*pbuf = (u8)index;
+
+	*(pbuf + 1) = (u8)len;
+
+	if (len > 0)
+		_rtw_memcpy((void *)(pbuf + 2), (void *)source, len);
+	
+	*frlen = *frlen + (len + 2);
+	
+	return (pbuf + len + 2);
+_func_exit_;	
+}
+
+inline u8 *rtw_set_ie_ch_switch(u8 *buf, u32 *buf_len, u8 ch_switch_mode,
+	u8 new_ch, u8 ch_switch_cnt)
+{
+	u8 ie_data[3];
+
+	ie_data[0] = ch_switch_mode;
+	ie_data[1] = new_ch;
+	ie_data[2] = ch_switch_cnt;
+	return rtw_set_ie(buf, WLAN_EID_CHANNEL_SWITCH,  3, ie_data, buf_len);
+}
+
+inline u8 secondary_ch_offset_to_hal_ch_offset(u8 ch_offset)
+{
+	if (ch_offset == SCN)
+		return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	else if(ch_offset == SCA)
+		return HAL_PRIME_CHNL_OFFSET_UPPER;
+	else if(ch_offset == SCB)
+		return HAL_PRIME_CHNL_OFFSET_LOWER;
+
+	return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+}
+
+inline u8 hal_ch_offset_to_secondary_ch_offset(u8 ch_offset)
+{
+	if (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
+		return SCN;
+	else if(ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
+		return SCB;
+	else if(ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
+		return SCA;
+
+	return SCN;
+}
+
+inline u8 *rtw_set_ie_secondary_ch_offset(u8 *buf, u32 *buf_len, u8 secondary_ch_offset)
+{
+	return rtw_set_ie(buf, WLAN_EID_SECONDARY_CHANNEL_OFFSET,  1, &secondary_ch_offset, buf_len);
+}
+
+inline u8 *rtw_set_ie_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl,
+	u8 flags, u16 reason, u16 precedence)
+{
+	u8 ie_data[6];
+
+	ie_data[0] = ttl;
+	ie_data[1] = flags;
+	RTW_PUT_LE16((u8*)&ie_data[2], reason);
+	RTW_PUT_LE16((u8*)&ie_data[4], precedence);
+
+	return rtw_set_ie(buf, 0x118,  6, ie_data, buf_len);
+}
+
+/*----------------------------------------------------------------------------
+index: the information element id index, limit is the limit for search
+-----------------------------------------------------------------------------*/
+u8 *rtw_get_ie(u8 *pbuf, sint index, sint *len, sint limit)
+{
+	sint tmp,i;
+	u8 *p;
+_func_enter_;
+	if (limit < 1){
+		_func_exit_;	
+		return NULL;
+	}
+
+	p = pbuf;
+	i = 0;
+	*len = 0;
+	while(1)
+	{
+		if (*p == index)
+		{
+			*len = *(p + 1);
+			return (p);
+		}
+		else
+		{
+			tmp = *(p + 1);
+			p += (tmp + 2);
+			i += (tmp + 2);
+		}
+		if (i >= limit)
+			break;
+	}
+_func_exit_;		
+	return NULL;
+}
+
+/**
+ * rtw_get_ie_ex - Search specific IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @eid: Element ID to match
+ * @oui: OUI to match
+ * @oui_len: OUI length
+ * @ie: If not NULL and the specific IE is found, the IE will be copied to the buf starting from the specific IE
+ * @ielen: If not NULL and the specific IE is found, will set to the length of the entire IE
+ *
+ * Returns: The address of the specific IE found, or NULL
+ */
+u8 *rtw_get_ie_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len, u8 *ie, uint *ielen)
+{
+	uint cnt;
+	u8 *target_ie = NULL;
+
+
+	if(ielen)
+		*ielen = 0;
+
+	if(!in_ie || in_len<=0)
+		return target_ie;
+
+	cnt = 0;
+
+	while(cnt<in_len)
+	{
+		if(eid == in_ie[cnt]
+			&& ( !oui || _rtw_memcmp(&in_ie[cnt+2], oui, oui_len) == _TRUE))
+		{
+			target_ie = &in_ie[cnt];
+
+			if(ie)
+				_rtw_memcpy(ie, &in_ie[cnt], in_ie[cnt+1]+2);
+			
+			if(ielen)
+				*ielen = in_ie[cnt+1]+2;
+
+			break;
+		}
+		else
+		{
+			cnt+=in_ie[cnt+1]+2; //goto next	
+		}		
+
+	}	
+
+	return target_ie;
+}
+
+/**
+ * rtw_ies_remove_ie - Find matching IEs and remove
+ * @ies: Address of IEs to search
+ * @ies_len: Pointer of length of ies, will update to new length
+ * @offset: The offset to start scarch
+ * @eid: Element ID to match
+ * @oui: OUI to match
+ * @oui_len: OUI length
+ *
+ * Returns: _SUCCESS: ies is updated, _FAIL: not updated
+ */
+int rtw_ies_remove_ie(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len)
+{
+	int ret = _FAIL;
+	u8 *target_ie;
+	u32 target_ielen;
+	u8 *start;
+	uint search_len;
+	
+	if(!ies || !ies_len || *ies_len <= offset)
+		goto exit;
+
+	start = ies + offset;
+	search_len = *ies_len - offset;
+
+	while (1) {
+		target_ie = rtw_get_ie_ex(start, search_len, eid, oui, oui_len, NULL, &target_ielen);
+		if (target_ie && target_ielen) {
+			u8 buf[MAX_IE_SZ] = {0};
+			u8 *remain_ies = target_ie + target_ielen;
+			uint remain_len = search_len - (remain_ies - start);
+			
+			_rtw_memcpy(buf, remain_ies, remain_len);
+			_rtw_memcpy(target_ie, buf, remain_len);
+			*ies_len = *ies_len - target_ielen;
+			ret = _SUCCESS;
+
+			start = target_ie;
+			search_len = remain_len;
+		} else {
+			break;
+		}
+	}
+exit:
+	return ret;
+}
+
+void rtw_set_supported_rate(u8* SupportedRates, uint mode) 
+{
+_func_enter_;
+
+	_rtw_memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
+	
+	switch (mode)
+	{
+		case WIRELESS_11B:
+			_rtw_memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+			break;
+		
+		case WIRELESS_11G:
+		case WIRELESS_11A:
+		case WIRELESS_11_5N:
+		case WIRELESS_11A_5N://Todo: no basic rate for ofdm ?
+			_rtw_memcpy(SupportedRates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
+			break;
+		
+		case WIRELESS_11BG:
+		case WIRELESS_11G_24N:
+		case WIRELESS_11_24N:
+		case WIRELESS_11BG_24N:
+			_rtw_memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+			_rtw_memcpy(SupportedRates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
+			break;
+	
+	}
+_func_exit_;	
+}
+
+uint	rtw_get_rateset_len(u8	*rateset)
+{
+	uint i = 0;
+_func_enter_;	
+	while(1)
+	{
+		if ((rateset[i]) == 0)
+			break;
+			
+		if (i > 12)
+			break;
+			
+		i++;			
+	}
+_func_exit_;		
+	return i;
+}
+
+int rtw_generate_ie(struct registry_priv *pregistrypriv)
+{
+	u8	wireless_mode;
+	int 	sz = 0, rateLen;
+	WLAN_BSSID_EX*	pdev_network = &pregistrypriv->dev_network;
+	u8*	ie = pdev_network->IEs;
+	
+_func_enter_;		
+
+	//timestamp will be inserted by hardware
+	sz += 8;	
+	ie += sz;
+	
+	//beacon interval : 2bytes
+	*(u16*)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod);//BCN_INTERVAL;
+	sz += 2; 
+	ie += 2;
+	
+	//capability info
+	*(u16*)ie = 0;
+	
+	*(u16*)ie |= cpu_to_le16(cap_IBSS);
+
+	if(pregistrypriv->preamble == PREAMBLE_SHORT)
+		*(u16*)ie |= cpu_to_le16(cap_ShortPremble);
+	
+	if (pdev_network->Privacy)
+		*(u16*)ie |= cpu_to_le16(cap_Privacy);
+	
+	sz += 2;
+	ie += 2;
+	
+	//SSID
+	ie = rtw_set_ie(ie, _SSID_IE_, pdev_network->Ssid.SsidLength, pdev_network->Ssid.Ssid, &sz);
+	
+	//supported rates
+	if(pregistrypriv->wireless_mode == WIRELESS_11ABGN)
+	{
+		if(pdev_network->Configuration.DSConfig > 14)
+			wireless_mode = WIRELESS_11A_5N;
+		else
+			wireless_mode = WIRELESS_11BG_24N;
+	}
+	else
+	{
+		wireless_mode = pregistrypriv->wireless_mode;
+	}
+	
+	rtw_set_supported_rate(pdev_network->SupportedRates, wireless_mode) ;
+	
+	rateLen = rtw_get_rateset_len(pdev_network->SupportedRates);
+
+	if (rateLen > 8)
+	{
+		ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, pdev_network->SupportedRates, &sz);
+		//ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz);
+	}
+	else
+	{
+		ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, pdev_network->SupportedRates, &sz);
+	}
+
+	//DS parameter set
+	ie = rtw_set_ie(ie, _DSSET_IE_, 1, (u8 *)&(pdev_network->Configuration.DSConfig), &sz);
+
+
+	//IBSS Parameter Set
+	
+	ie = rtw_set_ie(ie, _IBSS_PARA_IE_, 2, (u8 *)&(pdev_network->Configuration.ATIMWindow), &sz);
+
+	if (rateLen > 8)
+	{		
+		ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz);
+	}
+	
+#ifdef CONFIG_80211N_HT
+	//HT Cap.
+	if(((pregistrypriv->wireless_mode&WIRELESS_11_5N)||(pregistrypriv->wireless_mode&WIRELESS_11_24N)) 
+		&& (pregistrypriv->ht_enable==_TRUE))
+	{
+		//todo:
+	}
+#endif //CONFIG_80211N_HT
+
+	//pdev_network->IELength =  sz; //update IELength
+
+_func_exit_;
+
+	//return _SUCCESS;
+
+	return sz;
+
+}
+
+unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
+{
+	int len;
+	u16 val16;
+	unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
+	u8 *pbuf = pie;
+	int limit_new = limit;
+
+	while(1) 
+	{
+		pbuf = rtw_get_ie(pbuf, _WPA_IE_ID_, &len, limit_new);
+
+		if (pbuf) {
+
+			//check if oui matches...
+			if (_rtw_memcmp((pbuf + 2), wpa_oui_type, sizeof (wpa_oui_type)) == _FALSE) {
+
+				goto check_next_ie;
+			}
+
+			//check version...
+			_rtw_memcpy((u8 *)&val16, (pbuf + 6), sizeof(val16));
+
+			val16 = le16_to_cpu(val16);
+			if (val16 != 0x0001)
+				goto check_next_ie;
+
+			*wpa_ie_len = *(pbuf + 1);
+
+			return pbuf;
+
+		}
+		else {
+
+			*wpa_ie_len = 0;
+			return NULL;
+		}
+
+check_next_ie:
+
+		limit_new = limit - (pbuf - pie) - 2 - len;
+
+		if (limit_new <= 0)
+			break;
+
+		pbuf += (2 + len);
+
+	}
+
+	*wpa_ie_len = 0;
+
+	return NULL;
+
+}
+
+unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit)
+{	
+
+	return rtw_get_ie(pie, _WPA2_IE_ID_,rsn_ie_len, limit);
+
+}
+
+int rtw_get_wpa_cipher_suite(u8 *s)
+{
+	if (_rtw_memcmp(s, WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN) == _TRUE)
+		return WPA_CIPHER_NONE;
+	if (_rtw_memcmp(s, WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN) == _TRUE)
+		return WPA_CIPHER_WEP40;
+	if (_rtw_memcmp(s, WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN) == _TRUE)
+		return WPA_CIPHER_TKIP;
+	if (_rtw_memcmp(s, WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN) == _TRUE)
+		return WPA_CIPHER_CCMP;
+	if (_rtw_memcmp(s, WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN) == _TRUE)
+		return WPA_CIPHER_WEP104;
+
+	return 0;
+}
+
+int rtw_get_wpa2_cipher_suite(u8 *s)
+{
+	if (_rtw_memcmp(s, RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN) == _TRUE)
+		return WPA_CIPHER_NONE;
+	if (_rtw_memcmp(s, RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN) == _TRUE)
+		return WPA_CIPHER_WEP40;
+	if (_rtw_memcmp(s, RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN) == _TRUE)
+		return WPA_CIPHER_TKIP;
+	if (_rtw_memcmp(s, RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN) == _TRUE)
+		return WPA_CIPHER_CCMP;
+	if (_rtw_memcmp(s, RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN) == _TRUE)
+		return WPA_CIPHER_WEP104;
+
+	return 0;
+}
+
+
+int rtw_parse_wpa_ie(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+{
+	int i, ret=_SUCCESS;
+	int left, count;
+	u8 *pos;
+	u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
+
+	if (wpa_ie_len <= 0) {
+		/* No WPA IE - fail silently */
+		return _FAIL;
+	}
+
+	
+	if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie+1) != (u8)(wpa_ie_len - 2)) ||
+	   (_rtw_memcmp(wpa_ie+2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN) != _TRUE) )
+	{		
+		return _FAIL;
+	}
+
+	pos = wpa_ie;
+
+	pos += 8;
+	left = wpa_ie_len - 8;	
+
+
+	//group_cipher
+	if (left >= WPA_SELECTOR_LEN) {
+
+		*group_cipher = rtw_get_wpa_cipher_suite(pos);
+		
+		pos += WPA_SELECTOR_LEN;
+		left -= WPA_SELECTOR_LEN;
+		
+	} 
+	else if (left > 0)
+	{
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie length mismatch, %u too much", __FUNCTION__, left));
+		
+		return _FAIL;
+	}
+
+
+	//pairwise_cipher
+	if (left >= 2)
+	{		
+                //count = le16_to_cpu(*(u16*)pos);	
+		count = RTW_GET_LE16(pos);
+		pos += 2;
+		left -= 2;
+		
+		if (count == 0 || left < count * WPA_SELECTOR_LEN) {
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie count botch (pairwise), "
+				   		"count %u left %u", __FUNCTION__, count, left));
+			return _FAIL;
+		}
+		
+		for (i = 0; i < count; i++)
+		{
+			*pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
+			
+			pos += WPA_SELECTOR_LEN;
+			left -= WPA_SELECTOR_LEN;
+		}
+		
+	} 
+	else if (left == 1)
+	{
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie too short (for key mgmt)",   __FUNCTION__));
+		return _FAIL;
+	}
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (_rtw_memcmp(pos, SUITE_1X, 4) == 1) {
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("%s : there has 802.1x auth\n", __FUNCTION__));
+				*is_8021x = 1;
+			}
+		}
+	}
+	
+	return ret;
+	
+}
+
+int rtw_parse_wpa2_ie(u8* rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+{
+	int i, ret=_SUCCESS;
+	int left, count;
+	u8 *pos;
+	u8 SUITE_1X[4] = {0x00,0x0f, 0xac, 0x01};
+
+	if (rsn_ie_len <= 0) {
+		/* No RSN IE - fail silently */
+		return _FAIL;
+	}
+
+
+	if ((*rsn_ie!= _WPA2_IE_ID_) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2)))
+	{		
+		return _FAIL;
+	}
+	
+	pos = rsn_ie;
+	pos += 4;
+	left = rsn_ie_len - 4;	
+
+	//group_cipher
+	if (left >= RSN_SELECTOR_LEN) {
+
+		*group_cipher = rtw_get_wpa2_cipher_suite(pos);
+		
+		pos += RSN_SELECTOR_LEN;
+		left -= RSN_SELECTOR_LEN;
+		
+	} else if (left > 0) {
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie length mismatch, %u too much", __FUNCTION__, left));
+		return _FAIL;
+	}
+
+	//pairwise_cipher
+	if (left >= 2)
+	{		
+	        //count = le16_to_cpu(*(u16*)pos);
+		count = RTW_GET_LE16(pos);
+		pos += 2;
+		left -= 2;
+
+		if (count == 0 || left < count * RSN_SELECTOR_LEN) {
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie count botch (pairwise), "
+				  		 "count %u left %u", __FUNCTION__, count, left));
+			return _FAIL;
+		}
+		
+		for (i = 0; i < count; i++)
+		{			
+			*pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
+			
+			pos += RSN_SELECTOR_LEN;
+			left -= RSN_SELECTOR_LEN;
+		}
+
+	} 
+	else if (left == 1)
+	{
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie too short (for key mgmt)",  __FUNCTION__));
+		
+		return _FAIL;
+	}
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (_rtw_memcmp(pos, SUITE_1X, 4) == 1) {
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("%s (): there has 802.1x auth\n", __FUNCTION__));
+				*is_8021x = 1;
+			}
+		}
+	}
+
+	return ret;
+	
+}
+
+#ifdef CONFIG_WAPI_SUPPORT
+int rtw_get_wapi_ie(u8 *in_ie,uint in_len,u8 *wapi_ie,u16 *wapi_len)
+{
+	u8 authmode, i;
+	uint 	cnt;
+	u8 wapi_oui1[4]={0x0,0x14,0x72,0x01};
+	u8 wapi_oui2[4]={0x0,0x14,0x72,0x02};
+
+_func_enter_;
+	cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
+	while(cnt<in_len)
+	{
+		authmode=in_ie[cnt];
+
+		//if(authmode==_WAPI_IE_)
+		if(authmode==_WAPI_IE_ && (_rtw_memcmp(&in_ie[cnt+6], wapi_oui1,4)==_TRUE ||
+				       	_rtw_memcmp(&in_ie[cnt+6], wapi_oui2,4)==_TRUE))
+		{
+			if (wapi_ie) {
+				_rtw_memcpy(wapi_ie, &in_ie[cnt],in_ie[cnt+1]+2);
+
+				for(i=0;i<(in_ie[cnt+1]+2);i=i+8){
+					RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
+								wapi_ie[i],wapi_ie[i+1],wapi_ie[i+2],wapi_ie[i+3],wapi_ie[i+4],
+								wapi_ie[i+5],wapi_ie[i+6],wapi_ie[i+7]));
+				}
+			}
+
+			*wapi_len=in_ie[cnt+1]+2;
+			cnt+=in_ie[cnt+1]+2;  //get next
+		}
+		else
+		{
+			cnt+=in_ie[cnt+1]+2;   //get next
+		}
+	}
+
+	return *wapi_len;
+_func_exit_;
+
+
+}
+#endif
+
+int rtw_get_sec_ie(u8 *in_ie,uint in_len,u8 *rsn_ie,u16 *rsn_len,u8 *wpa_ie,u16 *wpa_len)
+{
+	u8 authmode, sec_idx, i;
+	u8 wpa_oui[4]={0x0,0x50,0xf2,0x01};
+	uint 	cnt;
+	
+_func_enter_;
+
+	//Search required WPA or WPA2 IE and copy to sec_ie[ ]
+	
+	cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
+	
+	sec_idx=0;
+		
+	while(cnt<in_len)
+	{
+		authmode=in_ie[cnt];
+		
+		if((authmode==_WPA_IE_ID_)&&(_rtw_memcmp(&in_ie[cnt+2], &wpa_oui[0],4)==_TRUE))
+		{	
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n rtw_get_wpa_ie: sec_idx=%d in_ie[cnt+1]+2=%d\n",sec_idx,in_ie[cnt+1]+2));		
+
+				if (wpa_ie) {
+				_rtw_memcpy(wpa_ie, &in_ie[cnt],in_ie[cnt+1]+2);
+
+				for(i=0;i<(in_ie[cnt+1]+2);i=i+8){
+						RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
+									wpa_ie[i],wpa_ie[i+1],wpa_ie[i+2],wpa_ie[i+3],wpa_ie[i+4],
+									wpa_ie[i+5],wpa_ie[i+6],wpa_ie[i+7]));
+					}
+				}
+
+				*wpa_len=in_ie[cnt+1]+2;
+				cnt+=in_ie[cnt+1]+2;  //get next
+		}
+		else
+		{
+			if(authmode==_WPA2_IE_ID_)
+			{
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n get_rsn_ie: sec_idx=%d in_ie[cnt+1]+2=%d\n",sec_idx,in_ie[cnt+1]+2));		
+
+				if (rsn_ie) {
+				_rtw_memcpy(rsn_ie, &in_ie[cnt],in_ie[cnt+1]+2);
+
+				for(i=0;i<(in_ie[cnt+1]+2);i=i+8){
+						RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
+									rsn_ie[i],rsn_ie[i+1],rsn_ie[i+2],rsn_ie[i+3],rsn_ie[i+4],
+									rsn_ie[i+5],rsn_ie[i+6],rsn_ie[i+7]));
+					}
+				}
+
+				*rsn_len=in_ie[cnt+1]+2;
+				cnt+=in_ie[cnt+1]+2;  //get next
+			}
+			else
+			{
+				cnt+=in_ie[cnt+1]+2;   //get next
+			}	
+		}
+		
+	}
+	
+_func_exit_;
+
+	return (*rsn_len+*wpa_len);
+	
+}
+
+u8 rtw_is_wps_ie(u8 *ie_ptr, uint *wps_ielen)
+{	
+	u8 match = _FALSE;
+	u8 eid, wps_oui[4]={0x0,0x50,0xf2,0x04};
+	
+	if(ie_ptr == NULL) return match;
+	
+	eid = ie_ptr[0];
+	
+	if((eid==_WPA_IE_ID_)&&(_rtw_memcmp(&ie_ptr[2], wps_oui, 4)==_TRUE))
+	{			
+		//DBG_8192C("==> found WPS_IE.....\n");
+		*wps_ielen = ie_ptr[1]+2;			
+		match=_TRUE;
+	}	
+	return match;
+}
+
+/**
+ * rtw_get_wps_ie - Search WPS IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie
+ * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE
+ *
+ * Returns: The address of the WPS IE found, or NULL
+ */
+u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
+{
+	uint cnt;
+	u8 *wpsie_ptr=NULL;
+	u8 eid, wps_oui[4]={0x0,0x50,0xf2,0x04};
+
+	if(wps_ielen)
+		*wps_ielen = 0;
+
+	if(!in_ie || in_len<=0)
+		return wpsie_ptr;
+
+	cnt = 0;
+
+	while(cnt<in_len)
+	{
+		eid = in_ie[cnt];
+
+		if((eid==_WPA_IE_ID_)&&(_rtw_memcmp(&in_ie[cnt+2], wps_oui, 4)==_TRUE))
+		{
+			wpsie_ptr = &in_ie[cnt];
+
+			if(wps_ie)
+				_rtw_memcpy(wps_ie, &in_ie[cnt], in_ie[cnt+1]+2);
+			
+			if(wps_ielen)
+				*wps_ielen = in_ie[cnt+1]+2;
+			
+			cnt+=in_ie[cnt+1]+2;
+
+			break;
+		}
+		else
+		{
+			cnt+=in_ie[cnt+1]+2; //goto next	
+		}		
+
+	}	
+
+	return wpsie_ptr;
+}
+
+/**
+ * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
+ * @wps_ie: Address of WPS IE to search
+ * @wps_ielen: Length limit from wps_ie
+ * @target_attr_id: The attribute ID of WPS attribute to search
+ * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
+ * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
+ *
+ * Returns: the address of the specific WPS attribute found, or NULL
+ */
+u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id ,u8 *buf_attr, u32 *len_attr)
+{
+	u8 *attr_ptr = NULL;
+	u8 * target_attr_ptr = NULL;
+	u8 wps_oui[4]={0x00,0x50,0xF2,0x04};
+
+	if(len_attr)
+		*len_attr = 0;
+
+	if ( ( wps_ie[0] != _VENDOR_SPECIFIC_IE_ ) ||
+		( _rtw_memcmp( wps_ie + 2, wps_oui , 4 ) != _TRUE ) )
+	{
+		return attr_ptr;
+	}
+
+	// 6 = 1(Element ID) + 1(Length) + 4(WPS OUI)
+	attr_ptr = wps_ie + 6; //goto first attr
+	
+	while(attr_ptr - wps_ie < wps_ielen)
+	{
+		// 4 = 2(Attribute ID) + 2(Length)
+		u16 attr_id = RTW_GET_BE16(attr_ptr);
+		u16 attr_data_len = RTW_GET_BE16(attr_ptr + 2);
+		u16 attr_len = attr_data_len + 4;
+		
+		//DBG_871X("%s attr_ptr:%p, id:%u, length:%u\n", __FUNCTION__, attr_ptr, attr_id, attr_data_len);
+		if( attr_id == target_attr_id )
+		{
+			target_attr_ptr = attr_ptr;
+		
+			if(buf_attr)
+				_rtw_memcpy(buf_attr, attr_ptr, attr_len);
+			
+			if(len_attr)
+				*len_attr = attr_len;
+			
+			break;
+		}
+		else
+		{
+			attr_ptr += attr_len; //goto next
+		}		
+		
+	}	
+
+	return target_attr_ptr;
+}
+
+/**
+ * rtw_get_wps_attr_content - Search a specific WPS attribute content from a given WPS IE
+ * @wps_ie: Address of WPS IE to search
+ * @wps_ielen: Length limit from wps_ie
+ * @target_attr_id: The attribute ID of WPS attribute to search
+ * @buf_content: If not NULL and the WPS attribute is found, WPS attribute content will be copied to the buf starting from buf_content
+ * @len_content: If not NULL and the WPS attribute is found, will set to the length of the WPS attribute content
+ *
+ * Returns: the address of the specific WPS attribute content found, or NULL
+ */
+u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id ,u8 *buf_content, uint *len_content)
+{
+	u8 *attr_ptr;
+	u32 attr_len;
+
+	if(len_content)
+		*len_content = 0;
+	
+	attr_ptr = rtw_get_wps_attr(wps_ie, wps_ielen, target_attr_id, NULL, &attr_len);
+
+	if(attr_ptr && attr_len)
+	{
+		if(buf_content)
+			_rtw_memcpy(buf_content, attr_ptr+4, attr_len-4);
+
+		if(len_content)
+			*len_content = attr_len-4;
+
+		return attr_ptr+4;
+	}
+
+	return NULL;
+}
+
+static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
+					    struct rtw_ieee802_11_elems *elems,
+					    int show_errors)
+{
+	unsigned int oui;
+
+	/* first 3 bytes in vendor specific information element are the IEEE
+	 * OUI of the vendor. The following byte is used a vendor specific
+	 * sub-type. */
+	if (elen < 4) {
+		if (show_errors) {
+			DBG_871X("short vendor specific "
+				   "information element ignored (len=%lu)\n",
+				   (unsigned long) elen);
+		}
+		return -1;
+	}
+
+	oui = RTW_GET_BE24(pos);
+	switch (oui) {
+	case OUI_MICROSOFT:
+		/* Microsoft/Wi-Fi information elements are further typed and
+		 * subtyped */
+		switch (pos[3]) {
+		case 1:
+			/* Microsoft OUI (00:50:F2) with OUI Type 1:
+			 * real WPA information element */
+			elems->wpa_ie = pos;
+			elems->wpa_ie_len = elen;
+			break;
+		case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
+			if (elen < 5) {
+				DBG_871X("short WME "
+					   "information element ignored "
+					   "(len=%lu)\n",
+					   (unsigned long) elen);
+				return -1;
+			}
+			switch (pos[4]) {
+			case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
+			case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
+				elems->wme = pos;
+				elems->wme_len = elen;
+				break;
+			case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
+				elems->wme_tspec = pos;
+				elems->wme_tspec_len = elen;
+				break;
+			default:
+				DBG_871X("unknown WME "
+					   "information element ignored "
+					   "(subtype=%d len=%lu)\n",
+					   pos[4], (unsigned long) elen);
+				return -1;
+			}
+			break;
+		case 4:
+			/* Wi-Fi Protected Setup (WPS) IE */
+			elems->wps_ie = pos;
+			elems->wps_ie_len = elen;
+			break;
+		default:
+			DBG_871X("Unknown Microsoft "
+				   "information element ignored "
+				   "(type=%d len=%lu)\n",
+				   pos[3], (unsigned long) elen);
+			return -1;
+		}
+		break;
+
+	case OUI_BROADCOM:
+		switch (pos[3]) {
+		case VENDOR_HT_CAPAB_OUI_TYPE:
+			elems->vendor_ht_cap = pos;
+			elems->vendor_ht_cap_len = elen;
+			break;
+		default:
+			DBG_871X("Unknown Broadcom "
+				   "information element ignored "
+				   "(type=%d len=%lu)\n",
+				   pos[3], (unsigned long) elen);
+			return -1;
+		}
+		break;
+
+	default:
+		DBG_871X("unknown vendor specific information "
+			   "element ignored (vendor OUI %02x:%02x:%02x "
+			   "len=%lu)\n",
+			   pos[0], pos[1], pos[2], (unsigned long) elen);
+		return -1;
+	}
+
+	return 0;
+	
+}
+
+/**
+ * ieee802_11_parse_elems - Parse information elements in management frames
+ * @start: Pointer to the start of IEs
+ * @len: Length of IE buffer in octets
+ * @elems: Data structure for parsed elements
+ * @show_errors: Whether to show parsing errors in debug log
+ * Returns: Parsing result
+ */
+ParseRes rtw_ieee802_11_parse_elems(u8 *start, uint len,
+				struct rtw_ieee802_11_elems *elems,
+				int show_errors)
+{
+	uint left = len;
+	u8 *pos = start;
+	int unknown = 0;
+
+	_rtw_memset(elems, 0, sizeof(*elems));
+
+	while (left >= 2) {
+		u8 id, elen;
+
+		id = *pos++;
+		elen = *pos++;
+		left -= 2;
+
+		if (elen > left) {
+			if (show_errors) {
+				DBG_871X("IEEE 802.11 element "
+					   "parse failed (id=%d elen=%d "
+					   "left=%lu)\n",
+					   id, elen, (unsigned long) left);				
+			}
+			return ParseFailed;
+		}
+
+		switch (id) {
+		case WLAN_EID_SSID:
+			elems->ssid = pos;
+			elems->ssid_len = elen;
+			break;
+		case WLAN_EID_SUPP_RATES:
+			elems->supp_rates = pos;
+			elems->supp_rates_len = elen;
+			break;
+		case WLAN_EID_FH_PARAMS:
+			elems->fh_params = pos;
+			elems->fh_params_len = elen;
+			break;
+		case WLAN_EID_DS_PARAMS:
+			elems->ds_params = pos;
+			elems->ds_params_len = elen;
+			break;
+		case WLAN_EID_CF_PARAMS:
+			elems->cf_params = pos;
+			elems->cf_params_len = elen;
+			break;
+		case WLAN_EID_TIM:
+			elems->tim = pos;
+			elems->tim_len = elen;
+			break;
+		case WLAN_EID_IBSS_PARAMS:
+			elems->ibss_params = pos;
+			elems->ibss_params_len = elen;
+			break;
+		case WLAN_EID_CHALLENGE:
+			elems->challenge = pos;
+			elems->challenge_len = elen;
+			break;
+		case WLAN_EID_ERP_INFO:
+			elems->erp_info = pos;
+			elems->erp_info_len = elen;
+			break;
+		case WLAN_EID_EXT_SUPP_RATES:
+			elems->ext_supp_rates = pos;
+			elems->ext_supp_rates_len = elen;
+			break;
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (rtw_ieee802_11_parse_vendor_specific(pos, elen,
+							     elems,
+							     show_errors))
+				unknown++;
+			break;
+		case WLAN_EID_RSN:
+			elems->rsn_ie = pos;
+			elems->rsn_ie_len = elen;
+			break;
+		case WLAN_EID_PWR_CAPABILITY:
+			elems->power_cap = pos;
+			elems->power_cap_len = elen;
+			break;
+		case WLAN_EID_SUPPORTED_CHANNELS:
+			elems->supp_channels = pos;
+			elems->supp_channels_len = elen;
+			break;
+		case WLAN_EID_MOBILITY_DOMAIN:
+			elems->mdie = pos;
+			elems->mdie_len = elen;
+			break;
+		case WLAN_EID_FAST_BSS_TRANSITION:
+			elems->ftie = pos;
+			elems->ftie_len = elen;
+			break;
+		case WLAN_EID_TIMEOUT_INTERVAL:
+			elems->timeout_int = pos;
+			elems->timeout_int_len = elen;
+			break;
+		case WLAN_EID_HT_CAP:
+			elems->ht_capabilities = pos;
+			elems->ht_capabilities_len = elen;
+			break;
+		case WLAN_EID_HT_OPERATION:
+			elems->ht_operation = pos;
+			elems->ht_operation_len = elen;
+			break;
+		default:
+			unknown++;
+			if (!show_errors)
+				break;
+			DBG_871X("IEEE 802.11 element parse "
+				   "ignored unknown element (id=%d elen=%d)\n",
+				   id, elen);
+			break;
+		}
+
+		left -= elen;
+		pos += elen;
+	}
+
+	if (left)
+		return ParseFailed;
+
+	return unknown ? ParseUnknown : ParseOK;
+	
+}
+
+static u8 key_char2num(u8 ch);
+static u8 key_char2num(u8 ch)
+{
+    if((ch>='0')&&(ch<='9'))
+        return ch - '0';
+    else if ((ch>='a')&&(ch<='f'))
+        return ch - 'a' + 10;
+    else if ((ch>='A')&&(ch<='F'))
+        return ch - 'A' + 10;
+    else
+	 return 0xff;
+}
+
+u8 str_2char2num(u8 hch, u8 lch);
+u8 str_2char2num(u8 hch, u8 lch)
+{
+    return ((key_char2num(hch) * 10 ) + key_char2num(lch));
+}
+
+u8 key_2char2num(u8 hch, u8 lch);
+u8 key_2char2num(u8 hch, u8 lch)
+{
+    return ((key_char2num(hch) << 4) | key_char2num(lch));
+}
+
+u8 convert_ip_addr(u8 hch, u8 mch, u8 lch)
+{
+    return ((key_char2num(hch) * 100) + (key_char2num(mch) * 10 ) + key_char2num(lch));
+}
+
+extern char* rtw_initmac;
+void rtw_macaddr_cfg(u8 *mac_addr)
+{
+	u8 mac[ETH_ALEN];
+	if(mac_addr == NULL)	return;
+	
+	if ( rtw_initmac )
+	{	//	Users specify the mac address
+		int jj,kk;
+
+		for( jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3 )
+		{
+			mac[jj] = key_2char2num(rtw_initmac[kk], rtw_initmac[kk+ 1]);
+		}
+		_rtw_memcpy(mac_addr, mac, ETH_ALEN);
+	}
+	else
+	{	//	Use the mac address stored in the Efuse
+		_rtw_memcpy(mac, mac_addr, ETH_ALEN);
+	}
+	
+	if (((mac[0]==0xff) &&(mac[1]==0xff) && (mac[2]==0xff) &&
+	     (mac[3]==0xff) && (mac[4]==0xff) &&(mac[5]==0xff)) ||
+	    ((mac[0]==0x0) && (mac[1]==0x0) && (mac[2]==0x0) &&
+	     (mac[3]==0x0) && (mac[4]==0x0) &&(mac[5]==0x0)))
+	{
+		mac[0] = 0x00;
+		mac[1] = 0xe0;
+		mac[2] = 0x4c;
+		mac[3] = 0x87;
+		mac[4] = 0x00;
+		mac[5] = 0x00;
+		// use default mac addresss
+		_rtw_memcpy(mac_addr, mac, ETH_ALEN);
+		DBG_871X("MAC Address from efuse error, assign default one !!!\n");
+	}	
+
+	DBG_871X("rtw_macaddr_cfg MAC Address  = "MAC_FMT"\n", MAC_ARG(mac_addr));
+}
+
+void dump_ies(u8 *buf, u32 buf_len) {
+	u8* pos = (u8*)buf;
+	u8 id, len;
+	
+	while(pos-buf<=buf_len){
+		id = *pos;
+		len = *(pos+1);
+
+		DBG_871X("%s ID:%u, LEN:%u\n", __FUNCTION__, id, len);
+		#ifdef CONFIG_P2P
+		dump_p2p_ie(pos, len);
+		#endif
+		dump_wps_ie(pos, len);
+
+		pos+=(2+len);
+	}	
+}
+
+void dump_wps_ie(u8 *ie, u32 ie_len) {
+	u8* pos = (u8*)ie;
+	u16 id;
+	u16 len;
+
+	u8 *wps_ie;
+	uint wps_ielen;
+	
+	wps_ie = rtw_get_wps_ie(ie, ie_len, NULL, &wps_ielen);
+	if(wps_ie != ie || wps_ielen == 0)
+		return;
+
+	pos+=6;
+	while(pos-ie < ie_len){
+		id = RTW_GET_BE16(pos);
+		len = RTW_GET_BE16(pos + 2);
+
+		DBG_871X("%s ID:0x%04x, LEN:%u\n", __FUNCTION__, id, len);
+
+		pos+=(4+len);
+	}	
+}
+
+#ifdef CONFIG_P2P
+void dump_p2p_ie(u8 *ie, u32 ie_len) {
+	u8* pos = (u8*)ie;
+	u8 id;
+	u16 len;
+
+	u8 *p2p_ie;
+	uint p2p_ielen;
+	
+	p2p_ie = rtw_get_p2p_ie(ie, ie_len, NULL, &p2p_ielen);
+	if(p2p_ie != ie || p2p_ielen == 0)
+		return;
+
+	pos+=6;
+	while(pos-ie < ie_len){
+		id = *pos;
+		len = RTW_GET_LE16(pos+1);
+
+		DBG_871X("%s ID:%u, LEN:%u\n", __FUNCTION__, id, len);
+
+		pos+=(3+len);
+	}	
+}
+
+/**
+ * rtw_get_p2p_ie - Search P2P IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @p2p_ie: If not NULL and P2P IE is found, P2P IE will be copied to the buf starting from p2p_ie
+ * @p2p_ielen: If not NULL and P2P IE is found, will set to the length of the entire P2P IE
+ *
+ * Returns: The address of the P2P IE found, or NULL
+ */
+u8 *rtw_get_p2p_ie(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen)
+{
+	uint cnt = 0;
+	u8 *p2p_ie_ptr;
+	u8 eid, p2p_oui[4]={0x50,0x6F,0x9A,0x09};
+
+	if ( p2p_ielen != NULL )
+		*p2p_ielen = 0;
+
+	while(cnt<in_len)
+	{
+		eid = in_ie[cnt];
+		if ((in_len < 0) || (cnt > MAX_IE_SZ)) {
+#ifdef PLATFORM_LINUX
+			dump_stack();
+#endif
+			return NULL;
+		}		
+		if( ( eid == _VENDOR_SPECIFIC_IE_ ) && ( _rtw_memcmp( &in_ie[cnt+2], p2p_oui, 4) == _TRUE ) )
+		{
+			p2p_ie_ptr = in_ie + cnt;
+		
+			if ( p2p_ie != NULL )
+			{
+				_rtw_memcpy( p2p_ie, &in_ie[ cnt ], in_ie[ cnt + 1 ] + 2 );
+			}
+
+			if ( p2p_ielen != NULL )
+			{
+				*p2p_ielen = in_ie[ cnt + 1 ] + 2;
+			}
+			
+			return p2p_ie_ptr;
+
+			break;
+		}
+		else
+		{
+			cnt += in_ie[ cnt + 1 ] +2; //goto next	
+		}		
+		
+	}	
+
+	return NULL;
+
+}
+
+/**
+ * rtw_get_p2p_attr - Search a specific P2P attribute from a given P2P IE
+ * @p2p_ie: Address of P2P IE to search
+ * @p2p_ielen: Length limit from p2p_ie
+ * @target_attr_id: The attribute ID of P2P attribute to search
+ * @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will be copied to the buf starting from buf_attr
+ * @len_attr: If not NULL and the P2P attribute is found, will set to the length of the entire P2P attribute
+ *
+ * Returns: the address of the specific WPS attribute found, or NULL
+ */
+u8 *rtw_get_p2p_attr(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id ,u8 *buf_attr, u32 *len_attr)
+{
+	u8 *attr_ptr = NULL;
+	u8 *target_attr_ptr = NULL;
+	u8 p2p_oui[4]={0x50,0x6F,0x9A,0x09};
+
+	if(len_attr)
+		*len_attr = 0;
+
+	if ( !p2p_ie || ( p2p_ie[0] != _VENDOR_SPECIFIC_IE_ ) ||
+		( _rtw_memcmp( p2p_ie + 2, p2p_oui , 4 ) != _TRUE ) )
+	{
+		return attr_ptr;
+	}
+
+	// 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type)
+	attr_ptr = p2p_ie + 6; //goto first attr
+	
+	while(attr_ptr - p2p_ie < p2p_ielen)
+	{
+		// 3 = 1(Attribute ID) + 2(Length)
+		u8 attr_id = *attr_ptr;
+		u16 attr_data_len = RTW_GET_LE16(attr_ptr + 1);
+		u16 attr_len = attr_data_len + 3;
+		
+		//DBG_871X("%s attr_ptr:%p, id:%u, length:%u\n", __FUNCTION__, attr_ptr, attr_id, attr_data_len);
+		if( attr_id == target_attr_id )
+		{
+			target_attr_ptr = attr_ptr;
+		
+			if(buf_attr)
+				_rtw_memcpy(buf_attr, attr_ptr, attr_len);
+			
+			if(len_attr)
+				*len_attr = attr_len;
+			
+			break;
+		}
+		else
+		{
+			attr_ptr += attr_len; //goto next
+		}		
+		
+	}	
+
+	return target_attr_ptr;
+}
+
+/**
+ * rtw_get_p2p_attr_content - Search a specific P2P attribute content from a given P2P IE
+ * @p2p_ie: Address of P2P IE to search
+ * @p2p_ielen: Length limit from p2p_ie
+ * @target_attr_id: The attribute ID of P2P attribute to search
+ * @buf_content: If not NULL and the P2P attribute is found, P2P attribute content will be copied to the buf starting from buf_content
+ * @len_content: If not NULL and the P2P attribute is found, will set to the length of the P2P attribute content
+ *
+ * Returns: the address of the specific P2P attribute content found, or NULL
+ */
+u8 *rtw_get_p2p_attr_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id ,u8 *buf_content, uint *len_content)
+{
+	u8 *attr_ptr;
+	u32 attr_len;
+
+	if(len_content)
+		*len_content = 0;
+	
+	attr_ptr = rtw_get_p2p_attr(p2p_ie, p2p_ielen, target_attr_id, NULL, &attr_len);
+
+	if(attr_ptr && attr_len)
+	{
+		if(buf_content)
+			_rtw_memcpy(buf_content, attr_ptr+3, attr_len-3);
+
+		if(len_content)
+			*len_content = attr_len-3;
+
+		return attr_ptr+3;
+	}
+
+	return NULL;
+}
+
+u32 rtw_set_p2p_attr_content(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr)
+{	
+	u32 a_len;
+
+	*pbuf = attr_id;
+		
+	//*(u16*)(pbuf + 1) = cpu_to_le16(attr_len);
+	RTW_PUT_LE16(pbuf + 1, attr_len);
+
+	if(pdata_attr)
+		_rtw_memcpy(pbuf + 3, pdata_attr, attr_len);		
+		
+	a_len = attr_len + 3;
+		
+	return a_len;
+}
+
+static uint rtw_p2p_attr_remove(u8 *ie, uint ielen_ori, u8 attr_id)
+{
+	u8 *target_attr;
+	u32 target_attr_len;
+	uint ielen = ielen_ori;
+	int index=0;
+
+	while(1) {
+		target_attr=rtw_get_p2p_attr(ie, ielen, attr_id, NULL, &target_attr_len);
+		if(target_attr && target_attr_len)
+		{
+			u8 *next_attr = target_attr+target_attr_len;
+			uint remain_len = ielen-(next_attr-ie);
+			//dump_ies(ie, ielen);
+			#if 0
+			DBG_871X("[%d] ie:%p, ielen:%u\n"
+				"target_attr:%p, target_attr_len:%u\n"
+				"next_attr:%p, remain_len:%u\n"
+				, index++
+				, ie, ielen
+				, target_attr, target_attr_len
+				, next_attr, remain_len
+			);
+			#endif
+
+			_rtw_memset(target_attr, 0, target_attr_len);
+			_rtw_memcpy(target_attr, next_attr, remain_len);
+			_rtw_memset(target_attr+remain_len, 0, target_attr_len);
+			*(ie+1) -= target_attr_len;
+			ielen-=target_attr_len;
+		}
+		else
+		{
+			//if(index>0)
+			//	dump_ies(ie, ielen);
+			break;
+		}
+	}
+
+	return ielen;
+}
+
+void rtw_WLAN_BSSID_EX_remove_p2p_attr(WLAN_BSSID_EX *bss_ex, u8 attr_id)
+{
+	u8 *p2p_ie;
+	uint p2p_ielen, p2p_ielen_ori;
+	int cnt;
+	
+	if( (p2p_ie=rtw_get_p2p_ie(bss_ex->IEs+_FIXED_IE_LENGTH_, bss_ex->IELength-_FIXED_IE_LENGTH_, NULL, &p2p_ielen_ori)) ) 
+	{
+		#if 0
+		if(rtw_get_p2p_attr(p2p_ie, p2p_ielen_ori, attr_id, NULL, NULL)) {
+			DBG_871X("rtw_get_p2p_attr: GOT P2P_ATTR:%u!!!!!!!!\n", attr_id);
+			dump_ies(bss_ex->IEs+_FIXED_IE_LENGTH_, bss_ex->IELength-_FIXED_IE_LENGTH_);
+		}
+		#endif
+
+		p2p_ielen=rtw_p2p_attr_remove(p2p_ie, p2p_ielen_ori, attr_id);
+		if(p2p_ielen != p2p_ielen_ori) {
+			
+			u8 *next_ie_ori = p2p_ie+p2p_ielen_ori;
+			u8 *next_ie = p2p_ie+p2p_ielen;
+			uint remain_len = bss_ex->IELength-(next_ie_ori-bss_ex->IEs);
+
+			_rtw_memcpy(next_ie, next_ie_ori, remain_len);
+			_rtw_memset(next_ie+remain_len, 0, p2p_ielen_ori-p2p_ielen);
+			bss_ex->IELength -= p2p_ielen_ori-p2p_ielen;
+
+			#if 0
+			DBG_871X("remove P2P_ATTR:%u!\n", attr_id);
+			dump_ies(bss_ex->IEs+_FIXED_IE_LENGTH_, bss_ex->IELength-_FIXED_IE_LENGTH_);
+			#endif
+		}
+	}
+}
+
+#endif //CONFIG_P2P
+
+#ifdef CONFIG_WFD
+int rtw_get_wfd_ie(u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen)
+{
+	int match;
+	uint cnt = 0;	
+	u8 eid, wfd_oui[4]={0x50,0x6F,0x9A,0x0A};
+
+
+	match=_FALSE;
+
+	if ( in_len < 0 )
+	{
+		return match;
+	}
+
+	while(cnt<in_len)
+	{
+		eid = in_ie[cnt];
+		
+		if( ( eid == _VENDOR_SPECIFIC_IE_ ) && ( _rtw_memcmp( &in_ie[cnt+2], wfd_oui, 4) == _TRUE ) )
+		{
+			if ( wfd_ie != NULL )
+			{
+				_rtw_memcpy( wfd_ie, &in_ie[ cnt ], in_ie[ cnt + 1 ] + 2 );
+				
+			}
+			else
+			{
+				if ( wfd_ielen != NULL )
+				{
+					*wfd_ielen = 0;
+				}
+			}
+			
+			if ( wfd_ielen != NULL )
+			{
+				*wfd_ielen = in_ie[ cnt + 1 ] + 2;
+			}
+			
+			cnt += in_ie[ cnt + 1 ] + 2;
+
+			match = _TRUE;
+			break;
+		}
+		else
+		{
+			cnt += in_ie[ cnt + 1 ] +2; //goto next	
+		}		
+		
+	}	
+
+	if ( match == _TRUE )
+	{
+		match = cnt;
+	}
+	
+	return match;
+
+}
+
+//	attr_content: The output buffer, contains the "body field" of WFD attribute.
+//	attr_contentlen: The data length of the "body field" of WFD attribute.
+int rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id ,u8 *attr_content, uint *attr_contentlen)
+{
+	int match;
+	uint cnt = 0;	
+	u8 attr_id, wfd_oui[4]={0x50,0x6F,0x9A,0x0A};
+
+
+	match=_FALSE;
+
+	if ( ( wfd_ie[ 0 ] != _VENDOR_SPECIFIC_IE_ ) ||
+		( _rtw_memcmp( wfd_ie + 2, wfd_oui , 4 ) != _TRUE ) )
+	{
+		return( match );
+	}
+
+	//	1 ( WFD IE ) + 1 ( Length ) + 3 ( OUI ) + 1 ( OUI Type )
+	cnt = 6;
+	while( cnt < wfd_ielen )
+	{
+		u16 attrlen = RTW_GET_BE16(wfd_ie + cnt + 1);
+		
+		attr_id = wfd_ie[cnt];
+		if( attr_id == target_attr_id )
+		{
+			//	3 -> 1 byte for attribute ID field, 2 bytes for length field
+			if(attr_content)
+				_rtw_memcpy( attr_content, &wfd_ie[ cnt + 3 ], attrlen );
+			
+			if(attr_contentlen)
+				*attr_contentlen = attrlen;
+			
+			cnt += attrlen + 3;
+
+			match = _TRUE;
+			break;
+		}
+		else
+		{
+			cnt += attrlen + 3; //goto next	
+		}		
+		
+	}	
+
+	return match;
+
+}
+#endif // CONFIG_WFD
+
+//Baron adds to avoid FreeBSD warning
+int ieee80211_is_empty_essid(const char *essid, int essid_len)
+{
+	/* Single white space is for Linksys APs */
+	if (essid_len == 1 && essid[0] == ' ')
+		return 1;
+
+	/* Otherwise, if the entire essid is 0, we assume it is hidden */
+	while (essid_len) {
+		essid_len--;
+		if (essid[essid_len] != '\0')
+			return 0;
+	}
+
+	return 1;
+}
+
+int ieee80211_get_hdrlen(u16 fc)
+{
+	int hdrlen = 24;
+
+	switch (WLAN_FC_GET_TYPE(fc)) {
+	case RTW_IEEE80211_FTYPE_DATA:
+		if (fc & RTW_IEEE80211_STYPE_QOS_DATA)
+			hdrlen += 2;
+		if ((fc & RTW_IEEE80211_FCTL_FROMDS) && (fc & RTW_IEEE80211_FCTL_TODS))
+			hdrlen += 6; /* Addr4 */
+		break;
+	case RTW_IEEE80211_FTYPE_CTL:
+		switch (WLAN_FC_GET_STYPE(fc)) {
+		case RTW_IEEE80211_STYPE_CTS:
+		case RTW_IEEE80211_STYPE_ACK:
+			hdrlen = 10;
+			break;
+		default:
+			hdrlen = 16;
+			break;
+		}
+		break;
+	}
+
+	return hdrlen;
+}
+
+int rtw_get_cipher_info(struct wlan_network *pnetwork)
+{
+	u32 wpa_ielen;
+	unsigned char *pbuf;
+	int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
+	int ret = _FAIL;
+	pbuf = rtw_get_wpa_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12);
+
+	if(pbuf && (wpa_ielen>0)) {
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_cipher_info: wpa_ielen: %d", wpa_ielen));
+		if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
+
+			pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+			pnetwork->BcnInfo.group_cipher = group_cipher;
+			pnetwork->BcnInfo.is_8021x = is8021x;
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("%s: pnetwork->pairwise_cipher: %d, is_8021x is %d",
+						__func__, pnetwork->BcnInfo.pairwise_cipher, pnetwork->BcnInfo.is_8021x));
+			ret = _SUCCESS;
+		}
+	} else {
+
+		pbuf = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12);
+
+		if(pbuf && (wpa_ielen>0)) {
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("get RSN IE\n"));
+			if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("get RSN IE  OK!!!\n"));
+				pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+				pnetwork->BcnInfo.group_cipher = group_cipher;
+				pnetwork->BcnInfo.is_8021x = is8021x;
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("%s: pnetwork->pairwise_cipher: %d,"
+							"pnetwork->group_cipher is %d, is_8021x is %d",	__func__, pnetwork->BcnInfo.pairwise_cipher,
+							pnetwork->BcnInfo.group_cipher,pnetwork->BcnInfo.is_8021x));
+				ret = _SUCCESS;
+			}
+		}
+	}
+
+	return ret;
+}
+
+void rtw_get_bcn_info(struct wlan_network *pnetwork)
+{
+	unsigned short cap = 0;
+	u8 bencrypt = 0;
+	//u8 wpa_ie[255],rsn_ie[255];
+	u16 wpa_len=0,rsn_len=0;
+	struct HT_info_element *pht_info = NULL;
+	struct rtw_ieee80211_ht_cap *pht_cap = NULL;
+	unsigned int		len;
+	unsigned char		*p;
+
+	_rtw_memcpy((u8 *)&cap, rtw_get_capability_from_ie(pnetwork->network.IEs), 2);
+	cap = le16_to_cpu(cap);
+	if (cap & WLAN_CAPABILITY_PRIVACY) {
+		bencrypt = 1;
+		pnetwork->network.Privacy = 1;
+	} else {
+		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
+	}
+	rtw_get_sec_ie(pnetwork->network.IEs ,pnetwork->network.IELength,NULL,&rsn_len,NULL,&wpa_len);
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: ssid=%s\n",pnetwork->network.Ssid.Ssid));
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: wpa_len=%d rsn_len=%d\n",wpa_len,rsn_len));
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: ssid=%s\n",pnetwork->network.Ssid.Ssid));
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: wpa_len=%d rsn_len=%d\n",wpa_len,rsn_len));
+
+	if (rsn_len > 0) {
+		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+	} else if (wpa_len > 0) {
+		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA;
+	} else {
+		if (bencrypt)
+			pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
+	}
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
+				pnetwork->BcnInfo.encryp_protocol));
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
+				pnetwork->BcnInfo.encryp_protocol));
+	rtw_get_cipher_info(pnetwork);
+
+	/* get bwmode and ch_offset */
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
+	if(p && len>0) {
+			pht_cap = (struct rtw_ieee80211_ht_cap *)(p + 2);
+			pnetwork->BcnInfo.ht_cap_info = pht_cap->cap_info;
+	} else {
+			pnetwork->BcnInfo.ht_cap_info = 0;
+	}
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
+	if(p && len>0) {
+			pht_info = (struct HT_info_element *)(p + 2);
+			pnetwork->BcnInfo.ht_info_infos_0 = pht_info->infos[0];
+	} else {
+			pnetwork->BcnInfo.ht_info_infos_0 = 0;
+	}
+}
+
+//show MCS rate, unit: 100Kbps
+u16 rtw_mcs_rate(u8 rf_type, u8 bw_40MHz, u8 short_GI_20, u8 short_GI_40, unsigned char * MCS_rate)
+{
+	u16 max_rate = 0;
+	
+	if(rf_type == RF_1T1R)
+	{
+		if(MCS_rate[0] & BIT(7))
+			max_rate = (bw_40MHz) ? ((short_GI_40)?1500:1350):((short_GI_20)?722:650);
+		else if(MCS_rate[0] & BIT(6))
+			max_rate = (bw_40MHz) ? ((short_GI_40)?1350:1215):((short_GI_20)?650:585);
+		else if(MCS_rate[0] & BIT(5))
+			max_rate = (bw_40MHz) ? ((short_GI_40)?1200:1080):((short_GI_20)?578:520);
+		else if(MCS_rate[0] & BIT(4))
+			max_rate = (bw_40MHz) ? ((short_GI_40)?900:810):((short_GI_20)?433:390);
+		else if(MCS_rate[0] & BIT(3))
+			max_rate = (bw_40MHz) ? ((short_GI_40)?600:540):((short_GI_20)?289:260);
+		else if(MCS_rate[0] & BIT(2))
+			max_rate = (bw_40MHz) ? ((short_GI_40)?450:405):((short_GI_20)?217:195);
+		else if(MCS_rate[0] & BIT(1))
+			max_rate = (bw_40MHz) ? ((short_GI_40)?300:270):((short_GI_20)?144:130);
+		else if(MCS_rate[0] & BIT(0))
+			max_rate = (bw_40MHz) ? ((short_GI_40)?150:135):((short_GI_20)?72:65);
+	}
+	else
+	{
+		if(MCS_rate[1])
+		{
+			if(MCS_rate[1] & BIT(7))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?3000:2700):((short_GI_20)?1444:1300);
+			else if(MCS_rate[1] & BIT(6))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?2700:2430):((short_GI_20)?1300:1170);
+			else if(MCS_rate[1] & BIT(5))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?2400:2160):((short_GI_20)?1156:1040);
+			else if(MCS_rate[1] & BIT(4))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?1800:1620):((short_GI_20)?867:780);
+			else if(MCS_rate[1] & BIT(3))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?1200:1080):((short_GI_20)?578:520);
+			else if(MCS_rate[1] & BIT(2))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?900:810):((short_GI_20)?433:390);
+			else if(MCS_rate[1] & BIT(1))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?600:540):((short_GI_20)?289:260);
+			else if(MCS_rate[1] & BIT(0))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?300:270):((short_GI_20)?144:130);
+		}
+		else
+		{
+			if(MCS_rate[0] & BIT(7))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?1500:1350):((short_GI_20)?722:650);
+			else if(MCS_rate[0] & BIT(6))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?1350:1215):((short_GI_20)?650:585);
+			else if(MCS_rate[0] & BIT(5))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?1200:1080):((short_GI_20)?578:520);
+			else if(MCS_rate[0] & BIT(4))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?900:810):((short_GI_20)?433:390);
+			else if(MCS_rate[0] & BIT(3))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?600:540):((short_GI_20)?289:260);
+			else if(MCS_rate[0] & BIT(2))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?450:405):((short_GI_20)?217:195);
+			else if(MCS_rate[0] & BIT(1))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?300:270):((short_GI_20)?144:130);
+			else if(MCS_rate[0] & BIT(0))
+				max_rate = (bw_40MHz) ? ((short_GI_40)?150:135):((short_GI_20)?72:65);
+		}
+	}
+	return max_rate;
+}
+
+int rtw_action_frame_parse(const u8 *frame, u32 frame_len, u8* category, u8 *action)
+{
+	const u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u16 fc;
+	u8 c, a;
+
+	fc = le16_to_cpu(((struct rtw_ieee80211_hdr_3addr *)frame)->frame_ctl);
+
+	if ((fc & (RTW_IEEE80211_FCTL_FTYPE|RTW_IEEE80211_FCTL_STYPE))
+		!= (RTW_IEEE80211_FTYPE_MGMT|RTW_IEEE80211_STYPE_ACTION)
+	)
+	{
+		return _FALSE;
+	}
+
+	c = frame_body[0];
+
+	switch(c) {
+	case RTW_WLAN_CATEGORY_P2P: /* vendor-specific */
+		break;
+	default:
+		a = frame_body[1];
+	}
+
+	if (category)
+		*category = c;
+	if (action)
+		*action = a;
+
+	return _TRUE;
+}
+
+static const char *_action_public_str[] = {
+	"ACT_PUB_BSSCOEXIST",
+	"ACT_PUB_DSE_ENABLE",
+	"ACT_PUB_DSE_DEENABLE",
+	"ACT_PUB_DSE_REG_LOCATION",
+	"ACT_PUB_EXT_CHL_SWITCH",
+	"ACT_PUB_DSE_MSR_REQ",
+	"ACT_PUB_DSE_MSR_RPRT",
+	"ACT_PUB_MP",
+	"ACT_PUB_DSE_PWR_CONSTRAINT",
+	"ACT_PUB_VENDOR",
+	"ACT_PUB_GAS_INITIAL_REQ",
+	"ACT_PUB_GAS_INITIAL_RSP",
+	"ACT_PUB_GAS_COMEBACK_REQ",
+	"ACT_PUB_GAS_COMEBACK_RSP",
+	"ACT_PUB_TDLS_DISCOVERY_RSP",
+	"ACT_PUB_LOCATION_TRACK",
+	"ACT_PUB_RSVD",
+};
+
+const char *action_public_str(u8 action)
+{
+	action = (action >= ACT_PUBLIC_MAX) ? ACT_PUBLIC_MAX : action;
+	return _action_public_str[action];
+}
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_io.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_io.c
new file mode 100755
index 00000000..83cd2fbb
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_io.c
@@ -0,0 +1,483 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+/*
+
+The purpose of rtw_io.c
+
+a. provides the API
+
+b. provides the protocol engine
+
+c. provides the software interface between caller and the hardware interface
+
+
+Compiler Flag Option:
+
+1. CONFIG_SDIO_HCI:
+    a. USE_SYNC_IRP:  Only sync operations are provided.
+    b. USE_ASYNC_IRP:Both sync/async operations are provided.
+
+2. CONFIG_USB_HCI:
+   a. USE_ASYNC_IRP: Both sync/async operations are provided.
+
+3. CONFIG_CFIO_HCI:
+   b. USE_SYNC_IRP: Only sync operations are provided.
+
+
+Only sync read/rtw_write_mem operations are provided.
+
+jackson@realtek.com.tw
+
+*/
+
+#define _RTW_IO_C_
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <rtw_io.h>
+#include <osdep_intf.h>
+
+#if defined (PLATFORM_LINUX) && defined (PLATFORM_WINDOWS)
+#error "Shall be Linux or Windows, but not both!\n"
+#endif
+
+#ifdef CONFIG_SDIO_HCI
+#include <sdio_ops.h>
+#endif
+
+#ifdef CONFIG_GSPI_HCI
+#include <gspi_ops.h>
+#endif
+
+#ifdef CONFIG_USB_HCI
+#include <usb_ops.h>
+#endif
+
+#ifdef CONFIG_PCI_HCI
+#include <pci_ops.h>
+#endif
+
+#ifdef CONFIG_SDIO_HCI
+#define rtw_le16_to_cpu(val) 		val
+#define rtw_le32_to_cpu(val)		val
+#define rtw_cpu_to_le16(val)		val
+#define rtw_cpu_to_le32(val)		val
+#else
+#define rtw_le16_to_cpu(val) 		le16_to_cpu(val)
+#define rtw_le32_to_cpu(val)		le32_to_cpu(val)
+#define rtw_cpu_to_le16(val)		cpu_to_le16(val)
+#define rtw_cpu_to_le32(val)		cpu_to_le32(val)
+#endif
+
+
+u8 _rtw_read8(_adapter *adapter, u32 addr)
+{
+	u8 r_val;
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u8 (*_read8)(struct intf_hdl *pintfhdl, u32 addr);
+	_func_enter_;
+	_read8 = pintfhdl->io_ops._read8;
+
+	r_val = _read8(pintfhdl, addr);
+	_func_exit_;
+	return r_val;
+}
+
+u16 _rtw_read16(_adapter *adapter, u32 addr)
+{
+	u16 r_val;
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u16 	(*_read16)(struct intf_hdl *pintfhdl, u32 addr);
+	_func_enter_;
+	_read16 = pintfhdl->io_ops._read16;
+
+	r_val = _read16(pintfhdl, addr);
+	_func_exit_;
+	return rtw_le16_to_cpu(r_val);
+}
+
+u32 _rtw_read32(_adapter *adapter, u32 addr)
+{
+	u32 r_val;
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u32 	(*_read32)(struct intf_hdl *pintfhdl, u32 addr);
+	_func_enter_;
+	_read32 = pintfhdl->io_ops._read32;
+
+	r_val = _read32(pintfhdl, addr);
+	_func_exit_;
+	return rtw_le32_to_cpu(r_val);
+
+}
+
+int _rtw_write8(_adapter *adapter, u32 addr, u8 val)
+{
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write8)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+	int ret;
+	_func_enter_;
+	_write8 = pintfhdl->io_ops._write8;
+
+	ret = _write8(pintfhdl, addr, val);
+	_func_exit_;
+	
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write16(_adapter *adapter, u32 addr, u16 val)
+{
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+	int ret;
+	_func_enter_;
+	_write16 = pintfhdl->io_ops._write16;
+
+	val = rtw_cpu_to_le16(val);
+	ret = _write16(pintfhdl, addr, val);
+	_func_exit_;
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write32(_adapter *adapter, u32 addr, u32 val)
+{
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+	int ret;
+	_func_enter_;
+	_write32 = pintfhdl->io_ops._write32;
+	
+	val = rtw_cpu_to_le32(val);
+	ret = _write32(pintfhdl, addr, val);
+	_func_exit_;
+
+	return RTW_STATUS_CODE(ret);
+}
+
+int _rtw_writeN(_adapter *adapter, u32 addr ,u32 length , u8 *pdata)
+{
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+        struct	intf_hdl	*pintfhdl = (struct intf_hdl*)(&(pio_priv->intf));
+	int (*_writeN)(struct intf_hdl *pintfhdl, u32 addr,u32 length, u8 *pdata);
+	int ret;
+	_func_enter_;
+	_writeN = pintfhdl->io_ops._writeN;
+
+	ret = _writeN(pintfhdl, addr,length,pdata);
+	_func_exit_;
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write8_async(_adapter *adapter, u32 addr, u8 val)
+{
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write8_async)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+	int ret;
+	_func_enter_;
+	_write8_async = pintfhdl->io_ops._write8_async;
+
+	ret = _write8_async(pintfhdl, addr, val);
+	_func_exit_;
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write16_async(_adapter *adapter, u32 addr, u16 val)
+{
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write16_async)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+	int ret;
+	_func_enter_;
+	_write16_async = pintfhdl->io_ops._write16_async;
+	val = rtw_cpu_to_le16(val);
+	ret = _write16_async(pintfhdl, addr, val);
+	_func_exit_;
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write32_async(_adapter *adapter, u32 addr, u32 val)
+{
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write32_async)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+	int ret;
+	_func_enter_;
+	_write32_async = pintfhdl->io_ops._write32_async;
+	val = rtw_cpu_to_le32(val);
+	ret = _write32_async(pintfhdl, addr, val);
+	_func_exit_;
+
+	return RTW_STATUS_CODE(ret);
+}
+
+void _rtw_read_mem(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	void (*_read_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+
+	_func_enter_;
+
+	if( (adapter->bDriverStopped ==_TRUE) || (adapter->bSurpriseRemoved == _TRUE))
+	{
+	     RT_TRACE(_module_rtl871x_io_c_, _drv_info_, ("rtw_read_mem:bDriverStopped(%d) OR bSurpriseRemoved(%d)", adapter->bDriverStopped, adapter->bSurpriseRemoved));	    
+	     return;
+	}
+
+	_read_mem = pintfhdl->io_ops._read_mem;
+
+	_read_mem(pintfhdl, addr, cnt, pmem);
+
+	_func_exit_;
+
+}
+
+void _rtw_write_mem(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	void (*_write_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+
+	_func_enter_;
+
+	_write_mem = pintfhdl->io_ops._write_mem;
+
+	_write_mem(pintfhdl, addr, cnt, pmem);
+
+	_func_exit_;
+
+}
+
+void _rtw_read_port(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	u32 (*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+
+	_func_enter_;
+
+	if( (adapter->bDriverStopped ==_TRUE) || (adapter->bSurpriseRemoved == _TRUE))
+	{
+	     RT_TRACE(_module_rtl871x_io_c_, _drv_info_, ("rtw_read_port:bDriverStopped(%d) OR bSurpriseRemoved(%d)", adapter->bDriverStopped, adapter->bSurpriseRemoved));	    
+	     return;
+	}
+
+	_read_port = pintfhdl->io_ops._read_port;
+
+	_read_port(pintfhdl, addr, cnt, pmem);
+
+	_func_exit_;
+
+}
+
+void _rtw_read_port_cancel(_adapter *adapter)
+{
+	void (*_read_port_cancel)(struct intf_hdl *pintfhdl);
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+
+	_read_port_cancel = pintfhdl->io_ops._read_port_cancel;
+
+	if(_read_port_cancel)
+		_read_port_cancel(pintfhdl);
+
+}
+
+u32 _rtw_write_port(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	u32 (*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	//struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u32 ret = _SUCCESS;
+
+	_func_enter_;
+
+	_write_port = pintfhdl->io_ops._write_port;
+	
+	ret = _write_port(pintfhdl, addr, cnt, pmem);
+
+	 _func_exit_;
+
+	return ret;
+}
+
+u32 _rtw_write_port_and_wait(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem, int timeout_ms)
+{
+	int ret = _SUCCESS;
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)pmem;
+	struct submit_ctx sctx;
+
+	rtw_sctx_init(&sctx, timeout_ms);
+	pxmitbuf->sctx = &sctx;
+
+	ret = _rtw_write_port(adapter, addr, cnt, pmem);
+
+	if (ret == _SUCCESS)
+		ret = rtw_sctx_wait(&sctx);
+
+	 return ret;
+}
+
+void _rtw_write_port_cancel(_adapter *adapter)
+{
+	void (*_write_port_cancel)(struct intf_hdl *pintfhdl);
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+
+	_write_port_cancel = pintfhdl->io_ops._write_port_cancel;
+
+	if(_write_port_cancel)
+		_write_port_cancel(pintfhdl);
+
+}
+
+int rtw_init_io_priv(_adapter *padapter, void (*set_intf_ops)(struct _io_ops *pops))
+{
+	struct io_priv	*piopriv = &padapter->iopriv;
+	struct intf_hdl *pintf = &piopriv->intf;
+
+	if (set_intf_ops == NULL)
+		return _FAIL;
+
+	piopriv->padapter = padapter;
+	pintf->padapter = padapter;
+	pintf->pintf_dev = adapter_to_dvobj(padapter);
+
+	set_intf_ops(&pintf->io_ops);
+
+	return _SUCCESS;
+}
+
+#ifdef DBG_IO
+
+u16 read_sniff_ranges[][2] = {
+	//{0x550, 0x551},
+}; 
+
+u16 write_sniff_ranges[][2] = {
+	//{0x550, 0x551},
+	//{0x4c, 0x4c},
+}; 
+
+int read_sniff_num = sizeof(read_sniff_ranges)/sizeof(u16)/2;
+int write_sniff_num = sizeof(write_sniff_ranges)/sizeof(u16)/2;
+
+bool match_read_sniff_ranges(u16 addr, u16 len)
+{
+	int i;
+	for (i = 0; i<read_sniff_num; i++) {
+		if (addr + len > read_sniff_ranges[i][0] && addr <= read_sniff_ranges[i][1])
+			return _TRUE;
+	}
+	
+	return _FALSE;
+}
+
+bool match_write_sniff_ranges(u16 addr, u16 len)
+{
+	int i;
+	for (i = 0; i<write_sniff_num; i++) {
+		if (addr + len > write_sniff_ranges[i][0] && addr <= write_sniff_ranges[i][1])
+			return _TRUE;
+	}
+	
+	return _FALSE;
+}
+
+u8 dbg_rtw_read8(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u8 val = _rtw_read8(adapter, addr);
+
+	if (match_read_sniff_ranges(addr, 1))
+		DBG_871X("DBG_IO %s:%d rtw_read8(0x%04x) return 0x%02x\n", caller, line, addr, val);
+
+	return val;
+}
+
+u16 dbg_rtw_read16(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u16 val = _rtw_read16(adapter, addr);
+	
+	if (match_read_sniff_ranges(addr, 2))
+		DBG_871X("DBG_IO %s:%d rtw_read16(0x%04x) return 0x%04x\n", caller, line, addr, val);
+
+	return val;
+}
+
+u32 dbg_rtw_read32(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u32 val = _rtw_read32(adapter, addr);
+	
+	if (match_read_sniff_ranges(addr, 4))
+		DBG_871X("DBG_IO %s:%d rtw_read32(0x%04x) return 0x%08x\n", caller, line, addr, val);
+
+	return val;
+}
+
+int dbg_rtw_write8(_adapter *adapter, u32 addr, u8 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 1))
+		DBG_871X("DBG_IO %s:%d rtw_write8(0x%04x, 0x%02x)\n", caller, line, addr, val);
+	
+	return _rtw_write8(adapter, addr, val);
+}
+int dbg_rtw_write16(_adapter *adapter, u32 addr, u16 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 2))
+		DBG_871X("DBG_IO %s:%d rtw_write16(0x%04x, 0x%04x)\n", caller, line, addr, val);
+	
+	return _rtw_write16(adapter, addr, val);
+}
+int dbg_rtw_write32(_adapter *adapter, u32 addr, u32 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 4))
+		DBG_871X("DBG_IO %s:%d rtw_write32(0x%04x, 0x%08x)\n", caller, line, addr, val);
+	
+	return _rtw_write32(adapter, addr, val);
+}
+int dbg_rtw_writeN(_adapter *adapter, u32 addr ,u32 length , u8 *data, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, length))
+		DBG_871X("DBG_IO %s:%d rtw_writeN(0x%04x, %u)\n", caller, line, addr, length);
+
+	return _rtw_writeN(adapter, addr, length, data);
+}
+#endif
+
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_ioctl_query.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_ioctl_query.c
new file mode 100755
index 00000000..06018867
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_ioctl_query.c
@@ -0,0 +1,196 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_IOCTL_QUERY_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <rtw_ioctl_query.h>
+#include <wifi.h>
+
+
+#ifdef PLATFORM_WINDOWS
+//
+// Added for WPA2-PSK, by Annie, 2005-09-20.
+//
+u8
+query_802_11_capability(
+	_adapter*		Adapter,
+	u8*			pucBuf,
+	u32 *		pulOutLen
+)
+{
+	static NDIS_802_11_AUTHENTICATION_ENCRYPTION szAuthEnc[] = 
+	{
+		{Ndis802_11AuthModeOpen, Ndis802_11EncryptionDisabled}, 
+		{Ndis802_11AuthModeOpen, Ndis802_11Encryption1Enabled},
+		{Ndis802_11AuthModeShared, Ndis802_11EncryptionDisabled}, 
+		{Ndis802_11AuthModeShared, Ndis802_11Encryption1Enabled},
+		{Ndis802_11AuthModeWPA, Ndis802_11Encryption2Enabled}, 
+		{Ndis802_11AuthModeWPA, Ndis802_11Encryption3Enabled},
+		{Ndis802_11AuthModeWPAPSK, Ndis802_11Encryption2Enabled}, 
+		{Ndis802_11AuthModeWPAPSK, Ndis802_11Encryption3Enabled},
+		{Ndis802_11AuthModeWPANone, Ndis802_11Encryption2Enabled}, 
+		{Ndis802_11AuthModeWPANone, Ndis802_11Encryption3Enabled},
+		{Ndis802_11AuthModeWPA2, Ndis802_11Encryption2Enabled}, 
+		{Ndis802_11AuthModeWPA2, Ndis802_11Encryption3Enabled},
+		{Ndis802_11AuthModeWPA2PSK, Ndis802_11Encryption2Enabled}, 
+		{Ndis802_11AuthModeWPA2PSK, Ndis802_11Encryption3Enabled}
+	};	
+	static ULONG	ulNumOfPairSupported = sizeof(szAuthEnc)/sizeof(NDIS_802_11_AUTHENTICATION_ENCRYPTION);
+	NDIS_802_11_CAPABILITY * pCap = (NDIS_802_11_CAPABILITY *)pucBuf;
+	u8*	pucAuthEncryptionSupported = (u8*) pCap->AuthenticationEncryptionSupported;
+
+
+	pCap->Length = sizeof(NDIS_802_11_CAPABILITY);
+	if(ulNumOfPairSupported > 1 )
+		pCap->Length += 	(ulNumOfPairSupported-1) * sizeof(NDIS_802_11_AUTHENTICATION_ENCRYPTION);
+	
+	pCap->Version = 2;	
+	pCap->NoOfPMKIDs = NUM_PMKID_CACHE;	
+	pCap->NoOfAuthEncryptPairsSupported = ulNumOfPairSupported;
+
+	if( sizeof (szAuthEnc) <= 240 )		// 240 = 256 - 4*4	// SecurityInfo.szCapability: only 256 bytes in size.
+	{
+		_rtw_memcpy( pucAuthEncryptionSupported, (u8*)szAuthEnc,  sizeof (szAuthEnc) );
+		*pulOutLen = pCap->Length;
+		return _TRUE;
+	}
+	else
+	{
+		*pulOutLen = 0;
+		RT_TRACE(_module_rtl871x_ioctl_query_c_,_drv_info_,("_query_802_11_capability(): szAuthEnc size is too large.\n"));
+		return _FALSE;
+	}
+}
+
+u8 query_802_11_association_information(	_adapter *padapter,PNDIS_802_11_ASSOCIATION_INFORMATION	pAssocInfo)
+{
+	struct wlan_network *tgt_network;
+	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct	security_priv  *psecuritypriv=&(padapter->securitypriv);
+	WLAN_BSSID_EX	*psecnetwork=(WLAN_BSSID_EX*)&(psecuritypriv->sec_bss);					
+	u8 *	pDest = (u8 *)pAssocInfo + sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+	unsigned char i,*auth_ie,*supp_ie;
+
+	//NdisZeroMemory(pAssocInfo, sizeof(NDIS_802_11_ASSOCIATION_INFORMATION));
+	_rtw_memset(pAssocInfo, 0, sizeof(NDIS_802_11_ASSOCIATION_INFORMATION));
+	//pAssocInfo->Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+
+	//------------------------------------------------------
+	// Association Request related information
+	//------------------------------------------------------
+	// Req_1. AvailableRequestFixedIEs
+	if(psecnetwork!=NULL){
+		
+	pAssocInfo->AvailableRequestFixedIEs |= NDIS_802_11_AI_REQFI_CAPABILITIES|NDIS_802_11_AI_REQFI_CURRENTAPADDRESS;
+	pAssocInfo->RequestFixedIEs.Capabilities = (unsigned short)* & psecnetwork->IEs[10];
+	_rtw_memcpy(pAssocInfo->RequestFixedIEs.CurrentAPAddress,
+		& psecnetwork->MacAddress, 6);
+
+	pAssocInfo->OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+
+	if(check_fwstate( pmlmepriv, _FW_UNDER_LINKING|_FW_LINKED)==_TRUE)
+	{
+		
+		if(psecuritypriv->ndisauthtype>=Ndis802_11AuthModeWPA2)
+			pDest[0] =48;		//RSN Information Element
+		else 
+			pDest[0] =221;	//WPA(SSN) Information Element
+		
+		RT_TRACE(_module_rtl871x_ioctl_query_c_,_drv_info_,("\n Adapter->ndisauthtype==Ndis802_11AuthModeWPA)?0xdd:0x30 [%d]",pDest[0]));
+		supp_ie=&psecuritypriv->supplicant_ie[0];
+		for(i=0;i<supp_ie[0];i++)
+		{
+			RT_TRACE(_module_rtl871x_ioctl_query_c_,_drv_info_,("IEs [%d] = 0x%x \n\n", i,supp_ie[i]));
+		}
+
+		i=13;	//0~11 is fixed information element		
+		RT_TRACE(_module_rtl871x_ioctl_query_c_,_drv_info_,("i= %d tgt_network->network.IELength=%d\n\n", i,(int)psecnetwork->IELength));
+		while((i<supp_ie[0]) && (i<256)){
+			if((unsigned char)supp_ie[i]==pDest[0]){
+						_rtw_memcpy((u8 *)(pDest),
+							&supp_ie[i], 
+							supp_ie[1+i]+2);
+			
+				break;
+			}
+			
+			i=i+supp_ie[i+1]+2;
+			if(supp_ie[1+i]==0)
+				i=i+1;
+			RT_TRACE(_module_rtl871x_ioctl_query_c_,_drv_info_,("iteration i=%d IEs [%d] = 0x%x \n\n", i,i,supp_ie[i+1]));
+			
+		}
+		
+
+		pAssocInfo->RequestIELength += (2 + supp_ie[1+i]);// (2 + psecnetwork->IEs[1+i]+4);
+
+	}
+	
+
+		RT_TRACE(_module_rtl871x_ioctl_query_c_,_drv_info_,("\n psecnetwork != NULL,fwstate==_FW_UNDER_LINKING \n"));
+
+	}
+	
+
+	//------------------------------------------------------
+	// Association Response related information
+	//------------------------------------------------------
+
+	if(check_fwstate( pmlmepriv, _FW_LINKED)==_TRUE)
+	{
+		tgt_network =&(pmlmepriv->cur_network);
+		if(tgt_network!=NULL){
+		pAssocInfo->AvailableResponseFixedIEs =
+				NDIS_802_11_AI_RESFI_CAPABILITIES
+				|NDIS_802_11_AI_RESFI_ASSOCIATIONID
+				;
+
+		pAssocInfo->ResponseFixedIEs.Capabilities =(unsigned short)* & tgt_network->network.IEs[10];
+		pAssocInfo->ResponseFixedIEs.StatusCode = 0;
+		pAssocInfo->ResponseFixedIEs.AssociationId =(unsigned short) tgt_network->aid;
+
+		pDest = (u8 *)pAssocInfo + sizeof(NDIS_802_11_ASSOCIATION_INFORMATION)+pAssocInfo->RequestIELength;
+		auth_ie=&psecuritypriv->authenticator_ie[0];
+
+		for(i=0;i<auth_ie[0];i++)
+			RT_TRACE(_module_rtl871x_ioctl_query_c_,_drv_info_,("IEs [%d] = 0x%x \n\n", i,auth_ie[i]));
+
+		i=auth_ie[0]-12;
+		if(i>0){
+			_rtw_memcpy((u8 *)&pDest[0],&auth_ie[1],i);
+			pAssocInfo->ResponseIELength =i; 
+		}
+
+
+		pAssocInfo->OffsetResponseIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION) + pAssocInfo->RequestIELength;  
+
+
+		RT_TRACE(_module_rtl871x_ioctl_query_c_,_drv_info_,("\n tgt_network != NULL,fwstate==_FW_LINKED \n"));
+		}
+	}												  	
+	RT_TRACE(_module_rtl871x_ioctl_query_c_,_drv_info_,("\n exit query_802_11_association_information \n"));
+_func_exit_;
+
+	return _TRUE;
+}
+#endif
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_ioctl_rtl.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_ioctl_rtl.c
new file mode 100755
index 00000000..d0454a97
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_ioctl_rtl.c
@@ -0,0 +1,1032 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define  _RTW_IOCTL_RTL_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <wlan_bssdef.h>
+#include <wifi.h>
+#include <rtw_ioctl.h>
+#include <rtw_ioctl_set.h>
+#include <rtw_ioctl_query.h>
+#include <rtw_ioctl_rtl.h>
+#include <mp_custom_oid.h>
+#ifdef CONFIG_MP_INCLUDED
+#include <rtw_mp.h>
+#include <rtw_mp_ioctl.h>
+#endif
+
+struct oid_obj_priv oid_rtl_seg_01_01[] =
+{
+	{1, &oid_null_function},										//0x80
+	{1, &oid_null_function},										//0x81
+	{1, &oid_null_function},										//0x82
+	{1, &oid_null_function},										//0x83//OID_RT_SET_SNIFFER_MODE
+	{1, &oid_rt_get_signal_quality_hdl},							//0x84
+	{1, &oid_rt_get_small_packet_crc_hdl},						//0x85
+	{1, &oid_rt_get_middle_packet_crc_hdl},						//0x86
+	{1, &oid_rt_get_large_packet_crc_hdl},						//0x87
+	{1, &oid_rt_get_tx_retry_hdl},								//0x88
+	{1, &oid_rt_get_rx_retry_hdl},								//0x89
+	{1, &oid_rt_pro_set_fw_dig_state_hdl},						//0x8A
+	{1, &oid_rt_pro_set_fw_ra_state_hdl}	,						//0x8B
+	{1, &oid_null_function},										//0x8C
+	{1, &oid_null_function},										//0x8D
+	{1, &oid_null_function},										//0x8E
+	{1, &oid_null_function},										//0x8F
+	{1, &oid_rt_get_rx_total_packet_hdl},							//0x90
+	{1, &oid_rt_get_tx_beacon_ok_hdl},							//0x91
+	{1, &oid_rt_get_tx_beacon_err_hdl},							//0x92
+	{1, &oid_rt_get_rx_icv_err_hdl},								//0x93
+	{1, &oid_rt_set_encryption_algorithm_hdl},					//0x94
+	{1, &oid_null_function},										//0x95
+	{1, &oid_rt_get_preamble_mode_hdl},							//0x96
+	{1, &oid_null_function},										//0x97
+	{1, &oid_rt_get_ap_ip_hdl},									//0x98
+	{1, &oid_rt_get_channelplan_hdl},							//0x99	
+	{1, &oid_rt_set_preamble_mode_hdl},	 						//0x9A
+	{1, &oid_rt_set_bcn_intvl_hdl},								//0x9B
+	{1, &oid_null_function},										//0x9C
+	{1, &oid_rt_dedicate_probe_hdl},								//0x9D
+	{1, &oid_null_function},										//0x9E
+	{1, &oid_null_function},										//0x9F
+	{1, &oid_null_function},										//0xA0
+	{1, &oid_null_function},										//0xA1
+	{1, &oid_null_function},										//0xA2
+	{1, &oid_null_function},										//0xA3
+	{1, &oid_null_function},										//0xA4
+	{1, &oid_null_function},										//0xA5
+	{1, &oid_null_function},										//0xA6
+	{1, &oid_rt_get_total_tx_bytes_hdl},							//0xA7
+	{1, &oid_rt_get_total_rx_bytes_hdl},							//0xA8
+	{1, &oid_rt_current_tx_power_level_hdl},						//0xA9	
+	{1, &oid_rt_get_enc_key_mismatch_count_hdl},	 			//0xAA
+	{1, &oid_rt_get_enc_key_match_count_hdl},					//0xAB
+	{1, &oid_rt_get_channel_hdl},								//0xAC
+	{1, &oid_rt_set_channelplan_hdl},								//0xAD
+	{1, &oid_rt_get_hardware_radio_off_hdl},						//0xAE
+	{1, &oid_null_function},										//0xAF
+	{1, &oid_null_function},										//0xB0
+	{1, &oid_null_function},										//0xB1
+	{1, &oid_null_function},										//0xB2
+	{1, &oid_null_function},										//0xB3
+	{1, &oid_rt_get_key_mismatch_hdl},							//0xB4
+	{1, &oid_null_function},										//0xB5
+	{1, &oid_null_function},										//0xB6
+	{1, &oid_null_function},										//0xB7
+	{1, &oid_null_function},										//0xB8
+	{1, &oid_null_function},										//0xB9	
+	{1, &oid_null_function},	 									//0xBA
+	{1, &oid_rt_supported_wireless_mode_hdl},					//0xBB
+	{1, &oid_rt_get_channel_list_hdl},							//0xBC
+	{1, &oid_rt_get_scan_in_progress_hdl},						//0xBD
+	{1, &oid_null_function},										//0xBE
+	{1, &oid_null_function},										//0xBF
+	{1, &oid_null_function},										//0xC0
+	{1, &oid_rt_forced_data_rate_hdl},							//0xC1
+	{1, &oid_rt_wireless_mode_for_scan_list_hdl},					//0xC2
+	{1, &oid_rt_get_bss_wireless_mode_hdl},						//0xC3
+	{1, &oid_rt_scan_with_magic_packet_hdl},					//0xC4
+	{1, &oid_null_function},										//0xC5
+	{1, &oid_null_function},										//0xC6
+	{1, &oid_null_function},										//0xC7
+	{1, &oid_null_function},										//0xC8
+	{1, &oid_null_function},										//0xC9	
+	{1, &oid_null_function},	 									//0xCA
+	{1, &oid_null_function},										//0xCB
+	{1, &oid_null_function},										//0xCC
+	{1, &oid_null_function},										//0xCD
+	{1, &oid_null_function},										//0xCE
+	{1, &oid_null_function},										//0xCF
+	
+};
+
+struct oid_obj_priv oid_rtl_seg_01_03[] =
+{
+	{1, &oid_rt_ap_get_associated_station_list_hdl},				//0x00
+	{1, &oid_null_function},										//0x01
+	{1, &oid_rt_ap_switch_into_ap_mode_hdl},					//0x02
+	{1, &oid_null_function},										//0x03
+	{1, &oid_rt_ap_supported_hdl},								//0x04
+	{1, &oid_rt_ap_set_passphrase_hdl},							//0x05
+
+};
+
+struct oid_obj_priv oid_rtl_seg_01_11[] =
+{
+	{1, &oid_null_function},					//0xC0	OID_RT_PRO_RX_FILTER	
+	{1, &oid_null_function},					//0xC1	OID_CE_USB_WRITE_REGISTRY
+	{1, &oid_null_function},					//0xC2	OID_CE_USB_READ_REGISTRY
+	{1, &oid_null_function},					//0xC3	OID_RT_PRO_SET_INITIAL_GAIN
+	{1, &oid_null_function},					//0xC4	OID_RT_PRO_SET_BB_RF_STANDBY_MODE
+	{1, &oid_null_function},					//0xC5	OID_RT_PRO_SET_BB_RF_SHUTDOWN_MODE
+	{1, &oid_null_function},					//0xC6	OID_RT_PRO_SET_TX_CHARGE_PUMP
+	{1, &oid_null_function},					//0xC7	OID_RT_PRO_SET_RX_CHARGE_PUMP
+	{1, &oid_rt_pro_rf_write_registry_hdl},	//0xC8	
+	{1, &oid_rt_pro_rf_read_registry_hdl},	//0xC9	
+	{1, &oid_null_function}					//0xCA	OID_RT_PRO_QUERY_RF_TYPE
+	
+};
+
+struct oid_obj_priv oid_rtl_seg_03_00[] =
+{
+	{1, &oid_null_function},										//0x00
+	{1, &oid_rt_get_connect_state_hdl},							//0x01
+	{1, &oid_null_function},										//0x02
+	{1, &oid_null_function},										//0x03
+	{1, &oid_rt_set_default_key_id_hdl},							//0x04
+
+	
+};
+
+
+//**************  oid_rtl_seg_01_01 section start ************** 
+
+NDIS_STATUS oid_rt_pro_set_fw_dig_state_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+#if 0
+	PADAPTER		Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	_irqL			oldirql;
+	
+	_func_enter_;
+	
+	if(poid_par_priv->type_of_oid != SET_OID) 
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+			
+	_irqlevel_changed_(&oldirql,LOWER);
+	if(poid_par_priv->information_buf_len >= sizeof(struct setdig_parm))
+	{
+		//DEBUG_ERR(("===> oid_rt_pro_set_fw_dig_state_hdl. type:0x%02x.\n",*((unsigned char*)poid_par_priv->information_buf )));	
+		if(!rtw_setfwdig_cmd(Adapter,*((unsigned char*)poid_par_priv->information_buf )))			
+		{
+			status = NDIS_STATUS_NOT_ACCEPTED;
+		}
+		                   
+	}
+	else{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+	}  
+	_irqlevel_changed_(&oldirql,RAISE);
+	_func_exit_;
+#endif
+	return status;
+}
+//-----------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_set_fw_ra_state_hdl(struct oid_par_priv* poid_par_priv)
+{
+
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+#if 0
+	PADAPTER		Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	_irqL			oldirql;
+	
+	_func_enter_;	
+	if(poid_par_priv->type_of_oid != SET_OID) 
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+					
+	_irqlevel_changed_(&oldirql,LOWER);
+	
+	if(poid_par_priv->information_buf_len >= sizeof(struct setra_parm))
+	{
+		//DEBUG_ERR(("===> oid_rt_pro_set_fw_ra_state_hdl. type:0x%02x.\n",*((unsigned char*)poid_par_priv->information_buf )));	
+		if(!rtw_setfwra_cmd(Adapter,*((unsigned char*)poid_par_priv->information_buf )))			
+		{
+			status = NDIS_STATUS_NOT_ACCEPTED;
+		}
+		                   
+	}
+	else{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+	}  
+	_irqlevel_changed_(&oldirql,RAISE);
+	_func_exit_;
+#endif
+	return status;
+}
+//-----------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_signal_quality_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	//DEBUG_ERR(("<**********************oid_rt_get_signal_quality_hdl \n"));
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+#if 0
+		if(pMgntInfo->mAssoc || pMgntInfo->mIbss)
+		{
+			ulInfo = pAdapter->RxStats.SignalQuality;
+			*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+		}
+		else
+		{
+			ulInfo = 0xffffffff; // It stands for -1 in 4-byte integer.
+		}
+		break;
+#endif
+
+	return status;
+}
+
+//------------------------------------------------------------------------------
+
+NDIS_STATUS oid_rt_get_small_packet_crc_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+		
+	if(poid_par_priv->information_buf_len >=  sizeof(ULONG) )
+	{		
+		*(ULONG *)poid_par_priv->information_buf = padapter->recvpriv.rx_smallpacket_crcerr;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;		
+	}
+	else
+	{
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_middle_packet_crc_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	if(poid_par_priv->information_buf_len >=  sizeof(ULONG) )
+	{		
+		*(ULONG *)poid_par_priv->information_buf = padapter->recvpriv.rx_middlepacket_crcerr;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;		
+	}
+	else
+	{
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}
+
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_large_packet_crc_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}	
+
+	if(poid_par_priv->information_buf_len >=  sizeof(ULONG) )
+	{		
+		*(ULONG *)poid_par_priv->information_buf = padapter->recvpriv.rx_largepacket_crcerr;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;		
+	}
+	else
+	{
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}
+
+
+	return status;
+}
+
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_tx_retry_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_rx_retry_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_rx_total_packet_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}	
+	if(poid_par_priv->information_buf_len >=  sizeof(ULONG) )
+	{		
+		*(u64 *)poid_par_priv->information_buf = padapter->recvpriv.rx_pkts + padapter->recvpriv.rx_drop;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;		
+	}
+	else
+	{
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}
+
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_tx_beacon_ok_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_tx_beacon_err_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_rx_icv_err_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	if(poid_par_priv->information_buf_len>= sizeof(u32))
+	{
+		//_rtw_memcpy(*(uint *)poid_par_priv->information_buf,padapter->recvpriv.rx_icv_err,sizeof(u32));
+		*(uint *)poid_par_priv->information_buf = padapter->recvpriv.rx_icv_err;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	}
+	else
+	{
+		status = NDIS_STATUS_INVALID_LENGTH ;
+	}
+	
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_set_encryption_algorithm_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != SET_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}	
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_preamble_mode_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+	ULONG			preamblemode = 0 ;			
+		
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+	if(poid_par_priv->information_buf_len>= sizeof(ULONG))
+	{		
+		if(padapter->registrypriv.preamble == PREAMBLE_LONG)
+			preamblemode = 0;
+		else if (padapter->registrypriv.preamble == PREAMBLE_AUTO)
+			preamblemode = 1;
+		else if (padapter->registrypriv.preamble == PREAMBLE_SHORT)
+			preamblemode = 2;
+		
+			
+		*(ULONG *)poid_par_priv->information_buf = preamblemode ;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	}
+	else
+	{
+		status = NDIS_STATUS_INVALID_LENGTH ;
+	}
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_ap_ip_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_get_channelplan_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+	struct eeprom_priv*	peeprompriv = &padapter->eeprompriv;	
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	*(u16 *)poid_par_priv->information_buf = peeprompriv->channel_plan ;
+
+	return status;
+}
+NDIS_STATUS oid_rt_set_channelplan_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+	struct eeprom_priv*	peeprompriv = &padapter->eeprompriv;	
+	
+	if(poid_par_priv->type_of_oid != SET_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+	
+	peeprompriv->channel_plan = *(u16 *)poid_par_priv->information_buf ;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_set_preamble_mode_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+	ULONG			preamblemode = 0;
+	if(poid_par_priv->type_of_oid != SET_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	 
+	if(poid_par_priv->information_buf_len>= sizeof(ULONG))
+	{		
+		preamblemode = *(ULONG *)poid_par_priv->information_buf ;
+		if( preamblemode == 0)
+			padapter->registrypriv.preamble = PREAMBLE_LONG;
+		else if (preamblemode==1 )
+			padapter->registrypriv.preamble = PREAMBLE_AUTO;
+		else if ( preamblemode==2 )
+			padapter->registrypriv.preamble = PREAMBLE_SHORT;		
+			
+		*(ULONG *)poid_par_priv->information_buf = preamblemode ;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	}
+	else
+	{
+		status = NDIS_STATUS_INVALID_LENGTH ;
+	}	
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_set_bcn_intvl_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != SET_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+NDIS_STATUS oid_rt_dedicate_probe_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_total_tx_bytes_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}	
+	if(poid_par_priv->information_buf_len>= sizeof(ULONG))
+	{		
+		*(u64 *)poid_par_priv->information_buf = padapter->xmitpriv.tx_bytes;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	}
+	else
+	{
+		status = NDIS_STATUS_INVALID_LENGTH ;
+	}
+	
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_total_rx_bytes_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	if(poid_par_priv->information_buf_len>= sizeof(ULONG))
+	{
+		//_rtw_memcpy(*(uint *)poid_par_priv->information_buf,padapter->recvpriv.rx_icv_err,sizeof(u32));
+		*(u64 *)poid_par_priv->information_buf = padapter->recvpriv.rx_bytes;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	}
+	else
+	{
+		status = NDIS_STATUS_INVALID_LENGTH ;
+	}
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_current_tx_power_level_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_enc_key_mismatch_count_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_enc_key_match_count_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_channel_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	NDIS_802_11_CONFIGURATION		*pnic_Config;
+
+	ULONG   channelnum;
+
+	_func_enter_;
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	if ( (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE))
+		pnic_Config = &pmlmepriv->cur_network.network.Configuration;
+	else
+		pnic_Config = &padapter->registrypriv.dev_network.Configuration;
+
+	channelnum = pnic_Config->DSConfig;
+	*(ULONG *)poid_par_priv->information_buf = channelnum;
+	
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+	_func_exit_;
+
+
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_hardware_radio_off_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_key_mismatch_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+NDIS_STATUS oid_rt_supported_wireless_mode_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+	ULONG 			ulInfo = 0 ;
+	//DEBUG_ERR(("<**********************oid_rt_supported_wireless_mode_hdl \n"));	
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+	if(poid_par_priv->information_buf_len >= sizeof(ULONG)){
+		ulInfo |= 0x0100; //WIRELESS_MODE_B
+		ulInfo |= 0x0200; //WIRELESS_MODE_G
+		ulInfo |= 0x0400; //WIRELESS_MODE_A
+
+		*(ULONG *) poid_par_priv->information_buf = ulInfo;		
+		//DEBUG_ERR(("<===oid_rt_supported_wireless_mode %x\n",ulInfo));	
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	}
+	else{
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}		
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_channel_list_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_scan_in_progress_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+
+
+NDIS_STATUS oid_rt_forced_data_rate_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+NDIS_STATUS oid_rt_wireless_mode_for_scan_list_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_bss_wireless_mode_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_scan_with_magic_packet_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+//**************  oid_rtl_seg_01_01 section end ************** 
+
+//**************  oid_rtl_seg_01_03 section start ************** 
+NDIS_STATUS oid_rt_ap_get_associated_station_list_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+NDIS_STATUS oid_rt_ap_switch_into_ap_mode_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+NDIS_STATUS oid_rt_ap_supported_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+NDIS_STATUS oid_rt_ap_set_passphrase_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != SET_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	return status;
+}
+
+//**************  oid_rtl_seg_01_03 section end ************** 
+
+//****************  oid_rtl_seg_01_11   section start ****************
+NDIS_STATUS oid_rt_pro_rf_write_registry_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	_irqL			oldirql;
+	_func_enter_;
+	//DEBUG_ERR(("<**********************oid_rt_pro_rf_write_registry_hdl \n"));
+	if(poid_par_priv->type_of_oid != SET_OID) //QUERY_OID
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	
+	_irqlevel_changed_(&oldirql,LOWER);
+	if(poid_par_priv->information_buf_len== (sizeof(unsigned long)*3))
+	{      
+		//RegOffsetValue	- The offset of RF register to write.
+		//RegDataWidth	- The data width of RF register to write.
+		//RegDataValue	- The value to write. 
+		//RegOffsetValue = *((unsigned long*)InformationBuffer);
+		//RegDataWidth = *((unsigned long*)InformationBuffer+1);	   
+		//RegDataValue =  *((unsigned long*)InformationBuffer+2);	
+		if(!rtw_setrfreg_cmd(Adapter, 
+						*(unsigned char*)poid_par_priv->information_buf, 
+						(unsigned long)(*((unsigned long*)poid_par_priv->information_buf+2))))
+		{
+			status = NDIS_STATUS_NOT_ACCEPTED;
+		}
+	                   
+	}
+	else{
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}   
+	_irqlevel_changed_(&oldirql,RAISE);
+	_func_exit_;
+
+	return status;
+}
+
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_rf_read_registry_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+#if 0
+	PADAPTER		Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	_irqL	oldirql;
+	_func_enter_;
+
+	//DEBUG_ERR(("<**********************oid_rt_pro_rf_read_registry_hdl \n"));
+	if(poid_par_priv->type_of_oid != SET_OID) //QUERY_OID
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}	
+	
+	_irqlevel_changed_(&oldirql,LOWER);
+	if(poid_par_priv->information_buf_len== (sizeof(unsigned long)*3))
+	{
+		if(Adapter->mppriv.act_in_progress == _TRUE)
+		{
+			status = NDIS_STATUS_NOT_ACCEPTED;
+		}
+		else
+		{
+			//init workparam
+			Adapter->mppriv.act_in_progress = _TRUE;
+			Adapter->mppriv.workparam.bcompleted= _FALSE;
+			Adapter->mppriv.workparam.act_type = MPT_READ_RF;
+			Adapter->mppriv.workparam.io_offset = *(unsigned long*)poid_par_priv->information_buf;		
+			Adapter->mppriv.workparam.io_value = 0xcccccccc;
+				       
+			//RegOffsetValue	- The offset of RF register to read.
+			//RegDataWidth	- The data width of RF register to read.
+			//RegDataValue	- The value to read. 
+			//RegOffsetValue = *((unsigned long*)InformationBuffer);
+			//RegDataWidth = *((unsigned long*)InformationBuffer+1);	   
+			//RegDataValue =  *((unsigned long*)InformationBuffer+2);	   	 	                   
+			if(!rtw_getrfreg_cmd(Adapter, 
+							*(unsigned char*)poid_par_priv->information_buf, 
+							(unsigned char*)&Adapter->mppriv.workparam.io_value))
+			{
+				status = NDIS_STATUS_NOT_ACCEPTED;
+			}
+		}
+				      		   
+		                   
+	}
+	else	{
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}
+	_irqlevel_changed_(&oldirql,RAISE);
+	_func_exit_;
+#endif
+	return status;
+}
+
+//****************  oid_rtl_seg_01_11   section end****************	
+
+
+//**************  oid_rtl_seg_03_00 section start **************  
+enum _CONNECT_STATE_{
+	CHECKINGSTATUS,
+	ASSOCIATED,
+	ADHOCMODE,
+	NOTASSOCIATED
+};
+
+NDIS_STATUS oid_rt_get_connect_state_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+
+	ULONG ulInfo;
+		
+	if(poid_par_priv->type_of_oid != QUERY_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}		
+
+	// nStatus==0	CheckingStatus
+	// nStatus==1	Associated
+	// nStatus==2	AdHocMode
+	// nStatus==3	NotAssociated
+	
+	if(check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == _TRUE)
+		ulInfo = CHECKINGSTATUS;
+	else if(check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)	
+		ulInfo = ASSOCIATED;
+	else if(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)== _TRUE)
+		ulInfo = ADHOCMODE;
+	else
+		ulInfo = NOTASSOCIATED ;
+
+	*(ULONG *)poid_par_priv->information_buf = ulInfo;
+	*poid_par_priv->bytes_rw =  poid_par_priv->information_buf_len;
+
+#if 0
+	// Rearrange the order to let the UI still shows connection when scan is in progress
+	RT_TRACE(COMP_OID_QUERY, DBG_LOUD, ("===> Query OID_RT_GET_CONNECT_STATE.\n"));
+	if(pMgntInfo->mAssoc)
+		ulInfo = 1;
+	else if(pMgntInfo->mIbss)
+		ulInfo = 2;
+	else if(pMgntInfo->bScanInProgress)
+		ulInfo = 0;
+	else
+		ulInfo = 3;
+	ulInfoLen = sizeof(ULONG);
+	RT_TRACE(COMP_OID_QUERY, DBG_LOUD, ("<=== Query OID_RT_GET_CONNECT_STATE: %d\n", ulInfo));
+#endif
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_set_default_key_id_hdl(struct oid_par_priv* poid_par_priv)
+{
+	NDIS_STATUS	 	status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if(poid_par_priv->type_of_oid != SET_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	
+	return status;
+}
+//**************  oid_rtl_seg_03_00 section end **************  
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_ioctl_set.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_ioctl_set.c
new file mode 100755
index 00000000..4df2e4e1
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_ioctl_set.c
@@ -0,0 +1,1449 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_IOCTL_SET_C_
+
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <rtw_ioctl_set.h>
+#include <hal_intf.h>
+
+#ifdef CONFIG_USB_HCI
+#include <usb_osintf.h>
+#include <usb_ops.h>
+#endif
+#ifdef CONFIG_SDIO_HCI
+#include <sdio_osintf.h>
+#endif
+
+#ifdef CONFIG_GSPI_HCI
+#include <gspi_osintf.h>
+#endif
+
+extern void indicate_wx_scan_complete_event(_adapter *padapter);
+
+#define IS_MAC_ADDRESS_BROADCAST(addr) \
+( \
+	( (addr[0] == 0xff) && (addr[1] == 0xff) && \
+		(addr[2] == 0xff) && (addr[3] == 0xff) && \
+		(addr[4] == 0xff) && (addr[5] == 0xff) )  ? _TRUE : _FALSE \
+)
+
+u8 rtw_validate_ssid(NDIS_802_11_SSID *ssid)
+{
+	u8	 i;
+	u8	ret=_TRUE;
+
+_func_enter_;	
+
+	if (ssid->SsidLength > 32) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("ssid length >32\n"));
+		ret= _FALSE;
+		goto exit;
+	}
+
+	for(i = 0; i < ssid->SsidLength; i++)
+	{
+		//wifi, printable ascii code must be supported
+		if(!( (ssid->Ssid[i] >= 0x20) && (ssid->Ssid[i] <= 0x7e) )){
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("ssid has nonprintabl ascii\n"));
+			ret= _FALSE;
+			break;
+		}
+	}
+
+exit:	
+
+_func_exit_;
+
+	return ret;
+}
+
+u8 rtw_do_join(_adapter * padapter);
+u8 rtw_do_join(_adapter * padapter)
+{
+	_irqL	irqL;
+	_list	*plist, *phead;
+	u8* pibss = NULL;
+	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	_queue	*queue	= &(pmlmepriv->scanned_queue);
+	u8 ret=_SUCCESS;
+
+_func_enter_;
+
+	_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+	phead = get_list_head(queue);
+	plist = get_next(phead);
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("\n rtw_do_join: phead = %p; plist = %p \n\n\n", phead, plist));
+
+	pmlmepriv->cur_network.join_res = -2;
+		
+	set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+	pmlmepriv->pscanned = plist;
+
+	pmlmepriv->to_join = _TRUE;
+
+	if(_rtw_queue_empty(queue)== _TRUE)
+	{	
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+            		
+		//when set_ssid/set_bssid for rtw_do_join(), but scanning queue is empty
+		//we try to issue sitesurvey firstly	
+            		
+		if (pmlmepriv->LinkDetectInfo.bBusyTraffic ==_FALSE
+			|| rtw_to_roaming(padapter) > 0
+		)
+		{
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_do_join(): site survey if scanned_queue is empty\n."));
+			// submit site_survey_cmd
+			if(_SUCCESS!=(ret=rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0)) ) {
+				pmlmepriv->to_join = _FALSE;
+				RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("rtw_do_join(): site survey return error\n."));
+			}
+		}
+		else
+		{
+			pmlmepriv->to_join = _FALSE;
+			ret = _FAIL;
+		}
+		
+		goto exit;
+	}	
+	else 	
+	{
+		int select_ret;
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+		if((select_ret=rtw_select_and_join_from_scanned_queue(pmlmepriv))==_SUCCESS)
+		{
+			pmlmepriv->to_join = _FALSE;
+			_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+		}
+		else	
+		{
+			if(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)==_TRUE)
+			{
+				// submit createbss_cmd to change to a ADHOC_MASTER
+
+ 				//pmlmepriv->lock has been acquired by caller...
+				WLAN_BSSID_EX    *pdev_network = &(padapter->registrypriv.dev_network);
+
+				pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+				
+				pibss = padapter->registrypriv.dev_network.MacAddress;
+
+				_rtw_memset(&pdev_network->Ssid, 0, sizeof(NDIS_802_11_SSID));
+				_rtw_memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(NDIS_802_11_SSID));
+	
+				rtw_update_registrypriv_dev_network(padapter);
+
+				rtw_generate_random_ibss(pibss);
+					
+				if(rtw_createbss_cmd(padapter)!=_SUCCESS)
+				{
+					RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("***Error=>do_goin: rtw_createbss_cmd status FAIL*** \n "));						
+					ret =  _FALSE;
+					goto exit;
+				}
+
+			     	pmlmepriv->to_join = _FALSE;
+
+				RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("***Error=> rtw_select_and_join_from_scanned_queue FAIL under STA_Mode*** \n "));						
+
+			}			
+			else
+			{ 
+				// can't associate ; reset under-linking			
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+#if 0	
+				if((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE))
+				{
+					if(_rtw_memcmp(pmlmepriv->cur_network.network.Ssid.Ssid, pmlmepriv->assoc_ssid.Ssid, pmlmepriv->assoc_ssid.SsidLength))
+					{ 
+						// for funk to do roaming
+						// funk will reconnect, but funk will not sitesurvey before reconnect
+						RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("for funk to do roaming"));
+						if(pmlmepriv->sitesurveyctrl.traffic_busy==_FALSE)
+							rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0);
+					}
+				
+				}				
+#endif
+
+				//when set_ssid/set_bssid for rtw_do_join(), but there are no desired bss in scanning queue
+				//we try to issue sitesurvey firstly			
+				if(pmlmepriv->LinkDetectInfo.bBusyTraffic==_FALSE
+					|| rtw_to_roaming(padapter) > 0
+				)
+				{
+					//DBG_871X("rtw_do_join() when   no desired bss in scanning queue \n");
+					if( _SUCCESS!=(ret=rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0)) ){
+						pmlmepriv->to_join = _FALSE;
+						RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("do_join(): site survey return error\n."));
+					}
+				}				
+				else
+				{
+					ret = _FAIL;
+					pmlmepriv->to_join = _FALSE;
+				}
+			}
+
+		}
+
+	}
+	
+exit:
+	
+_func_exit_;	
+
+	return ret;	
+}
+
+#ifdef PLATFORM_WINDOWS
+u8 rtw_pnp_set_power_wakeup(_adapter* padapter)
+{
+	u8 res=_SUCCESS;
+
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("==>rtw_pnp_set_power_wakeup!!!\n"));
+	
+	res = rtw_setstandby_cmd(padapter, 0);
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("<==rtw_pnp_set_power_wakeup!!!\n"));
+
+_func_exit_;
+	
+	return res;
+}
+
+u8 rtw_pnp_set_power_sleep(_adapter* padapter)
+{
+	u8 res=_SUCCESS;	
+	
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("==>rtw_pnp_set_power_sleep!!!\n"));
+	//DbgPrint("+rtw_pnp_set_power_sleep\n");
+
+	res = rtw_setstandby_cmd(padapter, 1);
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("<==rtw_pnp_set_power_sleep!!!\n"));
+
+_func_exit_;
+
+	return res;
+}
+
+u8 rtw_set_802_11_reload_defaults(_adapter * padapter, NDIS_802_11_RELOAD_DEFAULTS reloadDefaults)
+{
+_func_enter_;
+
+	switch( reloadDefaults)
+	{
+		case Ndis802_11ReloadWEPKeys:
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("SetInfo OID_802_11_RELOAD_DEFAULTS : Ndis802_11ReloadWEPKeys\n"));
+			break;
+	}
+
+	// SecClearAllKeys(Adapter);
+	// 8711 CAM was not for En/Decrypt only
+	// so, we can't clear all keys.
+	// should we disable WPAcfg (ox0088) bit 1-2, instead of clear all CAM
+	
+	//TO DO...
+
+_func_exit_;
+	
+	return _TRUE;
+}
+
+u8 set_802_11_test(_adapter* padapter, NDIS_802_11_TEST *test)
+{
+	u8 ret=_TRUE;
+	
+_func_enter_;
+
+	switch(test->Type)
+	{
+		case 1:
+			NdisMIndicateStatus(padapter->hndis_adapter, NDIS_STATUS_MEDIA_SPECIFIC_INDICATION, (PVOID)&test->AuthenticationEvent, test->Length - 8);
+			NdisMIndicateStatusComplete(padapter->hndis_adapter);
+			break;
+
+		case 2:
+			NdisMIndicateStatus(padapter->hndis_adapter, NDIS_STATUS_MEDIA_SPECIFIC_INDICATION, (PVOID)&test->RssiTrigger, sizeof(NDIS_802_11_RSSI));
+			NdisMIndicateStatusComplete(padapter->hndis_adapter);
+			break;
+
+		default:
+			ret=_FALSE;
+			break;
+	}
+
+_func_exit_;
+
+	return ret;	
+}
+
+u8	rtw_set_802_11_pmkid(_adapter*	padapter, NDIS_802_11_PMKID *pmkid)
+{
+	u8	ret=_SUCCESS;
+
+	return ret;
+}
+
+#endif
+
+u8 rtw_set_802_11_bssid(_adapter* padapter, u8 *bssid)
+{	
+	_irqL irqL;	
+	u8 status=_SUCCESS;
+	u32 cur_time = 0;
+
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	
+_func_enter_;
+	
+	DBG_871X_LEVEL(_drv_always_, "set bssid:%pM\n", bssid);
+
+	if ((bssid[0]==0x00 && bssid[1]==0x00 && bssid[2]==0x00 && bssid[3]==0x00 && bssid[4]==0x00 &&bssid[5]==0x00) ||
+	    (bssid[0]==0xFF && bssid[1]==0xFF && bssid[2]==0xFF && bssid[3]==0xFF && bssid[4]==0xFF &&bssid[5]==0xFF))
+	{
+		status = _FAIL;
+		goto exit;
+	}
+		
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+
+	DBG_871X("Set BSSID under fw_state=0x%08x\n", get_fwstate(pmlmepriv));
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == _TRUE) {
+		goto handle_tkip_countermeasure;
+	} else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == _TRUE) {
+		goto release_mlme_lock;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE) == _TRUE)
+	{
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_bssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n"));
+
+		if (_rtw_memcmp(&pmlmepriv->cur_network.network.MacAddress, bssid, ETH_ALEN) == _TRUE)
+		{		
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _FALSE)
+				goto release_mlme_lock;//it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again.
+		} else {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("Set BSSID not the same bssid\n"));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("set_bssid="MAC_FMT"\n", MAC_ARG(bssid) ));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("cur_bssid="MAC_FMT"\n", MAC_ARG(pmlmepriv->cur_network.network.MacAddress) ));
+
+			rtw_disassoc_cmd(padapter, 0, _TRUE);
+
+			if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+				rtw_indicate_disconnect(padapter);
+
+			rtw_free_assoc_resources(padapter, 1);
+
+			if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE)) {
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			}		
+		}
+	}
+
+handle_tkip_countermeasure:
+	//should we add something here...?
+
+#ifdef PLATFORM_LINUX
+	if (padapter->securitypriv.btkip_countermeasure == _TRUE) {
+		cur_time = rtw_get_current_time();
+
+		if( (cur_time - padapter->securitypriv.btkip_countermeasure_time) > 60 * HZ )
+		{
+			padapter->securitypriv.btkip_countermeasure = _FALSE;
+			padapter->securitypriv.btkip_countermeasure_time = 0;
+		}
+		else
+		{
+			status = _FAIL;
+			goto release_mlme_lock;
+		}
+	}
+#endif
+
+	_rtw_memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN);
+	pmlmepriv->assoc_by_bssid=_TRUE;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == _TRUE) {
+		pmlmepriv->to_join = _TRUE;	
+	}
+	else {
+		status = rtw_do_join(padapter);
+	}
+
+release_mlme_lock:
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+		
+exit:
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+		("rtw_set_802_11_bssid: status=%d\n", status));
+	
+_func_exit_;
+
+	return status;
+}
+
+u8 rtw_set_802_11_ssid(_adapter* padapter, NDIS_802_11_SSID *ssid)
+{	
+	_irqL irqL;
+	u8 status = _SUCCESS;
+	u32 cur_time = 0;
+
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *pnetwork = &pmlmepriv->cur_network;
+	
+_func_enter_;
+	
+	DBG_871X_LEVEL(_drv_always_, "set ssid [%s] fw_state=0x%08x\n",
+		       	ssid->Ssid, get_fwstate(pmlmepriv));
+
+	if(padapter->hw_init_completed==_FALSE){
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+			 ("set_ssid: hw_init_completed==_FALSE=>exit!!!\n"));
+		status = _FAIL;
+		goto exit;
+	}
+		
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	DBG_871X("Set SSID under fw_state=0x%08x\n", get_fwstate(pmlmepriv));
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == _TRUE) {	
+		goto handle_tkip_countermeasure;
+	} else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == _TRUE) {
+		goto release_mlme_lock;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE) == _TRUE)
+	{
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+			 ("set_ssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n"));
+
+		if ((pmlmepriv->assoc_ssid.SsidLength == ssid->SsidLength) &&
+		    (_rtw_memcmp(&pmlmepriv->assoc_ssid.Ssid, ssid->Ssid, ssid->SsidLength) == _TRUE))
+		{			
+			if((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _FALSE))
+			{
+				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+					 ("Set SSID is the same ssid, fw_state=0x%08x\n",
+					  get_fwstate(pmlmepriv)));
+
+				if(rtw_is_same_ibss(padapter, pnetwork) == _FALSE)
+				{				
+					//if in WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE, create bss or rejoin again
+					rtw_disassoc_cmd(padapter, 0, _TRUE);
+
+					if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+						rtw_indicate_disconnect(padapter);
+						
+					rtw_free_assoc_resources(padapter, 1);
+
+					if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE) {
+						_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+						set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+					}
+				}
+				else
+				{
+					goto release_mlme_lock;//it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again.
+				}
+			}
+#ifdef CONFIG_LPS
+			else {
+				rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_JOINBSS, 1);
+			}
+#endif
+		}
+		else
+		{
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("Set SSID not the same ssid\n"));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("set_ssid=[%s] len=0x%x\n", ssid->Ssid, (unsigned int)ssid->SsidLength));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("assoc_ssid=[%s] len=0x%x\n", pmlmepriv->assoc_ssid.Ssid, (unsigned int)pmlmepriv->assoc_ssid.SsidLength));
+
+			rtw_disassoc_cmd(padapter, 0, _TRUE);
+
+			if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+				rtw_indicate_disconnect(padapter);
+			
+			rtw_free_assoc_resources(padapter, 1);
+
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE) {
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+		}		
+	}
+
+handle_tkip_countermeasure:
+#ifdef PLATFORM_WINDOWS
+	if (padapter->securitypriv.btkip_countermeasure==_TRUE)
+	{
+		LARGE_INTEGER	sys_time;
+		u32  diff_time,cur_time ;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_ssid:padapter->securitypriv.btkip_countermeasure==_TRUE\n"));
+		NdisGetCurrentSystemTime(&sys_time);	
+		cur_time=(u32)(sys_time.QuadPart/10);  // In micro-second.
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_ssid:cur_time=0x%x\n",cur_time));
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_ssid:psecuritypriv->last_mic_err_time=0x%x\n",padapter->securitypriv.btkip_countermeasure_time));
+		diff_time = cur_time -padapter->securitypriv.btkip_countermeasure_time; // In micro-second.
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_ssid:diff_time=0x%x\n",diff_time));
+
+		if (diff_time > 60000000) {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_ssid(): countermeasure time >60s.\n"));
+			padapter->securitypriv.btkip_countermeasure=_FALSE;
+			// Update MIC error time.
+			padapter->securitypriv.btkip_countermeasure_time=0;
+		} else {
+			// can't join  in 60 seconds.
+			status = _FAIL;
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_ssid(): countermeasure time <60s.\n"));
+			goto release_mlme_lock;
+		}
+	}
+#endif
+
+#ifdef PLATFORM_LINUX
+	if (padapter->securitypriv.btkip_countermeasure == _TRUE) {
+		cur_time = rtw_get_current_time();
+
+		if( (cur_time - padapter->securitypriv.btkip_countermeasure_time) > 60 * HZ )
+		{
+			padapter->securitypriv.btkip_countermeasure = _FALSE;
+			padapter->securitypriv.btkip_countermeasure_time = 0;
+		}
+		else
+		{
+			status = _FAIL;
+			goto release_mlme_lock;
+		}
+	}
+#endif
+
+	#ifdef CONFIG_VALIDATE_SSID
+	if (rtw_validate_ssid(ssid) == _FALSE) {
+		status = _FAIL;
+		goto release_mlme_lock;
+	}
+	#endif
+
+	_rtw_memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(NDIS_802_11_SSID));
+	pmlmepriv->assoc_by_bssid=_FALSE;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == _TRUE) {
+		pmlmepriv->to_join = _TRUE;	
+	}
+	else {
+		status = rtw_do_join(padapter);
+	}
+
+release_mlme_lock:
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+exit:
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+		("-rtw_set_802_11_ssid: status=%d\n", status));
+	
+_func_exit_;
+
+	return status;
+	
+}
+
+u8 rtw_set_802_11_infrastructure_mode(_adapter* padapter, 
+	NDIS_802_11_NETWORK_INFRASTRUCTURE networktype)
+{
+	_irqL irqL;
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct	wlan_network	*cur_network = &pmlmepriv->cur_network;
+	NDIS_802_11_NETWORK_INFRASTRUCTURE* pold_state = &(cur_network->network.InfrastructureMode);
+	
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_notice_,
+		 ("+rtw_set_802_11_infrastructure_mode: old=%d new=%d fw_state=0x%08x\n",
+		  *pold_state, networktype, get_fwstate(pmlmepriv)));
+	
+	if(*pold_state != networktype)
+	{
+		_enter_critical_bh(&pmlmepriv->lock, &irqL);
+		
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,(" change mode!"));
+		//DBG_871X("change mode, old_mode=%d, new_mode=%d, fw_state=0x%x\n", *pold_state, networktype, get_fwstate(pmlmepriv));
+
+		if(*pold_state==Ndis802_11APMode)
+		{		
+			//change to other mode from Ndis802_11APMode			
+			cur_network->join_res = -1;
+			
+#ifdef CONFIG_NATIVEAP_MLME
+			stop_ap_mode(padapter);
+#endif
+		}
+
+		if((check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE) ||(*pold_state==Ndis802_11IBSS))
+			rtw_disassoc_cmd(padapter, 0, _TRUE);
+
+		if((check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE) ||
+			(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)== _TRUE) )
+			rtw_free_assoc_resources(padapter, 1);
+
+		if((*pold_state == Ndis802_11Infrastructure) ||(*pold_state == Ndis802_11IBSS))
+	       {
+			if(check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+			{		
+				rtw_indicate_disconnect(padapter); //will clr Linked_state; before this function, we must have chked whether  issue dis-assoc_cmd or not
+			}
+	       }
+		
+		*pold_state = networktype;
+
+		_clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE);
+				
+		switch(networktype)
+		{
+			case Ndis802_11IBSS:
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+				break;
+				
+			case Ndis802_11Infrastructure:
+				set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+				break;
+				
+			case Ndis802_11APMode:
+				set_fwstate(pmlmepriv, WIFI_AP_STATE);
+#ifdef CONFIG_NATIVEAP_MLME
+				start_ap_mode(padapter);
+				//rtw_indicate_connect(padapter);
+#endif				
+				
+				break;
+
+			case Ndis802_11AutoUnknown:
+			case Ndis802_11InfrastructureMax:
+				break;                        				
+		}
+
+		//SecClearAllKeys(adapter);
+		
+		//RT_TRACE(COMP_OID_SET, DBG_LOUD, ("set_infrastructure: fw_state:%x after changing mode\n",
+		//									get_fwstate(pmlmepriv) ));
+
+		_exit_critical_bh(&pmlmepriv->lock, &irqL);
+	}
+
+_func_exit_;
+
+	return _TRUE;
+}
+
+
+u8 rtw_set_802_11_disassociate(_adapter *padapter)
+{
+	_irqL irqL;
+	struct mlme_priv * pmlmepriv = &padapter->mlmepriv;
+
+_func_enter_;
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+	{
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("MgntActrtw_set_802_11_disassociate: rtw_indicate_disconnect\n"));
+
+		rtw_disassoc_cmd(padapter, 0, _TRUE);
+		rtw_indicate_disconnect(padapter);
+		rtw_free_assoc_resources(padapter, 1);	
+		rtw_pwr_wakeup(padapter);		
+	}
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+	
+_func_exit_;
+
+	return _TRUE;	
+}
+
+u8 rtw_set_802_11_bssid_list_scan(_adapter* padapter, NDIS_802_11_SSID *pssid, int ssid_max_num)
+{	
+	_irqL	irqL;
+	struct	mlme_priv		*pmlmepriv= &padapter->mlmepriv;
+	u8	res=_TRUE;
+	
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("+rtw_set_802_11_bssid_list_scan(), fw_state=%x\n", get_fwstate(pmlmepriv)));
+
+	if (padapter == NULL) {
+		res=_FALSE;
+		goto exit;
+	}
+	if (padapter->hw_init_completed==_FALSE){
+		res = _FALSE;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("\n===rtw_set_802_11_bssid_list_scan:hw_init_completed==_FALSE===\n"));
+		goto exit;
+	}
+	
+	if ((check_fwstate(pmlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == _TRUE) ||
+		(pmlmepriv->LinkDetectInfo.bBusyTraffic == _TRUE))
+	{
+		// Scan or linking is in progress, do nothing.
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("rtw_set_802_11_bssid_list_scan fail since fw_state = %x\n", get_fwstate(pmlmepriv)));
+		res = _TRUE;
+
+		if(check_fwstate(pmlmepriv, (_FW_UNDER_SURVEY|_FW_UNDER_LINKING))== _TRUE){
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("\n###_FW_UNDER_SURVEY|_FW_UNDER_LINKING\n\n"));
+		} else {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("\n###pmlmepriv->sitesurveyctrl.traffic_busy==_TRUE\n\n"));
+		}
+	} else {		
+		if (rtw_is_scan_deny(padapter)) {
+			DBG_871X(FUNC_ADPT_FMT": scan deny\n", FUNC_ADPT_ARG(padapter));
+			indicate_wx_scan_complete_event(padapter);
+			return _SUCCESS;
+		}
+		
+		_enter_critical_bh(&pmlmepriv->lock, &irqL);		
+		
+		res = rtw_sitesurvey_cmd(padapter, pssid, ssid_max_num, NULL, 0);
+		
+		_exit_critical_bh(&pmlmepriv->lock, &irqL);
+	}
+exit:
+	
+_func_exit_;
+
+	return res;	
+}
+
+u8 rtw_set_802_11_authentication_mode(_adapter* padapter, NDIS_802_11_AUTHENTICATION_MODE authmode) 
+{
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	int res;
+	u8 ret;
+	
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("set_802_11_auth.mode(): mode=%x\n", authmode));
+
+	psecuritypriv->ndisauthtype=authmode;
+	
+	RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_authentication_mode:psecuritypriv->ndisauthtype=%d", psecuritypriv->ndisauthtype));
+	
+	if(psecuritypriv->ndisauthtype>3)
+		psecuritypriv->dot11AuthAlgrthm=dot11AuthAlgrthm_8021X;
+	
+#ifdef CONFIG_WAPI_SUPPORT
+	if(psecuritypriv->ndisauthtype == 6)
+		psecuritypriv->dot11AuthAlgrthm=dot11AuthAlgrthm_WAPI;
+#endif
+
+	res=rtw_set_auth(padapter,psecuritypriv);
+	
+	if(res==_SUCCESS)
+		ret=_TRUE;
+	else
+		ret=_FALSE;
+	
+_func_exit_;
+
+	return ret;
+}
+
+u8 rtw_set_802_11_add_wep(_adapter* padapter, NDIS_802_11_WEP *wep){
+
+	u8		bdefaultkey;
+	u8		btransmitkey;
+	sint		keyid,res;
+	struct security_priv* psecuritypriv=&(padapter->securitypriv);
+	u8		ret=_SUCCESS;
+
+_func_enter_;
+
+	bdefaultkey=(wep->KeyIndex & 0x40000000) > 0 ? _FALSE : _TRUE;   //for ???
+	btransmitkey= (wep->KeyIndex & 0x80000000) > 0 ? _TRUE  : _FALSE;	//for ???
+	keyid=wep->KeyIndex & 0x3fffffff;
+
+	if(keyid>4)
+	{
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("MgntActrtw_set_802_11_add_wep:keyid>4=>fail\n"));
+		ret=_FALSE;
+		goto exit;
+	}
+	
+	switch(wep->KeyLength)
+	{
+		case 5:
+			psecuritypriv->dot11PrivacyAlgrthm=_WEP40_;
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("MgntActrtw_set_802_11_add_wep:wep->KeyLength=5\n"));
+			break;
+		case 13:
+			psecuritypriv->dot11PrivacyAlgrthm=_WEP104_;
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("MgntActrtw_set_802_11_add_wep:wep->KeyLength=13\n"));
+			break;
+		default:
+			psecuritypriv->dot11PrivacyAlgrthm=_NO_PRIVACY_;
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("MgntActrtw_set_802_11_add_wep:wep->KeyLength!=5 or 13\n"));
+			break;
+	}
+	
+	RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_add_wep:befor memcpy, wep->KeyLength=0x%x wep->KeyIndex=0x%x  keyid =%x\n",wep->KeyLength,wep->KeyIndex,keyid));
+
+	_rtw_memcpy(&(psecuritypriv->dot11DefKey[keyid].skey[0]),&(wep->KeyMaterial),wep->KeyLength);
+
+	psecuritypriv->dot11DefKeylen[keyid]=wep->KeyLength;
+
+	psecuritypriv->dot11PrivacyKeyIndex=keyid;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_add_wep:security key material : %x %x %x %x %x %x %x %x %x %x %x %x %x \n",
+		psecuritypriv->dot11DefKey[keyid].skey[0],psecuritypriv->dot11DefKey[keyid].skey[1],psecuritypriv->dot11DefKey[keyid].skey[2],
+		psecuritypriv->dot11DefKey[keyid].skey[3],psecuritypriv->dot11DefKey[keyid].skey[4],psecuritypriv->dot11DefKey[keyid].skey[5],
+		psecuritypriv->dot11DefKey[keyid].skey[6],psecuritypriv->dot11DefKey[keyid].skey[7],psecuritypriv->dot11DefKey[keyid].skey[8],
+		psecuritypriv->dot11DefKey[keyid].skey[9],psecuritypriv->dot11DefKey[keyid].skey[10],psecuritypriv->dot11DefKey[keyid].skey[11],
+		psecuritypriv->dot11DefKey[keyid].skey[12]));
+
+	res=rtw_set_key(padapter,psecuritypriv, keyid, 1);
+	
+	if(res==_FAIL)
+		ret= _FALSE;
+exit:
+	
+_func_exit_;
+
+	return ret;
+	
+}
+
+u8 rtw_set_802_11_remove_wep(_adapter* padapter, u32 keyindex){
+	
+	u8 ret=_SUCCESS;
+	
+_func_enter_;
+
+	if (keyindex >= 0x80000000 || padapter == NULL){
+		
+		ret=_FALSE;
+		goto exit;
+
+	}
+	else 
+	{
+		int res;
+		struct security_priv* psecuritypriv=&(padapter->securitypriv);
+		if( keyindex < 4 ){
+			
+			_rtw_memset(&psecuritypriv->dot11DefKey[keyindex], 0, 16);
+			
+			res=rtw_set_key(padapter,psecuritypriv,keyindex, 0);
+			
+			psecuritypriv->dot11DefKeylen[keyindex]=0;
+			
+			if(res==_FAIL)
+				ret=_FAIL;
+			
+		}
+		else
+		{			
+			ret=_FAIL;
+		}
+		
+	}
+	
+exit:	
+	
+_func_exit_;
+
+	return ret;
+	
+}
+
+u8 rtw_set_802_11_add_key(_adapter* padapter, NDIS_802_11_KEY *key){
+
+	uint	encryptionalgo;
+	u8 * pbssid;
+	struct sta_info *stainfo;
+	u8	bgroup = _FALSE;
+	u8	bgrouptkey = _FALSE;//can be remove later
+	u8	ret=_SUCCESS;
+	
+_func_enter_;
+
+	if (((key->KeyIndex & 0x80000000) == 0) && ((key->KeyIndex & 0x40000000) > 0)){
+
+		// It is invalid to clear bit 31 and set bit 30. If the miniport driver encounters this combination, 
+		// it must fail the request and return NDIS_STATUS_INVALID_DATA.
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_add_key: ((key->KeyIndex & 0x80000000) == 0)[=%d] ",(int)(key->KeyIndex & 0x80000000) == 0));
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_add_key:((key->KeyIndex & 0x40000000) > 0)[=%d]" , (int)(key->KeyIndex & 0x40000000) > 0));
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_info_,("rtw_set_802_11_add_key: key->KeyIndex=%d \n" ,(int)key->KeyIndex));
+		ret= _FAIL;
+		goto exit;
+	}
+
+	if(key->KeyIndex & 0x40000000)
+	{ 
+		// Pairwise key
+
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("OID_802_11_ADD_KEY: +++++ Pairwise key +++++\n"));
+	
+		pbssid=get_bssid(&padapter->mlmepriv);
+		stainfo=rtw_get_stainfo(&padapter->stapriv, pbssid);
+
+		if((stainfo!=NULL)&&(padapter->securitypriv.dot11AuthAlgrthm==dot11AuthAlgrthm_8021X)){
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("OID_802_11_ADD_KEY:( stainfo!=NULL)&&(Adapter->securitypriv.dot11AuthAlgrthm==dot11AuthAlgrthm_8021X)\n"));
+			encryptionalgo=stainfo->dot118021XPrivacy;
+		}
+		else{
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("OID_802_11_ADD_KEY: stainfo==NULL)||(Adapter->securitypriv.dot11AuthAlgrthm!=dot11AuthAlgrthm_8021X)\n"));
+			encryptionalgo=padapter->securitypriv.dot11PrivacyAlgrthm;
+		}
+
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("rtw_set_802_11_add_key: (encryptionalgo ==%d)!\n",encryptionalgo ));
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("rtw_set_802_11_add_key: (Adapter->securitypriv.dot11PrivacyAlgrthm ==%d)!\n",padapter->securitypriv.dot11PrivacyAlgrthm));
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("rtw_set_802_11_add_key: (Adapter->securitypriv.dot11AuthAlgrthm ==%d)!\n",padapter->securitypriv.dot11AuthAlgrthm));
+
+		if((stainfo!=NULL)){
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("rtw_set_802_11_add_key: (stainfo->dot118021XPrivacy ==%d)!\n", stainfo->dot118021XPrivacy));
+		}
+		
+		if(key->KeyIndex & 0x000000FF){
+			// The key index is specified in the lower 8 bits by values of zero to 255.
+			// The key index should be set to zero for a Pairwise key, and the driver should fail with
+			// NDIS_STATUS_INVALID_DATA if the lower 8 bits is not zero
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,(" key->KeyIndex & 0x000000FF.\n"));
+			ret= _FAIL;
+			goto exit;
+		}
+
+		// check BSSID
+		if (IS_MAC_ADDRESS_BROADCAST(key->BSSID) == _TRUE){
+
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("MacAddr_isBcst(key->BSSID)\n"));
+			ret= _FALSE;
+			goto exit;
+		}
+
+		// Check key length for TKIP.
+		//if(encryptionAlgorithm == RT_ENC_TKIP_ENCRYPTION && key->KeyLength != 32)
+		if((encryptionalgo== _TKIP_)&& (key->KeyLength != 32)){
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("TKIP KeyLength:0x%x != 32\n", key->KeyLength));
+			ret=_FAIL;
+			goto exit;
+
+		}
+
+		// Check key length for AES.
+		if((encryptionalgo== _AES_)&& (key->KeyLength != 16)) {
+			// For our supplicant, EAPPkt9x.vxd, cannot differentiate TKIP and AES case.
+			if(key->KeyLength == 32) {
+				key->KeyLength = 16; 
+			} else {
+				ret= _FAIL;
+				goto exit;
+			}
+		}
+
+		// Check key length for WEP. For NDTEST, 2005.01.27, by rcnjko.
+		if(	(encryptionalgo== _WEP40_|| encryptionalgo== _WEP104_) && (key->KeyLength != 5 || key->KeyLength != 13)) {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("WEP KeyLength:0x%x != 5 or 13\n", key->KeyLength));
+			ret=_FAIL;
+			goto exit;
+		}
+
+		bgroup = _FALSE;
+
+		// Check the pairwise key. Added by Annie, 2005-07-06.
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("------------------------------------------\n"));
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("[Pairwise Key set]\n"));
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("------------------------------------------\n"));
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("key index: 0x%8x(0x%8x)\n", key->KeyIndex,(key->KeyIndex&0x3)));
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("key Length: %d\n", key->KeyLength));
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("------------------------------------------\n"));
+	
+	}
+	else 
+	{	
+		// Group key - KeyIndex(BIT30==0)
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("OID_802_11_ADD_KEY: +++++ Group key +++++\n"));
+
+
+		// when add wep key through add key and didn't assigned encryption type before
+		if((padapter->securitypriv.ndisauthtype<=3)&&(padapter->securitypriv.dot118021XGrpPrivacy==0))
+		{
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("keylen=%d( Adapter->securitypriv.dot11PrivacyAlgrthm=%x  )padapter->securitypriv.dot118021XGrpPrivacy(%x)\n", key->KeyLength,padapter->securitypriv.dot11PrivacyAlgrthm,padapter->securitypriv.dot118021XGrpPrivacy));
+
+			switch(key->KeyLength)
+			{
+				case 5:
+					padapter->securitypriv.dot11PrivacyAlgrthm=_WEP40_;
+					RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("Adapter->securitypriv.dot11PrivacyAlgrthm= %x key->KeyLength=%u\n", padapter->securitypriv.dot11PrivacyAlgrthm,key->KeyLength));
+					break;
+				case 13:
+					padapter->securitypriv.dot11PrivacyAlgrthm=_WEP104_;
+					RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("Adapter->securitypriv.dot11PrivacyAlgrthm= %x key->KeyLength=%u\n", padapter->securitypriv.dot11PrivacyAlgrthm,key->KeyLength));
+					break;
+				default:
+					padapter->securitypriv.dot11PrivacyAlgrthm=_NO_PRIVACY_;
+					RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("Adapter->securitypriv.dot11PrivacyAlgrthm= %x key->KeyLength=%u \n", padapter->securitypriv.dot11PrivacyAlgrthm,key->KeyLength));
+					break;
+			}
+			
+			encryptionalgo=padapter->securitypriv.dot11PrivacyAlgrthm;
+			
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,(" Adapter->securitypriv.dot11PrivacyAlgrthm=%x\n", padapter->securitypriv.dot11PrivacyAlgrthm));
+			
+		}
+		else 
+		{
+			encryptionalgo=padapter->securitypriv.dot118021XGrpPrivacy;
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("( Adapter->securitypriv.dot11PrivacyAlgrthm=%x  )encryptionalgo(%x)=padapter->securitypriv.dot118021XGrpPrivacy(%x)keylen=%d\n", padapter->securitypriv.dot11PrivacyAlgrthm,encryptionalgo,padapter->securitypriv.dot118021XGrpPrivacy,key->KeyLength));
+
+		}
+		
+		if((check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE)==_TRUE) && (IS_MAC_ADDRESS_BROADCAST(key->BSSID) == _FALSE)) {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,(" IBSS but BSSID is not Broadcast Address.\n"));
+			ret= _FAIL;
+			goto exit;
+		}
+
+		// Check key length for TKIP
+		if((encryptionalgo== _TKIP_) && (key->KeyLength != 32)) {
+
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,(" TKIP GTK KeyLength:%u != 32\n", key->KeyLength));
+			ret= _FAIL;
+			goto exit;
+
+		} else if(encryptionalgo== _AES_ && (key->KeyLength != 16 && key->KeyLength != 32) ) {
+			
+			// Check key length for AES
+			// For NDTEST, we allow keylen=32 in this case. 2005.01.27, by rcnjko.
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("<=== SetInfo, OID_802_11_ADD_KEY: AES GTK KeyLength:%u != 16 or 32\n", key->KeyLength));
+			ret= _FAIL;
+			goto exit;
+		}
+
+		// Change the key length for EAPPkt9x.vxd. Added by Annie, 2005-11-03.
+		if((encryptionalgo==  _AES_) && (key->KeyLength == 32) ) {
+			key->KeyLength = 16; 
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("AES key length changed: %u\n", key->KeyLength) );
+		}
+
+		if(key->KeyIndex & 0x8000000) {//error ??? 0x8000_0000
+			bgrouptkey = _TRUE;
+		}
+
+		if((check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE)==_TRUE)&&(check_fwstate(&padapter->mlmepriv, _FW_LINKED)==_TRUE))
+		{
+			bgrouptkey = _TRUE;
+		}
+
+		bgroup = _TRUE;
+
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("------------------------------------------\n") );
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("[Group Key set]\n") );
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("------------------------------------------\n")) ;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("key index: 0x%8x(0x%8x)\n", key->KeyIndex,(key->KeyIndex&0x3)));
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("key Length: %d\n", key->KeyLength)) ;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("------------------------------------------\n"));
+		
+	}	
+
+	// If WEP encryption algorithm, just call rtw_set_802_11_add_wep().
+	if((padapter->securitypriv.dot11AuthAlgrthm !=dot11AuthAlgrthm_8021X)&&(encryptionalgo== _WEP40_  || encryptionalgo== _WEP104_))
+	{
+		u8 ret;		
+		u32 keyindex;		
+		u32 len = FIELD_OFFSET(NDIS_802_11_KEY, KeyMaterial) + key->KeyLength;
+		NDIS_802_11_WEP *wep = &padapter->securitypriv.ndiswep;
+				
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("OID_802_11_ADD_KEY: +++++ WEP key +++++\n"));
+
+		wep->Length = len;
+		keyindex = key->KeyIndex&0x7fffffff;
+		wep->KeyIndex = keyindex ;
+		wep->KeyLength = key->KeyLength;
+		
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("OID_802_11_ADD_KEY:Before memcpy \n"));
+
+		_rtw_memcpy(wep->KeyMaterial, key->KeyMaterial, key->KeyLength);	
+		_rtw_memcpy(&(padapter->securitypriv.dot11DefKey[keyindex].skey[0]), key->KeyMaterial, key->KeyLength);
+
+		padapter->securitypriv.dot11DefKeylen[keyindex]=key->KeyLength;		
+		padapter->securitypriv.dot11PrivacyKeyIndex=keyindex;
+		
+		ret = rtw_set_802_11_add_wep(padapter, wep);
+	
+		goto exit;
+		
+	}
+
+	if(key->KeyIndex & 0x20000000){
+		// SetRSC
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("OID_802_11_ADD_KEY: +++++ SetRSC+++++\n"));
+		if(bgroup == _TRUE)
+		{
+			NDIS_802_11_KEY_RSC keysrc=key->KeyRSC & 0x00FFFFFFFFFFFFULL;
+			_rtw_memcpy(&padapter->securitypriv.dot11Grprxpn, &keysrc, 8);			
+		} 
+		else 
+		{		
+			NDIS_802_11_KEY_RSC keysrc=key->KeyRSC & 0x00FFFFFFFFFFFFULL;	
+			_rtw_memcpy(&padapter->securitypriv.dot11Grptxpn, &keysrc, 8);			
+		}
+			
+	}
+
+	// Indicate this key idx is used for TX
+	// Save the key in KeyMaterial
+	if(bgroup == _TRUE) // Group transmit key
+	{
+		int res;
+		
+		if(bgrouptkey == _TRUE)
+		{		
+			padapter->securitypriv.dot118021XGrpKeyid=(u8)key->KeyIndex;
+		}
+		
+		if((key->KeyIndex&0x3) == 0){
+			ret = _FAIL;
+			goto exit;
+		}		
+		
+		_rtw_memset(&padapter->securitypriv.dot118021XGrpKey[(u8)((key->KeyIndex) & 0x03)], 0, 16);
+		_rtw_memset(&padapter->securitypriv.dot118021XGrptxmickey[(u8)((key->KeyIndex) & 0x03)], 0, 16);
+		_rtw_memset(&padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)], 0, 16);
+		
+		if((key->KeyIndex & 0x10000000))
+		{
+			_rtw_memcpy(&padapter->securitypriv.dot118021XGrptxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 16, 8);
+			_rtw_memcpy(&padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 24, 8);
+			
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("\n rtw_set_802_11_add_key:rx mic :0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+				padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[0],padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[1],
+				padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[2],padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[3],
+				padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[4],padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[5],
+				padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[6],padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[7]));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("\n rtw_set_802_11_add_key:set Group mic key!!!!!!!!\n"));
+
+		}
+		else
+		{
+			_rtw_memcpy(&padapter->securitypriv.dot118021XGrptxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 24, 8);
+			_rtw_memcpy(&padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 16, 8);
+			
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("\n rtw_set_802_11_add_key:rx mic :0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+				padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[0],padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[1],
+				padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[2],padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[3],
+				padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[4],padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[5],
+				padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)].skey[6],padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex-1) & 0x03)].skey[7]));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("\n rtw_set_802_11_add_key:set Group mic key!!!!!!!!\n"));
+		
+		}
+
+		//set group key by index
+		_rtw_memcpy(&padapter->securitypriv.dot118021XGrpKey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial, key->KeyLength);
+		
+		key->KeyIndex=key->KeyIndex & 0x03;
+		
+		padapter->securitypriv.binstallGrpkey=_TRUE;
+		
+		padapter->securitypriv.bcheck_grpkey=_FALSE;
+		
+		RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("reset group key"));
+		
+		res=rtw_set_key(padapter,&padapter->securitypriv, key->KeyIndex, 1);
+
+		if(res==_FAIL)
+			ret= _FAIL;
+
+		goto exit;
+			
+	}
+	else // Pairwise Key
+	{
+		u8 res;
+		
+		pbssid=get_bssid(&padapter->mlmepriv);
+		stainfo=rtw_get_stainfo(&padapter->stapriv , pbssid );
+		
+		if(stainfo!=NULL)
+		{			
+			_rtw_memset( &stainfo->dot118021x_UncstKey, 0, 16);// clear keybuffer
+			
+			_rtw_memcpy(&stainfo->dot118021x_UncstKey, key->KeyMaterial, 16);
+			
+			if(encryptionalgo== _TKIP_)
+			{
+				padapter->securitypriv.busetkipkey=_FALSE;
+				
+				//_set_timer(&padapter->securitypriv.tkip_timer, 50);
+				
+				RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("\n ==========_set_timer\n"));
+				
+				// if TKIP, save the Receive/Transmit MIC key in KeyMaterial[128-255]
+				if((key->KeyIndex & 0x10000000)){
+					_rtw_memcpy(&stainfo->dot11tkiptxmickey, key->KeyMaterial + 16, 8);
+					_rtw_memcpy(&stainfo->dot11tkiprxmickey, key->KeyMaterial + 24, 8);
+
+				} else {
+					_rtw_memcpy(&stainfo->dot11tkiptxmickey, key->KeyMaterial + 24, 8);
+					_rtw_memcpy(&stainfo->dot11tkiprxmickey, key->KeyMaterial + 16, 8);
+
+				}
+		
+			}
+			else if(encryptionalgo == _AES_)
+			{		
+	
+			}
+
+		
+			//Set key to CAM through H2C command
+			if(bgrouptkey)//never go to here
+			{
+				res=rtw_setstakey_cmd(padapter, (unsigned char *)stainfo, _FALSE);
+				RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("\n rtw_set_802_11_add_key:rtw_setstakey_cmd(group)\n"));
+			}
+			else{
+				res=rtw_setstakey_cmd(padapter, (unsigned char *)stainfo, _TRUE);
+				RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("\n rtw_set_802_11_add_key:rtw_setstakey_cmd(unicast)\n"));
+			}
+			
+			if(res ==_FALSE)
+				ret= _FAIL;
+			
+		}
+
+	}
+
+exit:
+	
+_func_exit_;
+
+	return ret;	
+}
+
+u8 rtw_set_802_11_remove_key(_adapter*	padapter, NDIS_802_11_REMOVE_KEY *key){
+	
+	uint				encryptionalgo;
+	u8 * pbssid;
+	struct sta_info *stainfo;
+	u8	bgroup = (key->KeyIndex & 0x4000000) > 0 ? _FALSE: _TRUE;
+	u8	keyIndex = (u8)key->KeyIndex & 0x03;
+	u8	ret=_SUCCESS;
+	
+_func_enter_;
+
+	if ((key->KeyIndex & 0xbffffffc) > 0) {
+		ret=_FAIL;
+		goto exit;
+	}
+
+	if (bgroup == _TRUE) {
+		encryptionalgo= padapter->securitypriv.dot118021XGrpPrivacy;
+		// clear group key by index
+		//NdisZeroMemory(Adapter->MgntInfo.SecurityInfo.KeyBuf[keyIndex], MAX_WEP_KEY_LEN);
+		//Adapter->MgntInfo.SecurityInfo.KeyLen[keyIndex] = 0;
+		
+		_rtw_memset(&padapter->securitypriv.dot118021XGrpKey[keyIndex], 0, 16);
+		
+		//! \todo Send a H2C Command to Firmware for removing this Key in CAM Entry.
+	
+	} else {
+	
+		pbssid=get_bssid(&padapter->mlmepriv);
+		stainfo=rtw_get_stainfo(&padapter->stapriv , pbssid );
+		if(stainfo !=NULL){
+			encryptionalgo=stainfo->dot118021XPrivacy;
+
+		// clear key by BSSID
+		_rtw_memset(&stainfo->dot118021x_UncstKey, 0, 16);
+		
+		//! \todo Send a H2C Command to Firmware for disable this Key in CAM Entry.
+
+		}
+		else{
+			ret= _FAIL;
+			goto exit;
+		}
+	}
+
+exit:
+	
+_func_exit_;
+
+	return _TRUE;
+	
+}
+
+/*
+* rtw_get_cur_max_rate - 
+* @adapter: pointer to _adapter structure
+* 
+* Return 0 or 100Kbps
+*/
+u16 rtw_get_cur_max_rate(_adapter *adapter)
+{
+	int	i = 0;
+	u8	*p;
+	u16	rate = 0, max_rate = 0;
+	struct mlme_ext_priv	*pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	WLAN_BSSID_EX  *pcur_bss = &pmlmepriv->cur_network.network;
+#ifdef CONFIG_80211N_HT
+	struct rtw_ieee80211_ht_cap *pht_capie;
+	u8	rf_type = 0;
+	u8	bw_40MHz=0, short_GI_20=0, short_GI_40=0;
+	u16	mcs_rate=0;
+	u32	ht_ielen = 0;	
+#endif
+
+#ifdef CONFIG_MP_INCLUDED
+	if (adapter->registrypriv.mp_mode == 1)
+	{	
+		if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE)
+			return 0;
+	}
+#endif
+
+	if((check_fwstate(pmlmepriv, _FW_LINKED) != _TRUE) 
+		&& (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) != _TRUE))
+		return 0;
+
+#ifdef CONFIG_80211N_HT
+	if (pmlmeext->cur_wireless_mode & (WIRELESS_11_24N|WIRELESS_11_5N)) {
+		p = rtw_get_ie(&pcur_bss->IEs[12], _HT_CAPABILITY_IE_, &ht_ielen, pcur_bss->IELength-12);
+		if(p && ht_ielen>0)
+		{
+			pht_capie = (struct rtw_ieee80211_ht_cap *)(p+2);
+		
+			_rtw_memcpy(&mcs_rate , pht_capie->supp_mcs_set, 2);
+
+			//bw_40MHz = (pht_capie->cap_info&IEEE80211_HT_CAP_SUP_WIDTH) ? 1:0;
+			//cur_bwmod is updated by beacon, pmlmeinfo is updated by association response
+			bw_40MHz = (pmlmeext->cur_bwmode && (HT_INFO_HT_PARAM_REC_TRANS_CHNL_WIDTH & pmlmeinfo->HT_info.infos[0])) ? 1:0;
+			
+			//short_GI = (pht_capie->cap_info&(IEEE80211_HT_CAP_SGI_20|IEEE80211_HT_CAP_SGI_40)) ? 1:0;
+			short_GI_20 = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info&IEEE80211_HT_CAP_SGI_20) ? 1:0;
+			short_GI_40 = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info&IEEE80211_HT_CAP_SGI_40) ? 1:0;
+
+			rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+			max_rate = rtw_mcs_rate(
+				rf_type,
+				bw_40MHz & (pregistrypriv->cbw40_enable), 
+				short_GI_20,
+				short_GI_40,
+				pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate
+			);
+		}
+	} 
+	else
+#endif //CONFIG_80211N_HT
+	{
+		while( (pcur_bss->SupportedRates[i]!=0) && (pcur_bss->SupportedRates[i]!=0xFF))
+		{
+			rate = pcur_bss->SupportedRates[i]&0x7F;
+			if(rate>max_rate)
+				max_rate = rate;
+			i++;
+		}
+	
+		max_rate = max_rate*10/2;
+	}
+
+	return max_rate;
+}
+
+/*
+* rtw_set_scan_mode - 
+* @adapter: pointer to _adapter structure
+* @scan_mode: 
+* 
+* Return _SUCCESS or _FAIL
+*/
+int rtw_set_scan_mode(_adapter *adapter, RT_SCAN_TYPE scan_mode)
+{
+	if(scan_mode != SCAN_ACTIVE && scan_mode != SCAN_PASSIVE)
+		return _FAIL;
+	
+	adapter->mlmepriv.scan_mode = scan_mode;
+
+	return _SUCCESS;
+}
+
+/*
+* rtw_set_channel_plan - 
+* @adapter: pointer to _adapter structure
+* @channel_plan: 
+* 
+* Return _SUCCESS or _FAIL
+*/
+int rtw_set_channel_plan(_adapter *adapter, u8 channel_plan)
+{
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	//handle by cmd_thread to sync with scan operation
+	return rtw_set_chplan_cmd(adapter, channel_plan, 1);
+}
+
+/*
+* rtw_set_country - 
+* @adapter: pointer to _adapter structure
+* @country_code: string of country code
+* 
+* Return _SUCCESS or _FAIL
+*/
+int rtw_set_country(_adapter *adapter, const char *country_code)
+{
+	int channel_plan = RT_CHANNEL_DOMAIN_WORLD_WIDE_5G;
+
+	DBG_871X("%s country_code:%s\n", __func__, country_code);
+
+	//TODO: should have a table to match country code and RT_CHANNEL_DOMAIN
+	//TODO: should consider 2-character and 3-character country code
+	if(0 == strcmp(country_code, "US"))
+		channel_plan = RT_CHANNEL_DOMAIN_FCC;
+	else if(0 == strcmp(country_code, "EU"))
+		channel_plan = RT_CHANNEL_DOMAIN_ETSI;
+	else if(0 == strcmp(country_code, "JP"))
+		channel_plan = RT_CHANNEL_DOMAIN_MKK;
+	else if(0 == strcmp(country_code, "CN"))
+		channel_plan = RT_CHANNEL_DOMAIN_CHINA;
+	else
+		DBG_871X("%s unknown country_code:%s\n", __FUNCTION__, country_code);
+	
+	return rtw_set_channel_plan(adapter, channel_plan);
+}
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_iol.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_iol.c
new file mode 100755
index 00000000..0e833be8
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_iol.c
@@ -0,0 +1,403 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+
+#include<rtw_iol.h>
+
+#ifdef CONFIG_IOL
+struct xmit_frame	*rtw_IOL_accquire_xmit_frame(ADAPTER *adapter)
+{
+	struct xmit_frame	*xmit_frame;
+	struct xmit_buf	*xmitbuf;
+	struct pkt_attrib	*pattrib;
+	struct xmit_priv	*pxmitpriv = &(adapter->xmitpriv);
+
+#if 1
+	if ((xmit_frame = rtw_alloc_xmitframe(pxmitpriv)) == NULL)
+	{
+		DBG_871X("%s rtw_alloc_xmitframe return null\n", __FUNCTION__);
+		goto exit;
+	}
+	
+	if ((xmitbuf = rtw_alloc_xmitbuf(pxmitpriv)) == NULL)
+	{
+		DBG_871X("%s rtw_alloc_xmitbuf return null\n", __FUNCTION__);
+		rtw_free_xmitframe(pxmitpriv, xmit_frame);
+		xmit_frame=NULL;
+		goto exit;
+	}
+	
+	xmit_frame->frame_tag = MGNT_FRAMETAG;
+	xmit_frame->pxmitbuf = xmitbuf;
+	xmit_frame->buf_addr = xmitbuf->pbuf;
+	xmitbuf->priv_data = xmit_frame;
+
+	pattrib = &xmit_frame->attrib;
+	update_mgntframe_attrib(adapter, pattrib);
+	pattrib->qsel = 0x10;//Beacon	
+	pattrib->subtype = WIFI_BEACON;	
+	pattrib->pktlen = pattrib->last_txcmdsz = 0;
+
+#else
+	if ((xmit_frame = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		DBG_871X("%s alloc_mgtxmitframe return null\n", __FUNCTION__);
+	}
+	else {
+		pattrib = &xmit_frame->attrib;
+		update_mgntframe_attrib(adapter, pattrib);
+		pattrib->qsel = 0x10;
+		pattrib->pktlen = pattrib->last_txcmdsz = 0;
+	}
+#endif
+
+exit:
+	return xmit_frame;
+}
+
+
+int rtw_IOL_append_cmds(struct xmit_frame *xmit_frame, u8 *IOL_cmds, u32 cmd_len)
+{
+	struct pkt_attrib	*pattrib = &xmit_frame->attrib;
+	u16 buf_offset;
+	u32 ori_len;
+
+	buf_offset = TXDESC_OFFSET;
+	ori_len = buf_offset+pattrib->pktlen;
+
+	//check if the io_buf can accommodate new cmds
+	if(ori_len + cmd_len + 8 > MAX_XMITBUF_SZ) {
+		DBG_871X("%s %u is large than MAX_XMITBUF_SZ:%u, can't accommodate new cmds\n", __FUNCTION__
+			, ori_len + cmd_len + 8, MAX_XMITBUF_SZ);
+		return _FAIL;
+	}
+
+	_rtw_memcpy(xmit_frame->buf_addr + buf_offset + pattrib->pktlen, IOL_cmds, cmd_len);
+	pattrib->pktlen += cmd_len;
+	pattrib->last_txcmdsz += cmd_len;
+
+	//DBG_871X("%s ori:%u + cmd_len:%u = %u\n", __FUNCTION__, ori_len, cmd_len, buf_offset+pattrib->pktlen);
+	
+	return _SUCCESS;
+}
+bool rtw_IOL_applied(ADAPTER *adapter)
+{	
+	if(1 == adapter->registrypriv.fw_iol)
+		return _TRUE;
+
+#ifdef CONFIG_USB_HCI
+	if((2 == adapter->registrypriv.fw_iol) && (!adapter_to_dvobj(adapter)->ishighspeed))
+		return _TRUE;
+#endif
+
+	return _FALSE;
+}
+/*
+bool rtw_IOL_applied(ADAPTER *adapter)
+{
+	if(adapter->registrypriv.fw_iol)
+		return _TRUE;
+
+#ifdef CONFIG_USB_HCI
+	if(!adapter_to_dvobj(adapter)->ishighspeed)
+		return _TRUE;
+#endif
+
+	return _FALSE;
+}
+*/
+
+int rtw_IOL_exec_cmds_sync(ADAPTER *adapter, struct xmit_frame *xmit_frame, u32 max_wating_ms, u32 bndy_cnt)
+{
+	return rtw_hal_iol_cmd(adapter, xmit_frame, max_wating_ms,bndy_cnt);
+}
+
+#ifdef CONFIG_IOL_NEW_GENERATION
+int rtw_IOL_append_LLT_cmd(struct xmit_frame *xmit_frame, u8 page_boundary)
+{
+	return _SUCCESS;
+}
+int _rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value, u8 mask)
+{
+	struct ioreg_cfg cmd = {8,IOREG_CMD_WB_REG,0x0, 0x0,0x0};
+
+	//RTW_PUT_LE16((u8*)&cmd.address, addr);	
+	//RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	
+	cmd.address = cpu_to_le16(addr);	
+	cmd.data = cpu_to_le32(value);
+	
+	if(mask!=0xFF)
+	{
+		cmd.length = 12;
+		//RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	
+		cmd.mask = cpu_to_le32(mask);
+	}	
+		
+	//DBG_871X("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FUNCTION__, addr,value,mask);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, cmd.length);
+
+}
+int _rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value, u16 mask)
+{
+	struct ioreg_cfg cmd = {8,IOREG_CMD_WW_REG,0x0, 0x0,0x0};
+
+	//RTW_PUT_LE16((u8*)&cmd.address, addr);	
+	//RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	
+	cmd.address = cpu_to_le16(addr);	
+	cmd.data = cpu_to_le32(value);
+	
+	if(mask!=0xFFFF)
+	{
+		cmd.length = 12;
+		//RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	
+		cmd.mask =  cpu_to_le32(mask);
+	}	
+		
+	//DBG_871X("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FUNCTION__, addr,value,mask);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, cmd.length);
+	
+}
+int _rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value, u32 mask)
+{
+	struct ioreg_cfg cmd = {8,IOREG_CMD_WD_REG,0x0, 0x0,0x0};
+
+	//RTW_PUT_LE16((u8*)&cmd.address, addr);	
+	//RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	
+	cmd.address = cpu_to_le16(addr);	
+	cmd.data = cpu_to_le32(value);
+	
+	if(mask!=0xFFFFFFFF)
+	{
+		cmd.length = 12;
+		//RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	
+		cmd.mask =  cpu_to_le32(mask);
+	}	
+		
+	//DBG_871X("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FU2NCTION__, addr,value,mask);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, cmd.length);
+	
+}
+
+int _rtw_IOL_append_WRF_cmd(struct xmit_frame *xmit_frame, u8 rf_path, u16 addr, u32 value, u32 mask)
+{
+	struct ioreg_cfg cmd = {8,IOREG_CMD_W_RF,0x0, 0x0,0x0};
+
+	//RTW_PUT_LE16((u8*)&cmd.address, addr);	
+	//RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	
+	cmd.address = (rf_path<<8) |((addr) &0xFF);	
+	cmd.data = cpu_to_le32(value);
+	
+	if(mask!=0x000FFFFF)
+	{
+		cmd.length = 12;
+		//RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	
+		cmd.mask =  cpu_to_le32(mask);
+	}	
+		
+	//DBG_871X("%s rf_path:0x%02x addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FU2NCTION__,rf_path, addr,value,mask);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, cmd.length);
+	
+}
+
+
+
+int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us)
+{
+	struct ioreg_cfg cmd = {4,IOREG_CMD_DELAY_US,0x0, 0x0,0x0};
+	//RTW_PUT_LE16((u8*)&cmd.address, us);	
+	cmd.address = cpu_to_le16(us);	
+
+	//DBG_871X("%s %u\n", __FUNCTION__, us);
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, 4);
+}
+
+int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms)
+{
+	struct ioreg_cfg cmd = {4,IOREG_CMD_DELAY_US,0x0, 0x0,0x0};
+
+	//RTW_PUT_LE16((u8*)&cmd.address, ms);	
+	cmd.address = cpu_to_le16(ms);	
+
+	//DBG_871X("%s %u\n", __FUNCTION__, ms);
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, 4);
+}
+int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame)
+{	
+	struct ioreg_cfg cmd = {4,IOREG_CMD_END,0xFFFF, 0xFF,0x0};
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, 4);
+
+}
+
+u8 rtw_IOL_cmd_boundary_handle(struct xmit_frame *pxmit_frame)
+{	
+	u8 is_cmd_bndy = _FALSE;
+	if(((pxmit_frame->attrib.pktlen+32)%256) + 8 >= 256){
+		rtw_IOL_append_END_cmd(pxmit_frame);
+		pxmit_frame->attrib.pktlen = ((((pxmit_frame->attrib.pktlen+32)/256)+1)*256 );
+		
+		//printk("==> %s, pktlen(%d)\n",__FUNCTION__,pxmit_frame->attrib.pktlen);
+		pxmit_frame->attrib.last_txcmdsz = pxmit_frame->attrib.pktlen;
+		is_cmd_bndy = _TRUE;	
+	}
+	return is_cmd_bndy;
+}
+
+void rtw_IOL_cmd_buf_dump(ADAPTER *Adapter,int buf_len,u8 *pbuf)
+{
+	int i;
+	int j=1;
+	
+	printk("###### %s ######\n",__FUNCTION__);
+	for(i=0;i< buf_len;i++){
+		printk("%02x-",*(pbuf+i));
+			
+		if(j%32 ==0) printk("\n");j++;				
+	}
+	printk("\n");
+	printk("============= ioreg_cmd len = %d =============== \n",buf_len);	
+}
+
+
+#else //CONFIG_IOL_NEW_GENERATION
+int rtw_IOL_append_LLT_cmd(struct xmit_frame *xmit_frame, u8 page_boundary)
+{	
+	IOL_CMD cmd = {0x0, IOL_CMD_LLT, 0x0, 0x0};
+	
+	RTW_PUT_BE32((u8*)&cmd.value, (u32)page_boundary);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, 8);
+}
+
+int _rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_WB_REG, 0x0, 0x0};
+	
+	RTW_PUT_BE16((u8*)&cmd.address, (u16)addr);
+	RTW_PUT_BE32((u8*)&cmd.value, (u32)value);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, 8);
+}
+
+int _rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_WW_REG, 0x0, 0x0};
+	
+	RTW_PUT_BE16((u8*)&cmd.address, (u16)addr);
+	RTW_PUT_BE32((u8*)&cmd.value, (u32)value);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, 8);
+}
+
+int _rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_WD_REG, 0x0, 0x0};
+	u8* pos = (u8 *)&cmd;
+	
+	RTW_PUT_BE16((u8*)&cmd.address, (u16)addr);
+	RTW_PUT_BE32((u8*)&cmd.value, (u32)value);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, 8);
+}
+
+#ifdef DBG_IO
+int dbg_rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 1))
+		DBG_871X("DBG_IO %s:%d IOL_WB(0x%04x, 0x%02x)\n", caller, line, addr, value);
+
+	return _rtw_IOL_append_WB_cmd(xmit_frame, addr, value);
+}
+
+int dbg_rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 2))
+		DBG_871X("DBG_IO %s:%d IOL_WW(0x%04x, 0x%04x)\n", caller, line, addr, value);
+
+	return _rtw_IOL_append_WW_cmd(xmit_frame, addr, value);
+}
+
+int dbg_rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 4))
+		DBG_871X("DBG_IO %s:%d IOL_WD(0x%04x, 0x%08x)\n", caller, line, addr, value);
+
+	return _rtw_IOL_append_WD_cmd(xmit_frame, addr, value);
+}
+#endif
+
+int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_DELAY_US, 0x0, 0x0};
+	
+	RTW_PUT_BE32((u8*)&cmd.value, (u32)us);
+
+	//DBG_871X("%s %u\n", __FUNCTION__, us);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, 8);
+}
+
+int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_DELAY_MS, 0x0, 0x0};
+	
+	RTW_PUT_BE32((u8*)&cmd.value, (u32)ms);
+
+	//DBG_871X("%s %u\n", __FUNCTION__, ms);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, 8);
+}
+
+int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame)
+{	
+	IOL_CMD end_cmd = {0x0, IOL_CMD_END, 0x0, 0x0};
+
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8*)&end_cmd, 8);
+
+}
+
+int rtw_IOL_exec_cmd_array_sync(PADAPTER adapter, u8 *IOL_cmds, u32 cmd_num, u32 max_wating_ms)
+{
+	struct xmit_frame	*xmit_frame;
+
+	if((xmit_frame=rtw_IOL_accquire_xmit_frame(adapter)) == NULL)
+		return _FAIL;
+
+	if(rtw_IOL_append_cmds(xmit_frame, IOL_cmds, cmd_num<<3) == _FAIL)
+		return _FAIL;
+
+	return rtw_IOL_exec_cmds_sync(adapter, xmit_frame, max_wating_ms,0);
+}
+
+int rtw_IOL_exec_empty_cmds_sync(ADAPTER *adapter, u32 max_wating_ms)
+{
+	IOL_CMD end_cmd = {0x0, IOL_CMD_END, 0x0, 0x0};
+	return rtw_IOL_exec_cmd_array_sync(adapter, (u8*)&end_cmd, 1, max_wating_ms);
+}
+#endif //CONFIG_IOL_NEW_GENERATION
+
+
+
+
+#endif //CONFIG_IOL
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_led.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_led.c
new file mode 100755
index 00000000..5d0c8471
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_led.c
@@ -0,0 +1,2422 @@
+/******************************************************************************

+ *

+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.

+ *                                        

+ * This program is free software; you can redistribute it and/or modify it

+ * under the terms of version 2 of the GNU General Public License as

+ * published by the Free Software Foundation.

+ *

+ * 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.,

+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA

+ *

+ *

+ ******************************************************************************/

+

+#include <drv_types.h>

+

+//

+//	Description:

+//		Callback function of LED BlinkTimer, 

+//		it just schedules to corresponding BlinkWorkItem/led_blink_hdl

+//

+void BlinkTimerCallback(void *data)

+{

+	PLED_871x	 pLed = (PLED_871x)data;

+	_adapter		*padapter = pLed->padapter;

+

+	//DBG_871X("%s\n", __FUNCTION__);

+

+	if( (padapter->bSurpriseRemoved == _TRUE) || ( padapter->bDriverStopped == _TRUE))	

+	{

+		//DBG_871X("%s bSurpriseRemoved:%d, bDriverStopped:%d\n", __FUNCTION__, padapter->bSurpriseRemoved, padapter->bDriverStopped);

+		return;

+	}

+

+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI)

+	#ifdef CONFIG_LED_HANDLED_BY_CMD_THREAD

+	rtw_led_blink_cmd(padapter, pLed);

+	#else

+	_set_workitem(&(pLed->BlinkWorkItem));

+	#endif

+#elif defined(CONFIG_PCI_HCI)

+	BlinkHandler(pLed);

+#endif

+

+}

+

+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI)

+//

+//	Description:

+//		Callback function of LED BlinkWorkItem.

+//		We dispatch acture LED blink action according to LedStrategy.

+//

+void BlinkWorkItemCallback(struct work_struct *work)

+{

+	PLED_871x	 pLed = container_of(work, LED_871x, BlinkWorkItem);

+	BlinkHandler(pLed);

+}

+#endif

+

+//

+//	Description:

+//		Reset status of LED_871x object.

+//

+void ResetLedStatus(PLED_871x pLed) {

+

+	pLed->CurrLedState = RTW_LED_OFF; // Current LED state.

+	pLed->bLedOn = _FALSE; // true if LED is ON, false if LED is OFF.

+

+	pLed->bLedBlinkInProgress = _FALSE; // true if it is blinking, false o.w..

+	pLed->bLedWPSBlinkInProgress = _FALSE;

+	

+	pLed->BlinkTimes = 0; // Number of times to toggle led state for blinking.

+	pLed->BlinkingLedState = LED_UNKNOWN; // Next state for blinking, either RTW_LED_ON or RTW_LED_OFF are.

+

+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI)

+	pLed->bLedNoLinkBlinkInProgress = _FALSE;

+	pLed->bLedLinkBlinkInProgress = _FALSE;

+	pLed->bLedStartToLinkBlinkInProgress = _FALSE;

+	pLed->bLedScanBlinkInProgress = _FALSE;

+#endif

+}

+

+ //

+//	Description:

+//		Initialize an LED_871x object.

+//

+void

+InitLed871x(

+	_adapter			*padapter,

+	PLED_871x		pLed,

+	LED_PIN_871x	LedPin

+	)

+{

+	pLed->padapter = padapter;

+	pLed->LedPin = LedPin;

+

+	ResetLedStatus(pLed);

+

+	_init_timer(&(pLed->BlinkTimer), padapter->pnetdev, BlinkTimerCallback, pLed);

+

+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI)

+	_init_workitem(&(pLed->BlinkWorkItem), BlinkWorkItemCallback, pLed);

+#endif

+}

+

+

+//

+//	Description:

+//		DeInitialize an LED_871x object.

+//

+void

+DeInitLed871x(

+	PLED_871x			pLed

+	)

+{

+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)

+	_cancel_workitem_sync(&(pLed->BlinkWorkItem));

+#endif

+	_cancel_timer_ex(&(pLed->BlinkTimer));

+	ResetLedStatus(pLed);

+}

+

+

+//

+//	Description:

+//		Implementation of LED blinking behavior.

+//		It toggle off LED and schedule corresponding timer if necessary.

+//

+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)

+

+void SwLedOn(_adapter *padapter, PLED_871x pLed);

+void SwLedOff(_adapter	*padapter, PLED_871x	pLed);

+

+#define CONFIG_LED_REMOVE_HAL

+

+void

+SwLedBlink(

+	PLED_871x			pLed

+	)

+{

+	_adapter			*padapter = pLed->padapter;

+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);

+	u8				bStopBlinking = _FALSE;

+

+	// Change LED according to BlinkingLedState specified.

+	if( pLed->BlinkingLedState == RTW_LED_ON )

+	{

+		SwLedOn(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn on\n", pLed->BlinkTimes));

+	}

+	else

+	{

+		SwLedOff(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,( "Blinktimes (%d): turn off\n", pLed->BlinkTimes));

+	}

+

+	// Determine if we shall change LED state again.

+	pLed->BlinkTimes--;

+	switch(pLed->CurrLedState)

+	{

+

+	case LED_BLINK_NORMAL:

+		if(pLed->BlinkTimes == 0)

+		{

+			bStopBlinking = _TRUE;

+		}

+		break;

+

+	case LED_BLINK_StartToBlink:

+		if( check_fwstate(pmlmepriv, _FW_LINKED) && check_fwstate(pmlmepriv, WIFI_STATION_STATE) )

+		{

+			bStopBlinking = _TRUE;

+		}

+		if( check_fwstate(pmlmepriv, _FW_LINKED) &&

+			(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) )

+		{

+			bStopBlinking = _TRUE;

+		}

+		else if(pLed->BlinkTimes == 0)

+		{

+			bStopBlinking = _TRUE;

+		}

+		break;

+

+	case LED_BLINK_WPS:

+		if( pLed->BlinkTimes == 0 )

+		{

+			bStopBlinking = _TRUE;

+		}

+		break;

+

+

+	default:

+		bStopBlinking = _TRUE;

+		break;

+

+	}

+

+	if(bStopBlinking)

+	{

+		//if( padapter->pwrctrlpriv.cpwm >= PS_STATE_S2)

+		if(0)

+		{

+			SwLedOff(padapter, pLed);

+		}

+		else if( (check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE) && (pLed->bLedOn == _FALSE))

+		{

+			SwLedOn(padapter, pLed);

+		}

+		else if( (check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE) &&  pLed->bLedOn == _TRUE)

+		{

+			SwLedOff(padapter, pLed);

+		}

+

+		pLed->BlinkTimes = 0;

+		pLed->bLedBlinkInProgress = _FALSE;

+	}

+	else

+	{

+		// Assign LED state to toggle.

+		if( pLed->BlinkingLedState == RTW_LED_ON )

+			pLed->BlinkingLedState = RTW_LED_OFF;

+		else

+			pLed->BlinkingLedState = RTW_LED_ON;

+

+		// Schedule a timer to toggle LED state.

+		switch( pLed->CurrLedState )

+		{

+		case LED_BLINK_NORMAL:

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);

+			break;

+

+		case LED_BLINK_SLOWLY:

+		case LED_BLINK_StartToBlink:

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);

+			break;

+

+		case LED_BLINK_WPS:

+			{

+				if( pLed->BlinkingLedState == RTW_LED_ON )

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_LONG_INTERVAL);

+				else

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_LONG_INTERVAL);

+			}

+			break;

+

+		default:

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);

+			break;

+		}

+	}

+}

+

+void

+SwLedBlink1(

+	PLED_871x			pLed

+	)

+{

+	_adapter				*padapter = pLed->padapter;

+#ifndef CONFIG_LED_REMOVE_HAL

+	HAL_DATA_TYPE		*pHalData = GET_HAL_DATA(padapter);

+#endif

+	struct led_priv		*ledpriv = &(padapter->ledpriv);

+	struct mlme_priv		*pmlmepriv = &(padapter->mlmepriv);

+	PLED_871x 			pLed1 = &(ledpriv->SwLed1);

+	u8					bStopBlinking = _FALSE;

+

+#ifndef CONFIG_LED_REMOVE_HAL

+	if(pHalData->EEPROMCustomerID == RT_CID_819x_CAMEO)

+		pLed = &(ledpriv->SwLed1);

+#endif

+

+	// Change LED according to BlinkingLedState specified.

+	if( pLed->BlinkingLedState == RTW_LED_ON )

+	{

+		SwLedOn(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,( "Blinktimes (%d): turn on\n", pLed->BlinkTimes));

+	}

+	else

+	{

+		SwLedOff(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn off\n", pLed->BlinkTimes));

+	}

+

+#ifndef CONFIG_LED_REMOVE_HAL

+	if(pHalData->EEPROMCustomerID == RT_CID_DEFAULT)

+	{

+		if(check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE)

+		{

+			if(!pLed1->bSWLedCtrl)

+			{

+				SwLedOn(padapter, pLed1);

+				pLed1->bSWLedCtrl = _TRUE;

+			}

+			else if(!pLed1->bLedOn)

+				SwLedOn(padapter, pLed1);

+			RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (): turn on pLed1\n"));

+		}

+		else

+		{

+			if(!pLed1->bSWLedCtrl)

+			{

+				SwLedOff(padapter, pLed1);

+				pLed1->bSWLedCtrl = _TRUE;

+			}

+			else if(pLed1->bLedOn)

+				SwLedOff(padapter, pLed1);

+			RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (): turn off pLed1\n"));

+		}

+	}

+

+#endif

+

+	if( padapter->pwrctrlpriv.rf_pwrstate != rf_on )

+	{

+		SwLedOff(padapter, pLed);

+		ResetLedStatus(pLed);

+		return;

+	}

+

+	switch(pLed->CurrLedState)

+	{

+		case LED_BLINK_SLOWLY:

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+			break;

+

+		case LED_BLINK_NORMAL:

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);

+			break;

+

+		case LED_BLINK_SCAN:

+			pLed->BlinkTimes--;

+			if( pLed->BlinkTimes == 0 )

+			{

+				bStopBlinking = _TRUE;

+			}

+

+			if(bStopBlinking)

+			{

+				if(check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE)

+				{

+					pLed->bLedLinkBlinkInProgress = _TRUE;

+					pLed->CurrLedState = LED_BLINK_NORMAL;

+					if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+

+				}

+				else if(check_fwstate(pmlmepriv, _FW_LINKED)== _FALSE)

+				{

+					pLed->bLedNoLinkBlinkInProgress = _TRUE;

+					pLed->CurrLedState = LED_BLINK_SLOWLY;

+					if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+				}

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+			else

+			{

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_BLINK_TXRX:

+			pLed->BlinkTimes--;

+			if( pLed->BlinkTimes == 0 )

+			{

+				bStopBlinking = _TRUE;

+			}

+			if(bStopBlinking)

+			{

+				if(check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE)

+				{

+					pLed->bLedLinkBlinkInProgress = _TRUE;

+					pLed->CurrLedState = LED_BLINK_NORMAL;

+					if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+				}

+				else if(check_fwstate(pmlmepriv, _FW_LINKED)== _FALSE)

+				{

+					pLed->bLedNoLinkBlinkInProgress = _TRUE;

+					pLed->CurrLedState = LED_BLINK_SLOWLY;

+					if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+				}

+				pLed->BlinkTimes = 0;

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			else

+			{

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_BLINK_WPS:

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+			break;

+

+		case LED_BLINK_WPS_STOP:	//WPS success

+			if(pLed->BlinkingLedState == RTW_LED_ON)

+				bStopBlinking = _FALSE;

+			else

+				bStopBlinking = _TRUE;

+

+			if(bStopBlinking)

+			{

+				pLed->bLedLinkBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_NORMAL;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);

+				RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+

+				pLed->bLedWPSBlinkInProgress = _FALSE;	

+			}

+			else

+			{

+				pLed->BlinkingLedState = RTW_LED_OFF;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);

+			}

+			break;

+

+		default:

+			break;

+	}

+

+}

+

+void

+SwLedBlink2(

+	PLED_871x			pLed

+	)

+{

+	_adapter				*padapter = pLed->padapter;

+	struct mlme_priv		*pmlmepriv = &(padapter->mlmepriv);

+	u8					bStopBlinking = _FALSE;

+

+	// Change LED according to BlinkingLedState specified.

+	if( pLed->BlinkingLedState == RTW_LED_ON)

+	{

+		SwLedOn(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn on\n", pLed->BlinkTimes));

+	}

+	else

+	{

+		SwLedOff(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn off\n", pLed->BlinkTimes));

+	}

+

+	switch(pLed->CurrLedState)

+	{

+		case LED_BLINK_SCAN:

+			pLed->BlinkTimes--;

+			if( pLed->BlinkTimes == 0 )

+			{

+				bStopBlinking = _TRUE;

+			}

+

+			if(bStopBlinking)

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on )

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else if(check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE)

+				{

+					pLed->CurrLedState = RTW_LED_ON;

+					pLed->BlinkingLedState = RTW_LED_ON;

+					SwLedOn(padapter, pLed);

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("stop scan blink CurrLedState %d\n", pLed->CurrLedState));

+

+				}

+				else if(check_fwstate(pmlmepriv, _FW_LINKED)== _FALSE)

+				{

+					pLed->CurrLedState = RTW_LED_OFF;

+					pLed->BlinkingLedState = RTW_LED_OFF;

+					SwLedOff(padapter, pLed);

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("stop scan blink CurrLedState %d\n", pLed->CurrLedState));

+				}

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+			else

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on )

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else

+				{

+					 if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+				}

+			}

+			break;

+

+		case LED_BLINK_TXRX:

+			pLed->BlinkTimes--;

+			if( pLed->BlinkTimes == 0 )

+			{

+				bStopBlinking = _TRUE;

+			}

+			if(bStopBlinking)

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on )

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else if(check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE)

+				{

+					pLed->CurrLedState = RTW_LED_ON;

+					pLed->BlinkingLedState = RTW_LED_ON;

+					SwLedOn(padapter, pLed);

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("stop CurrLedState %d\n", pLed->CurrLedState));

+

+				}

+				else if(check_fwstate(pmlmepriv, _FW_LINKED)== _FALSE)

+				{

+					pLed->CurrLedState = RTW_LED_OFF;

+					pLed->BlinkingLedState = RTW_LED_OFF;

+					SwLedOff(padapter, pLed);

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("stop CurrLedState %d\n", pLed->CurrLedState));

+				}

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			else

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on )

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else

+				{

+					 if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+				}

+			}

+			break;

+

+		default:

+			break;

+	}

+

+}

+

+void

+SwLedBlink3(

+	PLED_871x			pLed

+	)

+{

+	_adapter			*padapter = pLed->padapter;

+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);

+	u8				bStopBlinking = _FALSE;

+

+	// Change LED according to BlinkingLedState specified.

+	if( pLed->BlinkingLedState == RTW_LED_ON )

+	{

+		SwLedOn(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn on\n", pLed->BlinkTimes));

+	}

+	else

+	{

+		if(pLed->CurrLedState != LED_BLINK_WPS_STOP)

+			SwLedOff(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn off\n", pLed->BlinkTimes));

+	}

+

+	switch(pLed->CurrLedState)

+	{

+		case LED_BLINK_SCAN:

+			pLed->BlinkTimes--;

+			if( pLed->BlinkTimes == 0 )

+			{

+				bStopBlinking = _TRUE;

+			}

+

+			if(bStopBlinking)

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on )

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else if(check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE)

+				{

+					pLed->CurrLedState = RTW_LED_ON;

+					pLed->BlinkingLedState = RTW_LED_ON;

+					if( !pLed->bLedOn )

+						SwLedOn(padapter, pLed);

+

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+				}

+				else if(check_fwstate(pmlmepriv, _FW_LINKED)== _FALSE)

+				{

+					pLed->CurrLedState = RTW_LED_OFF;

+					pLed->BlinkingLedState = RTW_LED_OFF;

+					if( pLed->bLedOn )

+						SwLedOff(padapter, pLed);

+

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+				}

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+			else

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on )

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else

+				{

+				 	if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+				}

+			}

+			break;

+

+		case LED_BLINK_TXRX:

+			pLed->BlinkTimes--;

+			if( pLed->BlinkTimes == 0 )

+			{

+				bStopBlinking = _TRUE;

+			}

+			if(bStopBlinking)

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on )

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else if(check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE)

+				{

+					pLed->CurrLedState = RTW_LED_ON;

+					pLed->BlinkingLedState = RTW_LED_ON;

+

+					if( !pLed->bLedOn )

+						SwLedOn(padapter, pLed);

+

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+				}

+				else if(check_fwstate(pmlmepriv, _FW_LINKED)== _FALSE)

+				{

+					pLed->CurrLedState = RTW_LED_OFF;

+					pLed->BlinkingLedState = RTW_LED_OFF;

+

+					if( pLed->bLedOn )

+						SwLedOff(padapter, pLed);

+

+

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+				}

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			else

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on )

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else

+				{

+					if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+				}

+			}

+			break;

+

+		case LED_BLINK_WPS:

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+			break;

+

+		case LED_BLINK_WPS_STOP:	//WPS success

+			if(pLed->BlinkingLedState == RTW_LED_ON)

+			{

+				pLed->BlinkingLedState = RTW_LED_OFF;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);

+				bStopBlinking = _FALSE;

+			}

+			else

+			{

+				bStopBlinking = _TRUE;

+			}

+

+			if(bStopBlinking)

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on )

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else

+				{

+					pLed->CurrLedState = RTW_LED_ON;

+					pLed->BlinkingLedState = RTW_LED_ON;

+					SwLedOn(padapter, pLed);

+					RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+				}

+				pLed->bLedWPSBlinkInProgress = _FALSE;

+			}

+			break;

+

+

+		default:

+			break;

+	}

+

+}

+

+

+void

+SwLedBlink4(

+	PLED_871x			pLed

+	)

+{

+	_adapter			*padapter = pLed->padapter;

+	struct led_priv	*ledpriv = &(padapter->ledpriv);

+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);

+	PLED_871x 		pLed1 = &(ledpriv->SwLed1);

+	u8				bStopBlinking = _FALSE;

+

+	// Change LED according to BlinkingLedState specified.

+	if( pLed->BlinkingLedState == RTW_LED_ON )

+	{

+		SwLedOn(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn on\n", pLed->BlinkTimes));

+	}

+	else

+	{

+		SwLedOff(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn off\n", pLed->BlinkTimes));

+	}

+

+	if(!pLed1->bLedWPSBlinkInProgress && pLed1->BlinkingLedState == LED_UNKNOWN)

+	{

+		pLed1->BlinkingLedState = RTW_LED_OFF;

+		pLed1->CurrLedState = RTW_LED_OFF;

+		SwLedOff(padapter, pLed1);

+	}

+

+	switch(pLed->CurrLedState)

+	{

+		case LED_BLINK_SLOWLY:

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+			break;

+

+		case LED_BLINK_StartToBlink:

+			if( pLed->bLedOn )

+			{

+				pLed->BlinkingLedState = RTW_LED_OFF;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);

+			}

+			else

+			{

+				pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);

+			}

+			break;

+

+		case LED_BLINK_SCAN:

+			pLed->BlinkTimes--;

+			if( pLed->BlinkTimes == 0 )

+			{

+				bStopBlinking = _FALSE;

+			}

+

+			if(bStopBlinking)

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS)

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else

+				{

+					pLed->bLedNoLinkBlinkInProgress = _FALSE;

+					pLed->CurrLedState = LED_BLINK_SLOWLY;

+					if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+				}

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+			else

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS)

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else

+				{

+					 if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+				}

+			}

+			break;

+

+		case LED_BLINK_TXRX:

+			pLed->BlinkTimes--;

+			if( pLed->BlinkTimes == 0 )

+			{

+				bStopBlinking = _TRUE;

+			}

+			if(bStopBlinking)

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS)

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else

+				{

+					pLed->bLedNoLinkBlinkInProgress = _TRUE;

+					pLed->CurrLedState = LED_BLINK_SLOWLY;

+					if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+				}

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			else

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS)

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else

+				{

+					 if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+				}

+			}

+			break;

+

+		case LED_BLINK_WPS:

+			if( pLed->bLedOn )

+			{

+				pLed->BlinkingLedState = RTW_LED_OFF;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);

+			}

+			else

+			{

+				pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);

+			}

+			break;

+

+		case LED_BLINK_WPS_STOP:	//WPS authentication fail

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);

+			break;

+

+		case LED_BLINK_WPS_STOP_OVERLAP:	//WPS session overlap

+			pLed->BlinkTimes--;

+			if(pLed->BlinkTimes == 0)

+			{

+				if(pLed->bLedOn)

+				{

+					pLed->BlinkTimes = 1;

+				}

+				else

+				{

+					bStopBlinking = _TRUE;

+				}

+			}

+

+			if(bStopBlinking)

+			{

+				pLed->BlinkTimes = 10;

+				pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);

+			}

+			else

+			{

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);

+			}

+			break;

+

+

+		default:

+			break;

+	}

+

+	RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("SwLedBlink4 CurrLedState %d\n", pLed->CurrLedState));

+

+

+}

+

+void

+SwLedBlink5(

+	PLED_871x			pLed

+	)

+{

+	_adapter			*padapter = pLed->padapter;

+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);

+	u8				bStopBlinking = _FALSE;

+

+	// Change LED according to BlinkingLedState specified.

+	if( pLed->BlinkingLedState == RTW_LED_ON )

+	{

+		SwLedOn(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn on\n", pLed->BlinkTimes));

+	}

+	else

+	{

+		SwLedOff(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn off\n", pLed->BlinkTimes));

+	}

+

+	switch(pLed->CurrLedState)

+	{

+		case LED_BLINK_SCAN:

+			pLed->BlinkTimes--;

+			if( pLed->BlinkTimes == 0 )

+			{

+				bStopBlinking = _TRUE;

+			}

+

+			if(bStopBlinking)

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS)

+				{

+					pLed->CurrLedState = RTW_LED_OFF;

+					pLed->BlinkingLedState = RTW_LED_OFF;

+					if(pLed->bLedOn)

+						SwLedOff(padapter, pLed);

+				}

+				else

+				{		pLed->CurrLedState = RTW_LED_ON;

+						pLed->BlinkingLedState = RTW_LED_ON;

+						if(!pLed->bLedOn)

+							_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+				}

+

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+			else

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS)

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else

+				{

+					if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+				}

+			}

+			break;

+

+

+		case LED_BLINK_TXRX:

+			pLed->BlinkTimes--;

+			if( pLed->BlinkTimes == 0 )

+			{

+				bStopBlinking = _TRUE;

+			}

+

+			if(bStopBlinking)

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS)

+				{

+					pLed->CurrLedState = RTW_LED_OFF;

+					pLed->BlinkingLedState = RTW_LED_OFF;

+					if(pLed->bLedOn)

+						SwLedOff(padapter, pLed);

+				}

+				else

+				{

+					pLed->CurrLedState = RTW_LED_ON;

+					pLed->BlinkingLedState = RTW_LED_ON;

+					if(!pLed->bLedOn)

+						_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+				}

+

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			else

+			{

+				if( padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS)

+				{

+					SwLedOff(padapter, pLed);

+				}

+				else

+				{

+					 if( pLed->bLedOn )

+						pLed->BlinkingLedState = RTW_LED_OFF;

+					else

+						pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+				}

+			}

+			break;

+

+		default:

+			break;

+	}

+

+	RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("SwLedBlink5 CurrLedState %d\n", pLed->CurrLedState));

+

+

+}

+

+void

+SwLedBlink6(

+	PLED_871x			pLed

+	)

+{

+	_adapter			*padapter = pLed->padapter;

+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);

+	u8				bStopBlinking = _FALSE;

+

+	// Change LED according to BlinkingLedState specified.

+	if( pLed->BlinkingLedState == RTW_LED_ON )

+	{

+		SwLedOn(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn on\n", pLed->BlinkTimes));

+	}

+	else

+	{

+		SwLedOff(padapter, pLed);

+		RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Blinktimes (%d): turn off\n", pLed->BlinkTimes));

+	}

+

+	RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("<==== blink6\n"));

+}

+

+static void

+SwLedControlMode0(

+	_adapter		*padapter,

+	LED_CTL_MODE		LedAction

+)

+{

+	struct led_priv	*ledpriv = &(padapter->ledpriv);

+	PLED_871x	pLed = &(ledpriv->SwLed1);

+

+	// Decide led state

+	switch(LedAction)

+	{

+	case LED_CTL_TX:

+	case LED_CTL_RX:

+		if( pLed->bLedBlinkInProgress == _FALSE )

+		{

+			pLed->bLedBlinkInProgress = _TRUE;

+

+			pLed->CurrLedState = LED_BLINK_NORMAL;

+			pLed->BlinkTimes = 2;

+

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);

+		}

+		break;

+

+	case LED_CTL_START_TO_LINK:

+		if( pLed->bLedBlinkInProgress == _FALSE )

+		{

+			pLed->bLedBlinkInProgress = _TRUE;

+

+			pLed->CurrLedState = LED_BLINK_StartToBlink;

+			pLed->BlinkTimes = 24;

+

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);

+		}

+		else

+		{

+			pLed->CurrLedState = LED_BLINK_StartToBlink;

+		}

+		break;

+

+	case LED_CTL_LINK:

+		pLed->CurrLedState = RTW_LED_ON;

+		if( pLed->bLedBlinkInProgress == _FALSE )

+		{

+			SwLedOn(padapter, pLed);

+		}

+		break;

+

+	case LED_CTL_NO_LINK:

+		pLed->CurrLedState = RTW_LED_OFF;

+		if( pLed->bLedBlinkInProgress == _FALSE )

+		{

+			SwLedOff(padapter, pLed);

+		}

+		break;

+

+	case LED_CTL_POWER_OFF:

+		pLed->CurrLedState = RTW_LED_OFF;

+		if(pLed->bLedBlinkInProgress)

+		{

+			_cancel_timer_ex(&(pLed->BlinkTimer));

+			pLed->bLedBlinkInProgress = _FALSE;

+		}

+		SwLedOff(padapter, pLed);

+		break;

+

+	case LED_CTL_START_WPS:

+		if( pLed->bLedBlinkInProgress == _FALSE || pLed->CurrLedState == RTW_LED_ON)

+		{

+			pLed->bLedBlinkInProgress = _TRUE;

+

+			pLed->CurrLedState = LED_BLINK_WPS;

+			pLed->BlinkTimes = 20;

+

+			if( pLed->bLedOn )

+			{

+				pLed->BlinkingLedState = RTW_LED_OFF;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_LONG_INTERVAL);

+			}

+			else

+			{

+				pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_LONG_INTERVAL);

+			}

+		}

+		break;

+

+	case LED_CTL_STOP_WPS:

+		if(pLed->bLedBlinkInProgress)

+		{

+			pLed->CurrLedState = RTW_LED_OFF;

+			_cancel_timer_ex(&(pLed->BlinkTimer));

+			pLed->bLedBlinkInProgress = _FALSE;

+		}

+		break;

+

+

+	default:

+		break;

+	}

+

+	RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Led %d\n", pLed->CurrLedState));

+

+}

+

+ //ALPHA, added by chiyoko, 20090106

+static void

+SwLedControlMode1(

+	_adapter		*padapter,

+	LED_CTL_MODE		LedAction

+)

+{

+#ifndef CONFIG_LED_REMOVE_HAL

+	HAL_DATA_TYPE		*pHalData = GET_HAL_DATA(padapter);

+#endif

+	struct led_priv		*ledpriv = &(padapter->ledpriv);

+	PLED_871x			pLed = &(ledpriv->SwLed0);

+	struct mlme_priv		*pmlmepriv = &(padapter->mlmepriv);

+

+#ifndef CONFIG_LED_REMOVE_HAL

+	if(pHalData->EEPROMCustomerID == RT_CID_819x_CAMEO)

+		pLed = &(ledpriv->SwLed1);

+#endif

+

+	switch(LedAction)

+	{

+		case LED_CTL_POWER_ON:

+		case LED_CTL_START_TO_LINK:

+		case LED_CTL_NO_LINK:

+			if( pLed->bLedNoLinkBlinkInProgress == _FALSE )

+			{

+				if(pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))

+				{

+					return;

+				}

+				if( pLed->bLedLinkBlinkInProgress == _TRUE )

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedLinkBlinkInProgress = _FALSE;

+				}

+	 			if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+	 			}

+

+				pLed->bLedNoLinkBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_SLOWLY;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_LINK:

+			if( pLed->bLedLinkBlinkInProgress == _FALSE )

+			{

+				if(pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))

+				{

+					return;

+				}

+				if(pLed->bLedNoLinkBlinkInProgress == _TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedNoLinkBlinkInProgress = _FALSE;

+				}

+				if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+	 			}

+				pLed->bLedLinkBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_NORMAL;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_SITE_SURVEY:

+			 if((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE))

+			 	;

+			 else if(pLed->bLedScanBlinkInProgress ==_FALSE)

+			 {

+			 	if(IS_LED_WPS_BLINKING(pLed))

+					return;

+

+	  			if(pLed->bLedNoLinkBlinkInProgress == _TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedNoLinkBlinkInProgress = _FALSE;

+				}

+				if( pLed->bLedLinkBlinkInProgress == _TRUE )

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					 pLed->bLedLinkBlinkInProgress = _FALSE;

+				}

+	 			if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+				}

+				pLed->bLedScanBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_SCAN;

+				pLed->BlinkTimes = 24;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+			 }

+			break;

+

+		case LED_CTL_TX:

+		case LED_CTL_RX:

+			if(pLed->bLedBlinkInProgress ==_FALSE)

+			{

+				if(pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))

+				{

+					return;

+				}

+				if(pLed->bLedNoLinkBlinkInProgress == _TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedNoLinkBlinkInProgress = _FALSE;

+				}

+				if( pLed->bLedLinkBlinkInProgress == _TRUE )

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedLinkBlinkInProgress = _FALSE;

+				}

+				pLed->bLedBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_TXRX;

+				pLed->BlinkTimes = 2;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_START_WPS: //wait until xinpin finish

+		case LED_CTL_START_WPS_BOTTON:

+			 if(pLed->bLedWPSBlinkInProgress ==_FALSE)

+			 {

+				if(pLed->bLedNoLinkBlinkInProgress == _TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedNoLinkBlinkInProgress = _FALSE;

+				}

+				if( pLed->bLedLinkBlinkInProgress == _TRUE )

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					 pLed->bLedLinkBlinkInProgress = _FALSE;

+				}

+				if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+				}

+				if(pLed->bLedScanBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedScanBlinkInProgress = _FALSE;

+				}

+				pLed->bLedWPSBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_WPS;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+			 }

+			break;

+

+

+		case LED_CTL_STOP_WPS:

+			if(pLed->bLedNoLinkBlinkInProgress == _TRUE)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedNoLinkBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedLinkBlinkInProgress == _TRUE )

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				 pLed->bLedLinkBlinkInProgress = _FALSE;

+			}

+			if(pLed->bLedBlinkInProgress ==_TRUE)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			if(pLed->bLedScanBlinkInProgress ==_TRUE)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+			if(pLed->bLedWPSBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+			}

+			else

+			{

+				pLed->bLedWPSBlinkInProgress = _TRUE;

+			}

+

+			pLed->CurrLedState = LED_BLINK_WPS_STOP;

+			if(pLed->bLedOn)

+			{

+				pLed->BlinkingLedState = RTW_LED_OFF;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);

+			}

+			else

+			{

+				pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), 0);

+			}

+			break;

+

+		case LED_CTL_STOP_WPS_FAIL:

+			if(pLed->bLedWPSBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedWPSBlinkInProgress = _FALSE;

+			}

+

+			pLed->bLedNoLinkBlinkInProgress = _TRUE;

+			pLed->CurrLedState = LED_BLINK_SLOWLY;

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+			break;

+

+		case LED_CTL_POWER_OFF:

+			pLed->CurrLedState = RTW_LED_OFF;

+			pLed->BlinkingLedState = RTW_LED_OFF;

+			if( pLed->bLedNoLinkBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedNoLinkBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedLinkBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedLinkBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedWPSBlinkInProgress )

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedWPSBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedScanBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+

+			SwLedOff(padapter, pLed);

+			break;

+

+		default:

+			break;

+

+	}

+

+	RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Led %d\n", pLed->CurrLedState));

+}

+

+ //Arcadyan/Sitecom , added by chiyoko, 20090216

+static void

+SwLedControlMode2(

+	_adapter				*padapter,

+	LED_CTL_MODE		LedAction

+)

+{

+	struct led_priv	*ledpriv = &(padapter->ledpriv);

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	PLED_871x 		pLed = &(ledpriv->SwLed0);

+

+	switch(LedAction)

+	{

+		case LED_CTL_SITE_SURVEY:

+			 if(pmlmepriv->LinkDetectInfo.bBusyTraffic)

+			 	;

+			 else if(pLed->bLedScanBlinkInProgress ==_FALSE)

+			 {

+			 	if(IS_LED_WPS_BLINKING(pLed))

+					return;

+

+	 			if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+				}

+				pLed->bLedScanBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_SCAN;

+				pLed->BlinkTimes = 24;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+			 }

+			break;

+

+		case LED_CTL_TX:

+		case LED_CTL_RX:

+	 		if((pLed->bLedBlinkInProgress ==_FALSE) && (check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE))

+	  		{

+	  		  	if(pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))

+				{

+					return;

+				}

+

+				pLed->bLedBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_TXRX;

+				pLed->BlinkTimes = 2;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_LINK:

+			pLed->CurrLedState = RTW_LED_ON;

+			pLed->BlinkingLedState = RTW_LED_ON;

+			if( pLed->bLedBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedScanBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+

+			_set_timer(&(pLed->BlinkTimer), 0);

+			break;

+

+		case LED_CTL_START_WPS: //wait until xinpin finish

+		case LED_CTL_START_WPS_BOTTON:

+			if(pLed->bLedWPSBlinkInProgress ==_FALSE)

+			{

+				if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+				}

+				if(pLed->bLedScanBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedScanBlinkInProgress = _FALSE;

+				}

+				pLed->bLedWPSBlinkInProgress = _TRUE;

+				pLed->CurrLedState = RTW_LED_ON;

+				pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), 0);

+			 }

+			break;

+

+		case LED_CTL_STOP_WPS:

+			pLed->bLedWPSBlinkInProgress = _FALSE;

+			if(padapter->pwrctrlpriv.rf_pwrstate != rf_on)

+			{

+				SwLedOff(padapter, pLed);

+			}

+			else

+			{

+				pLed->CurrLedState = RTW_LED_ON;

+				pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), 0);

+				RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+			}

+			break;

+

+		case LED_CTL_STOP_WPS_FAIL:

+			pLed->bLedWPSBlinkInProgress = _FALSE;

+			if(padapter->pwrctrlpriv.rf_pwrstate != rf_on)

+			{

+				SwLedOff(padapter, pLed);

+			}

+			else

+			{

+				pLed->CurrLedState = RTW_LED_OFF;

+				pLed->BlinkingLedState = RTW_LED_OFF;

+				_set_timer(&(pLed->BlinkTimer), 0);

+				RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+			}

+			break;

+

+		case LED_CTL_START_TO_LINK:

+		case LED_CTL_NO_LINK:

+			if(!IS_LED_BLINKING(pLed))

+			{

+				pLed->CurrLedState = RTW_LED_OFF;

+				pLed->BlinkingLedState = RTW_LED_OFF;

+				_set_timer(&(pLed->BlinkTimer), 0);

+			}

+			break;

+

+		case LED_CTL_POWER_OFF:

+			pLed->CurrLedState = RTW_LED_OFF;

+			pLed->BlinkingLedState = RTW_LED_OFF;

+			if( pLed->bLedBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedScanBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedWPSBlinkInProgress )

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedWPSBlinkInProgress = _FALSE;

+			}

+

+			_set_timer(&(pLed->BlinkTimer), 0);

+			break;

+

+		default:

+			break;

+

+	}

+

+	RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+}

+

+  //COREGA, added by chiyoko, 20090316

+ static void

+ SwLedControlMode3(

+	_adapter				*padapter,

+	LED_CTL_MODE		LedAction

+)

+{

+	struct led_priv	*ledpriv = &(padapter->ledpriv);

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	PLED_871x		pLed = &(ledpriv->SwLed0);

+

+	switch(LedAction)

+	{

+		case LED_CTL_SITE_SURVEY:

+			if(pmlmepriv->LinkDetectInfo.bBusyTraffic)

+				;

+			else if(pLed->bLedScanBlinkInProgress ==_FALSE)

+			{

+				if(IS_LED_WPS_BLINKING(pLed))

+					return;

+

+				if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+				}

+				pLed->bLedScanBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_SCAN;

+				pLed->BlinkTimes = 24;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_TX:

+		case LED_CTL_RX:

+	 		if((pLed->bLedBlinkInProgress ==_FALSE) && (check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE))

+	  		{

+	  		  	if(pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))

+				{

+					return;

+				}

+

+				pLed->bLedBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_TXRX;

+				pLed->BlinkTimes = 2;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_LINK:

+			if(IS_LED_WPS_BLINKING(pLed))

+				return;

+

+			pLed->CurrLedState = RTW_LED_ON;

+			pLed->BlinkingLedState = RTW_LED_ON;

+			if( pLed->bLedBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedScanBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+

+			_set_timer(&(pLed->BlinkTimer), 0);

+			break;

+

+		case LED_CTL_START_WPS: //wait until xinpin finish

+		case LED_CTL_START_WPS_BOTTON:

+			if(pLed->bLedWPSBlinkInProgress ==_FALSE)

+			{

+				if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+				}

+				if(pLed->bLedScanBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedScanBlinkInProgress = _FALSE;

+				}

+				pLed->bLedWPSBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_WPS;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_STOP_WPS:

+			if(pLed->bLedWPSBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedWPSBlinkInProgress = _FALSE;

+			}

+			else

+			{

+				pLed->bLedWPSBlinkInProgress = _TRUE;

+			}

+

+			pLed->CurrLedState = LED_BLINK_WPS_STOP;

+			if(pLed->bLedOn)

+			{

+				pLed->BlinkingLedState = RTW_LED_OFF;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);

+			}

+			else

+			{

+				pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), 0);

+			}

+

+			break;

+

+		case LED_CTL_STOP_WPS_FAIL:

+			if(pLed->bLedWPSBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedWPSBlinkInProgress = _FALSE;

+			}

+

+			pLed->CurrLedState = RTW_LED_OFF;

+			pLed->BlinkingLedState = RTW_LED_OFF;

+			_set_timer(&(pLed->BlinkTimer), 0);

+			break;

+

+		case LED_CTL_START_TO_LINK:

+		case LED_CTL_NO_LINK:

+			if(!IS_LED_BLINKING(pLed))

+			{

+				pLed->CurrLedState = RTW_LED_OFF;

+				pLed->BlinkingLedState = RTW_LED_OFF;

+				_set_timer(&(pLed->BlinkTimer), 0);

+			}

+			break;

+

+		case LED_CTL_POWER_OFF:

+			pLed->CurrLedState = RTW_LED_OFF;

+			pLed->BlinkingLedState = RTW_LED_OFF;

+			if( pLed->bLedBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedScanBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedWPSBlinkInProgress )

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedWPSBlinkInProgress = _FALSE;

+			}

+

+			_set_timer(&(pLed->BlinkTimer), 0);

+			break;

+

+		default:

+			break;

+

+	}

+

+	RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("CurrLedState %d\n", pLed->CurrLedState));

+}

+

+

+ //Edimax-Belkin, added by chiyoko, 20090413

+static void

+SwLedControlMode4(

+	_adapter				*padapter,

+	LED_CTL_MODE		LedAction

+)

+{

+	struct led_priv	*ledpriv = &(padapter->ledpriv);

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	PLED_871x		pLed = &(ledpriv->SwLed0);

+	PLED_871x		pLed1 = &(ledpriv->SwLed1);

+

+	switch(LedAction)

+	{

+		case LED_CTL_START_TO_LINK:

+			if(pLed1->bLedWPSBlinkInProgress)

+			{

+				pLed1->bLedWPSBlinkInProgress = _FALSE;

+				_cancel_timer_ex(&(pLed1->BlinkTimer));

+

+				pLed1->BlinkingLedState = RTW_LED_OFF;

+				pLed1->CurrLedState = RTW_LED_OFF;

+

+				if(pLed1->bLedOn)

+					_set_timer(&(pLed->BlinkTimer), 0);

+			}

+

+			if( pLed->bLedStartToLinkBlinkInProgress == _FALSE )

+			{

+				if(pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))

+				{

+					return;

+				}

+	 			if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+	 			}

+	 			if(pLed->bLedNoLinkBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedNoLinkBlinkInProgress = _FALSE;

+	 			}

+

+				pLed->bLedStartToLinkBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_StartToBlink;

+				if( pLed->bLedOn )

+				{

+					pLed->BlinkingLedState = RTW_LED_OFF;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);

+				}

+				else

+				{

+					pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);

+				}

+			}

+			break;

+

+		case LED_CTL_LINK:

+		case LED_CTL_NO_LINK:

+			//LED1 settings

+			if(LedAction == LED_CTL_LINK)

+			{

+				if(pLed1->bLedWPSBlinkInProgress)

+				{

+					pLed1->bLedWPSBlinkInProgress = _FALSE;

+					_cancel_timer_ex(&(pLed1->BlinkTimer));

+

+					pLed1->BlinkingLedState = RTW_LED_OFF;

+					pLed1->CurrLedState = RTW_LED_OFF;

+

+					if(pLed1->bLedOn)

+						_set_timer(&(pLed->BlinkTimer), 0);

+				}

+			}

+

+			if( pLed->bLedNoLinkBlinkInProgress == _FALSE )

+			{

+				if(pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))

+				{

+					return;

+				}

+	 			if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+	 			}

+

+				pLed->bLedNoLinkBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_SLOWLY;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_SITE_SURVEY:

+			if((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE))

+				;

+			else if(pLed->bLedScanBlinkInProgress ==_FALSE)

+			{

+				if(IS_LED_WPS_BLINKING(pLed))

+					return;

+

+				if(pLed->bLedNoLinkBlinkInProgress == _TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedNoLinkBlinkInProgress = _FALSE;

+				}

+				if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+				}

+				pLed->bLedScanBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_SCAN;

+				pLed->BlinkTimes = 24;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_TX:

+		case LED_CTL_RX:

+	 		if(pLed->bLedBlinkInProgress ==_FALSE)

+	  		{

+	  		  	if(pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))

+				{

+					return;

+				}

+	  		  	if(pLed->bLedNoLinkBlinkInProgress == _TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedNoLinkBlinkInProgress = _FALSE;

+				}

+				pLed->bLedBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_TXRX;

+				pLed->BlinkTimes = 2;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_START_WPS: //wait until xinpin finish

+		case LED_CTL_START_WPS_BOTTON:

+			if(pLed1->bLedWPSBlinkInProgress)

+			{

+				pLed1->bLedWPSBlinkInProgress = _FALSE;

+				_cancel_timer_ex(&(pLed1->BlinkTimer));

+

+				pLed1->BlinkingLedState = RTW_LED_OFF;

+				pLed1->CurrLedState = RTW_LED_OFF;

+

+				if(pLed1->bLedOn)

+					_set_timer(&(pLed->BlinkTimer), 0);

+			}

+

+			if(pLed->bLedWPSBlinkInProgress ==_FALSE)

+			{

+				if(pLed->bLedNoLinkBlinkInProgress == _TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedNoLinkBlinkInProgress = _FALSE;

+				}

+				if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+				}

+				if(pLed->bLedScanBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedScanBlinkInProgress = _FALSE;

+				}

+				pLed->bLedWPSBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_WPS;

+				if( pLed->bLedOn )

+				{

+					pLed->BlinkingLedState = RTW_LED_OFF;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);

+				}

+				else

+				{

+					pLed->BlinkingLedState = RTW_LED_ON;

+					_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);

+				}

+			}

+			break;

+

+		case LED_CTL_STOP_WPS:	//WPS connect success

+			if(pLed->bLedWPSBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedWPSBlinkInProgress = _FALSE;

+			}

+

+			pLed->bLedNoLinkBlinkInProgress = _TRUE;

+			pLed->CurrLedState = LED_BLINK_SLOWLY;

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+

+			break;

+

+		case LED_CTL_STOP_WPS_FAIL:		//WPS authentication fail

+			if(pLed->bLedWPSBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedWPSBlinkInProgress = _FALSE;

+			}

+

+			pLed->bLedNoLinkBlinkInProgress = _TRUE;

+			pLed->CurrLedState = LED_BLINK_SLOWLY;

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+

+			//LED1 settings

+			if(pLed1->bLedWPSBlinkInProgress)

+				_cancel_timer_ex(&(pLed1->BlinkTimer));

+			else

+				pLed1->bLedWPSBlinkInProgress = _TRUE;

+

+			pLed1->CurrLedState = LED_BLINK_WPS_STOP;

+			if( pLed1->bLedOn )

+				pLed1->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed1->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);

+

+			break;

+

+		case LED_CTL_STOP_WPS_FAIL_OVERLAP:	//WPS session overlap

+			if(pLed->bLedWPSBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedWPSBlinkInProgress = _FALSE;

+			}

+

+			pLed->bLedNoLinkBlinkInProgress = _TRUE;

+			pLed->CurrLedState = LED_BLINK_SLOWLY;

+			if( pLed->bLedOn )

+				pLed->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);

+

+			//LED1 settings

+			if(pLed1->bLedWPSBlinkInProgress)

+				_cancel_timer_ex(&(pLed1->BlinkTimer));

+			else

+				pLed1->bLedWPSBlinkInProgress = _TRUE;

+

+			pLed1->CurrLedState = LED_BLINK_WPS_STOP_OVERLAP;

+			pLed1->BlinkTimes = 10;

+			if( pLed1->bLedOn )

+				pLed1->BlinkingLedState = RTW_LED_OFF;

+			else

+				pLed1->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);

+

+			break;

+

+		case LED_CTL_POWER_OFF:

+			pLed->CurrLedState = RTW_LED_OFF;

+			pLed->BlinkingLedState = RTW_LED_OFF;

+

+			if( pLed->bLedNoLinkBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedNoLinkBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedLinkBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedLinkBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedWPSBlinkInProgress )

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedWPSBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedScanBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedScanBlinkInProgress = _FALSE;

+			}

+			if( pLed->bLedStartToLinkBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedStartToLinkBlinkInProgress = _FALSE;

+			}

+

+			if( pLed1->bLedWPSBlinkInProgress )

+			{

+				_cancel_timer_ex(&(pLed1->BlinkTimer));

+				pLed1->bLedWPSBlinkInProgress = _FALSE;

+			}

+

+			pLed1->BlinkingLedState = LED_UNKNOWN;

+			SwLedOff(padapter, pLed);

+			SwLedOff(padapter, pLed1);

+			break;

+

+		default:

+			break;

+

+	}

+

+	RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Led %d\n", pLed->CurrLedState));

+}

+

+

+

+ //Sercomm-Belkin, added by chiyoko, 20090415

+static void

+SwLedControlMode5(

+	_adapter				*padapter,

+	LED_CTL_MODE		LedAction

+)

+{

+#ifndef CONFIG_LED_REMOVE_HAL

+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);

+#endif

+	struct led_priv	*ledpriv = &(padapter->ledpriv);

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	PLED_871x		pLed = &(ledpriv->SwLed0);

+

+#ifndef CONFIG_LED_REMOVE_HAL

+	if(pHalData->EEPROMCustomerID == RT_CID_819x_CAMEO)

+		pLed = &(ledpriv->SwLed1);

+#endif

+

+	switch(LedAction)

+	{

+		case LED_CTL_POWER_ON:

+		case LED_CTL_NO_LINK:

+		case LED_CTL_LINK: 	//solid blue

+			pLed->CurrLedState = RTW_LED_ON;

+			pLed->BlinkingLedState = RTW_LED_ON;

+

+			_set_timer(&(pLed->BlinkTimer), 0);

+			break;

+

+		case LED_CTL_SITE_SURVEY:

+			if((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE))

+				;

+			else if(pLed->bLedScanBlinkInProgress ==_FALSE)

+			{

+				if(pLed->bLedBlinkInProgress ==_TRUE)

+				{

+					_cancel_timer_ex(&(pLed->BlinkTimer));

+					pLed->bLedBlinkInProgress = _FALSE;

+				}

+				pLed->bLedScanBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_SCAN;

+				pLed->BlinkTimes = 24;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_TX:

+		case LED_CTL_RX:

+	 		if(pLed->bLedBlinkInProgress ==_FALSE)

+	  		{

+	  		  	if(pLed->CurrLedState == LED_BLINK_SCAN)

+				{

+					return;

+				}

+				pLed->bLedBlinkInProgress = _TRUE;

+				pLed->CurrLedState = LED_BLINK_TXRX;

+				pLed->BlinkTimes = 2;

+				if( pLed->bLedOn )

+					pLed->BlinkingLedState = RTW_LED_OFF;

+				else

+					pLed->BlinkingLedState = RTW_LED_ON;

+				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);

+			}

+			break;

+

+		case LED_CTL_POWER_OFF:

+			pLed->CurrLedState = RTW_LED_OFF;

+			pLed->BlinkingLedState = RTW_LED_OFF;

+

+			if( pLed->bLedBlinkInProgress)

+			{

+				_cancel_timer_ex(&(pLed->BlinkTimer));

+				pLed->bLedBlinkInProgress = _FALSE;

+			}

+

+			SwLedOff(padapter, pLed);

+			break;

+

+		default:

+			break;

+

+	}

+

+	RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("Led %d\n", pLed->CurrLedState));

+}

+

+ //WNC-Corega, added by chiyoko, 20090902

+static void

+SwLedControlMode6(

+	_adapter				*padapter,

+	LED_CTL_MODE		LedAction

+)

+{

+	struct led_priv	*ledpriv = &(padapter->ledpriv);

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	PLED_871x pLed0 = &(ledpriv->SwLed0);

+

+	switch(LedAction)

+	{

+		case LED_CTL_POWER_ON:

+		case LED_CTL_LINK:

+		case LED_CTL_NO_LINK:

+			_cancel_timer_ex(&(pLed0->BlinkTimer));

+			pLed0->CurrLedState = RTW_LED_ON;

+			pLed0->BlinkingLedState = RTW_LED_ON;

+			_set_timer(&(pLed0->BlinkTimer), 0);

+			break;

+

+		case LED_CTL_POWER_OFF:

+			SwLedOff(padapter, pLed0);

+			break;

+

+		default:

+			break;

+	}

+

+	RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("ledcontrol 6 Led %d\n", pLed0->CurrLedState));

+}

+

+//

+//	Description:

+//		Handler function of LED Blinking.

+//		We dispatch acture LED blink action according to LedStrategy.

+//

+void BlinkHandler(PLED_871x	 pLed)

+{

+	_adapter		*padapter = pLed->padapter;

+	struct led_priv	*ledpriv = &(padapter->ledpriv);

+

+	//DBG_871X("%s (%s:%d)\n",__FUNCTION__, current->comm, current->pid);

+

+	if( (padapter->bSurpriseRemoved == _TRUE) || ( padapter->bDriverStopped == _TRUE))	

+	{

+		//DBG_871X("%s bSurpriseRemoved:%d, bDriverStopped:%d\n", __FUNCTION__, padapter->bSurpriseRemoved, padapter->bDriverStopped);

+		return;

+	}

+

+	switch(ledpriv->LedStrategy)

+	{

+		case SW_LED_MODE0:

+			SwLedBlink(pLed);

+			break;

+		

+		case SW_LED_MODE1:

+			SwLedBlink1(pLed);

+			break;

+		

+		case SW_LED_MODE2:

+			SwLedBlink2(pLed);

+			break;

+			

+		case SW_LED_MODE3:

+			SwLedBlink3(pLed);

+			break;

+

+		case SW_LED_MODE4:

+			SwLedBlink4(pLed);

+			break;			

+

+		case SW_LED_MODE5:

+			SwLedBlink5(pLed);

+			break;

+

+		case SW_LED_MODE6:

+			SwLedBlink6(pLed);

+			break;

+

+		default:

+			//RT_TRACE(COMP_LED, DBG_LOUD, ("BlinkWorkItemCallback 0x%x \n", pHalData->LedStrategy));

+			//SwLedBlink(pLed);

+			break;

+	}

+}

+

+void

+LedControl871x(

+	_adapter				*padapter,

+	LED_CTL_MODE		LedAction

+	)

+{

+	struct led_priv	*ledpriv = &(padapter->ledpriv);

+

+       if( (padapter->bSurpriseRemoved == _TRUE) || ( padapter->bDriverStopped == _TRUE)

+	   	||(padapter->hw_init_completed == _FALSE) )

+       {

+             return;

+       }

+

+

+	if( ledpriv->bRegUseLed == _FALSE)

+		return;

+

+	//if (!priv->up)

+	//	return;

+

+	//if(priv->bInHctTest)

+	//	return;

+

+	if( (padapter->pwrctrlpriv.rf_pwrstate != rf_on &&

+		padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) &&

+		(LedAction == LED_CTL_TX || LedAction == LED_CTL_RX ||

+		 LedAction == LED_CTL_SITE_SURVEY ||

+		 LedAction == LED_CTL_LINK ||

+		 LedAction == LED_CTL_NO_LINK ||

+		 LedAction == LED_CTL_POWER_ON) )

+	{

+		return;

+	}

+

+	switch(ledpriv->LedStrategy)

+	{

+		case SW_LED_MODE0:

+			//SwLedControlMode0(padapter, LedAction);

+			break;

+

+		case SW_LED_MODE1:

+			SwLedControlMode1(padapter, LedAction);

+			break;

+		case SW_LED_MODE2:

+			SwLedControlMode2(padapter, LedAction);

+                	break;

+

+		case SW_LED_MODE3:

+			SwLedControlMode3(padapter, LedAction);

+			break;

+

+		case SW_LED_MODE4:

+			SwLedControlMode4(padapter, LedAction);

+			break;

+

+		case SW_LED_MODE5:

+			SwLedControlMode5(padapter, LedAction);

+			break;

+

+		case SW_LED_MODE6:

+			SwLedControlMode6(padapter, LedAction);

+			break;

+

+		default:

+			break;

+	}

+

+	RT_TRACE(_module_rtl8712_led_c_,_drv_info_,("LedStrategy:%d, LedAction %d\n", ledpriv->LedStrategy,LedAction));

+}

+

+#endif

+

diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_mlme.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_mlme.c
new file mode 100755
index 00000000..a827c022
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_mlme.c
@@ -0,0 +1,4039 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_MLME_C_
+
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <recv_osdep.h>
+#include <xmit_osdep.h>
+#include <hal_intf.h>
+#include <mlme_osdep.h>
+#include <sta_info.h>
+#include <wifi.h>
+#include <wlan_bssdef.h>
+#include <rtw_ioctl_set.h>
+
+extern void indicate_wx_scan_complete_event(_adapter *padapter);
+extern u8 rtw_do_join(_adapter * padapter);
+
+#ifdef CONFIG_DISABLE_MCS13TO15
+extern unsigned char	MCS_rate_2R_MCS13TO15_OFF[16];
+extern unsigned char	MCS_rate_2R[16];
+#else //CONFIG_DISABLE_MCS13TO15
+extern unsigned char	MCS_rate_2R[16];
+#endif //CONFIG_DISABLE_MCS13TO15
+extern unsigned char	MCS_rate_1R[16];
+
+sint	_rtw_init_mlme_priv (_adapter* padapter)
+{
+	sint	i;
+	u8	*pbuf;
+	struct wlan_network	*pnetwork;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	sint	res = _SUCCESS;
+
+_func_enter_;
+
+	// We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc().
+	//_rtw_memset((u8 *)pmlmepriv, 0, sizeof(struct mlme_priv));
+
+	pmlmepriv->nic_hdl = (u8 *)padapter;
+
+	pmlmepriv->pscanned = NULL;
+	pmlmepriv->fw_state = 0;
+	pmlmepriv->cur_network.network.InfrastructureMode = Ndis802_11AutoUnknown;
+	pmlmepriv->scan_mode=SCAN_ACTIVE;// 1: active, 0: pasive. Maybe someday we should rename this varable to "active_mode" (Jeff)
+
+	_rtw_spinlock_init(&(pmlmepriv->lock));	
+	_rtw_init_queue(&(pmlmepriv->free_bss_pool));
+	_rtw_init_queue(&(pmlmepriv->scanned_queue));
+
+	set_scanned_network_val(pmlmepriv, 0);
+	
+	_rtw_memset(&pmlmepriv->assoc_ssid,0,sizeof(NDIS_802_11_SSID));
+
+	pbuf = rtw_zvmalloc(MAX_BSS_CNT * (sizeof(struct wlan_network)));
+	
+	if (pbuf == NULL){
+		res=_FAIL;
+		goto exit;
+	}
+	pmlmepriv->free_bss_buf = pbuf;
+		
+	pnetwork = (struct wlan_network *)pbuf;
+	
+	for(i = 0; i < MAX_BSS_CNT; i++)
+	{		
+		_rtw_init_listhead(&(pnetwork->list));
+
+		rtw_list_insert_tail(&(pnetwork->list), &(pmlmepriv->free_bss_pool.queue));
+
+		pnetwork++;
+	}
+
+	//allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf
+
+	rtw_clear_scan_deny(padapter);
+
+	#ifdef CONFIG_FTP_PROTECT
+	pmlmepriv->ftp_lock_flag = 0;
+	#endif //CONFIG_FTP_PROTECT
+
+	rtw_init_mlme_timer(padapter);
+
+exit:
+
+_func_exit_;
+
+	return res;
+}	
+
+void rtw_mfree_mlme_priv_lock (struct mlme_priv *pmlmepriv);
+void rtw_mfree_mlme_priv_lock (struct mlme_priv *pmlmepriv)
+{
+	_rtw_spinlock_free(&pmlmepriv->lock);
+	_rtw_spinlock_free(&(pmlmepriv->free_bss_pool.lock));
+	_rtw_spinlock_free(&(pmlmepriv->scanned_queue.lock));
+}
+
+static void rtw_free_mlme_ie_data(u8 **ppie, u32 *plen)
+{
+	if(*ppie)
+	{		
+		_rtw_mfree(*ppie, *plen);
+		*plen = 0;
+		*ppie=NULL;
+	}	
+}
+
+void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
+{
+#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
+	rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_beacon_ie, &pmlmepriv->wps_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_req_ie, &pmlmepriv->wps_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_resp_ie, &pmlmepriv->wps_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_assoc_resp_ie, &pmlmepriv->wps_assoc_resp_ie_len);
+	
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_beacon_ie, &pmlmepriv->p2p_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_req_ie, &pmlmepriv->p2p_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_resp_ie, &pmlmepriv->p2p_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_go_probe_resp_ie, &pmlmepriv->p2p_go_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_req_ie, &pmlmepriv->p2p_assoc_req_ie_len);
+#endif
+
+#if defined(CONFIG_WFD) && defined(CONFIG_IOCTL_CFG80211)	
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_beacon_ie, &pmlmepriv->wfd_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_req_ie, &pmlmepriv->wfd_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_resp_ie, &pmlmepriv->wfd_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_go_probe_resp_ie, &pmlmepriv->wfd_go_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_assoc_req_ie, &pmlmepriv->wfd_assoc_req_ie_len);
+#endif
+
+}
+
+void _rtw_free_mlme_priv (struct mlme_priv *pmlmepriv)
+{
+_func_enter_;
+
+	rtw_free_mlme_priv_ie_data(pmlmepriv);
+
+	if(pmlmepriv){
+		rtw_mfree_mlme_priv_lock (pmlmepriv);
+
+		if (pmlmepriv->free_bss_buf) {
+			rtw_vmfree(pmlmepriv->free_bss_buf, MAX_BSS_CNT * sizeof(struct wlan_network));
+		}
+	}
+_func_exit_;	
+}
+
+sint	_rtw_enqueue_network(_queue *queue, struct wlan_network *pnetwork)
+{
+	_irqL irqL;
+
+_func_enter_;	
+
+	if (pnetwork == NULL)
+		goto exit;
+	
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	rtw_list_insert_tail(&pnetwork->list, &queue->queue);
+
+	_exit_critical_bh(&queue->lock, &irqL);
+
+exit:	
+
+_func_exit_;		
+
+	return _SUCCESS;
+}
+
+struct	wlan_network *_rtw_dequeue_network(_queue *queue)
+{
+	_irqL irqL;
+
+	struct wlan_network *pnetwork;
+
+_func_enter_;	
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	if (_rtw_queue_empty(queue) == _TRUE)
+
+		pnetwork = NULL;
+	
+	else
+	{
+		pnetwork = LIST_CONTAINOR(get_next(&queue->queue), struct wlan_network, list);
+		
+		rtw_list_delete(&(pnetwork->list));
+	}
+	
+	_exit_critical_bh(&queue->lock, &irqL);
+
+_func_exit_;		
+
+	return pnetwork;
+}
+
+struct	wlan_network *_rtw_alloc_network(struct	mlme_priv *pmlmepriv )//(_queue *free_queue)
+{
+	_irqL	irqL;
+	struct	wlan_network	*pnetwork;	
+	_queue *free_queue = &pmlmepriv->free_bss_pool;
+	_list* plist = NULL;
+	
+_func_enter_;	
+
+	_enter_critical_bh(&free_queue->lock, &irqL);
+	
+	if (_rtw_queue_empty(free_queue) == _TRUE) {
+		pnetwork=NULL;
+		goto exit;
+	}
+	plist = get_next(&(free_queue->queue));
+	
+	pnetwork = LIST_CONTAINOR(plist , struct wlan_network, list);
+	
+	rtw_list_delete(&pnetwork->list);
+	
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("_rtw_alloc_network: ptr=%p\n", plist));
+	pnetwork->network_type = 0;
+	pnetwork->fixed = _FALSE;
+	pnetwork->last_scanned = rtw_get_current_time();
+	pnetwork->aid=0;	
+	pnetwork->join_res=0;
+
+	pmlmepriv->num_of_scanned ++;
+	
+exit:
+	_exit_critical_bh(&free_queue->lock, &irqL);
+
+_func_exit_;		
+
+	return pnetwork;	
+}
+
+void _rtw_free_network(struct	mlme_priv *pmlmepriv ,struct wlan_network *pnetwork, u8 isfreeall)
+{
+	u32 curr_time, delta_time;
+	u32 lifetime = SCANQUEUE_LIFETIME;
+	_irqL irqL;	
+	_queue *free_queue = &(pmlmepriv->free_bss_pool);
+	
+_func_enter_;		
+
+	if (pnetwork == NULL)
+		goto exit;
+
+	if (pnetwork->fixed == _TRUE)
+		goto exit;
+
+	curr_time = rtw_get_current_time();	
+
+	if ( (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)==_TRUE ) || 
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)==_TRUE ) )
+		lifetime = 1;
+
+	if(!isfreeall)
+	{
+#ifdef PLATFORM_WINDOWS
+
+		delta_time = (curr_time -pnetwork->last_scanned)/10;
+
+		if(delta_time  < lifetime*1000000)// unit:usec
+		{
+			goto exit;
+		}
+
+#endif
+
+#ifdef PLATFORM_LINUX
+
+		delta_time = (curr_time -pnetwork->last_scanned)/HZ;	
+
+		if(delta_time < lifetime)// unit:sec
+		{		
+			goto exit;
+		}
+
+#endif
+
+#ifdef PLATFORM_FREEBSD
+        //i think needs to check again
+		delta_time = (curr_time -pnetwork->last_scanned)/hz;	
+
+		if(delta_time < lifetime)// unit:sec
+		{		
+			goto exit;
+		}
+
+#endif
+	}
+
+	_enter_critical_bh(&free_queue->lock, &irqL);
+	
+	rtw_list_delete(&(pnetwork->list));
+
+	rtw_list_insert_tail(&(pnetwork->list),&(free_queue->queue));
+		
+	pmlmepriv->num_of_scanned --;
+	
+
+	//DBG_871X("_rtw_free_network:SSID=%s\n", pnetwork->network.Ssid.Ssid);
+	
+	_exit_critical_bh(&free_queue->lock, &irqL);
+	
+exit:		
+	
+_func_exit_;			
+
+}
+
+void _rtw_free_network_nolock(struct	mlme_priv *pmlmepriv, struct wlan_network *pnetwork)
+{
+
+	_queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+_func_enter_;		
+
+	if (pnetwork == NULL)
+		goto exit;
+
+	if (pnetwork->fixed == _TRUE)
+		goto exit;
+
+	//_enter_critical(&free_queue->lock, &irqL);
+	
+	rtw_list_delete(&(pnetwork->list));
+
+	rtw_list_insert_tail(&(pnetwork->list), get_list_head(free_queue));
+		
+	pmlmepriv->num_of_scanned --;
+	
+	//_exit_critical(&free_queue->lock, &irqL);
+	
+exit:		
+
+_func_exit_;			
+
+}
+
+
+/*
+	return the wlan_network with the matching addr
+
+	Shall be calle under atomic context... to avoid possible racing condition...
+*/
+struct wlan_network *_rtw_find_network(_queue *scanned_queue, u8 *addr)
+{
+
+	//_irqL irqL;
+	_list	*phead, *plist;
+	struct	wlan_network *pnetwork = NULL;
+	u8 zero_addr[ETH_ALEN] = {0,0,0,0,0,0};
+	
+_func_enter_;	
+
+	if(_rtw_memcmp(zero_addr, addr, ETH_ALEN)){
+		pnetwork=NULL;
+		goto exit;
+	}
+	
+	//_enter_critical_bh(&scanned_queue->lock, &irqL);
+	
+	phead = get_list_head(scanned_queue);
+	plist = get_next(phead);
+	 
+	while (plist != phead)
+       {
+                pnetwork = LIST_CONTAINOR(plist, struct wlan_network ,list);
+
+		if (_rtw_memcmp(addr, pnetwork->network.MacAddress, ETH_ALEN) == _TRUE)
+                        break;
+		
+		plist = get_next(plist);
+        }
+
+	if(plist == phead)
+		pnetwork = NULL;
+
+	//_exit_critical_bh(&scanned_queue->lock, &irqL);
+	
+exit:		
+	
+_func_exit_;		
+
+	return pnetwork;
+	
+}
+
+
+void _rtw_free_network_queue(_adapter *padapter, u8 isfreeall)
+{
+	_irqL irqL;
+	_list *phead, *plist;
+	struct wlan_network *pnetwork;
+	struct mlme_priv* pmlmepriv = &padapter->mlmepriv;
+	_queue *scanned_queue = &pmlmepriv->scanned_queue;
+
+_func_enter_;	
+	
+
+	_enter_critical_bh(&scanned_queue->lock, &irqL);
+
+	phead = get_list_head(scanned_queue);
+	plist = get_next(phead);
+
+	while (rtw_end_of_queue_search(phead, plist) == _FALSE)
+	{
+
+		pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		plist = get_next(plist);
+
+		_rtw_free_network(pmlmepriv,pnetwork, isfreeall);
+		
+	}
+
+	_exit_critical_bh(&scanned_queue->lock, &irqL);
+	
+_func_exit_;		
+
+}
+
+
+
+
+sint rtw_if_up(_adapter *padapter)	{
+
+	sint res;
+_func_enter_;		
+
+	if( padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+		(check_fwstate(&padapter->mlmepriv, _FW_LINKED)== _FALSE)){		
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_if_up:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved));	
+		res=_FALSE;
+	}
+	else
+		res=  _TRUE;
+	
+_func_exit_;
+	return res;
+}
+
+
+void rtw_generate_random_ibss(u8* pibss)
+{
+	u32	curtime = rtw_get_current_time();
+
+_func_enter_;
+	pibss[0] = 0x02;  //in ad-hoc mode bit1 must set to 1
+	pibss[1] = 0x11;
+	pibss[2] = 0x87;
+	pibss[3] = (u8)(curtime & 0xff) ;//p[0];
+	pibss[4] = (u8)((curtime>>8) & 0xff) ;//p[1];
+	pibss[5] = (u8)((curtime>>16) & 0xff) ;//p[2];
+_func_exit_;
+	return;
+}
+
+u8 *rtw_get_capability_from_ie(u8 *ie)
+{
+	return (ie + 8 + 2);
+}
+
+
+u16 rtw_get_capability(WLAN_BSSID_EX *bss)
+{
+	u16	val;
+_func_enter_;	
+
+	_rtw_memcpy((u8 *)&val, rtw_get_capability_from_ie(bss->IEs), 2); 
+
+_func_exit_;		
+	return le16_to_cpu(val);
+}
+
+u8 *rtw_get_timestampe_from_ie(u8 *ie)
+{
+	return (ie + 0);	
+}
+
+u8 *rtw_get_beacon_interval_from_ie(u8 *ie)
+{
+	return (ie + 8);	
+}
+
+
+int	rtw_init_mlme_priv (_adapter *padapter)//(struct	mlme_priv *pmlmepriv)
+{
+	int	res;
+_func_enter_;	
+	res = _rtw_init_mlme_priv(padapter);// (pmlmepriv);
+_func_exit_;	
+	return res;
+}
+
+void rtw_free_mlme_priv (struct mlme_priv *pmlmepriv)
+{
+_func_enter_;
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("rtw_free_mlme_priv\n"));
+	_rtw_free_mlme_priv (pmlmepriv);
+_func_exit_;	
+}
+
+int	rtw_enqueue_network(_queue *queue, struct wlan_network *pnetwork);
+int	rtw_enqueue_network(_queue *queue, struct wlan_network *pnetwork)
+{
+	int	res;
+_func_enter_;		
+	res = _rtw_enqueue_network(queue, pnetwork);
+_func_exit_;		
+	return res;
+}
+
+
+#ifndef PLATFORM_FREEBSD //Baron
+static struct	wlan_network *rtw_dequeue_network(_queue *queue)
+{
+	struct wlan_network *pnetwork;
+_func_enter_;		
+	pnetwork = _rtw_dequeue_network(queue);
+_func_exit_;		
+	return pnetwork;
+}
+#endif //PLATFORM_FREEBSD
+
+struct	wlan_network *rtw_alloc_network(struct	mlme_priv *pmlmepriv );
+struct	wlan_network *rtw_alloc_network(struct	mlme_priv *pmlmepriv )//(_queue	*free_queue)
+{
+	struct	wlan_network	*pnetwork;
+_func_enter_;			
+	pnetwork = _rtw_alloc_network(pmlmepriv);
+_func_exit_;			
+	return pnetwork;
+}
+
+void rtw_free_network(struct mlme_priv *pmlmepriv, struct	wlan_network *pnetwork, u8 is_freeall);
+void rtw_free_network(struct mlme_priv *pmlmepriv, struct	wlan_network *pnetwork, u8 is_freeall)//(struct	wlan_network *pnetwork, _queue	*free_queue)
+{
+_func_enter_;		
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("rtw_free_network==> ssid = %s \n\n" , pnetwork->network.Ssid.Ssid));
+	_rtw_free_network(pmlmepriv, pnetwork, is_freeall);
+_func_exit_;		
+}
+
+void rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork );
+void rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork )
+{
+_func_enter_;		
+	//RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("rtw_free_network==> ssid = %s \n\n" , pnetwork->network.Ssid.Ssid));
+	_rtw_free_network_nolock(pmlmepriv, pnetwork);
+_func_exit_;		
+}
+
+
+void rtw_free_network_queue(_adapter* dev, u8 isfreeall)
+{
+_func_enter_;		
+	_rtw_free_network_queue(dev, isfreeall);
+_func_exit_;			
+}
+
+/*
+	return the wlan_network with the matching addr
+
+	Shall be calle under atomic context... to avoid possible racing condition...
+*/
+struct	wlan_network *rtw_find_network(_queue *scanned_queue, u8 *addr)
+{
+	struct	wlan_network *pnetwork = _rtw_find_network(scanned_queue, addr);
+
+	return pnetwork;
+}
+
+int rtw_is_same_ibss(_adapter *adapter, struct wlan_network *pnetwork)
+{
+	int ret=_TRUE;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+
+	if ( (psecuritypriv->dot11PrivacyAlgrthm != _NO_PRIVACY_ ) &&
+		    ( pnetwork->network.Privacy == 0 ) )
+	{
+		ret=_FALSE;
+	}
+	else if((psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_ ) &&
+		 ( pnetwork->network.Privacy == 1 ) )
+	{
+		ret=_FALSE;
+	}
+	else
+	{
+		ret=_TRUE;
+	}
+	
+	return ret;
+	
+}
+
+inline int is_same_ess(WLAN_BSSID_EX *a, WLAN_BSSID_EX *b);
+inline int is_same_ess(WLAN_BSSID_EX *a, WLAN_BSSID_EX *b)
+{
+	//RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("(%s,%d)(%s,%d)\n",
+	//		a->Ssid.Ssid,a->Ssid.SsidLength,b->Ssid.Ssid,b->Ssid.SsidLength));
+	return (a->Ssid.SsidLength == b->Ssid.SsidLength) 
+		&&  _rtw_memcmp(a->Ssid.Ssid, b->Ssid.Ssid, a->Ssid.SsidLength)==_TRUE;
+}
+
+int is_same_network(WLAN_BSSID_EX *src, WLAN_BSSID_EX *dst)
+{
+	 u16 s_cap, d_cap;
+	 
+_func_enter_;	
+
+#ifdef PLATFORM_OS_XP
+	 if ( ((uint)dst) <= 0x7fffffff || 
+	 	((uint)src) <= 0x7fffffff ||
+	 	((uint)&s_cap) <= 0x7fffffff ||
+	 	((uint)&d_cap) <= 0x7fffffff)
+	{
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("\n@@@@ error address of dst\n"));
+			
+		KeBugCheckEx(0x87110000, (ULONG_PTR)dst, (ULONG_PTR)src,(ULONG_PTR)&s_cap, (ULONG_PTR)&d_cap);
+
+		return _FALSE;
+	}
+#endif
+
+
+	_rtw_memcpy((u8 *)&s_cap, rtw_get_capability_from_ie(src->IEs), 2);
+	_rtw_memcpy((u8 *)&d_cap, rtw_get_capability_from_ie(dst->IEs), 2);
+
+	
+	s_cap = le16_to_cpu(s_cap);
+	d_cap = le16_to_cpu(d_cap);
+	
+_func_exit_;			
+
+	return ((src->Ssid.SsidLength == dst->Ssid.SsidLength) &&
+		//	(src->Configuration.DSConfig == dst->Configuration.DSConfig) &&
+			( (_rtw_memcmp(src->MacAddress, dst->MacAddress, ETH_ALEN)) == _TRUE) &&
+			( (_rtw_memcmp(src->Ssid.Ssid, dst->Ssid.Ssid, src->Ssid.SsidLength)) == _TRUE) &&
+			((s_cap & WLAN_CAPABILITY_IBSS) == 
+			(d_cap & WLAN_CAPABILITY_IBSS)) &&
+			((s_cap & WLAN_CAPABILITY_BSS) == 
+			(d_cap & WLAN_CAPABILITY_BSS)));
+	
+}
+
+struct	wlan_network	* rtw_get_oldest_wlan_network(_queue *scanned_queue)
+{
+	_list	*plist, *phead;
+
+	
+	struct	wlan_network	*pwlan = NULL;
+	struct	wlan_network	*oldest = NULL;
+_func_enter_;		
+	phead = get_list_head(scanned_queue);
+	
+	plist = get_next(phead);
+
+	while(1)
+	{
+		
+		if (rtw_end_of_queue_search(phead,plist)== _TRUE)
+			break;
+		
+		pwlan= LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		if(pwlan->fixed!=_TRUE)
+		{		
+			if (oldest == NULL ||time_after(oldest->last_scanned, pwlan->last_scanned))
+				oldest = pwlan;
+		}
+		
+		plist = get_next(plist);
+	}
+_func_exit_;		
+	return oldest;
+	
+}
+
+void update_network(WLAN_BSSID_EX *dst, WLAN_BSSID_EX *src,
+	_adapter * padapter, bool update_ie)
+{
+	u8 ss_ori = dst->PhyInfo.SignalStrength;
+	u8 sq_ori = dst->PhyInfo.SignalQuality;
+	long rssi_ori = dst->Rssi;
+
+	u8 ss_smp = src->PhyInfo.SignalStrength;
+	u8 sq_smp = src->PhyInfo.SignalQuality;
+	long rssi_smp = src->Rssi;
+
+	u8 ss_final;
+	u8 sq_final;
+	long rssi_final;
+
+_func_enter_;		
+
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	rtw_hal_antdiv_rssi_compared(padapter, dst, src); //this will update src.Rssi, need consider again
+#endif
+
+	#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1	
+	if(strcmp(dst->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
+		DBG_871X("%s %s("MAC_FMT", ch%u) ss_ori:%3u, sq_ori:%3u, rssi_ori:%3ld, ss_smp:%3u, sq_smp:%3u, rssi_smp:%3ld\n"
+			, __FUNCTION__
+			, src->Ssid.Ssid, MAC_ARG(src->MacAddress), src->Configuration.DSConfig
+			,ss_ori, sq_ori, rssi_ori
+			,ss_smp, sq_smp, rssi_smp
+		);
+	}
+	#endif
+
+	/* The rule below is 1/5 for sample value, 4/5 for history value */
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) && is_same_network(&(padapter->mlmepriv.cur_network.network), src)) {
+		/* Take the recvpriv's value for the connected AP*/
+		ss_final = padapter->recvpriv.signal_strength;
+		sq_final = padapter->recvpriv.signal_qual;
+		/* the rssi value here is undecorated, and will be used for antenna diversity */
+		if(sq_smp != 101) /* from the right channel */
+			rssi_final = (src->Rssi+dst->Rssi*4)/5;
+		else
+			rssi_final = rssi_ori;
+	}
+	else {
+		if(sq_smp != 101) { /* from the right channel */
+			ss_final = ((u32)(src->PhyInfo.SignalStrength)+(u32)(dst->PhyInfo.SignalStrength)*4)/5;
+			sq_final = ((u32)(src->PhyInfo.SignalQuality)+(u32)(dst->PhyInfo.SignalQuality)*4)/5;
+			rssi_final = (src->Rssi+dst->Rssi*4)/5;
+		} else {
+			/* bss info not receving from the right channel, use the original RX signal infos */
+			ss_final = dst->PhyInfo.SignalStrength;
+			sq_final = dst->PhyInfo.SignalQuality;
+			rssi_final = dst->Rssi;
+		}
+		
+	}
+
+	if (update_ie)
+		_rtw_memcpy((u8 *)dst, (u8 *)src, get_WLAN_BSSID_EX_sz(src));
+
+	dst->PhyInfo.SignalStrength = ss_final;
+	dst->PhyInfo.SignalQuality = sq_final;
+	dst->Rssi = rssi_final;
+
+	#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1
+	if(strcmp(dst->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
+		DBG_871X("%s %s("MAC_FMT"), SignalStrength:%u, SignalQuality:%u, RawRSSI:%ld\n"
+			, __FUNCTION__
+			, dst->Ssid.Ssid, MAC_ARG(dst->MacAddress), dst->PhyInfo.SignalStrength, dst->PhyInfo.SignalQuality, dst->Rssi);
+	}
+	#endif
+
+#if 0 // old codes, may be useful one day...
+//	DBG_871X("update_network: rssi=0x%lx dst->Rssi=%d ,dst->Rssi=0x%lx , src->Rssi=0x%lx",(dst->Rssi+src->Rssi)/2,dst->Rssi,dst->Rssi,src->Rssi);
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) && is_same_network(&(padapter->mlmepriv.cur_network.network), src))
+	{
+	
+		//DBG_871X("b:ssid=%s update_network: src->rssi=0x%d padapter->recvpriv.ui_rssi=%d\n",src->Ssid.Ssid,src->Rssi,padapter->recvpriv.signal);
+		if(padapter->recvpriv.signal_qual_data.total_num++ >= PHY_LINKQUALITY_SLID_WIN_MAX)
+	        {
+	              padapter->recvpriv.signal_qual_data.total_num = PHY_LINKQUALITY_SLID_WIN_MAX;
+	              last_evm = padapter->recvpriv.signal_qual_data.elements[padapter->recvpriv.signal_qual_data.index];
+	              padapter->recvpriv.signal_qual_data.total_val -= last_evm;
+	        }
+               	padapter->recvpriv.signal_qual_data.total_val += query_rx_pwr_percentage(src->Rssi);
+
+              	padapter->recvpriv.signal_qual_data.elements[padapter->recvpriv.signal_qual_data.index++] = query_rx_pwr_percentage(src->Rssi);
+                if(padapter->recvpriv.signal_qual_data.index >= PHY_LINKQUALITY_SLID_WIN_MAX)
+                       padapter->recvpriv.signal_qual_data.index = 0;
+
+		//DBG_871X("Total SQ=%d  pattrib->signal_qual= %d\n", padapter->recvpriv.signal_qual_data.total_val, src->Rssi);
+
+		// <1> Showed on UI for user,in percentage.
+		tmpVal = padapter->recvpriv.signal_qual_data.total_val/padapter->recvpriv.signal_qual_data.total_num;
+                padapter->recvpriv.signal=(u8)tmpVal;//Link quality
+
+		src->Rssi= translate_percentage_to_dbm(padapter->recvpriv.signal) ;
+	}
+	else{
+//	DBG_871X("ELSE:ssid=%s update_network: src->rssi=0x%d dst->rssi=%d\n",src->Ssid.Ssid,src->Rssi,dst->Rssi);
+		src->Rssi=(src->Rssi +dst->Rssi)/2;//dBM
+	}	
+
+//	DBG_871X("a:update_network: src->rssi=0x%d padapter->recvpriv.ui_rssi=%d\n",src->Rssi,padapter->recvpriv.signal);
+
+#endif
+
+_func_exit_;		
+}
+
+static void update_current_network(_adapter *adapter, WLAN_BSSID_EX *pnetwork)
+{
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+	
+_func_enter_;		
+
+#ifdef PLATFORM_OS_XP
+	if ((unsigned long)(&(pmlmepriv->cur_network.network)) < 0x7ffffff)
+	{		
+		KeBugCheckEx(0x87111c1c, (ULONG_PTR)(&(pmlmepriv->cur_network.network)), 0, 0,0);
+	}
+#endif
+
+	if ( (check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE) && (is_same_network(&(pmlmepriv->cur_network.network), pnetwork)))
+	{
+		//RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,"Same Network\n");
+
+		//if(pmlmepriv->cur_network.network.IELength<= pnetwork->IELength)
+		{
+			update_network(&(pmlmepriv->cur_network.network), pnetwork,adapter, _TRUE);
+			rtw_update_protection(adapter, (pmlmepriv->cur_network.network.IEs) + sizeof (NDIS_802_11_FIXED_IEs), 
+									pmlmepriv->cur_network.network.IELength);
+		}
+	}
+
+_func_exit_;			
+
+}
+
+
+/*
+
+Caller must hold pmlmepriv->lock first.
+
+
+*/
+void rtw_update_scanned_network(_adapter *adapter, WLAN_BSSID_EX *target)
+{
+	_irqL irqL;
+	_list	*plist, *phead;
+	ULONG	bssid_ex_sz;
+	struct mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+	_queue	*queue	= &(pmlmepriv->scanned_queue);
+	struct wlan_network	*pnetwork = NULL;
+	struct wlan_network	*oldest = NULL;
+
+_func_enter_;
+
+	_enter_critical_bh(&queue->lock, &irqL);
+	phead = get_list_head(queue);
+	plist = get_next(phead);
+
+	while(1)
+	{
+		if (rtw_end_of_queue_search(phead,plist)== _TRUE)
+			break;
+
+		pnetwork	= LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		if ((unsigned long)(pnetwork) < 0x7ffffff)
+		{
+#ifdef PLATFORM_OS_XP
+			KeBugCheckEx(0x87111c1c, (ULONG_PTR)pnetwork, 0, 0,0);
+#endif
+		}
+
+		if (is_same_network(&(pnetwork->network), target))
+			break;
+
+		if ((oldest == ((struct wlan_network *)0)) ||
+		time_after(oldest->last_scanned, pnetwork->last_scanned))
+			oldest = pnetwork;
+
+		plist = get_next(plist);
+
+	}
+	
+	
+	/* If we didn't find a match, then get a new network slot to initialize
+	 * with this beacon's information */
+	if (rtw_end_of_queue_search(phead,plist)== _TRUE) {
+		
+		if (_rtw_queue_empty(&(pmlmepriv->free_bss_pool)) == _TRUE) {
+			/* If there are no more slots, expire the oldest */
+			//list_del_init(&oldest->list);
+			pnetwork = oldest;
+
+#ifdef CONFIG_ANTENNA_DIVERSITY
+			//target->PhyInfo.Optimum_antenna = pHalData->CurAntenna;//optimum_antenna=>For antenna diversity
+			rtw_hal_get_def_var(adapter, HAL_DEF_CURRENT_ANTENNA, &(target->PhyInfo.Optimum_antenna));
+#endif
+			_rtw_memcpy(&(pnetwork->network), target,  get_WLAN_BSSID_EX_sz(target));
+			//pnetwork->last_scanned = rtw_get_current_time();
+			// variable initialize
+			pnetwork->fixed = _FALSE;
+			pnetwork->last_scanned = rtw_get_current_time();
+
+			pnetwork->network_type = 0;	
+			pnetwork->aid=0;		
+			pnetwork->join_res=0;
+
+			/* bss info not receving from the right channel */
+			if (pnetwork->network.PhyInfo.SignalQuality == 101)
+				pnetwork->network.PhyInfo.SignalQuality = 0;
+		}
+		else {
+			/* Otherwise just pull from the free list */
+
+			pnetwork = rtw_alloc_network(pmlmepriv); // will update scan_time
+
+			if(pnetwork==NULL){ 
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("\n\n\nsomething wrong here\n\n\n"));
+				goto exit;
+			}
+
+			bssid_ex_sz = get_WLAN_BSSID_EX_sz(target);
+			target->Length = bssid_ex_sz;
+#ifdef CONFIG_ANTENNA_DIVERSITY
+			//target->PhyInfo.Optimum_antenna = pHalData->CurAntenna;
+			rtw_hal_get_def_var(adapter, HAL_DEF_CURRENT_ANTENNA, &(target->PhyInfo.Optimum_antenna));
+#endif
+			_rtw_memcpy(&(pnetwork->network), target, bssid_ex_sz );
+
+			pnetwork->last_scanned = rtw_get_current_time();
+
+			/* bss info not receving from the right channel */
+			if (pnetwork->network.PhyInfo.SignalQuality == 101)
+				pnetwork->network.PhyInfo.SignalQuality = 0;
+
+			rtw_list_insert_tail(&(pnetwork->list),&(queue->queue)); 
+
+		}
+	}
+	else {
+		/* we have an entry and we are going to update it. But this entry may
+		 * be already expired. In this case we do the same as we found a new 
+		 * net and call the new_net handler
+		 */
+		bool update_ie = _TRUE;
+
+		pnetwork->last_scanned = rtw_get_current_time();
+
+		//target.Reserved[0]==1, means that scaned network is a bcn frame.
+		if((pnetwork->network.IELength>target->IELength) && (target->Reserved[0]==1))
+			update_ie = _FALSE;
+
+		update_network(&(pnetwork->network), target,adapter, update_ie);
+	}
+
+exit:
+	_exit_critical_bh(&queue->lock, &irqL);
+
+_func_exit_;
+}
+
+void rtw_add_network(_adapter *adapter, WLAN_BSSID_EX *pnetwork);
+void rtw_add_network(_adapter *adapter, WLAN_BSSID_EX *pnetwork)
+{
+	_irqL irqL;
+	struct	mlme_priv	*pmlmepriv = &(((_adapter *)adapter)->mlmepriv);
+	//_queue	*queue	= &(pmlmepriv->scanned_queue);
+
+_func_enter_;		
+
+	//_enter_critical_bh(&queue->lock, &irqL);
+
+	#if defined(CONFIG_P2P) && defined(CONFIG_P2P_REMOVE_GROUP_INFO)
+	rtw_WLAN_BSSID_EX_remove_p2p_attr(pnetwork, P2P_ATTR_GROUP_INFO);
+	#endif
+	
+	update_current_network(adapter, pnetwork);
+	
+	rtw_update_scanned_network(adapter, pnetwork);
+
+	//_exit_critical_bh(&queue->lock, &irqL);
+	
+_func_exit_;		
+}
+
+//select the desired network based on the capability of the (i)bss.
+// check items: (1) security
+//			   (2) network_type
+//			   (3) WMM
+//			   (4) HT
+//                     (5) others
+int rtw_is_desired_network(_adapter *adapter, struct wlan_network *pnetwork);
+int rtw_is_desired_network(_adapter *adapter, struct wlan_network *pnetwork)
+{
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u32 desired_encmode;
+	u32 privacy;
+
+	//u8 wps_ie[512];
+	uint wps_ielen;
+
+	int bselected = _TRUE;
+	
+	desired_encmode = psecuritypriv->ndisencryptstatus;
+	privacy = pnetwork->network.Privacy;
+
+	if(check_fwstate(pmlmepriv, WIFI_UNDER_WPS))
+	{
+		if(rtw_get_wps_ie(pnetwork->network.IEs+_FIXED_IE_LENGTH_, pnetwork->network.IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen)!=NULL)
+		{
+			return _TRUE;
+		}
+		else
+		{	
+			return _FALSE;
+		}	
+	}
+	if (adapter->registrypriv.wifi_spec == 1) //for  correct flow of 8021X  to do....
+	{
+		if ((desired_encmode == Ndis802_11EncryptionDisabled) && (privacy != 0))
+	            bselected = _FALSE;
+	}
+	
+
+ 	if ((desired_encmode != Ndis802_11EncryptionDisabled) && (privacy == 0)) {
+		DBG_871X("desired_encmode: %d, privacy: %d\n", desired_encmode, privacy);
+		bselected = _FALSE;
+ 	}
+
+	if(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE)
+	{
+		if(pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode)
+			bselected = _FALSE;
+	}	
+		
+
+	return bselected;
+}
+
+/* TODO: Perry : For Power Management */
+void rtw_atimdone_event_callback(_adapter	*adapter , u8 *pbuf)
+{
+
+_func_enter_;		
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("receive atimdone_evet\n"));	
+_func_exit_;			
+	return;	
+}
+
+
+void rtw_survey_event_callback(_adapter	*adapter, u8 *pbuf)
+{
+	_irqL  irqL;
+	u32 len;
+	WLAN_BSSID_EX *pnetwork;
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+
+_func_enter_;		
+
+	pnetwork = (WLAN_BSSID_EX *)pbuf;
+
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_survey_event_callback, ssid=%s\n",  pnetwork->Ssid.Ssid));
+
+#ifdef CONFIG_RTL8712
+        //endian_convert
+ 	pnetwork->Length = le32_to_cpu(pnetwork->Length);
+  	pnetwork->Ssid.SsidLength = le32_to_cpu(pnetwork->Ssid.SsidLength);	
+	pnetwork->Privacy =le32_to_cpu( pnetwork->Privacy);
+	pnetwork->Rssi = le32_to_cpu(pnetwork->Rssi);
+	pnetwork->NetworkTypeInUse =le32_to_cpu(pnetwork->NetworkTypeInUse);	
+	pnetwork->Configuration.ATIMWindow = le32_to_cpu(pnetwork->Configuration.ATIMWindow);
+	pnetwork->Configuration.BeaconPeriod = le32_to_cpu(pnetwork->Configuration.BeaconPeriod);
+	pnetwork->Configuration.DSConfig =le32_to_cpu(pnetwork->Configuration.DSConfig);
+	pnetwork->Configuration.FHConfig.DwellTime=le32_to_cpu(pnetwork->Configuration.FHConfig.DwellTime);
+	pnetwork->Configuration.FHConfig.HopPattern=le32_to_cpu(pnetwork->Configuration.FHConfig.HopPattern);
+	pnetwork->Configuration.FHConfig.HopSet=le32_to_cpu(pnetwork->Configuration.FHConfig.HopSet);
+	pnetwork->Configuration.FHConfig.Length=le32_to_cpu(pnetwork->Configuration.FHConfig.Length);	
+	pnetwork->Configuration.Length = le32_to_cpu(pnetwork->Configuration.Length);
+	pnetwork->InfrastructureMode = le32_to_cpu(pnetwork->InfrastructureMode);
+	pnetwork->IELength = le32_to_cpu(pnetwork->IELength);
+#endif	
+
+	len = get_WLAN_BSSID_EX_sz(pnetwork);
+	if(len > (sizeof(WLAN_BSSID_EX)))
+	{
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("\n ****rtw_survey_event_callback: return a wrong bss ***\n"));
+		return;
+	}
+
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	// update IBSS_network 's timestamp
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) == _TRUE)
+	{
+		//RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,"rtw_survey_event_callback : WIFI_ADHOC_MASTER_STATE \n\n");
+		if(_rtw_memcmp(&(pmlmepriv->cur_network.network.MacAddress), pnetwork->MacAddress, ETH_ALEN))
+		{
+			struct wlan_network* ibss_wlan = NULL;
+			_irqL	irqL;
+			
+			_rtw_memcpy(pmlmepriv->cur_network.network.IEs, pnetwork->IEs, 8);
+			_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+			ibss_wlan = rtw_find_network(&pmlmepriv->scanned_queue,  pnetwork->MacAddress);
+			if(ibss_wlan)
+			{
+				_rtw_memcpy(ibss_wlan->network.IEs , pnetwork->IEs, 8);			
+				_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);		
+				goto exit;
+			}
+			_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+		}
+	}
+
+	// lock pmlmepriv->lock when you accessing network_q
+	if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == _FALSE)
+	{		
+   	        if( pnetwork->Ssid.Ssid[0] == 0 )
+		{
+			pnetwork->Ssid.SsidLength = 0;
+		}	
+		rtw_add_network(adapter, pnetwork);
+	}	
+
+exit:	
+		
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+_func_exit_;		
+
+	return;	
+}
+
+
+
+void rtw_surveydone_event_callback(_adapter	*adapter, u8 *pbuf)
+{
+	_irqL  irqL;
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+	
+#ifdef CONFIG_MLME_EXT	
+
+	mlmeext_surveydone_event_callback(adapter);
+
+#endif
+
+_func_enter_;			
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	if(pmlmepriv->wps_probe_req_ie)
+	{
+		u32 free_len = pmlmepriv->wps_probe_req_ie_len;
+		pmlmepriv->wps_probe_req_ie_len = 0;
+		rtw_mfree(pmlmepriv->wps_probe_req_ie, free_len);
+		pmlmepriv->wps_probe_req_ie = NULL;			
+	}
+	
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_surveydone_event_callback: fw_state:%x\n\n", get_fwstate(pmlmepriv)));
+	
+	if (check_fwstate(pmlmepriv,_FW_UNDER_SURVEY))
+	{
+		u8 timer_cancelled;
+		
+		_cancel_timer(&pmlmepriv->scan_to_timer, &timer_cancelled);
+		
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	}
+	else {
+	
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("nic status =%x, survey done event comes too late!\n", get_fwstate(pmlmepriv)));	
+	}
+
+	#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+	rtw_set_signal_stat_timer(&adapter->recvpriv);
+	#endif
+
+	if(pmlmepriv->to_join == _TRUE)
+	{
+		if((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)==_TRUE) )
+		{
+			if(check_fwstate(pmlmepriv, _FW_LINKED)==_FALSE)
+			{
+				set_fwstate(pmlmepriv, _FW_UNDER_LINKING);	
+				
+		   		if(rtw_select_and_join_from_scanned_queue(pmlmepriv)==_SUCCESS)
+		   		{
+		       			_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT );	 
+                  		}
+		   		else	
+		  		{
+					WLAN_BSSID_EX    *pdev_network = &(adapter->registrypriv.dev_network); 			
+					u8 *pibss = adapter->registrypriv.dev_network.MacAddress;
+
+					//pmlmepriv->fw_state ^= _FW_UNDER_SURVEY;//because don't set assoc_timer
+					_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+					RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("switching to adhoc master\n"));
+				
+					_rtw_memset(&pdev_network->Ssid, 0, sizeof(NDIS_802_11_SSID));
+					_rtw_memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(NDIS_802_11_SSID));
+	
+					rtw_update_registrypriv_dev_network(adapter);
+					rtw_generate_random_ibss(pibss);
+
+                       			pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+			
+					if(rtw_createbss_cmd(adapter)!=_SUCCESS)
+					{
+	                     		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Error=>rtw_createbss_cmd status FAIL\n"));						
+					}	
+
+			     		pmlmepriv->to_join = _FALSE;
+		   		}
+		 	}
+		}
+		else
+		{
+			int s_ret;
+			set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+			pmlmepriv->to_join = _FALSE;
+			if(_SUCCESS == (s_ret=rtw_select_and_join_from_scanned_queue(pmlmepriv)))
+			{
+	     		     _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);	 
+			}
+			else if(s_ret == 2)//there is no need to wait for join
+			{
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+				rtw_indicate_connect(adapter);
+			}
+			else
+			{
+				DBG_871X("try_to_join, but select scanning queue fail, to_roaming:%d\n", rtw_to_roaming(adapter));
+				#ifdef CONFIG_LAYER2_ROAMING
+				if (rtw_to_roaming(adapter) != 0) {
+					if( --pmlmepriv->to_roaming == 0
+						|| _SUCCESS != rtw_sitesurvey_cmd(adapter, &pmlmepriv->assoc_ssid, 1, NULL, 0)
+					) {
+						rtw_set_roaming(adapter, 0);
+#ifdef CONFIG_INTEL_WIDI
+						if(adapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING)
+						{
+							_rtw_memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN);
+							intel_widi_wk_cmd(adapter, INTEL_WIDI_LISTEN_WK, NULL);
+							DBG_871X("change to widi listen\n");
+						}
+#endif // CONFIG_INTEL_WIDI
+						rtw_free_assoc_resources(adapter, 1);
+						rtw_indicate_disconnect(adapter);
+					} else {
+						pmlmepriv->to_join = _TRUE;
+					}
+				}
+				#endif
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+			}
+		}
+	}
+
+	indicate_wx_scan_complete_event(adapter);
+	//DBG_871X("scan complete in %dms\n",rtw_get_passing_time_ms(pmlmepriv->scan_start_time));
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+#ifdef CONFIG_P2P_PS
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE) {
+		p2p_ps_wk_cmd(adapter, P2P_PS_SCAN_DONE, 0);
+	}
+#endif // CONFIG_P2P_PS
+
+	rtw_os_xmit_schedule(adapter);
+#ifdef CONFIG_CONCURRENT_MODE	
+	rtw_os_xmit_schedule(adapter->pbuddy_adapter);
+#endif
+#ifdef CONFIG_DUALMAC_CONCURRENT
+	dc_resume_xmit(adapter);
+#endif
+
+#ifdef CONFIG_DRVEXT_MODULE_WSC
+	drvext_surveydone_callback(&adapter->drvextpriv);
+#endif
+
+#ifdef DBG_CONFIG_ERROR_DETECT
+	{
+		struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;		
+		if(pmlmeext->sitesurvey_res.bss_cnt == 0){
+			rtw_hal_sreset_reset(adapter);			
+		}
+	}
+#endif
+
+#ifdef CONFIG_IOCTL_CFG80211
+	rtw_cfg80211_surveydone_event_callback(adapter);
+#endif //CONFIG_IOCTL_CFG80211
+
+_func_exit_;	
+
+}
+
+void rtw_dummy_event_callback(_adapter *adapter , u8 *pbuf)
+{
+
+}
+
+void rtw_fwdbg_event_callback(_adapter *adapter , u8 *pbuf)
+{
+
+}
+
+static void free_scanqueue(struct	mlme_priv *pmlmepriv)
+{
+	_irqL irqL, irqL0;
+	_queue *free_queue = &pmlmepriv->free_bss_pool;
+	_queue *scan_queue = &pmlmepriv->scanned_queue;
+	_list	*plist, *phead, *ptemp;
+	
+_func_enter_;		
+	
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+free_scanqueue\n"));
+	_enter_critical_bh(&scan_queue->lock, &irqL0);
+	_enter_critical_bh(&free_queue->lock, &irqL);
+
+	phead = get_list_head(scan_queue);
+	plist = get_next(phead);
+
+	while (plist != phead)
+       {
+		ptemp = get_next(plist);
+		rtw_list_delete(plist);
+		rtw_list_insert_tail(plist, &free_queue->queue);
+		plist =ptemp;
+		pmlmepriv->num_of_scanned --;
+        }
+	
+	_exit_critical_bh(&free_queue->lock, &irqL);
+	_exit_critical_bh(&scan_queue->lock, &irqL0);
+	
+_func_exit_;
+}
+	
+/*
+*rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock
+*/
+void rtw_free_assoc_resources(_adapter *adapter, int lock_scanned_queue)
+{
+	_irqL irqL;
+	struct wlan_network* pwlan = NULL;
+     	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+   	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+	
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &adapter->tdlsinfo;
+#endif //CONFIG_TDLS
+_func_enter_;			
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+rtw_free_assoc_resources\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("tgt_network->network.MacAddress="MAC_FMT" ssid=%s\n",
+		MAC_ARG(tgt_network->network.MacAddress), tgt_network->network.Ssid.Ssid));
+
+	if(check_fwstate( pmlmepriv, WIFI_STATION_STATE|WIFI_AP_STATE))
+	{
+		struct sta_info* psta;
+		
+		psta = rtw_get_stainfo(&adapter->stapriv, tgt_network->network.MacAddress);
+
+#ifdef CONFIG_TDLS
+		if(ptdlsinfo->setup_state != TDLS_STATE_NONE)
+		{
+			rtw_tdls_cmd(adapter, myid(&(adapter->eeprompriv)), TDLS_RS_RCR);
+			rtw_reset_tdls_info(adapter);
+			rtw_free_all_stainfo(adapter);
+			_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
+		}
+		else
+#endif //CONFIG_TDLS
+		{
+			_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
+			rtw_free_stainfo(adapter,  psta);
+		}
+
+		_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
+		
+	}
+
+	if(check_fwstate( pmlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE))
+	{
+		struct sta_info* psta;
+	
+		rtw_free_all_stainfo(adapter);
+
+		psta = rtw_get_bcmc_stainfo(adapter);
+		_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		
+		rtw_free_stainfo(adapter, psta);
+		_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		
+
+		rtw_init_bcmc_stainfo(adapter);	
+	}
+
+	if(lock_scanned_queue)
+		_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+	
+	pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+	if(pwlan)		
+	{
+		pwlan->fixed = _FALSE;
+	}	
+	else
+	{
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("rtw_free_assoc_resources : pwlan== NULL \n\n"));
+	}
+
+
+	if((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) && (adapter->stapriv.asoc_sta_count== 1))
+		/*||check_fwstate(pmlmepriv, WIFI_STATION_STATE)*/)
+	{
+		rtw_free_network_nolock(pmlmepriv, pwlan); 
+	}
+
+	if(lock_scanned_queue)
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+	
+	pmlmepriv->key_mask = 0;
+
+_func_exit_;	
+	
+}
+
+/*
+*rtw_indicate_connect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_connect(_adapter *padapter)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_connect\n"));
+ 
+	pmlmepriv->to_join = _FALSE;
+
+	if(!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) 
+	{
+
+#ifdef CONFIG_SW_ANTENNA_DIVERSITY
+		rtw_hal_set_hwreg(padapter, HW_VAR_ANTENNA_DIVERSITY_LINK, 0);
+#endif
+
+		set_fwstate(pmlmepriv, _FW_LINKED);
+
+		rtw_led_control(padapter, LED_CTL_LINK);
+
+	
+#ifdef CONFIG_DRVEXT_MODULE
+		if(padapter->drvextpriv.enable_wpa)
+		{
+			indicate_l2_connect(padapter);
+		}
+		else
+#endif
+		{
+			rtw_os_indicate_connect(padapter);
+		}
+
+	}
+
+	rtw_set_roaming(padapter, 0);
+#ifdef CONFIG_INTEL_WIDI
+	if(padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING)
+	{
+		_rtw_memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN);
+		intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_WK, NULL);
+		DBG_871X("change to widi listen\n");
+	}
+#endif // CONFIG_INTEL_WIDI
+
+	rtw_set_scan_deny(padapter, 3000);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("-rtw_indicate_connect: fw_state=0x%08x\n", get_fwstate(pmlmepriv)));
+ 
+_func_exit_;
+
+}
+
+
+/*
+*rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_disconnect( _adapter *padapter )
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;	
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX	*cur_network = &(pmlmeinfo->network);
+	struct sta_info *psta;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+_func_enter_;	
+	
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_disconnect\n"));
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING|WIFI_UNDER_WPS);
+
+        //DBG_871X("clear wps when %s\n", __func__);
+
+	if(rtw_to_roaming(padapter) > 0)
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+
+#ifdef CONFIG_WAPI_SUPPORT
+	psta = rtw_get_stainfo(pstapriv,cur_network->MacAddress);
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+	{
+		rtw_wapi_return_one_sta_info(padapter, psta->hwaddr);
+	}
+	else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+		check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
+	{
+		rtw_wapi_return_all_sta_info(padapter);
+	}
+#endif
+
+	if(check_fwstate(&padapter->mlmepriv, _FW_LINKED) 
+		|| (rtw_to_roaming(padapter) <= 0)
+	)
+	{
+		rtw_os_indicate_disconnect(padapter);
+
+		//set ips_deny_time to avoid enter IPS before LPS leave
+		padapter->pwrctrlpriv.ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime(3000);
+
+	      _clr_fwstate_(pmlmepriv, _FW_LINKED);
+
+		rtw_led_control(padapter, LED_CTL_NO_LINK);
+
+		rtw_clear_scan_deny(padapter);
+
+	}
+
+#ifdef CONFIG_P2P_PS
+	p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+#endif // CONFIG_P2P_PS
+
+#ifdef CONFIG_LPS
+#ifdef CONFIG_WOWLAN
+	if(padapter->pwrctrlpriv.wowlan_mode==_FALSE)
+#endif //CONFIG_WOWLAN
+	rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 1);
+#endif
+
+_func_exit_;	
+}
+
+inline void rtw_indicate_scan_done( _adapter *padapter, bool aborted)
+{
+	rtw_os_indicate_scan_done(padapter, aborted);
+}
+
+void rtw_scan_abort(_adapter *adapter)
+{
+	u32 cnt=0;
+	u32 start;
+	struct mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(adapter->mlmeextpriv);
+
+	start = rtw_get_current_time();
+	pmlmeext->scan_abort = _TRUE;
+	while (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)
+		&& rtw_get_passing_time_ms(start) <= 200) {
+
+		if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+			break;
+
+		DBG_871X(FUNC_NDEV_FMT"fw_state=_FW_UNDER_SURVEY!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+		rtw_msleep_os(20);
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+		if (!adapter->bDriverStopped && !adapter->bSurpriseRemoved)
+			DBG_871X(FUNC_NDEV_FMT"waiting for scan_abort time out!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+		#ifdef CONFIG_PLATFORM_MSTAR_TITANIA12	
+		//_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+		set_survey_timer(pmlmeext, 0);
+		_set_timer(&pmlmepriv->scan_to_timer, 50);
+		#endif
+		rtw_indicate_scan_done(adapter, _TRUE);
+	}
+	pmlmeext->scan_abort = _FALSE;
+}
+
+static struct sta_info *rtw_joinbss_update_stainfo(_adapter *padapter, struct wlan_network *pnetwork)
+{
+	int i;
+	struct sta_info *bmc_sta, *psta=NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct sta_priv *pstapriv = &padapter->stapriv;	
+
+	psta = rtw_get_stainfo(pstapriv, pnetwork->network.MacAddress);
+	if(psta==NULL) {
+		psta = rtw_alloc_stainfo(pstapriv, pnetwork->network.MacAddress);
+	}
+
+	if(psta) //update ptarget_sta
+	{
+		DBG_871X("%s\n", __FUNCTION__);
+	
+		psta->aid  = pnetwork->join_res;
+#ifdef CONFIG_CONCURRENT_MODE	
+
+			if(PRIMARY_ADAPTER == padapter->adapter_type)
+				psta->mac_id=0;
+			else
+				psta->mac_id=2;
+#else
+			psta->mac_id=0;
+#endif		
+
+		//sta mode
+		rtw_hal_set_odm_var(padapter,HAL_ODM_STA_INFO,psta,_TRUE);
+
+		//security related
+		if(padapter->securitypriv.dot11AuthAlgrthm== dot11AuthAlgrthm_8021X)
+		{						
+			padapter->securitypriv.binstallGrpkey=_FALSE;
+			padapter->securitypriv.busetkipkey=_FALSE;						
+			padapter->securitypriv.bgrpkey_handshake=_FALSE;
+
+			psta->ieee8021x_blocked=_TRUE;
+			psta->dot118021XPrivacy=padapter->securitypriv.dot11PrivacyAlgrthm;
+						
+			_rtw_memset((u8 *)&psta->dot118021x_UncstKey, 0, sizeof (union Keytype));
+						
+			_rtw_memset((u8 *)&psta->dot11tkiprxmickey, 0, sizeof (union Keytype));
+			_rtw_memset((u8 *)&psta->dot11tkiptxmickey, 0, sizeof (union Keytype));
+						
+			_rtw_memset((u8 *)&psta->dot11txpn, 0, sizeof (union pn48));	
+			_rtw_memset((u8 *)&psta->dot11rxpn, 0, sizeof (union pn48));	
+		}
+
+		//	Commented by Albert 2012/07/21
+		//	When doing the WPS, the wps_ie_len won't equal to 0
+		//	And the Wi-Fi driver shouldn't allow the data packet to be tramsmitted.
+		if ( padapter->securitypriv.wps_ie_len != 0 )
+		{
+			psta->ieee8021x_blocked=_TRUE;
+			padapter->securitypriv.wps_ie_len = 0;
+		}
+
+
+		//for A-MPDU Rx reordering buffer control for bmc_sta & sta_info
+		//if A-MPDU Rx is enabled, reseting  rx_ordering_ctrl wstart_b(indicate_seq) to default value=0xffff
+		//todo: check if AP can send A-MPDU packets
+		for(i=0; i < 16 ; i++)
+		{
+			//preorder_ctrl = &precvpriv->recvreorder_ctrl[i];
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+			preorder_ctrl->enable = _FALSE;
+			preorder_ctrl->indicate_seq = 0xffff;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
+				preorder_ctrl->indicate_seq);
+			#endif
+			preorder_ctrl->wend_b= 0xffff;
+			preorder_ctrl->wsize_b = 64;//max_ampdu_sz;//ex. 32(kbytes) -> wsize_b=32
+		}
+
+		
+		bmc_sta = rtw_get_bcmc_stainfo(padapter);
+		if(bmc_sta)
+		{
+			for(i=0; i < 16 ; i++)
+			{
+				//preorder_ctrl = &precvpriv->recvreorder_ctrl[i];
+				preorder_ctrl = &bmc_sta->recvreorder_ctrl[i];
+				preorder_ctrl->enable = _FALSE;
+				preorder_ctrl->indicate_seq = 0xffff;
+				#ifdef DBG_RX_SEQ
+				DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
+					preorder_ctrl->indicate_seq);
+				#endif
+				preorder_ctrl->wend_b= 0xffff;
+				preorder_ctrl->wsize_b = 64;//max_ampdu_sz;//ex. 32(kbytes) -> wsize_b=32
+			}
+		}
+
+	
+		//misc.
+		update_sta_info(padapter, psta);
+		
+	}
+					
+	return psta;
+	
+}
+
+//pnetwork : returns from rtw_joinbss_event_callback
+//ptarget_wlan: found from scanned_queue
+static void rtw_joinbss_update_network(_adapter *padapter, struct wlan_network *ptarget_wlan, struct wlan_network  *pnetwork)
+{
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);	
+	struct wlan_network  *cur_network = &(pmlmepriv->cur_network);
+
+	DBG_871X("%s\n", __FUNCTION__);
+	
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\nfw_state:%x, BSSID:"MAC_FMT"\n"
+		,get_fwstate(pmlmepriv), MAC_ARG(pnetwork->network.MacAddress)));
+
+				
+	// why not use ptarget_wlan??
+	_rtw_memcpy(&cur_network->network, &pnetwork->network, pnetwork->network.Length);
+	// some IEs in pnetwork is wrong, so we should use ptarget_wlan IEs
+	cur_network->network.IELength = ptarget_wlan->network.IELength;
+	_rtw_memcpy(&cur_network->network.IEs[0], &ptarget_wlan->network.IEs[0], MAX_IE_SZ);
+
+	cur_network->aid = pnetwork->join_res;
+
+				
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+#endif
+	padapter->recvpriv.signal_strength = ptarget_wlan->network.PhyInfo.SignalStrength;
+	padapter->recvpriv.signal_qual = ptarget_wlan->network.PhyInfo.SignalQuality;
+	//the ptarget_wlan->network.Rssi is raw data, we use ptarget_wlan->network.PhyInfo.SignalStrength instead (has scaled)
+	padapter->recvpriv.rssi = translate_percentage_to_dbm(ptarget_wlan->network.PhyInfo.SignalStrength);
+	#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1
+		DBG_871X("%s signal_strength:%3u, rssi:%3d, signal_qual:%3u"
+			"\n"
+			, __FUNCTION__
+			, padapter->recvpriv.signal_strength
+			, padapter->recvpriv.rssi
+			, padapter->recvpriv.signal_qual
+	);
+	#endif
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+#endif
+				
+	//update fw_state //will clr _FW_UNDER_LINKING here indirectly
+	switch(pnetwork->network.InfrastructureMode)
+	{	
+		case Ndis802_11Infrastructure:						
+			
+				if(pmlmepriv->fw_state&WIFI_UNDER_WPS)
+					pmlmepriv->fw_state = WIFI_STATION_STATE|WIFI_UNDER_WPS;
+				else
+					pmlmepriv->fw_state = WIFI_STATION_STATE;
+				
+				break;
+		case Ndis802_11IBSS:		
+				pmlmepriv->fw_state = WIFI_ADHOC_STATE;
+				break;
+		default:
+				pmlmepriv->fw_state = WIFI_NULL_STATE;
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Invalid network_mode\n"));
+				break;
+	}
+
+	rtw_update_protection(padapter, (cur_network->network.IEs) + sizeof (NDIS_802_11_FIXED_IEs), 
+									(cur_network->network.IELength));
+			
+#ifdef CONFIG_80211N_HT			
+	rtw_update_ht_cap(padapter, cur_network->network.IEs, cur_network->network.IELength);
+#endif
+
+
+}
+
+//Notes: the fucntion could be > passive_level (the same context as Rx tasklet)
+//pnetwork : returns from rtw_joinbss_event_callback
+//ptarget_wlan: found from scanned_queue
+//if join_res > 0, for (fw_state==WIFI_STATION_STATE), we check if  "ptarget_sta" & "ptarget_wlan" exist.	
+//if join_res > 0, for (fw_state==WIFI_ADHOC_STATE), we only check if "ptarget_wlan" exist.
+//if join_res > 0, update "cur_network->network" from "pnetwork->network" if (ptarget_wlan !=NULL).
+//
+//#define REJOIN
+void rtw_joinbss_event_prehandle(_adapter *adapter, u8 *pbuf)
+{
+	_irqL irqL,irqL2;
+	static u8 retry=0;
+	u8 timer_cancelled;
+	struct sta_info *ptarget_sta= NULL, *pcur_sta = NULL;
+   	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+	struct wlan_network 	*pnetwork	= (struct wlan_network *)pbuf;
+	struct wlan_network 	*cur_network = &(pmlmepriv->cur_network);
+	struct wlan_network	*pcur_wlan = NULL, *ptarget_wlan = NULL;
+	unsigned int 		the_same_macaddr = _FALSE;	
+
+_func_enter_;	
+
+#ifdef CONFIG_RTL8712
+       //endian_convert
+	pnetwork->join_res = le32_to_cpu(pnetwork->join_res);
+	pnetwork->network_type = le32_to_cpu(pnetwork->network_type);
+	pnetwork->network.Length = le32_to_cpu(pnetwork->network.Length);
+	pnetwork->network.Ssid.SsidLength = le32_to_cpu(pnetwork->network.Ssid.SsidLength);
+	pnetwork->network.Privacy =le32_to_cpu( pnetwork->network.Privacy);
+	pnetwork->network.Rssi = le32_to_cpu(pnetwork->network.Rssi);
+	pnetwork->network.NetworkTypeInUse =le32_to_cpu(pnetwork->network.NetworkTypeInUse) ;	
+	pnetwork->network.Configuration.ATIMWindow = le32_to_cpu(pnetwork->network.Configuration.ATIMWindow);
+	pnetwork->network.Configuration.BeaconPeriod = le32_to_cpu(pnetwork->network.Configuration.BeaconPeriod);
+	pnetwork->network.Configuration.DSConfig = le32_to_cpu(pnetwork->network.Configuration.DSConfig);
+	pnetwork->network.Configuration.FHConfig.DwellTime=le32_to_cpu(pnetwork->network.Configuration.FHConfig.DwellTime);
+	pnetwork->network.Configuration.FHConfig.HopPattern=le32_to_cpu(pnetwork->network.Configuration.FHConfig.HopPattern);
+	pnetwork->network.Configuration.FHConfig.HopSet=le32_to_cpu(pnetwork->network.Configuration.FHConfig.HopSet);
+	pnetwork->network.Configuration.FHConfig.Length=le32_to_cpu(pnetwork->network.Configuration.FHConfig.Length);	
+	pnetwork->network.Configuration.Length = le32_to_cpu(pnetwork->network.Configuration.Length);
+	pnetwork->network.InfrastructureMode = le32_to_cpu(pnetwork->network.InfrastructureMode);
+	pnetwork->network.IELength = le32_to_cpu(pnetwork->network.IELength );
+#endif
+
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("joinbss event call back received with res=%d\n", pnetwork->join_res));
+
+	rtw_get_encrypt_decrypt_from_registrypriv(adapter);
+	
+
+	if (pmlmepriv->assoc_ssid.SsidLength == 0)
+	{
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("@@@@@   joinbss event call back  for Any SSid\n"));		
+	}
+	else
+	{
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("@@@@@   rtw_joinbss_event_callback for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
+	}
+	
+	the_same_macaddr = _rtw_memcmp(pnetwork->network.MacAddress, cur_network->network.MacAddress, ETH_ALEN);
+
+	pnetwork->network.Length = get_WLAN_BSSID_EX_sz(&pnetwork->network);
+	if(pnetwork->network.Length > sizeof(WLAN_BSSID_EX))
+	{
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("\n\n ***joinbss_evt_callback return a wrong bss ***\n\n"));
+		goto ignore_joinbss_callback;
+	}
+		
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+	
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n rtw_joinbss_event_callback !! _enter_critical \n"));
+
+	if(pnetwork->join_res > 0)
+	{
+		_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+		retry = 0;
+		if (check_fwstate(pmlmepriv,_FW_UNDER_LINKING) )
+		{
+			//s1. find ptarget_wlan
+			if(check_fwstate(pmlmepriv, _FW_LINKED) )
+			{
+				if(the_same_macaddr == _TRUE)
+				{
+					ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);					
+				}
+				else
+				{
+					pcur_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
+					if(pcur_wlan)	pcur_wlan->fixed = _FALSE;
+
+					pcur_sta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress);
+					if(pcur_sta){
+						_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+						rtw_free_stainfo(adapter,  pcur_sta);
+						_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+					}
+
+					ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
+					if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE){
+						if(ptarget_wlan)	ptarget_wlan->fixed = _TRUE;			
+					}
+				}
+
+			}
+			else
+			{
+				ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
+				if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE){
+					if(ptarget_wlan)	ptarget_wlan->fixed = _TRUE;			
+				}
+			}
+		
+			//s2. update cur_network 
+			if(ptarget_wlan)
+			{			
+				rtw_joinbss_update_network(adapter, ptarget_wlan, pnetwork);
+			}
+			else
+			{			
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Can't find ptarget_wlan when joinbss_event callback\n"));
+				_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+				goto ignore_joinbss_callback;
+			}
+					
+			
+			//s3. find ptarget_sta & update ptarget_sta after update cur_network only for station mode 
+			if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE)
+			{ 
+				ptarget_sta = rtw_joinbss_update_stainfo(adapter, pnetwork);
+				if(ptarget_sta==NULL)
+				{
+					RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Can't update stainfo when joinbss_event callback\n"));
+					_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+					goto ignore_joinbss_callback;
+				}
+			}
+
+			//s4. indicate connect			
+				if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE)
+				{
+					rtw_indicate_connect(adapter);
+				}
+				else
+				{
+					//adhoc mode will rtw_indicate_connect when rtw_stassoc_event_callback
+					RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("adhoc mode, fw_state:%x", get_fwstate(pmlmepriv)));
+				}
+
+				
+			//s5. Cancle assoc_timer					
+			_cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
+		
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("Cancle assoc_timer \n"));		
+		
+		}
+		else
+		{
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("rtw_joinbss_event_callback err: fw_state:%x", get_fwstate(pmlmepriv)));	
+			_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+			goto ignore_joinbss_callback;
+		}
+		
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);	
+				
+	}
+	else if(pnetwork->join_res == -4) 
+	{
+		rtw_reset_securitypriv(adapter);
+		_set_timer(&pmlmepriv->assoc_timer, 1);					
+
+		//rtw_free_assoc_resources(adapter, 1);
+
+		if((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == _TRUE)
+		{		
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("fail! clear _FW_UNDER_LINKING ^^^fw_state=%x\n", get_fwstate(pmlmepriv)));
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+		}	
+		
+	}
+	else //if join_res < 0 (join fails), then try again
+	{
+	
+		#ifdef REJOIN
+		res = _FAIL;
+		if(retry < 2) {
+			res = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("rtw_select_and_join_from_scanned_queue again! res:%d\n",res));
+		}
+
+		 if(res == _SUCCESS)
+		{
+			//extend time of assoc_timer
+			_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+			retry++;
+		}
+		else if(res == 2)//there is no need to wait for join
+		{
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+			rtw_indicate_connect(adapter);
+		}	
+		else
+		{
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Set Assoc_Timer = 1; can't find match ssid in scanned_q \n"));
+		#endif
+			
+			_set_timer(&pmlmepriv->assoc_timer, 1);
+			//rtw_free_assoc_resources(adapter, 1);
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+			
+		#ifdef REJOIN
+			retry = 0;	
+		}
+		#endif
+	}
+
+ignore_joinbss_callback:
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+	_func_exit_;	
+}
+
+void rtw_joinbss_event_callback(_adapter *adapter, u8 *pbuf)
+{
+	struct wlan_network 	*pnetwork	= (struct wlan_network *)pbuf;
+
+_func_enter_;
+
+	mlmeext_joinbss_event_callback(adapter, pnetwork->join_res);
+
+	rtw_os_xmit_schedule(adapter);
+
+#ifdef CONFIG_CONCURRENT_MODE	
+	rtw_os_xmit_schedule(adapter->pbuddy_adapter);
+#endif	
+
+#ifdef CONFIG_DUALMAC_CONCURRENT
+	dc_resume_xmit(adapter);
+#endif
+
+_func_exit_;
+}
+
+u8 search_max_mac_id(_adapter *padapter)
+{
+	u8 mac_id, aid;
+#if (RATE_ADAPTIVE_SUPPORT==1)	//for 88E RA		
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)	
+	if(check_fwstate(pmlmepriv, WIFI_AP_STATE)){		
+		
+		for (aid = (pstapriv->max_num_sta); aid > 0; aid--)
+		{
+			if (pstapriv->sta_aid[aid-1] != NULL)
+				break;
+		}
+/*
+		for (mac_id = (pstapriv->max_num_sta-1); mac_id >= 0; mac_id--)
+		{
+			if (pstapriv->sta_aid[mac_id] != NULL)
+				break;
+		}	
+*/
+		mac_id = aid + 1;
+	}
+	else
+#endif
+	{//adhoc  id =  31~2
+		for (mac_id = (NUM_STA-1); mac_id >= IBSS_START_MAC_ID ; mac_id--)
+		{
+			if (pmlmeinfo->FW_sta_info[mac_id].status == 1)
+			{
+				break;
+			}
+		}
+	}
+#endif	
+	return mac_id;
+
+}		
+
+//FOR AP ,AD-HOC mode
+void rtw_stassoc_hw_rpt(_adapter *adapter,struct sta_info *psta)
+{
+	u16 media_status;
+
+	if(psta==NULL)	return;
+
+	#if (RATE_ADAPTIVE_SUPPORT==1)	//for 88E RA	
+	{
+		u8 macid = search_max_mac_id(adapter);				
+		rtw_hal_set_hwreg(adapter,HW_VAR_TX_RPT_MAX_MACID, (u8*)&macid);
+	}
+	#endif
+	media_status = (psta->mac_id<<8)|1; //  MACID|OPMODE:1 connect				
+	rtw_hal_set_hwreg(adapter,HW_VAR_H2C_MEDIA_STATUS_RPT,(u8 *)&media_status);			
+}
+
+void rtw_stassoc_event_callback(_adapter *adapter, u8 *pbuf)
+{
+	_irqL irqL;	
+	struct sta_info *psta;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct stassoc_event	*pstassoc	= (struct stassoc_event*)pbuf;
+	struct wlan_network 	*cur_network = &(pmlmepriv->cur_network);
+	struct wlan_network	*ptarget_wlan = NULL;
+	
+_func_enter_;	
+	
+	if(rtw_access_ctrl(adapter, pstassoc->macaddr) == _FALSE)
+		return;
+
+#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	if(check_fwstate(pmlmepriv, WIFI_AP_STATE))
+	{
+		psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);	
+		if(psta)
+		{
+#ifdef CONFIG_IOCTL_CFG80211
+			#ifdef COMPAT_KERNEL_RELEASE
+
+			#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,37)) || defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)
+			u8 *passoc_req = NULL;
+			u32 assoc_req_len;
+
+			_enter_critical_bh(&psta->lock, &irqL);
+			if(psta->passoc_req && psta->assoc_req_len>0)
+			{				
+				passoc_req = rtw_zmalloc(psta->assoc_req_len);
+				if(passoc_req)
+				{
+					assoc_req_len = psta->assoc_req_len;
+					_rtw_memcpy(passoc_req, psta->passoc_req, assoc_req_len);
+					
+					_rtw_mfree(psta->passoc_req , psta->assoc_req_len);
+					psta->passoc_req = NULL;
+					psta->assoc_req_len = 0;
+				}
+			}			
+			_exit_critical_bh(&psta->lock, &irqL);
+
+			if(passoc_req && assoc_req_len>0)
+			{
+				rtw_cfg80211_indicate_sta_assoc(adapter, passoc_req, assoc_req_len);
+
+				_rtw_mfree(passoc_req, assoc_req_len);
+			}
+			#endif //(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,37)) || defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)
+#endif //CONFIG_IOCTL_CFG80211	
+
+			//bss_cap_update_on_sta_join(adapter, psta);
+			//sta_info_update(adapter, psta);
+			ap_sta_info_defer_update(adapter, psta);
+
+			rtw_stassoc_hw_rpt(adapter,psta);
+
+		}	
+		
+		goto exit;
+	}	
+#endif	
+	//for AD-HOC mode
+	psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);	
+	if( psta != NULL)
+	{
+		//the sta have been in sta_info_queue => do nothing 
+		
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Error: rtw_stassoc_event_callback: sta has been in sta_hash_queue \n"));
+		
+		goto exit; //(between drv has received this event before and  fw have not yet to set key to CAM_ENTRY)
+	}
+
+	psta = rtw_alloc_stainfo(&adapter->stapriv, pstassoc->macaddr);	
+	if (psta == NULL) {
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Can't alloc sta_info when rtw_stassoc_event_callback\n"));
+		goto exit;
+	}	
+	
+	//to do : init sta_info variable
+	psta->qos_option = 0;
+	psta->mac_id = (uint)pstassoc->cam_id;
+	//psta->aid = (uint)pstassoc->cam_id;
+	DBG_871X("%s\n",__FUNCTION__);
+	//for ad-hoc mode
+	rtw_hal_set_odm_var(adapter,HAL_ODM_STA_INFO,psta,_TRUE);
+
+	rtw_stassoc_hw_rpt(adapter,psta);	
+	
+	if(adapter->securitypriv.dot11AuthAlgrthm==dot11AuthAlgrthm_8021X)
+		psta->dot118021XPrivacy = adapter->securitypriv.dot11PrivacyAlgrthm;
+	
+
+	psta->ieee8021x_blocked = _FALSE;		
+	
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	if ( (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)==_TRUE ) || 
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)==_TRUE ) )
+	{
+		if(adapter->stapriv.asoc_sta_count== 2)
+		{
+			_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+			ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
+			if(ptarget_wlan)	ptarget_wlan->fixed = _TRUE;
+			_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+			// a sta + bc/mc_stainfo (not Ibss_stainfo)
+			rtw_indicate_connect(adapter);
+		}
+	}
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+
+	mlmeext_sta_add_event_callback(adapter, psta);
+	
+#ifdef CONFIG_RTL8711
+	//submit SetStaKey_cmd to tell fw, fw will allocate an CAM entry for this sta	
+	rtw_setstakey_cmd(adapter, (unsigned char*)psta, _FALSE);
+#endif
+		
+exit:
+	
+_func_exit_;	
+
+}
+
+void rtw_stadel_event_callback(_adapter *adapter, u8 *pbuf)
+{
+	_irqL irqL,irqL2;
+	int mac_id=-1;
+	struct sta_info *psta;
+	struct wlan_network* pwlan = NULL;
+	WLAN_BSSID_EX    *pdev_network=NULL;
+	u8* pibss = NULL;
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+	struct 	stadel_event *pstadel	= (struct stadel_event*)pbuf;
+   	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
+	
+_func_enter_;	
+	
+	psta = rtw_get_stainfo(&adapter->stapriv, pstadel->macaddr);
+	if(psta)
+		mac_id = psta->mac_id;
+	else
+		mac_id = pstadel->mac_id;
+
+	DBG_871X("%s(mac_id=%d)=" MAC_FMT "\n", __func__, mac_id, MAC_ARG(pstadel->macaddr));
+
+	if(mac_id>=0){
+		u16 media_status;
+		media_status = (mac_id<<8)|0; //  MACID|OPMODE:0 means disconnect
+		//for STA,AP,ADHOC mode, report disconnect stauts to FW
+		rtw_hal_set_hwreg(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
+	}	
+
+        if(check_fwstate(pmlmepriv, WIFI_AP_STATE))
+        {
+#ifdef CONFIG_IOCTL_CFG80211
+		#ifdef COMPAT_KERNEL_RELEASE
+
+		#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,37)) || defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)
+		rtw_cfg80211_indicate_sta_disassoc(adapter, pstadel->macaddr, *(u16*)pstadel->rsvd);
+		#endif //(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,37)) || defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)
+#endif //CONFIG_IOCTL_CFG80211
+
+		return;
+        }
+
+
+	mlmeext_sta_del_event_callback(adapter);
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL2);
+
+	if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) )
+	{
+		#ifdef CONFIG_LAYER2_ROAMING
+		if (rtw_to_roaming(adapter) > 0)
+			pmlmepriv->to_roaming--; /* this stadel_event is caused by roaming, decrease to_roaming */
+		else if (rtw_to_roaming(adapter) == 0)
+			rtw_set_roaming(adapter, adapter->registrypriv.max_roaming_times);
+#ifdef CONFIG_INTEL_WIDI
+		if(adapter->mlmepriv.widi_state != INTEL_WIDI_STATE_CONNECTED)
+#endif // CONFIG_INTEL_WIDI
+		if(*((unsigned short *)(pstadel->rsvd)) != WLAN_REASON_EXPIRATION_CHK)
+			rtw_set_roaming(adapter, 0); /* don't roam */
+		#endif
+
+		rtw_free_uc_swdec_pending_queue(adapter);
+
+		rtw_free_assoc_resources(adapter, 1);
+		rtw_indicate_disconnect(adapter);
+		_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+		// remove the network entry in scanned_queue
+		pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);	
+		if (pwlan) {			
+			pwlan->fixed = _FALSE;
+			rtw_free_network_nolock(pmlmepriv, pwlan);
+		}
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+		#ifdef CONFIG_LAYER2_ROAMING
+		_rtw_roaming(adapter, tgt_network);
+		#else
+#ifdef CONFIG_INTEL_WIDI
+		process_intel_widi_disconnect(adapter, 1);
+#endif // CONFIG_INTEL_WIDI
+		#endif //CONFIG_LAYER2_ROAMING
+
+	}
+
+	if ( check_fwstate(pmlmepriv,WIFI_ADHOC_MASTER_STATE) || 
+	      check_fwstate(pmlmepriv,WIFI_ADHOC_STATE))
+	{
+		
+		_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
+		rtw_free_stainfo(adapter,  psta);
+		_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
+		
+		if(adapter->stapriv.asoc_sta_count== 1) //a sta + bc/mc_stainfo (not Ibss_stainfo)
+		{ 
+			//rtw_indicate_disconnect(adapter);//removed@20091105
+			_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+			//free old ibss network
+			//pwlan = rtw_find_network(&pmlmepriv->scanned_queue, pstadel->macaddr);
+			pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+			if(pwlan)	
+			{
+				pwlan->fixed = _FALSE;
+				rtw_free_network_nolock(pmlmepriv, pwlan); 
+			}
+			_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+			//re-create ibss
+			pdev_network = &(adapter->registrypriv.dev_network);			
+			pibss = adapter->registrypriv.dev_network.MacAddress;
+
+			_rtw_memcpy(pdev_network, &tgt_network->network, get_WLAN_BSSID_EX_sz(&tgt_network->network));
+			
+			_rtw_memset(&pdev_network->Ssid, 0, sizeof(NDIS_802_11_SSID));
+			_rtw_memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(NDIS_802_11_SSID));
+	
+			rtw_update_registrypriv_dev_network(adapter);			
+
+			rtw_generate_random_ibss(pibss);
+			
+			if(check_fwstate(pmlmepriv,WIFI_ADHOC_STATE))
+			{
+				set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+
+			if(rtw_createbss_cmd(adapter)!=_SUCCESS)
+			{
+
+				RT_TRACE(_module_rtl871x_ioctl_set_c_,_drv_err_,("***Error=>stadel_event_callback: rtw_createbss_cmd status FAIL*** \n "));										
+
+			}
+
+			
+		}
+		
+	}
+	
+	_exit_critical_bh(&pmlmepriv->lock, &irqL2);
+	
+_func_exit_;	
+
+}
+
+
+void rtw_cpwm_event_callback(PADAPTER padapter, u8 *pbuf)
+{
+#ifdef CONFIG_LPS_LCLK
+	struct reportpwrstate_parm *preportpwrstate;
+#endif
+
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("+rtw_cpwm_event_callback !!!\n"));
+#ifdef CONFIG_LPS_LCLK
+	preportpwrstate = (struct reportpwrstate_parm*)pbuf;
+	preportpwrstate->state |= (u8)(padapter->pwrctrlpriv.cpwm_tog + 0x80);
+	cpwm_int_hdl(padapter, preportpwrstate);
+#endif
+
+_func_exit_;
+
+}
+
+/*
+* _rtw_join_timeout_handler - Timeout/faliure handler for CMD JoinBss
+* @adapter: pointer to _adapter structure
+*/
+void _rtw_join_timeout_handler (_adapter *adapter)
+{
+	_irqL irqL;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+#ifdef CONFIG_LAYER2_ROAMING
+	int do_join_r;
+#endif //CONFIG_LAYER2_ROAMING
+
+#if 0
+	if (adapter->bDriverStopped == _TRUE){
+		_rtw_up_sema(&pmlmepriv->assoc_terminate);
+		return;
+	}
+#endif	
+
+_func_enter_;		
+#ifdef PLATFORM_FREEBSD
+		rtw_mtx_lock(NULL);
+		 if (callout_pending(&adapter->mlmepriv.assoc_timer.callout)) {
+			 /* callout was reset */
+			 //mtx_unlock(&sc->sc_mtx);
+			 rtw_mtx_unlock(NULL);
+			 return;
+		 }
+		 if (!callout_active(&adapter->mlmepriv.assoc_timer.callout)) {
+			 /* callout was stopped */
+			 //mtx_unlock(&sc->sc_mtx);
+			 rtw_mtx_unlock(NULL);
+			 return;
+		 }
+		 callout_deactivate(&adapter->mlmepriv.assoc_timer.callout);
+	
+	
+#endif
+
+	DBG_871X("%s, fw_state=%x\n", __FUNCTION__, get_fwstate(pmlmepriv));
+	
+	if(adapter->bDriverStopped ||adapter->bSurpriseRemoved)
+		return;
+
+	
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	#ifdef CONFIG_LAYER2_ROAMING
+	if (rtw_to_roaming(adapter) > 0) { /* join timeout caused by roaming */
+		while(1) {
+			pmlmepriv->to_roaming--;
+			if (rtw_to_roaming(adapter) != 0) { /* try another */
+				DBG_871X("%s try another roaming\n", __FUNCTION__);
+				if( _SUCCESS!=(do_join_r=rtw_do_join(adapter)) ) {
+					DBG_871X("%s roaming do_join return %d\n", __FUNCTION__ ,do_join_r);
+					continue;
+				}
+				break;
+			} else {
+#ifdef CONFIG_INTEL_WIDI
+				if(adapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING)
+				{
+					_rtw_memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN);
+					intel_widi_wk_cmd(adapter, INTEL_WIDI_LISTEN_WK, NULL);
+					DBG_871X("change to widi listen\n");
+				}
+#endif // CONFIG_INTEL_WIDI
+				DBG_871X("%s We've try roaming but fail\n", __FUNCTION__);
+				rtw_indicate_disconnect(adapter);
+				break;
+			}
+		}
+		
+	} else 
+	#endif
+	{
+		rtw_indicate_disconnect(adapter);
+		free_scanqueue(pmlmepriv);//???
+
+#ifdef CONFIG_IOCTL_CFG80211
+		//indicate disconnect for the case that join_timeout and check_fwstate != FW_LINKED
+		rtw_cfg80211_indicate_disconnect(adapter);
+#endif //CONFIG_IOCTL_CFG80211
+
+ 	}
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+	
+
+#ifdef CONFIG_DRVEXT_MODULE_WSC	
+	drvext_assoc_fail_indicate(&adapter->drvextpriv);	
+#endif	
+#ifdef PLATFORM_FREEBSD
+		rtw_mtx_unlock(NULL);
+#endif
+	
+_func_exit_;
+
+}
+
+/*
+* rtw_scan_timeout_handler - Timeout/Faliure handler for CMD SiteSurvey
+* @adapter: pointer to _adapter structure
+*/
+void rtw_scan_timeout_handler (_adapter *adapter)
+{	
+	_irqL irqL;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	
+	DBG_871X(FUNC_ADPT_FMT" fw_state=%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv));
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+	
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+	rtw_indicate_scan_done(adapter, _TRUE);
+
+}
+
+static void rtw_auto_scan_handler(_adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+
+	//auto site survey per 60sec
+	if(pmlmepriv->scan_interval >0)
+	{
+		pmlmepriv->scan_interval--;
+		if(pmlmepriv->scan_interval==0)
+		{
+/*		
+			if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == _TRUE) 
+			{
+				DBG_871X("exit %s when _FW_UNDER_SURVEY|_FW_UNDER_LINKING -> \n", __FUNCTION__);
+				return;
+			}
+			
+			if(pmlmepriv->sitesurveyctrl.traffic_busy == _TRUE)
+			{
+				DBG_871X("%s exit cause traffic_busy(%x)\n",__FUNCTION__, pmlmepriv->sitesurveyctrl.traffic_busy);
+				return;
+			}
+*/
+
+#ifdef CONFIG_CONCURRENT_MODE
+			if (rtw_buddy_adapter_up(padapter))
+			{
+				if ((check_buddy_fwstate(padapter, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == _TRUE) ||
+					(padapter->pbuddy_adapter->mlmepriv.LinkDetectInfo.bBusyTraffic == _TRUE))
+				{		
+					DBG_871X("%s, but buddy_intf is under scanning or linking or BusyTraffic\n", __FUNCTION__);
+					return;
+				}
+			}
+#endif
+
+			DBG_871X("%s\n", __FUNCTION__);
+
+			rtw_set_802_11_bssid_list_scan(padapter, NULL, 0);			
+			
+			pmlmepriv->scan_interval = SCAN_INTERVAL;// 30*2 sec = 60sec
+			
+		}
+		
+	}
+
+}
+
+void rtw_dynamic_check_timer_handlder(_adapter *adapter)
+{
+#ifdef CONFIG_AP_MODE
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+#endif //CONFIG_AP_MODE
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+#ifdef CONFIG_CONCURRENT_MODE	
+	PADAPTER pbuddy_adapter = adapter->pbuddy_adapter;	
+#endif
+
+	if(!adapter)
+		return;	
+#if defined(CONFIG_CHECK_BT_HANG) && defined(CONFIG_BT_COEXIST)	
+	if(adapter->HalFunc.hal_checke_bt_hang)
+		adapter->HalFunc.hal_checke_bt_hang(adapter);
+#endif
+	if(adapter->hw_init_completed == _FALSE)
+		return;
+
+	if ((adapter->bDriverStopped == _TRUE)||(adapter->bSurpriseRemoved== _TRUE))
+		return;
+
+	
+#ifdef CONFIG_CONCURRENT_MODE
+	if(pbuddy_adapter)
+	{
+		if(adapter->net_closed == _TRUE && pbuddy_adapter->net_closed == _TRUE)
+		{
+			return;
+		}		
+	}
+	else
+#endif //CONFIG_CONCURRENT_MODE
+	if(adapter->net_closed == _TRUE)
+	{
+		return;
+	}	
+
+	rtw_dynamic_chk_wk_cmd(adapter);
+
+	if(pregistrypriv->wifi_spec==1)
+	{	
+#ifdef CONFIG_P2P
+		struct wifidirect_info *pwdinfo = &adapter->wdinfo;
+		if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+#endif	
+		{
+			//auto site survey
+			rtw_auto_scan_handler(adapter);
+		}	
+	}
+
+#ifndef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+#ifdef CONFIG_AP_MODE
+	if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+	{
+		expire_timeout_chk(adapter);
+	}	
+#endif
+#endif //!CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+
+#ifdef CONFIG_BR_EXT
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35))
+	rcu_read_lock();
+#endif	// (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35))
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) 
+	if( adapter->pnetdev->br_port 
+#else	// (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+	if( rcu_dereference(adapter->pnetdev->rx_handler_data)
+#endif	// (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+		&& (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) == _TRUE) )
+	{
+		// expire NAT2.5 entry
+		void nat25_db_expire(_adapter *priv);
+		nat25_db_expire(adapter);
+
+		if (adapter->pppoe_connection_in_progress > 0) {
+			adapter->pppoe_connection_in_progress--;
+		}
+		
+		// due to rtw_dynamic_check_timer_handlder() is called every 2 seconds
+		if (adapter->pppoe_connection_in_progress > 0) {
+			adapter->pppoe_connection_in_progress--;
+		}
+	}
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35))
+	rcu_read_unlock();
+#endif	// (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35))
+
+#endif	// CONFIG_BR_EXT
+	
+}
+
+
+#ifdef CONFIG_SET_SCAN_DENY_TIMER
+inline bool rtw_is_scan_deny(_adapter *adapter)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+	return (ATOMIC_READ(&mlmepriv->set_scan_deny) != 0) ? _TRUE : _FALSE;
+}
+
+inline void rtw_clear_scan_deny(_adapter *adapter)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+	ATOMIC_SET(&mlmepriv->set_scan_deny, 0);
+	if (0)
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
+}
+
+void rtw_set_scan_deny_timer_hdl(_adapter *adapter)
+{
+	rtw_clear_scan_deny(adapter);
+}
+
+void rtw_set_scan_deny(_adapter *adapter, u32 ms)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+#ifdef CONFIG_CONCURRENT_MODE
+	struct mlme_priv *b_mlmepriv;
+#endif
+
+	if (0)
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
+	ATOMIC_SET(&mlmepriv->set_scan_deny, 1);
+	_set_timer(&mlmepriv->set_scan_deny_timer, ms);
+	
+#ifdef CONFIG_CONCURRENT_MODE
+	if (!adapter->pbuddy_adapter)
+		return;
+
+	if (0)
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter->pbuddy_adapter));
+ 	b_mlmepriv = &adapter->pbuddy_adapter->mlmepriv;
+	ATOMIC_SET(&b_mlmepriv->set_scan_deny, 1);
+	_set_timer(&b_mlmepriv->set_scan_deny_timer, ms);	
+#endif
+	
+}
+#endif
+
+#if defined(IEEE80211_SCAN_RESULT_EXPIRE)
+#define RTW_SCAN_RESULT_EXPIRE IEEE80211_SCAN_RESULT_EXPIRE/HZ*1000 -1000 //3000 -1000
+#else
+#define RTW_SCAN_RESULT_EXPIRE 2000
+#endif
+
+#ifndef PLATFORM_FREEBSD
+/*
+* Select a new join candidate from the original @param candidate and @param competitor
+* @return _TRUE: candidate is updated
+* @return _FALSE: candidate is not updated
+*/
+static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
+	, struct wlan_network **candidate, struct wlan_network *competitor)
+{
+	int updated = _FALSE;
+	_adapter *adapter = container_of(pmlmepriv, _adapter, mlmepriv);
+
+
+	//check bssid, if needed
+	if(pmlmepriv->assoc_by_bssid==_TRUE) {
+		if(_rtw_memcmp(competitor->network.MacAddress, pmlmepriv->assoc_bssid, ETH_ALEN) ==_FALSE)
+			goto exit;
+	}
+
+	//check ssid, if needed
+	if(pmlmepriv->assoc_ssid.Ssid && pmlmepriv->assoc_ssid.SsidLength) {
+		if(	competitor->network.Ssid.SsidLength != pmlmepriv->assoc_ssid.SsidLength
+			|| _rtw_memcmp(competitor->network.Ssid.Ssid, pmlmepriv->assoc_ssid.Ssid, pmlmepriv->assoc_ssid.SsidLength) == _FALSE
+		)
+			goto exit;
+	}
+
+	if(rtw_is_desired_network(adapter, competitor)  == _FALSE)
+		goto exit;
+
+#ifdef  CONFIG_LAYER2_ROAMING
+	if(rtw_to_roaming(adapter) > 0) {
+		if(	rtw_get_passing_time_ms((u32)competitor->last_scanned) >= RTW_SCAN_RESULT_EXPIRE
+			|| is_same_ess(&competitor->network, &pmlmepriv->cur_network.network) == _FALSE
+		)
+			goto exit;
+	}
+#endif
+	
+	if(*candidate == NULL ||(*candidate)->network.Rssi<competitor->network.Rssi )
+	{
+		*candidate = competitor;
+		updated = _TRUE;
+	}
+
+#if 0
+	if(pmlmepriv->assoc_by_bssid==_TRUE) { // associate with bssid
+		if(	(*candidate == NULL ||(*candidate)->network.Rssi<competitor->network.Rssi )
+			&& _rtw_memcmp(competitor->network.MacAddress, pmlmepriv->assoc_bssid, ETH_ALEN)==_TRUE
+		) {
+			*candidate = competitor;
+			updated = _TRUE;
+		}
+	} else  if (pmlmepriv->assoc_ssid.SsidLength == 0 ) { // associate with ssid, but ssidlength is 0
+		if(	(*candidate == NULL ||(*candidate)->network.Rssi<competitor->network.Rssi ) ) {
+			*candidate = competitor;
+			updated = _TRUE;
+		}
+	} else
+#ifdef  CONFIG_LAYER2_ROAMING
+	if(rtw_to_roaming(adapter)) { // roaming
+		if(	(*candidate == NULL ||(*candidate)->network.Rssi<competitor->network.Rssi )
+			&& is_same_ess(&competitor->network, &pmlmepriv->cur_network.network) 
+			//&&(!is_same_network(&competitor->network, &pmlmepriv->cur_network.network))
+			&& rtw_get_passing_time_ms((u32)competitor->last_scanned) < RTW_SCAN_RESULT_EXPIRE
+			&& rtw_is_desired_network(adapter, competitor)
+		) {
+			*candidate = competitor;
+			updated = _TRUE;
+		}
+		
+	} else
+#endif
+	{ // associate with ssid
+		if(	(*candidate == NULL ||(*candidate)->network.Rssi<competitor->network.Rssi )
+			&& (competitor->network.Ssid.SsidLength==pmlmepriv->assoc_ssid.SsidLength)
+			&&((_rtw_memcmp(competitor->network.Ssid.Ssid, pmlmepriv->assoc_ssid.Ssid, pmlmepriv->assoc_ssid.SsidLength)) == _TRUE)
+			&& rtw_is_desired_network(adapter, competitor)
+		) {
+			*candidate = competitor;
+			updated = _TRUE;
+		}
+	}
+#endif
+
+	if(updated){
+		DBG_871X("[by_bssid:%u][assoc_ssid:%s]"
+			#ifdef  CONFIG_LAYER2_ROAMING
+			"[to_roaming:%u] "
+			#endif
+			"new candidate: %s("MAC_FMT") rssi:%d\n",
+			pmlmepriv->assoc_by_bssid,
+			pmlmepriv->assoc_ssid.Ssid,
+			#ifdef  CONFIG_LAYER2_ROAMING
+			rtw_to_roaming(adapter),
+			#endif
+			(*candidate)->network.Ssid.Ssid,
+			MAC_ARG((*candidate)->network.MacAddress),
+			(int)(*candidate)->network.Rssi
+		);
+	}
+
+exit:
+	return updated;
+}
+
+/*
+Calling context:
+The caller of the sub-routine will be in critical section...
+
+The caller must hold the following spinlock
+
+pmlmepriv->lock
+
+
+*/
+
+int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv )
+{
+	_irqL	irqL;
+	int ret;
+	_list	*phead;
+	_adapter *adapter;	
+	_queue	*queue	= &(pmlmepriv->scanned_queue);
+	struct	wlan_network	*pnetwork = NULL;
+	struct	wlan_network	*candidate = NULL;
+	u8 		bSupportAntDiv = _FALSE;
+
+_func_enter_;
+
+	_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+	phead = get_list_head(queue);		
+	adapter = (_adapter *)pmlmepriv->nic_hdl;
+
+	pmlmepriv->pscanned = get_next( phead );
+
+	while (!rtw_end_of_queue_search(phead, pmlmepriv->pscanned)) {
+
+		pnetwork = LIST_CONTAINOR(pmlmepriv->pscanned, struct wlan_network, list);
+		if(pnetwork==NULL){
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s return _FAIL:(pnetwork==NULL)\n", __FUNCTION__));
+			ret = _FAIL;
+			goto exit;
+		}
+		
+		pmlmepriv->pscanned = get_next(pmlmepriv->pscanned);
+
+		#if 0
+		DBG_871X("MacAddress:"MAC_FMT" ssid:%s\n", MAC_ARG(pnetwork->network.MacAddress), pnetwork->network.Ssid.Ssid);
+		#endif
+
+		rtw_check_join_candidate(pmlmepriv, &candidate, pnetwork);
+ 
+ 	}
+
+	if(candidate == NULL) {
+		DBG_871X("%s: return _FAIL(candidate == NULL)\n", __FUNCTION__);
+#ifdef CONFIG_WOWLAN
+		_clr_fwstate_(pmlmepriv, _FW_LINKED|_FW_UNDER_LINKING);
+#endif
+		ret = _FAIL;
+		goto exit;
+	} else {
+		DBG_871X("%s: candidate: %s("MAC_FMT", ch:%u)\n", __FUNCTION__,
+			candidate->network.Ssid.Ssid, MAC_ARG(candidate->network.MacAddress),
+			candidate->network.Configuration.DSConfig);
+	}
+	
+
+	// check for situation of  _FW_LINKED 
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+	{
+		DBG_871X("%s: _FW_LINKED while ask_for_joinbss!!!\n", __FUNCTION__);
+
+		#if 0 // for WPA/WPA2 authentication, wpa_supplicant will expect authentication from AP, it is needed to reconnect AP...
+		if(is_same_network(&pmlmepriv->cur_network.network, &candidate->network))
+		{
+			DBG_871X("%s: _FW_LINKED and is same network, it needn't join again\n", __FUNCTION__);
+
+			rtw_indicate_connect(adapter);//rtw_indicate_connect again
+				
+			ret = 2;
+			goto exit;
+		}
+		else
+		#endif
+		{
+			rtw_disassoc_cmd(adapter, 0, _TRUE);
+			rtw_indicate_disconnect(adapter);
+			rtw_free_assoc_resources(adapter, 0);
+		}
+	}
+	
+	#ifdef CONFIG_ANTENNA_DIVERSITY
+	rtw_hal_get_def_var(adapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &(bSupportAntDiv));
+	if(_TRUE == bSupportAntDiv)	
+	{
+		u8 CurrentAntenna;
+		rtw_hal_get_def_var(adapter, HAL_DEF_CURRENT_ANTENNA, &(CurrentAntenna));			
+		DBG_871X("#### Opt_Ant_(%s) , cur_Ant(%s)\n",
+			(2==candidate->network.PhyInfo.Optimum_antenna)?"A":"B",
+			(2==CurrentAntenna)?"A":"B"
+		);
+	}
+	#endif
+	set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+	ret = rtw_joinbss_cmd(adapter, candidate);
+	
+exit:
+	_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+_func_exit_;
+
+	return ret;
+}
+#else
+int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv )
+{	
+	_irqL	irqL;
+	_list	*phead;
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	u8 CurrentAntenna;
+#endif
+	unsigned char *dst_ssid, *src_ssid;
+	_adapter *adapter;	
+	_queue	*queue	= &(pmlmepriv->scanned_queue);
+	struct	wlan_network	*pnetwork = NULL;
+	struct	wlan_network	*pnetwork_max_rssi = NULL;
+	#ifdef CONFIG_LAYER2_ROAMING
+	struct wlan_network * roaming_candidate=NULL;
+	u32 cur_time=rtw_get_current_time();
+	#endif
+
+_func_enter_;
+	_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+	phead = get_list_head(queue);		
+	adapter = (_adapter *)pmlmepriv->nic_hdl;
+
+	pmlmepriv->pscanned = get_next( phead );
+
+	while (!rtw_end_of_queue_search(phead, pmlmepriv->pscanned)) {
+
+		pnetwork = LIST_CONTAINOR(pmlmepriv->pscanned, struct wlan_network, list);
+		if(pnetwork==NULL){
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("(2)rtw_select_and_join_from_scanned_queue return _FAIL:(pnetwork==NULL)\n"));
+			_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+			return _FAIL;	
+		}
+
+		dst_ssid = pnetwork->network.Ssid.Ssid;
+		src_ssid = pmlmepriv->assoc_ssid.Ssid;
+		
+		pmlmepriv->pscanned = get_next(pmlmepriv->pscanned);
+
+		#if 0
+		DBG_871X("MacAddress:"MAC_FMT" ssid:%s\n", MAC_ARG(pnetwork->network.MacAddress), pnetwork->network.Ssid.Ssid);
+		#endif
+
+		if(pmlmepriv->assoc_by_bssid==_TRUE)
+		{
+			if(_rtw_memcmp(pnetwork->network.MacAddress, pmlmepriv->assoc_bssid, ETH_ALEN)==_TRUE)
+			{
+				//remove the condition @ 20081125
+				//if((pmlmepriv->cur_network.network.InfrastructureMode==Ndis802_11AutoUnknown)||
+				//	pmlmepriv->cur_network.network.InfrastructureMode == pnetwork->network.InfrastructureMode)
+				//		goto ask_for_joinbss;
+				
+				if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+				{
+					if(is_same_network(&pmlmepriv->cur_network.network, &pnetwork->network))
+					{
+						//DBG_871X("select_and_join(1): _FW_LINKED and is same network, it needn't join again\n");
+
+						rtw_indicate_connect(adapter);//rtw_indicate_connect again
+						_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);	
+						return 2;
+					}
+					else
+					{
+						rtw_disassoc_cmd(adapter, 0, _TRUE);
+						rtw_indicate_disconnect(adapter);
+						rtw_free_assoc_resources(adapter, 0);
+						_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+						goto ask_for_joinbss;
+						
+					}
+				}
+				else
+				{
+					_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+					goto ask_for_joinbss;
+				}
+							
+			}
+			
+		} else if (pmlmepriv->assoc_ssid.SsidLength == 0) {			
+			goto ask_for_joinbss;//anyway, join first selected(dequeued) pnetwork if ssid_len=0				
+	
+		#ifdef CONFIG_LAYER2_ROAMING
+		} else if (rtw_to_roaming(adapter) > 0) {
+		
+			if(	(roaming_candidate == NULL ||roaming_candidate->network.Rssi<pnetwork->network.Rssi )
+				&& is_same_ess(&pnetwork->network, &pmlmepriv->cur_network.network) 
+				//&&(!is_same_network(&pnetwork->network, &pmlmepriv->cur_network.network))
+				&&  rtw_get_time_interval_ms((u32)pnetwork->last_scanned,cur_time) < 5000
+				) {
+				roaming_candidate = pnetwork;
+				//RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+				DBG_871X
+					("roaming_candidate???: %s("MAC_FMT")\n",
+					roaming_candidate->network.Ssid.Ssid, MAC_ARG(roaming_candidate->network.MacAddress) )
+					//)
+					;
+			}
+			continue;
+		#endif
+
+		} else if ( (pnetwork->network.Ssid.SsidLength==pmlmepriv->assoc_ssid.SsidLength)
+			&&((_rtw_memcmp(dst_ssid, src_ssid, pmlmepriv->assoc_ssid.SsidLength)) == _TRUE)
+			)
+		{
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("dst_ssid=%s, src_ssid=%s \n", dst_ssid, src_ssid));
+#ifdef CONFIG_ANTENNA_DIVERSITY
+			rtw_hal_get_def_var(adapter, HAL_DEF_CURRENT_ANTENNA, &(CurrentAntenna));			
+			DBG_871X("#### dst_ssid=(%s) Opt_Ant_(%s) , cur_Ant(%s)\n", dst_ssid,
+				(2==pnetwork->network.PhyInfo.Optimum_antenna)?"A":"B",
+				(2==CurrentAntenna)?"A":"B");
+#endif
+			//remove the condition @ 20081125
+			//if((pmlmepriv->cur_network.network.InfrastructureMode==Ndis802_11AutoUnknown)||
+			//	pmlmepriv->cur_network.network.InfrastructureMode == pnetwork->network.InfrastructureMode)
+			//{
+			//	_rtw_memcpy(pmlmepriv->assoc_bssid, pnetwork->network.MacAddress, ETH_ALEN);
+			//	goto ask_for_joinbss;
+			//}
+
+			if(pmlmepriv->assoc_by_rssi==_TRUE)//if the ssid is the same, select the bss which has the max rssi
+			{
+				if( NULL==pnetwork_max_rssi|| pnetwork->network.Rssi > pnetwork_max_rssi->network.Rssi)
+						pnetwork_max_rssi = pnetwork;					
+			}
+			else if(rtw_is_desired_network(adapter, pnetwork) == _TRUE)
+			{
+				if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+				{
+#if 0				
+					if(is_same_network(&pmlmepriv->cur_network.network, &pnetwork->network))
+					{
+						DBG_871X("select_and_join(2): _FW_LINKED and is same network, it needn't join again\n");
+						
+						rtw_indicate_connect(adapter);//rtw_indicate_connect again
+						
+						return 2;
+					}
+					else
+#endif						
+					{
+						rtw_disassoc_cmd(adapter, 0, _TRUE);
+						//rtw_indicate_disconnect(adapter);//
+						rtw_free_assoc_resources(adapter, 0);
+						_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+						goto ask_for_joinbss;						
+					}
+				}
+				else
+				{
+					_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+					goto ask_for_joinbss;
+				}				
+
+			}
+		
+			
+		}
+ 	
+ 	}
+	_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+	#ifdef CONFIG_LAYER2_ROAMING
+	if(rtw_to_roaming(adapter) > 0 && roaming_candidate ){
+		pnetwork=roaming_candidate;
+		DBG_871X("select_and_join_from_scanned_queue: roaming_candidate: %s("MAC_FMT")\n",
+			pnetwork->network.Ssid.Ssid, MAC_ARG(pnetwork->network.MacAddress));
+		goto ask_for_joinbss;
+	}
+	#endif
+
+	if((pmlmepriv->assoc_by_rssi==_TRUE)  && (pnetwork_max_rssi!=NULL))
+	{
+		pnetwork = pnetwork_max_rssi;
+		DBG_871X("select_and_join_from_scanned_queue: pnetwork_max_rssi: %s("MAC_FMT")\n",
+			pnetwork->network.Ssid.Ssid, MAC_ARG(pnetwork->network.MacAddress));
+		goto ask_for_joinbss;
+	}
+
+	DBG_871X("(1)rtw_select_and_join_from_scanned_queue return _FAIL\n");
+	
+_func_exit_;	
+
+     return _FAIL;
+
+ask_for_joinbss:
+	
+_func_exit_;
+
+	return rtw_joinbss_cmd(adapter, pnetwork);
+
+}
+#endif //PLATFORM_FREEBSD
+
+
+sint rtw_set_auth(_adapter * adapter,struct security_priv *psecuritypriv)
+{
+	struct	cmd_obj* pcmd;
+	struct 	setauth_parm *psetauthparm;
+	struct	cmd_priv	*pcmdpriv=&(adapter->cmdpriv);
+	sint		res=_SUCCESS;
+	
+_func_enter_;	
+
+	pcmd = (struct	cmd_obj*)rtw_zmalloc(sizeof(struct	cmd_obj));
+	if(pcmd==NULL){
+		res= _FAIL;  //try again
+		goto exit;
+	}
+	
+	psetauthparm=(struct setauth_parm*)rtw_zmalloc(sizeof(struct setauth_parm));
+	if(psetauthparm==NULL){
+		rtw_mfree((unsigned char *)pcmd, sizeof(struct	cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	_rtw_memset(psetauthparm, 0, sizeof(struct setauth_parm));
+	psetauthparm->mode=(unsigned char)psecuritypriv->dot11AuthAlgrthm;
+	
+	pcmd->cmdcode = _SetAuth_CMD_;
+	pcmd->parmbuf = (unsigned char *)psetauthparm;   
+	pcmd->cmdsz =  (sizeof(struct setauth_parm));  
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+
+
+	_rtw_init_listhead(&pcmd->list);
+
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("after enqueue set_auth_cmd, auth_mode=%x\n", psecuritypriv->dot11AuthAlgrthm));
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+
+_func_exit_;
+
+	return res;
+
+}
+
+
+sint rtw_set_key(_adapter * adapter,struct security_priv *psecuritypriv,sint keyid, u8 set_tx)
+{
+	u8	keylen;
+	struct cmd_obj		*pcmd;
+	struct setkey_parm	*psetkeyparm;
+	struct cmd_priv		*pcmdpriv = &(adapter->cmdpriv);
+	struct mlme_priv		*pmlmepriv = &(adapter->mlmepriv);
+	sint	res=_SUCCESS;
+	
+_func_enter_;
+	
+	pcmd = (struct	cmd_obj*)rtw_zmalloc(sizeof(struct	cmd_obj));
+	if(pcmd==NULL){
+		res= _FAIL;  //try again
+		goto exit;
+	}
+	psetkeyparm=(struct setkey_parm*)rtw_zmalloc(sizeof(struct setkey_parm));
+	if(psetkeyparm==NULL){
+		rtw_mfree((unsigned char *)pcmd, sizeof(struct	cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	_rtw_memset(psetkeyparm, 0, sizeof(struct setkey_parm));
+
+	if(psecuritypriv->dot11AuthAlgrthm ==dot11AuthAlgrthm_8021X){		
+		psetkeyparm->algorithm=(unsigned char)psecuritypriv->dot118021XGrpPrivacy;	
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("\n rtw_set_key: psetkeyparm->algorithm=(unsigned char)psecuritypriv->dot118021XGrpPrivacy=%d \n", psetkeyparm->algorithm));
+	}	
+	else{
+		psetkeyparm->algorithm=(u8)psecuritypriv->dot11PrivacyAlgrthm;
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("\n rtw_set_key: psetkeyparm->algorithm=(u8)psecuritypriv->dot11PrivacyAlgrthm=%d \n", psetkeyparm->algorithm));
+
+	}
+	psetkeyparm->keyid = (u8)keyid;//0~3
+	psetkeyparm->set_tx = set_tx;
+	if (is_wep_enc(psetkeyparm->algorithm))
+		pmlmepriv->key_mask |= BIT(psetkeyparm->keyid);
+
+#ifdef CONFIG_AUTOSUSPEND
+	if( _TRUE  == adapter->pwrctrlpriv.bInternalAutoSuspend)
+	{
+		adapter->pwrctrlpriv.wepkeymask = pmlmepriv->key_mask;
+		DBG_871X("....AutoSuspend pwrctrlpriv.wepkeymask(%x)\n",adapter->pwrctrlpriv.wepkeymask);
+	}
+#endif
+	DBG_871X("==> rtw_set_key algorithm(%x),keyid(%x),key_mask(%x)\n",psetkeyparm->algorithm,psetkeyparm->keyid,pmlmepriv->key_mask);
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("\n rtw_set_key: psetkeyparm->algorithm=%d psetkeyparm->keyid=(u8)keyid=%d \n",psetkeyparm->algorithm, keyid));
+
+	switch(psetkeyparm->algorithm){
+		
+		case _WEP40_:
+			keylen=5;
+			_rtw_memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen);
+			break;
+		case _WEP104_:
+			keylen=13;
+			_rtw_memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen);
+			break;
+		case _TKIP_:
+			keylen=16;			
+			_rtw_memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen);
+			psetkeyparm->grpkey=1;
+			break;
+		case _AES_:
+			keylen=16;			
+			_rtw_memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen);
+			psetkeyparm->grpkey=1;
+			break;
+		default:
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("\n rtw_set_key:psecuritypriv->dot11PrivacyAlgrthm = %x (must be 1 or 2 or 4 or 5)\n",psecuritypriv->dot11PrivacyAlgrthm));
+			res= _FAIL;
+			goto exit;
+	}
+
+	
+	pcmd->cmdcode = _SetKey_CMD_;
+	pcmd->parmbuf = (u8 *)psetkeyparm;   
+	pcmd->cmdsz =  (sizeof(struct setkey_parm));  
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+
+
+	_rtw_init_listhead(&pcmd->list);
+
+	//_rtw_init_sema(&(pcmd->cmd_sem), 0);
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+_func_exit_;
+	return res;
+
+}
+
+
+//adjust IEs for rtw_joinbss_cmd in WMM
+int rtw_restruct_wmm_ie(_adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len, uint initial_out_len)
+{
+	unsigned	int ielength=0;
+	unsigned int i, j;
+
+	i = 12; //after the fixed IE
+	while(i<in_len)
+	{
+		ielength = initial_out_len;		
+		
+		if(in_ie[i] == 0xDD && in_ie[i+2] == 0x00 && in_ie[i+3] == 0x50  && in_ie[i+4] == 0xF2 && in_ie[i+5] == 0x02 && i+5 < in_len) //WMM element ID and OUI
+		{
+
+			//Append WMM IE to the last index of out_ie
+			/*
+			for(j=i; j< i+(in_ie[i+1]+2); j++)
+			{
+				out_ie[ielength] = in_ie[j];				
+				ielength++;
+			}
+			out_ie[initial_out_len+8] = 0x00; //force the QoS Info Field to be zero
+	                */
+                       
+                        for ( j = i; j < i + 9; j++ )
+                        {
+                            out_ie[ ielength] = in_ie[ j ];
+                            ielength++;
+                        } 
+                        out_ie[ initial_out_len + 1 ] = 0x07;
+                        out_ie[ initial_out_len + 6 ] = 0x00;
+                        out_ie[ initial_out_len + 8 ] = 0x00;
+	
+			break;
+		}
+
+		i+=(in_ie[i+1]+2); // to the next IE element
+	}
+	
+	return ielength;
+	
+}
+
+
+//
+// Ported from 8185: IsInPreAuthKeyList(). (Renamed from SecIsInPreAuthKeyList(), 2006-10-13.)
+// Added by Annie, 2006-05-07.
+//
+// Search by BSSID,
+// Return Value:
+//		-1 		:if there is no pre-auth key in the  table
+//		>=0		:if there is pre-auth key, and   return the entry id
+//
+//
+
+static int SecIsInPMKIDList(_adapter *Adapter, u8 *bssid)
+{
+	struct security_priv *psecuritypriv=&Adapter->securitypriv;
+	int i=0;
+
+	do
+	{
+		if( ( psecuritypriv->PMKIDList[i].bUsed ) && 
+                    (  _rtw_memcmp( psecuritypriv->PMKIDList[i].Bssid, bssid, ETH_ALEN ) == _TRUE ) )
+		{
+			break;
+		}
+		else
+		{	
+			i++;
+			//continue;
+		}
+		
+	}while(i<NUM_PMKID_CACHE);
+
+	if( i == NUM_PMKID_CACHE )
+	{ 
+		i = -1;// Could not find.
+	}
+	else
+	{ 
+		// There is one Pre-Authentication Key for the specific BSSID.
+	}
+
+	return (i);
+	
+}
+
+//
+// Check the RSN IE length
+// If the RSN IE length <= 20, the RSN IE didn't include the PMKID information
+// 0-11th element in the array are the fixed IE
+// 12th element in the array is the IE
+// 13th element in the array is the IE length  
+//
+
+static int rtw_append_pmkid(_adapter *Adapter,int iEntry, u8 *ie, uint ie_len)
+{
+	struct security_priv *psecuritypriv=&Adapter->securitypriv;
+
+	if(ie[13]<=20){	
+		// The RSN IE didn't include the PMK ID, append the PMK information 
+			ie[ie_len]=1;
+			ie_len++;
+			ie[ie_len]=0;	//PMKID count = 0x0100
+			ie_len++;
+			_rtw_memcpy(	&ie[ie_len], &psecuritypriv->PMKIDList[iEntry].PMKID, 16);
+		
+			ie_len+=16;
+			ie[13]+=18;//PMKID length = 2+16
+
+	}
+	return (ie_len);
+	
+}
+sint rtw_restruct_sec_ie(_adapter *adapter,u8 *in_ie, u8 *out_ie, uint in_len)
+{
+	u8 authmode, securitytype, match;
+	u8 sec_ie[255], uncst_oui[4], bkup_ie[255];
+	u8 wpa_oui[4]={0x0, 0x50, 0xf2, 0x01};
+	uint 	ielength, cnt, remove_cnt;
+	int iEntry;
+
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct security_priv *psecuritypriv=&adapter->securitypriv;
+	uint 	ndisauthmode=psecuritypriv->ndisauthtype;
+	uint ndissecuritytype = psecuritypriv->ndisencryptstatus;
+	
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+		 ("+rtw_restruct_sec_ie: ndisauthmode=%d ndissecuritytype=%d\n",
+		  ndisauthmode, ndissecuritytype));
+	
+	//copy fixed ie only
+	_rtw_memcpy(out_ie, in_ie,12);
+	ielength=12;
+	if((ndisauthmode==Ndis802_11AuthModeWPA)||(ndisauthmode==Ndis802_11AuthModeWPAPSK))
+			authmode=_WPA_IE_ID_;
+	if((ndisauthmode==Ndis802_11AuthModeWPA2)||(ndisauthmode==Ndis802_11AuthModeWPA2PSK))
+			authmode=_WPA2_IE_ID_;
+
+	if(check_fwstate(pmlmepriv, WIFI_UNDER_WPS))
+	{
+		_rtw_memcpy(out_ie+ielength, psecuritypriv->wps_ie, psecuritypriv->wps_ie_len);
+		
+		ielength += psecuritypriv->wps_ie_len;
+	}
+	else if((authmode==_WPA_IE_ID_)||(authmode==_WPA2_IE_ID_))
+	{		
+		//copy RSN or SSN		
+		_rtw_memcpy(&out_ie[ielength], &psecuritypriv->supplicant_ie[0], psecuritypriv->supplicant_ie[1]+2);
+		ielength+=psecuritypriv->supplicant_ie[1]+2;
+		rtw_report_sec_ie(adapter, authmode, psecuritypriv->supplicant_ie);
+	
+#ifdef CONFIG_DRVEXT_MODULE
+		drvext_report_sec_ie(&adapter->drvextpriv, authmode, sec_ie);	
+#endif
+	}
+
+	iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid);
+	if(iEntry<0)
+	{
+		return ielength;
+	}
+	else
+	{
+		if(authmode == _WPA2_IE_ID_)
+		{
+			ielength=rtw_append_pmkid(adapter, iEntry, out_ie, ielength);
+		}
+	}
+
+_func_exit_;
+	
+	return ielength;	
+}
+
+void rtw_init_registrypriv_dev_network(	_adapter* adapter)
+{
+	struct registry_priv* pregistrypriv = &adapter->registrypriv;
+	struct eeprom_priv* peepriv = &adapter->eeprompriv;
+	WLAN_BSSID_EX    *pdev_network = &pregistrypriv->dev_network;
+	u8 *myhwaddr = myid(peepriv);
+	
+_func_enter_;
+
+	_rtw_memcpy(pdev_network->MacAddress, myhwaddr, ETH_ALEN);
+
+	_rtw_memcpy(&pdev_network->Ssid, &pregistrypriv->ssid, sizeof(NDIS_802_11_SSID));
+	
+	pdev_network->Configuration.Length=sizeof(NDIS_802_11_CONFIGURATION);
+	pdev_network->Configuration.BeaconPeriod = 100;	
+	pdev_network->Configuration.FHConfig.Length = 0;
+	pdev_network->Configuration.FHConfig.HopPattern = 0;
+	pdev_network->Configuration.FHConfig.HopSet = 0;
+	pdev_network->Configuration.FHConfig.DwellTime = 0;
+	
+	
+_func_exit_;	
+	
+}
+
+void rtw_update_registrypriv_dev_network(_adapter* adapter) 
+{
+	int sz=0;
+	struct registry_priv* pregistrypriv = &adapter->registrypriv;	
+	WLAN_BSSID_EX    *pdev_network = &pregistrypriv->dev_network;
+	struct	security_priv*	psecuritypriv = &adapter->securitypriv;
+	struct	wlan_network	*cur_network = &adapter->mlmepriv.cur_network;
+	//struct	xmit_priv	*pxmitpriv = &adapter->xmitpriv;
+
+_func_enter_;
+
+#if 0
+	pxmitpriv->vcs_setting = pregistrypriv->vrtl_carrier_sense;
+	pxmitpriv->vcs = pregistrypriv->vcs_type;
+	pxmitpriv->vcs_type = pregistrypriv->vcs_type;
+	//pxmitpriv->rts_thresh = pregistrypriv->rts_thresh;
+	pxmitpriv->frag_len = pregistrypriv->frag_thresh;
+	
+	adapter->qospriv.qos_option = pregistrypriv->wmm_enable;
+#endif	
+
+	pdev_network->Privacy = (psecuritypriv->dot11PrivacyAlgrthm > 0 ? 1 : 0) ; // adhoc no 802.1x
+
+	pdev_network->Rssi = 0;
+
+	switch(pregistrypriv->wireless_mode)
+	{
+		case WIRELESS_11B:
+			pdev_network->NetworkTypeInUse = (Ndis802_11DS);
+			break;	
+		case WIRELESS_11G:
+		case WIRELESS_11BG:
+		case WIRELESS_11_24N:
+		case WIRELESS_11G_24N:
+		case WIRELESS_11BG_24N:
+			pdev_network->NetworkTypeInUse = (Ndis802_11OFDM24);
+			break;
+		case WIRELESS_11A:
+		case WIRELESS_11A_5N:
+			pdev_network->NetworkTypeInUse = (Ndis802_11OFDM5);
+			break;
+		case WIRELESS_11ABGN:
+			if(pregistrypriv->channel > 14)
+				pdev_network->NetworkTypeInUse = (Ndis802_11OFDM5);
+			else
+				pdev_network->NetworkTypeInUse = (Ndis802_11OFDM24);
+			break;
+		default :
+			// TODO
+			break;
+	}
+	
+	pdev_network->Configuration.DSConfig = (pregistrypriv->channel);
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("pregistrypriv->channel=%d, pdev_network->Configuration.DSConfig=0x%x\n", pregistrypriv->channel, pdev_network->Configuration.DSConfig));	
+
+	if(cur_network->network.InfrastructureMode == Ndis802_11IBSS)
+		pdev_network->Configuration.ATIMWindow = (0);
+
+	pdev_network->InfrastructureMode = (cur_network->network.InfrastructureMode);
+
+	// 1. Supported rates
+	// 2. IE
+
+	//rtw_set_supported_rate(pdev_network->SupportedRates, pregistrypriv->wireless_mode) ; // will be called in rtw_generate_ie
+	sz = rtw_generate_ie(pregistrypriv);
+
+	pdev_network->IELength = sz;
+
+	pdev_network->Length = get_WLAN_BSSID_EX_sz((WLAN_BSSID_EX  *)pdev_network);
+
+	//notes: translate IELength & Length after assign the Length to cmdsz in createbss_cmd();
+	//pdev_network->IELength = cpu_to_le32(sz);
+		
+_func_exit_;	
+
+}
+
+void rtw_get_encrypt_decrypt_from_registrypriv(_adapter* adapter)
+{
+_func_enter_;
+
+
+_func_exit_;	
+	
+}
+
+//the fucntion is at passive_level 
+void rtw_joinbss_reset(_adapter *padapter)
+{
+	u8	threshold;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+#ifdef CONFIG_80211N_HT	
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+#endif
+
+	//todo: if you want to do something io/reg/hw setting before join_bss, please add code here
+	
+
+
+
+#ifdef CONFIG_80211N_HT
+
+	pmlmepriv->num_FortyMHzIntolerant = 0;
+
+	pmlmepriv->num_sta_no_ht = 0;
+
+	phtpriv->ampdu_enable = _FALSE;//reset to disabled
+
+#ifdef CONFIG_USB_HCI
+	// TH=1 => means that invalidate usb rx aggregation
+	// TH=0 => means that validate usb rx aggregation, use init value.
+	if(phtpriv->ht_option)
+	{
+		if(padapter->registrypriv.wifi_spec==1)		
+			threshold = 1;
+		else
+			threshold = 0;
+		rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+	}
+	else
+	{
+		threshold = 1;
+		rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+	}
+#endif
+
+#endif	
+
+}
+
+
+#ifdef CONFIG_80211N_HT
+
+//the fucntion is >= passive_level 
+unsigned int rtw_restructure_ht_ie(_adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len)
+{
+	u32 ielen, out_len;
+	HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor;
+	unsigned char *p, *pframe;
+	struct rtw_ieee80211_ht_cap ht_capie;
+	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv   	*pqospriv= &pmlmepriv->qospriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+
+
+	phtpriv->ht_option = _FALSE;
+
+	p = rtw_get_ie(in_ie+12, _HT_CAPABILITY_IE_, &ielen, in_len-12);
+
+	if(p && ielen>0)
+	{
+		if(pqospriv->qos_option == 0)
+		{
+			out_len = *pout_len;
+			pframe = rtw_set_ie(out_ie+out_len, _VENDOR_SPECIFIC_IE_, 
+								_WMM_IE_Length_, WMM_IE, pout_len);
+
+			pqospriv->qos_option = 1;
+		}
+
+		out_len = *pout_len;
+
+		_rtw_memset(&ht_capie, 0, sizeof(struct rtw_ieee80211_ht_cap));
+
+		ht_capie.cap_info = IEEE80211_HT_CAP_SUP_WIDTH |IEEE80211_HT_CAP_SGI_20 |
+							IEEE80211_HT_CAP_SGI_40 | IEEE80211_HT_CAP_TX_STBC |
+							IEEE80211_HT_CAP_DSSSCCK40;
+
+
+		{
+			u32 rx_packet_offset, max_recvbuf_sz;
+			rtw_hal_get_def_var(padapter, HAL_DEF_RX_PACKET_OFFSET, &rx_packet_offset);
+			rtw_hal_get_def_var(padapter, HAL_DEF_MAX_RECVBUF_SZ, &max_recvbuf_sz);
+			//if(max_recvbuf_sz-rx_packet_offset>(8191-256)) {
+			//	DBG_871X("%s IEEE80211_HT_CAP_MAX_AMSDU is set\n", __FUNCTION__);
+			//	ht_capie.cap_info = ht_capie.cap_info |IEEE80211_HT_CAP_MAX_AMSDU;
+			//}
+		}
+		/* 	
+		AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+		AMPDU_para [4:2]:Min MPDU Start Spacing	
+		*/
+
+		/*		
+		#if defined(CONFIG_RTL8188E )&& defined (CONFIG_SDIO_HCI)
+		ht_capie.ampdu_params_info = 2;
+		#else
+		ht_capie.ampdu_params_info = (IEEE80211_HT_CAP_AMPDU_FACTOR&0x03);
+		#endif
+		*/
+		
+		rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
+		ht_capie.ampdu_params_info = (max_rx_ampdu_factor&0x03);
+		
+		if(padapter->securitypriv.dot11PrivacyAlgrthm == _AES_ )
+			ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&(0x07<<2));
+		else
+			ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&0x00);										
+
+		
+		pframe = rtw_set_ie(out_ie+out_len, _HT_CAPABILITY_IE_, 
+							sizeof(struct rtw_ieee80211_ht_cap), (unsigned char*)&ht_capie, pout_len);
+
+
+		//_rtw_memcpy(out_ie+out_len, p, ielen+2);//gtest
+		//*pout_len = *pout_len + (ielen+2);
+
+							
+		phtpriv->ht_option = _TRUE;
+
+		p = rtw_get_ie(in_ie+12, _HT_ADD_INFO_IE_, &ielen, in_len-12);
+		if(p && (ielen==sizeof(struct ieee80211_ht_addt_info)))
+		{
+			out_len = *pout_len;		
+			pframe = rtw_set_ie(out_ie+out_len, _HT_ADD_INFO_IE_, ielen, p+2 , pout_len);
+		}
+
+	}
+	
+	return (phtpriv->ht_option);
+	
+}
+
+//the fucntion is > passive_level (in critical_section)
+void rtw_update_ht_cap(_adapter *padapter, u8 *pie, uint ie_len)
+{	
+	u8 *p, max_ampdu_sz;
+	int len;		
+	//struct sta_info *bmc_sta, *psta;	
+	struct rtw_ieee80211_ht_cap *pht_capie;
+	struct ieee80211_ht_addt_info *pht_addtinfo;
+	//struct recv_reorder_ctrl *preorder_ctrl;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+	//struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	//struct wlan_network *pcur_network = &(pmlmepriv->cur_network);;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);	
+	
+
+	if(!phtpriv->ht_option)
+		return;
+
+	if ((!pmlmeinfo->HT_info_enable) || (!pmlmeinfo->HT_caps_enable))
+		return;
+
+	DBG_871X("+rtw_update_ht_cap()\n");
+
+	//maybe needs check if ap supports rx ampdu.
+	if((phtpriv->ampdu_enable==_FALSE) &&(pregistrypriv->ampdu_enable==1))
+	{
+		if(pregistrypriv->wifi_spec==1)
+		{
+			phtpriv->ampdu_enable = _FALSE;
+		}
+		else
+		{
+			phtpriv->ampdu_enable = _TRUE;
+		}
+	}
+	else if(pregistrypriv->ampdu_enable==2)
+	{
+		phtpriv->ampdu_enable = _TRUE;
+	}
+
+	
+	//check Max Rx A-MPDU Size 
+	len = 0;
+	p = rtw_get_ie(pie+sizeof (NDIS_802_11_FIXED_IEs), _HT_CAPABILITY_IE_, &len, ie_len-sizeof (NDIS_802_11_FIXED_IEs));
+	if(p && len>0)	
+	{
+		pht_capie = (struct rtw_ieee80211_ht_cap *)(p+2);
+		max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_CAP_AMPDU_FACTOR);
+		max_ampdu_sz = 1 << (max_ampdu_sz+3); // max_ampdu_sz (kbytes);
+		
+		//DBG_871X("rtw_update_ht_cap(): max_ampdu_sz=%d\n", max_ampdu_sz);
+		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
+		
+	}
+
+
+	len=0;
+	p = rtw_get_ie(pie+sizeof (NDIS_802_11_FIXED_IEs), _HT_ADD_INFO_IE_, &len, ie_len-sizeof (NDIS_802_11_FIXED_IEs));
+	if(p && len>0)	
+	{
+		pht_addtinfo = (struct ieee80211_ht_addt_info *)(p+2);
+		//todo:
+	}
+
+
+	//update cur_bwmode & cur_ch_offset
+	if ((pregistrypriv->cbw40_enable) &&
+		(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & BIT(1)) && 
+		(pmlmeinfo->HT_info.infos[0] & BIT(2)))
+	{
+		int i;
+		u8	rf_type;
+
+		padapter->HalFunc.GetHwRegHandler(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+		//update the MCS rates
+		for (i = 0; i < 16; i++)
+		{
+			if((rf_type == RF_1T1R) || (rf_type == RF_1T2R))
+			{
+				pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i];
+			}
+			else
+			{
+				#ifdef CONFIG_DISABLE_MCS13TO15
+				if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40 && pregistrypriv->wifi_spec != 1 )
+				{
+					pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R_MCS13TO15_OFF[i];
+				}
+				else
+					pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R[i];
+				#else
+				pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R[i];
+				#endif //CONFIG_DISABLE_MCS13TO15
+			}
+			#ifdef RTL8192C_RECONFIG_TO_1T1R
+			{
+				pmlmeinfo->HT_caps.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i];
+			}
+			#endif
+		}
+		//switch to the 40M Hz mode accoring to the AP
+		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+		switch ((pmlmeinfo->HT_info.infos[0] & 0x3))
+		{
+			case HT_EXTCHNL_OFFSET_UPPER:
+				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+				break;
+			
+			case HT_EXTCHNL_OFFSET_LOWER:
+				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+				break;
+				
+			default:
+				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+				break;
+		}		
+	}
+
+	//
+	// Config SM Power Save setting
+	//
+	pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2;
+	if(pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC)
+	{
+		/*u8 i;
+		//update the MCS rates
+		for (i = 0; i < 16; i++)
+		{
+			pmlmeinfo->HT_caps.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i];
+		}*/
+		DBG_871X("%s(): WLAN_HT_CAP_SM_PS_STATIC\n",__FUNCTION__);
+	}
+
+	//
+	// Config current HT Protection mode.
+	//
+	pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3;
+
+	
+	
+#if 0 //move to rtw_update_sta_info_client()
+	//for A-MPDU Rx reordering buffer control for bmc_sta & sta_info
+	//if A-MPDU Rx is enabled, reseting  rx_ordering_ctrl wstart_b(indicate_seq) to default value=0xffff
+	//todo: check if AP can send A-MPDU packets
+	bmc_sta = rtw_get_bcmc_stainfo(padapter);
+	if(bmc_sta)
+	{
+		for(i=0; i < 16 ; i++)
+		{
+			//preorder_ctrl = &precvpriv->recvreorder_ctrl[i];
+			preorder_ctrl = &bmc_sta->recvreorder_ctrl[i];
+			preorder_ctrl->enable = _FALSE;
+			preorder_ctrl->indicate_seq = 0xffff;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
+				preorder_ctrl->indicate_seq);
+			#endif
+			preorder_ctrl->wend_b= 0xffff;
+			preorder_ctrl->wsize_b = 64;//max_ampdu_sz;//ex. 32(kbytes) -> wsize_b=32
+		}
+	}
+
+	psta = rtw_get_stainfo(&padapter->stapriv, pcur_network->network.MacAddress);
+	if(psta)
+	{
+		for(i=0; i < 16 ; i++)
+		{
+			//preorder_ctrl = &precvpriv->recvreorder_ctrl[i];
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+			preorder_ctrl->enable = _FALSE;
+			preorder_ctrl->indicate_seq = 0xffff;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
+				preorder_ctrl->indicate_seq);
+			#endif
+			preorder_ctrl->wend_b= 0xffff;
+			preorder_ctrl->wsize_b = 64;//max_ampdu_sz;//ex. 32(kbytes) -> wsize_b=32
+		}
+	}
+#endif	
+
+}
+
+void rtw_issue_addbareq_cmd(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	u8 issued;
+	int priority;
+	struct sta_info *psta=NULL;
+	struct ht_priv	*phtpriv;
+	struct pkt_attrib *pattrib =&pxmitframe->attrib;
+	s32 bmcst = IS_MCAST(pattrib->ra);
+
+	//if(bmcst || (padapter->mlmepriv.LinkDetectInfo.bTxBusyTraffic == _FALSE))
+	if(bmcst || (padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod<100))	
+		return;
+	
+	priority = pattrib->priority;
+
+	if (pattrib->psta)
+		psta = pattrib->psta;
+	else
+	{
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	}	
+	
+	if(psta==NULL)
+	{
+		DBG_871X("%s, psta==NUL\n", __func__);
+		return;
+	}
+
+	if(!(psta->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return;
+	}	
+
+
+	phtpriv = &psta->htpriv;
+
+	if((phtpriv->ht_option==_TRUE) && (phtpriv->ampdu_enable==_TRUE)) 
+	{
+		issued = (phtpriv->agg_enable_bitmap>>priority)&0x1;
+		issued |= (phtpriv->candidate_tid_bitmap>>priority)&0x1;
+
+		if(0==issued)
+		{
+			DBG_871X("rtw_issue_addbareq_cmd, p=%d\n", priority);
+			psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority);
+			rtw_addbareq_cmd(padapter,(u8) priority, pattrib->ra);
+		}
+	}
+	
+}
+
+#endif
+
+#ifdef CONFIG_LAYER2_ROAMING
+inline void rtw_set_roaming(_adapter *adapter, u8 to_roaming)
+{
+	if (to_roaming == 0)
+		adapter->mlmepriv.to_join = _FALSE;
+	adapter->mlmepriv.to_roaming = to_roaming;
+}
+
+inline u8 rtw_to_roaming(_adapter *adapter)
+{
+	return adapter->mlmepriv.to_roaming;
+}
+
+void rtw_roaming(_adapter *padapter, struct wlan_network *tgt_network)
+{
+	_irqL irqL;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+	_rtw_roaming(padapter, tgt_network);
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+}
+void _rtw_roaming(_adapter *padapter, struct wlan_network *tgt_network)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	int do_join_r;
+
+	struct wlan_network *pnetwork;
+
+	if(tgt_network != NULL)
+		pnetwork = tgt_network;
+	else
+		pnetwork = &pmlmepriv->cur_network;
+	
+	if(0 < rtw_to_roaming(padapter)) {
+		DBG_871X("roaming from %s("MAC_FMT"), length:%d\n",
+				pnetwork->network.Ssid.Ssid, MAC_ARG(pnetwork->network.MacAddress),
+				pnetwork->network.Ssid.SsidLength);
+		_rtw_memcpy(&pmlmepriv->assoc_ssid, &pnetwork->network.Ssid, sizeof(NDIS_802_11_SSID));
+
+		pmlmepriv->assoc_by_bssid = _FALSE;
+
+#ifdef CONFIG_WAPI_SUPPORT
+		rtw_wapi_return_all_sta_info(padapter);
+#endif
+
+		while(1) {
+			if( _SUCCESS==(do_join_r=rtw_do_join(padapter)) ) {
+				break;
+			} else {
+				DBG_871X("roaming do_join return %d\n", do_join_r);
+				pmlmepriv->to_roaming--;
+				
+				if(0< rtw_to_roaming(padapter)) {
+					continue;
+				} else {
+					DBG_871X("%s(%d) -to roaming fail, indicate_disconnect\n", __FUNCTION__,__LINE__);
+					rtw_indicate_disconnect(padapter);
+					break;
+				}
+			}
+		}
+	}
+	
+}
+#endif
+
+sint rtw_linked_check(_adapter *padapter)
+{
+	if(	(check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == _TRUE) ||
+			(check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE) == _TRUE))
+	{
+		if(padapter->stapriv.asoc_sta_count > 2)
+			return _TRUE;
+	}
+	else
+	{	//Station mode
+		if(check_fwstate(&padapter->mlmepriv, _FW_LINKED)== _TRUE)
+			return _TRUE;
+	}
+	return _FALSE;
+}
+
+#ifdef CONFIG_CONCURRENT_MODE
+sint rtw_buddy_adapter_up(_adapter *padapter)
+{	
+	sint res = _FALSE;
+	
+	if(padapter == NULL)
+		return res;
+
+
+	if(padapter->pbuddy_adapter == NULL)
+	{
+		res = _FALSE;
+	}
+	else if( (padapter->pbuddy_adapter->bDriverStopped) || (padapter->pbuddy_adapter->bSurpriseRemoved) ||
+		(padapter->pbuddy_adapter->bup == _FALSE) || (padapter->pbuddy_adapter->hw_init_completed == _FALSE))
+	{		
+		res = _FALSE;
+	}
+	else
+	{
+		res = _TRUE;
+	}	
+
+	return res;	
+
+}
+
+sint check_buddy_fwstate(_adapter *padapter, sint state)
+{
+	if(padapter == NULL)
+		return _FALSE;	
+	
+	if(padapter->pbuddy_adapter == NULL)
+		return _FALSE;	
+		
+	if ((state == WIFI_FW_NULL_STATE) && 
+		(padapter->pbuddy_adapter->mlmepriv.fw_state == WIFI_FW_NULL_STATE))
+		return _TRUE;		
+	
+	if (padapter->pbuddy_adapter->mlmepriv.fw_state & state)
+		return _TRUE;
+
+	return _FALSE;
+}
+
+#endif //CONFIG_CONCURRENT_MODE
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_mlme_ext.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_mlme_ext.c
new file mode 100755
index 00000000..74fac72a
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_mlme_ext.c
@@ -0,0 +1,13017 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_MLME_EXT_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <wifi.h>
+#include <rtw_mlme_ext.h>
+#include <wlan_bssdef.h>
+#include <mlme_osdep.h>
+#include <recv_osdep.h>
+
+#ifdef CONFIG_BT_COEXIST
+#include <rtl8723a_hal.h>
+#endif
+
+struct mlme_handler mlme_sta_tbl[]={
+	{WIFI_ASSOCREQ,		"OnAssocReq",	&OnAssocReq},
+	{WIFI_ASSOCRSP,		"OnAssocRsp",	&OnAssocRsp},
+	{WIFI_REASSOCREQ,	"OnReAssocReq",	&OnAssocReq},
+	{WIFI_REASSOCRSP,	"OnReAssocRsp",	&OnAssocRsp},
+	{WIFI_PROBEREQ,		"OnProbeReq",	&OnProbeReq},
+	{WIFI_PROBERSP,		"OnProbeRsp",		&OnProbeRsp},
+
+	/*----------------------------------------------------------
+					below 2 are reserved
+	-----------------------------------------------------------*/
+	{0,					"DoReserved",		&DoReserved},
+	{0,					"DoReserved",		&DoReserved},
+	{WIFI_BEACON,		"OnBeacon",		&OnBeacon},
+	{WIFI_ATIM,			"OnATIM",		&OnAtim},
+	{WIFI_DISASSOC,		"OnDisassoc",		&OnDisassoc},
+	{WIFI_AUTH,			"OnAuth",		&OnAuthClient},
+	{WIFI_DEAUTH,		"OnDeAuth",		&OnDeAuth},
+	{WIFI_ACTION,		"OnAction",		&OnAction},
+};
+
+#ifdef _CONFIG_NATIVEAP_MLME_
+struct mlme_handler mlme_ap_tbl[]={
+	{WIFI_ASSOCREQ,		"OnAssocReq",	&OnAssocReq},
+	{WIFI_ASSOCRSP,		"OnAssocRsp",	&OnAssocRsp},
+	{WIFI_REASSOCREQ,	"OnReAssocReq",	&OnAssocReq},
+	{WIFI_REASSOCRSP,	"OnReAssocRsp",	&OnAssocRsp},
+	{WIFI_PROBEREQ,		"OnProbeReq",	&OnProbeReq},
+	{WIFI_PROBERSP,		"OnProbeRsp",		&OnProbeRsp},
+
+	/*----------------------------------------------------------
+					below 2 are reserved
+	-----------------------------------------------------------*/
+	{0,					"DoReserved",		&DoReserved},
+	{0,					"DoReserved",		&DoReserved},
+	{WIFI_BEACON,		"OnBeacon",		&OnBeacon},
+	{WIFI_ATIM,			"OnATIM",		&OnAtim},
+	{WIFI_DISASSOC,		"OnDisassoc",		&OnDisassoc},
+	{WIFI_AUTH,			"OnAuth",		&OnAuth},
+	{WIFI_DEAUTH,		"OnDeAuth",		&OnDeAuth},
+	{WIFI_ACTION,		"OnAction",		&OnAction},
+};
+#endif
+
+struct action_handler OnAction_tbl[]={
+	{RTW_WLAN_CATEGORY_SPECTRUM_MGMT,	 "ACTION_SPECTRUM_MGMT", on_action_spct},
+	{RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos},
+	{RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls},
+	{RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
+	{RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
+	{RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
+	{RTW_WLAN_CATEGORY_FT, "ACTION_FT",	&DoReserved},
+	{RTW_WLAN_CATEGORY_HT,	"ACTION_HT",	&OnAction_ht},
+	{RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved},
+	{RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm},	
+	{RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p},	
+};
+
+
+u8	null_addr[ETH_ALEN]= {0,0,0,0,0,0};
+
+/**************************************************
+OUI definitions for the vendor specific IE
+***************************************************/
+unsigned char	RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
+unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
+unsigned char	WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
+unsigned char	P2P_OUI[] = {0x50,0x6F,0x9A,0x09};
+unsigned char	WFD_OUI[] = {0x50,0x6F,0x9A,0x0A};
+
+unsigned char	WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+unsigned char	WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+
+unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
+unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
+
+extern unsigned char REALTEK_96B_IE[];
+
+/********************************************************
+MCS rate definitions
+*********************************************************/
+#ifdef CONFIG_DISABLE_MCS13TO15
+unsigned char	MCS_rate_2R_MCS13TO15_OFF[16] = {0xff, 0x1f, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+unsigned char	MCS_rate_2R[16] = {0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+#else //CONFIG_DISABLE_MCS13TO15
+unsigned char	MCS_rate_2R[16] = {0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+#endif //CONFIG_DISABLE_MCS13TO15
+unsigned char	MCS_rate_1R[16] = {0xff, 0x00, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+
+/********************************************************
+ChannelPlan definitions
+*********************************************************/
+/*static RT_CHANNEL_PLAN	DefaultChannelPlan[RT_CHANNEL_DOMAIN_MAX] = {
+	{{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},32},					// 0x00, RT_CHANNEL_DOMAIN_FCC
+	{{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},31},						// 0x01, RT_CHANNEL_DOMAIN_IC
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},32},						// 0x02, RT_CHANNEL_DOMAIN_ETSI
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13},13},																					// 0x03, RT_CHANNEL_DOMAIN_SPAIN
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13},13},																					// 0x04, RT_CHANNEL_DOMAIN_FRANCE
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13},13},																					// 0x05, RT_CHANNEL_DOMAIN_MKK
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13},13},																					// 0x06, RT_CHANNEL_DOMAIN_MKK1
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64},21},																// 0x07, RT_CHANNEL_DOMAIN_ISRAEL
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,14,36,40,44,48,52,56,60,64},22},															// 0x08, RT_CHANNEL_DOMAIN_TELEC
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},																				// 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13},13},																					// 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13
+	{{1,2,3,4,5,6,7,8,9,10,11,56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},26},									// 0x0B, RT_CHANNEL_DOMAIN_TAIWAN
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,149,153,157,161,165},18},																// 0x0C, RT_CHANNEL_DOMAIN_CHINA
+	{{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,149,153,157,161,165},24},												// 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO
+	{{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,149,153,157,161,165},31},						// 0x0E, RT_CHANNEL_DOMAIN_KOREA
+	{{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64},19},																	// 0x0F, RT_CHANNEL_DOMAIN_TURKEY
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},32},						// 0x10, RT_CHANNEL_DOMAIN_JAPAN
+	{{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,149,153,157,161,165},20},															// 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48},17},																		// 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140,149,153,157,161,165},37},	// 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G
+	{{1,2,3,4,5,6,7,8,9,10,11,56,60,64,149,153,157,161,165},19},																// 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS
+};*/
+
+static RT_CHANNEL_PLAN_2G	RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13},13},		// 0x00, RT_CHANNEL_DOMAIN_2G_WORLD , Passive scan CH 12, 13
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13},13},		// 0x01, RT_CHANNEL_DOMAIN_2G_ETSI1
+	{{1,2,3,4,5,6,7,8,9,10,11},11},			// 0x02, RT_CHANNEL_DOMAIN_2G_FCC1
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},	// 0x03, RT_CHANNEL_DOMAIN_2G_MIKK1
+	{{10,11,12,13},4},					// 0x04, RT_CHANNEL_DOMAIN_2G_ETSI2
+	{{},0},									// 0x05, RT_CHANNEL_DOMAIN_2G_NULL
+};
+
+static RT_CHANNEL_PLAN_5G	RTW_ChannelPlan5G[RT_CHANNEL_DOMAIN_5G_MAX] = {
+	{{},0},																					// 0x00, RT_CHANNEL_DOMAIN_5G_NULL
+	{{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},19},						// 0x01, RT_CHANNEL_DOMAIN_5G_ETSI1
+	{{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140,149,153,157,161,165},24},	// 0x02, RT_CHANNEL_DOMAIN_5G_ETSI2
+	{{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,149,153,157,161,165},22},			// 0x03, RT_CHANNEL_DOMAIN_5G_ETSI3
+	{{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140,149,153,157,161,165},24},	// 0x04, RT_CHANNEL_DOMAIN_5G_FCC1
+	{{36,40,44,48,149,153,157,161,165},9},														// 0x05, RT_CHANNEL_DOMAIN_5G_FCC2
+	{{36,40,44,48,52,56,60,64,149,153,157,161,165},13},											// 0x06, RT_CHANNEL_DOMAIN_5G_FCC3
+	{{36,40,44,48,52,56,60,64,149,153,157,161},12},												// 0x07, RT_CHANNEL_DOMAIN_5G_FCC4
+	{{149,153,157,161,165},5},																	// 0x08, RT_CHANNEL_DOMAIN_5G_FCC5
+	{{36,40,44,48,52,56,60,64},8},																// 0x09, RT_CHANNEL_DOMAIN_5G_FCC6
+	{{36,40,44,48,52,56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},20},					// 0x0A, RT_CHANNEL_DOMAIN_5G_FCC7_IC1
+	{{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,149,153,157,161,165},20},					// 0x0B, RT_CHANNEL_DOMAIN_5G_KCC1
+	{{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},19},						// 0x0C, RT_CHANNEL_DOMAIN_5G_MKK1
+	{{36,40,44,48,52,56,60,64},8},																// 0x0D, RT_CHANNEL_DOMAIN_5G_MKK2
+	{{100,104,108,112,116,120,124,128,132,136,140},11},											// 0x0E, RT_CHANNEL_DOMAIN_5G_MKK3
+	{{56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},15},								// 0x0F, RT_CHANNEL_DOMAIN_5G_NCC1
+	{{56,60,64,149,153,157,161,165},8},															// 0x10, RT_CHANNEL_DOMAIN_5G_NCC2
+
+	//===== Driver self defined for old channel plan Compatible ,Remember to modify if have new channel plan definition =====
+	{{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},21},				// 0x11, RT_CHANNEL_DOMAIN_5G_FCC
+	{{36,40,44,48},4},																			// 0x12, RT_CHANNEL_DOMAIN_5G_JAPAN_NO_DFS
+	{{36,40,44,48,149,153,157,161},8},																// 0x13, RT_CHANNEL_DOMAIN_5G_FCC4_NO_DFS
+};
+
+static RT_CHANNEL_PLAN_MAP	RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
+	//===== 0x00 ~ 0x1F , Old Define =====
+	{0x02,0x11},	//0x00, RT_CHANNEL_DOMAIN_FCC
+	{0x02,0x0A},	//0x01, RT_CHANNEL_DOMAIN_IC
+	{0x01,0x01},	//0x02, RT_CHANNEL_DOMAIN_ETSI
+	{0x01,0x00},	//0x03, RT_CHANNEL_DOMAIN_SPAIN
+	{0x01,0x00},	//0x04, RT_CHANNEL_DOMAIN_FRANCE
+	{0x03,0x00},	//0x05, RT_CHANNEL_DOMAIN_MKK
+	{0x03,0x00},	//0x06, RT_CHANNEL_DOMAIN_MKK1
+	{0x01,0x09},	//0x07, RT_CHANNEL_DOMAIN_ISRAEL
+	{0x03,0x09},	//0x08, RT_CHANNEL_DOMAIN_TELEC
+	{0x03,0x00},	//0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN
+	{0x00,0x00},	//0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13
+	{0x02,0x0F},	//0x0B, RT_CHANNEL_DOMAIN_TAIWAN
+	{0x01,0x08},	//0x0C, RT_CHANNEL_DOMAIN_CHINA
+	{0x02,0x06},	//0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO
+	{0x02,0x0B},	//0x0E, RT_CHANNEL_DOMAIN_KOREA
+	{0x02,0x09},	//0x0F, RT_CHANNEL_DOMAIN_TURKEY
+	{0x01,0x01},	//0x10, RT_CHANNEL_DOMAIN_JAPAN
+	{0x02,0x05},	//0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS
+	{0x01,0x12},	//0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS
+	{0x00,0x04},	//0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G
+	{0x02,0x10},	//0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS
+	{0x00,0x12},	//0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS
+	{0x00,0x13},	//0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS
+	{0x03,0x12},	//0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS
+	{0x05,0x08},	//0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS
+	{0x02,0x08},	//0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS
+	{0x00,0x00},	//0x1A,
+	{0x00,0x00},	//0x1B,
+	{0x00,0x00},	//0x1C,
+	{0x00,0x00},	//0x1D,
+	{0x00,0x00},	//0x1E,
+	{0x05,0x04},	//0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G
+	//===== 0x20 ~ 0x7F ,New Define =====
+	{0x00,0x00},	//0x20, RT_CHANNEL_DOMAIN_WORLD_NULL
+	{0x01,0x00},	//0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL
+	{0x02,0x00},	//0x22, RT_CHANNEL_DOMAIN_FCC1_NULL
+	{0x03,0x00},	//0x23, RT_CHANNEL_DOMAIN_MKK1_NULL
+	{0x04,0x00},	//0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL
+	{0x02,0x04},	//0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1
+	{0x00,0x01},	//0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1
+	{0x03,0x0C},	//0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1
+	{0x00,0x0B},	//0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1
+	{0x00,0x05},	//0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2
+	{0x00,0x00},	//0x2A,
+	{0x00,0x00},	//0x2B,
+	{0x00,0x00},	//0x2C,
+	{0x00,0x00},	//0x2D,
+	{0x00,0x00},	//0x2E,
+	{0x00,0x00},	//0x2F,
+	{0x00,0x06},	//0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3
+	{0x00,0x07},	//0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4
+	{0x00,0x08},	//0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5
+	{0x00,0x09},	//0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6
+	{0x02,0x0A},	//0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7
+	{0x00,0x02},	//0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2
+	{0x00,0x03},	//0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3
+	{0x03,0x0D},	//0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2
+	{0x03,0x0E},	//0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3
+	{0x02,0x0F},	//0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1
+	{0x00,0x00},	//0x3A,
+	{0x00,0x00},	//0x3B,
+	{0x00,0x00},	//0x3C,
+	{0x00,0x00},	//0x3D,
+	{0x00,0x00},	//0x3E,
+	{0x00,0x00},	//0x3F,
+	{0x02,0x10},	//0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2
+	{0x03,0x00},	//0x41, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G
+};
+
+static RT_CHANNEL_PLAN_MAP	RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03,0x02}; //use the conbination for max channel numbers
+
+/*
+ * Search the @param channel_num in given @param channel_set
+ * @ch_set: the given channel set
+ * @ch: the given channel number
+ * 
+ * return the index of channel_num in channel_set, -1 if not found
+ */
+int rtw_ch_set_search_ch(RT_CHANNEL_INFO *ch_set, const u32 ch)
+{
+	int i;
+	for(i=0;ch_set[i].ChannelNum!=0;i++){
+		if(ch == ch_set[i].ChannelNum)
+			break;
+	}
+	
+	if(i >= ch_set[i].ChannelNum)
+		return -1;
+	return i;
+}
+
+/****************************************************************************
+
+Following are the initialization functions for WiFi MLME
+
+*****************************************************************************/
+
+int init_hw_mlme_ext(_adapter *padapter)
+{
+	struct	mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	//set_opmode_cmd(padapter, infra_client_with_mlme);//removed
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	return _SUCCESS;
+}
+
+static void init_mlme_ext_priv_value(_adapter* padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+#ifdef CONFIG_TDLS
+	u8 i;
+#endif
+
+	//unsigned char default_channel_set[MAX_CHANNEL_NUM] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 0, 0};
+	unsigned char	mixed_datarate[NumRates] = {_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,_9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_, _48M_RATE_, _54M_RATE_, 0xff};
+	unsigned char	mixed_basicrate[NumRates] ={_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_, _12M_RATE_, _24M_RATE_, 0xff,};
+
+	ATOMIC_SET(&pmlmeext->event_seq, 0);
+	pmlmeext->mgnt_seq = 0;//reset to zero when disconnect at client mode
+
+	pmlmeext->cur_channel = padapter->registrypriv.channel;
+	pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	pmlmeext->retry = 0;
+
+	pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
+
+	//_rtw_memcpy(pmlmeext->channel_set, DefaultChannelPlan[padapter->mlmepriv.ChannelPlan].Channel, DefaultChannelPlan[padapter->mlmepriv.ChannelPlan].Len);
+	//_rtw_memcpy(pmlmeext->channel_set, default_channel_set, MAX_CHANNEL_NUM);
+	_rtw_memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
+	_rtw_memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
+
+	if(pmlmeext->cur_channel > 14)
+		pmlmeext->tx_rate = IEEE80211_OFDM_RATE_6MB;
+	else
+		pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
+
+	pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+	pmlmeext->sitesurvey_res.channel_idx = 0;
+	pmlmeext->sitesurvey_res.bss_cnt = 0;
+	pmlmeext->scan_abort = _FALSE;
+
+	pmlmeinfo->state = WIFI_FW_NULL_STATE;
+	pmlmeinfo->reauth_count = 0;
+	pmlmeinfo->reassoc_count = 0;
+	pmlmeinfo->link_count = 0;
+	pmlmeinfo->auth_seq = 0;
+	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+	pmlmeinfo->key_index = 0;
+	pmlmeinfo->iv = 0;
+
+	pmlmeinfo->enc_algo = _NO_PRIVACY_;
+	pmlmeinfo->authModeToggle = 0;
+
+	_rtw_memset(pmlmeinfo->chg_txt, 0, 128);
+
+	pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+	pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
+
+	pmlmeinfo->dialogToken = 0;
+
+	pmlmeext->action_public_rxseq = 0xffff;
+	pmlmeext->action_public_dialog_token = 0xff;
+}
+
+static int has_channel(RT_CHANNEL_INFO *channel_set,
+					   u8 chanset_size,
+					   u8 chan) {
+	int i;
+
+	for (i = 0; i < chanset_size; i++) {
+		if (channel_set[i].ChannelNum == chan) {
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+static void init_channel_list(_adapter *padapter, RT_CHANNEL_INFO *channel_set,
+							  u8 chanset_size,
+							  struct p2p_channels *channel_list) {
+
+	struct p2p_oper_class_map op_class[] = {
+		{ IEEE80211G,  81,   1,  13,  1, BW20 },
+		{ IEEE80211G,  82,  14,  14,  1, BW20 },
+#if 0 /* Do not enable HT40 on 2 GHz */
+		{ IEEE80211G,  83,   1,   9,  1, BW40PLUS },
+		{ IEEE80211G,  84,   5,  13,  1, BW40MINUS },
+#endif
+		{ IEEE80211A, 115,  36,  48,  4, BW20 },
+		{ IEEE80211A, 116,  36,  44,  8, BW40PLUS },
+		{ IEEE80211A, 117,  40,  48,  8, BW40MINUS },
+		{ IEEE80211A, 124, 149, 161,  4, BW20 },
+		{ IEEE80211A, 125, 149, 169,  4, BW20 },
+		{ IEEE80211A, 126, 149, 157,  8, BW40PLUS },
+		{ IEEE80211A, 127, 153, 161,  8, BW40MINUS },
+		{ -1, 0, 0, 0, 0, BW20 }
+	};
+
+	int cla, op;
+
+	cla = 0;
+
+	for (op = 0; op_class[op].op_class; op++) {
+		u8 ch;
+		struct p2p_oper_class_map *o = &op_class[op];
+		struct p2p_reg_class *reg = NULL;
+
+		for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
+			if (!has_channel(channel_set, chanset_size, ch)) {
+				continue;
+			}
+
+			if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
+				continue;
+
+			if ((0 == (padapter->registrypriv.cbw40_enable & BIT(1))) &&
+				((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
+				continue;
+
+			if (reg == NULL) {
+				reg = &channel_list->reg_class[cla];
+				cla++;
+				reg->reg_class = o->op_class;
+				reg->channels = 0;
+			}
+			reg->channel[reg->channels] = ch;
+			reg->channels++;
+		}
+	}
+	channel_list->reg_classes = cla;
+
+}
+
+static u8 init_channel_set(_adapter* padapter, u8 ChannelPlan, RT_CHANNEL_INFO *channel_set)
+{
+	u8	index,chanset_size = 0;
+	u8	b5GBand = _FALSE, b2_4GBand = _FALSE;
+	u8	Index2G = 0, Index5G=0;
+
+	_rtw_memset(channel_set, 0, sizeof(RT_CHANNEL_INFO)*MAX_CHANNEL_NUM);
+
+	if(ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE)
+	{
+		DBG_871X("ChannelPlan ID %x error !!!!!\n",ChannelPlan);
+		return chanset_size;
+	}
+
+	if(padapter->registrypriv.wireless_mode & WIRELESS_11G)
+	{
+		b2_4GBand = _TRUE;
+		if(RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
+			Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
+		else
+			Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
+	}
+
+	if(padapter->registrypriv.wireless_mode & WIRELESS_11A)
+	{
+		b5GBand = _TRUE;
+		if(RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
+			Index5G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index5G;
+		else
+			Index5G = RTW_ChannelPlanMap[ChannelPlan].Index5G;
+	}
+
+	if(b2_4GBand)
+	{
+		for(index=0;index<RTW_ChannelPlan2G[Index2G].Len;index++)
+		{
+			channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
+
+			if(	(RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||//Channel 1~11 is active, and 12~14 is passive
+				(RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == ChannelPlan)	)
+			{
+				if(channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else if((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
+					channel_set[chanset_size].ScanType  = SCAN_PASSIVE;			
+			}
+			else if(RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
+				RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan ||
+				RT_CHANNEL_DOMAIN_2G_WORLD == Index2G)// channel 12~13, passive scan
+			{
+				if(channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+			}
+			else
+			{
+				channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+			}
+
+			chanset_size++;
+		}
+	}
+
+	if(b5GBand)
+	{
+		for(index=0;index<RTW_ChannelPlan5G[Index5G].Len;index++)
+		{
+#ifdef CONFIG_DFS
+			channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
+			if ( channel_set[chanset_size].ChannelNum <= 48 
+				|| channel_set[chanset_size].ChannelNum >= 149 )
+			{
+				if(RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)//passive scan for all 5G channels
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+			}
+			else
+			{
+				channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+			}
+			chanset_size++;
+#else /* CONFIG_DFS */
+			if ( RTW_ChannelPlan5G[Index5G].Channel[index] <= 48 
+				|| RTW_ChannelPlan5G[Index5G].Channel[index] >= 149 ) {
+				channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
+				if(RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)//passive scan for all 5G channels
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				DBG_871X("%s(): channel_set[%d].ChannelNum = %d\n", __FUNCTION__, chanset_size, channel_set[chanset_size].ChannelNum);
+				chanset_size++;
+			}
+#endif /* CONFIG_DFS */
+		}
+	}
+
+	return chanset_size;
+}
+
+int	init_mlme_ext_priv(_adapter* padapter)
+{
+	int	res = _SUCCESS;
+	struct registry_priv* pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	// We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc().
+	//_rtw_memset((u8 *)pmlmeext, 0, sizeof(struct mlme_ext_priv));
+
+	pmlmeext->padapter = padapter;
+
+	//fill_fwpriv(padapter, &(pmlmeext->fwpriv));
+
+	init_mlme_ext_priv_value(padapter);
+	pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
+	
+	init_mlme_ext_timer(padapter);
+
+#ifdef CONFIG_AP_MODE
+	init_mlme_ap_info(padapter);	
+#endif
+
+	pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan,pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+	
+	pmlmeext->chan_scan_time = SURVEY_TO;
+	pmlmeext->mlmeext_init = _TRUE;
+
+
+#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK	
+	pmlmeext->active_keep_alive_check = _TRUE;
+#endif
+
+#ifdef DBG_FIXED_CHAN		
+	pmlmeext->fixed_chan = 0xFF;	
+#endif
+
+	return res;
+
+}
+
+void free_mlme_ext_priv (struct mlme_ext_priv *pmlmeext)
+{
+	_adapter *padapter = pmlmeext->padapter;
+
+	if (!padapter)
+		return;
+
+	if (padapter->bDriverStopped == _TRUE)
+	{
+		_cancel_timer_ex(&pmlmeext->survey_timer);
+		_cancel_timer_ex(&pmlmeext->link_timer);
+		//_cancel_timer_ex(&pmlmeext->ADDBA_timer);
+	}
+}
+
+static u8 cmp_pkt_chnl_diff(_adapter *padapter,u8* pframe,uint packet_len)
+{	// if the channel is same, return 0. else return channel differential	
+	uint len;
+	u8 channel;	
+	u8 *p;		
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_, _DSSET_IE_, &len, packet_len - _BEACON_IE_OFFSET_);	
+	if (p)	
+	{	
+		channel = *(p + 2);		
+		if(padapter->mlmeextpriv.cur_channel >= channel)		
+		{			
+			return (padapter->mlmeextpriv.cur_channel - channel);		
+		}		
+		else		
+		{			
+			return (channel-padapter->mlmeextpriv.cur_channel);		
+		}	
+	}	
+	else
+	{		
+		return 0;	
+	}
+}
+
+static void _mgt_dispatcher(_adapter *padapter, struct mlme_handler *ptable, union recv_frame *precv_frame)
+{
+	u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
+	u8 *pframe = precv_frame->u.hdr.rx_data; 
+
+	  if(ptable->func)
+        {
+       	 //receive the frames that ra(a1) is my address or ra(a1) is bc address.
+		if (!_rtw_memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
+			!_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN)) 
+		{
+			return;
+		}
+		
+		ptable->func(padapter, precv_frame);
+        }
+	
+}
+
+void mgt_dispatcher(_adapter *padapter, union recv_frame *precv_frame)
+{
+	int index;
+	struct mlme_handler *ptable;
+#ifdef CONFIG_AP_MODE
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+#endif //CONFIG_AP_MODE
+	u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+		 ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
+		  GetFrameType(pframe), GetFrameSubType(pframe)));
+
+#if 0
+	{
+		u8 *pbuf;
+		pbuf = GetAddr1Ptr(pframe);
+		DBG_871X("A1-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
+		pbuf = GetAddr2Ptr(pframe);
+		DBG_871X("A2-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
+		pbuf = GetAddr3Ptr(pframe);
+		DBG_871X("A3-%x:%x:%x:%x:%x:%x\n", *pbuf, *(pbuf+1), *(pbuf+2), *(pbuf+3), *(pbuf+4), *(pbuf+5));
+	}
+#endif
+
+	if (GetFrameType(pframe) != WIFI_MGT_TYPE)
+	{
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
+		return;
+	}
+
+	//receive the frames that ra(a1) is my address or ra(a1) is bc address.
+	if (!_rtw_memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
+		!_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
+	{
+		return;
+	}
+
+	ptable = mlme_sta_tbl;
+
+	index = GetFrameSubType(pframe) >> 4;
+
+#ifdef CONFIG_TDLS
+	if((index << 4)==WIFI_ACTION){
+		//category==public (4), action==TDLS_DISCOVERY_RESPONSE
+		if(*(pframe+24)==0x04 && *(pframe+25)==TDLS_DISCOVERY_RESPONSE){
+			DBG_871X("recv tdls discovery response frame\n");
+			On_TDLS_Dis_Rsp(padapter, precv_frame);
+		}
+	}
+#endif //CONFIG_TDLS
+
+	if (index > 13)
+	{
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Currently we do not support reserved sub-fr-type=%d\n", index));
+		return;
+	}
+	ptable += index;
+
+#if 1
+	if (psta != NULL)
+	{
+		if (GetRetry(pframe))
+		{
+			if (precv_frame->u.hdr.attrib.seq_num == psta->RxMgmtFrameSeqNum)
+			{
+				/* drop the duplicate management frame */
+				DBG_871X("Drop duplicate management frame with seq_num = %d.\n", precv_frame->u.hdr.attrib.seq_num);
+				return;
+			}
+		}
+		psta->RxMgmtFrameSeqNum = precv_frame->u.hdr.attrib.seq_num;
+	}
+#else
+
+	if(GetRetry(pframe))
+	{
+		//RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("drop due to decache!\n"));
+		//return;
+	}
+#endif
+
+#ifdef CONFIG_AP_MODE
+	switch (GetFrameSubType(pframe)) 
+	{
+		case WIFI_AUTH:
+			if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+				ptable->func = &OnAuth;
+			else
+				ptable->func = &OnAuthClient;
+			//pass through
+		case WIFI_ASSOCREQ:
+		case WIFI_REASSOCREQ:
+			_mgt_dispatcher(padapter, ptable, precv_frame);	
+#ifdef CONFIG_HOSTAPD_MLME				
+			if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+				rtw_hostapd_mlme_rx(padapter, precv_frame);
+#endif			
+			break;
+		case WIFI_PROBEREQ:
+			if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+			{
+#ifdef CONFIG_HOSTAPD_MLME		
+				rtw_hostapd_mlme_rx(padapter, precv_frame);		
+#else
+				_mgt_dispatcher(padapter, ptable, precv_frame);
+#endif
+			}
+			else
+				_mgt_dispatcher(padapter, ptable, precv_frame);
+			break;
+		case WIFI_BEACON:			
+			_mgt_dispatcher(padapter, ptable, precv_frame);
+			break;
+		case WIFI_ACTION:
+			//if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+			_mgt_dispatcher(padapter, ptable, precv_frame);		
+			break;
+		default:
+			_mgt_dispatcher(padapter, ptable, precv_frame);	
+			if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+				rtw_hostapd_mlme_rx(padapter, precv_frame);			
+			break;
+	}
+#else
+
+	_mgt_dispatcher(padapter, ptable, precv_frame);	
+	
+#endif
+
+}
+
+#ifdef CONFIG_P2P
+u32 p2p_listen_state_process(_adapter *padapter, unsigned char *da)
+{
+	bool response = _TRUE;
+	
+#ifdef CONFIG_IOCTL_CFG80211
+	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled == _FALSE
+		|| padapter->mlmepriv.wps_probe_resp_ie == NULL
+		|| padapter->mlmepriv.p2p_probe_resp_ie == NULL
+	)
+	{
+		DBG_871X("DON'T issue_probersp_p2p: p2p_enabled:%d, wps_probe_resp_ie:%p, p2p_probe_resp_ie:%p\n",
+			wdev_to_priv(padapter->rtw_wdev)->p2p_enabled,
+			padapter->mlmepriv.wps_probe_resp_ie,
+			padapter->mlmepriv.p2p_probe_resp_ie);
+		response = _FALSE;
+	}
+#else
+
+	//	do nothing if the device name is empty
+	if ( !padapter->wdinfo.device_name_len )
+	{
+		response	= _FALSE;
+	}
+#endif
+
+	if (response == _TRUE)
+		issue_probersp_p2p( padapter, da);
+	
+	return _SUCCESS;
+}
+#endif //CONFIG_P2P
+
+
+/****************************************************************************
+
+Following are the callback functions for each subtype of the management frames
+
+*****************************************************************************/
+
+unsigned int OnProbeReq(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int	ielen;
+	unsigned char	*p;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX 	*cur = &(pmlmeinfo->network);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	u8 is_valid_p2p_probereq = _FALSE;
+
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	struct rx_pkt_attrib	*pattrib = &precv_frame->u.hdr.attrib;
+	u8 wifi_test_chk_rate = 1;
+	
+	if (	!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && 
+		!rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) && 
+		!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
+		!rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
+		!rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)
+	   )
+	{
+		//	Commented by Albert 2011/03/17
+		//	mcs_rate = 0 -> CCK 1M rate
+		//	mcs_rate = 1 -> CCK 2M rate
+		//	mcs_rate = 2 -> CCK 5.5M rate
+		//	mcs_rate = 3 -> CCK 11M rate
+		//	In the P2P mode, the driver should not support the CCK rate
+
+		//	Commented by Kurt 2012/10/16
+		//	IOT issue: Google Nexus7 use 1M rate to send p2p_probe_req after GO nego completed and Nexus7 is client
+#ifdef CONFIG_WIFI_TEST
+		if ( pattrib->mcs_rate <= 3 )
+		{
+			wifi_test_chk_rate = 0;
+		}
+#endif //CONFIG_WIFI_TEST
+
+		if( wifi_test_chk_rate == 1 )
+		{
+			if((is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len)) == _TRUE)
+			{
+				if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE))
+				{
+#ifndef CONFIG_IOCTL_CFG80211
+// FIXME
+					report_survey_event(padapter, precv_frame);
+#endif					
+					p2p_listen_state_process( padapter,  get_sa(pframe));
+
+					return _SUCCESS;	
+				}
+
+				if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+				{
+					goto _continue;
+				}
+			}
+		}
+	}
+
+_continue:
+#endif //CONFIG_P2P
+
+	if(check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+	{
+		return _SUCCESS;
+	}
+
+	if(check_fwstate(pmlmepriv, _FW_LINKED) == _FALSE && 
+		check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE)==_FALSE)
+	{
+		return _SUCCESS;
+	}
+
+
+	//DBG_871X("+OnProbeReq\n");
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
+		check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
+	{
+		//don't process probe req
+		return _SUCCESS;
+	}
+#endif		
+
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
+			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+
+	//check (wildcard) SSID 
+	if (p != NULL)
+	{
+		if(is_valid_p2p_probereq == _TRUE)
+		{
+			goto _issue_probersp;
+		}
+
+		if ( (ielen != 0 && _FALSE ==_rtw_memcmp((void *)(p+2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength))
+			|| (ielen == 0 && pmlmeinfo->hidden_ssid_mode)
+		)
+		{
+			return _SUCCESS;
+		}
+
+_issue_probersp:
+
+		if(check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE && 
+			pmlmepriv->cur_network.join_res == _TRUE)
+		{
+			//DBG_871X("+issue_probersp during ap mode\n");
+			issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);		
+		}
+
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnProbeRsp(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct sta_info		*psta;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	u8	*pframe = precv_frame->u.hdr.rx_data;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+#endif
+
+
+#ifdef CONFIG_P2P
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
+	{
+		if ( _TRUE == pwdinfo->tx_prov_disc_info.benable )
+		{
+			if( _rtw_memcmp( pwdinfo->tx_prov_disc_info.peerIFAddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
+			{
+				if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT))
+				{
+					pwdinfo->tx_prov_disc_info.benable = _FALSE;
+					issue_p2p_provision_request( padapter,
+												pwdinfo->tx_prov_disc_info.ssid.Ssid, 
+												pwdinfo->tx_prov_disc_info.ssid.SsidLength,
+												pwdinfo->tx_prov_disc_info.peerDevAddr );
+				}
+				else if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
+				{
+					pwdinfo->tx_prov_disc_info.benable = _FALSE;
+					issue_p2p_provision_request( padapter,
+												NULL, 
+												0,
+												pwdinfo->tx_prov_disc_info.peerDevAddr );
+				}
+			}		
+		}
+		return _SUCCESS;
+	}
+	else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
+	{
+		if ( _TRUE == pwdinfo->nego_req_info.benable )
+		{
+			DBG_871X( "[%s] P2P State is GONEGO ING!\n", __FUNCTION__ );
+			if( _rtw_memcmp( pwdinfo->nego_req_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
+			{
+				pwdinfo->nego_req_info.benable = _FALSE;
+				issue_p2p_GO_request( padapter, pwdinfo->nego_req_info.peerDevAddr);
+			}
+		}
+	}
+	else if( rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ ) )
+	{
+		if ( _TRUE == pwdinfo->invitereq_info.benable )
+		{
+			DBG_871X( "[%s] P2P_STATE_TX_INVITE_REQ!\n", __FUNCTION__ );
+			if( _rtw_memcmp( pwdinfo->invitereq_info.peer_macaddr, GetAddr2Ptr(pframe), ETH_ALEN ) )
+			{
+				pwdinfo->invitereq_info.benable = _FALSE;
+				issue_p2p_invitation_request( padapter, pwdinfo->invitereq_info.peer_macaddr );
+			}
+		}
+	}
+#endif
+
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
+	{
+		report_survey_event(padapter, precv_frame);	
+#ifdef CONFIG_CONCURRENT_MODE
+		report_survey_event(padapter->pbuddy_adapter, precv_frame);	
+#endif
+#ifdef CONFIG_DUALMAC_CONCURRENT
+		dc_report_survey_event(padapter, precv_frame);
+#endif
+		return _SUCCESS;
+	}
+
+	#if 0 //move to validate_recv_mgnt_frame
+	if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
+	{
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+		{
+			if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
+			{
+				psta->sta_stats.rx_mgnt_pkts++;
+			}
+		}
+	}
+	#endif
+	
+	return _SUCCESS;
+	
+}
+
+unsigned int OnBeacon(_adapter *padapter, union recv_frame *precv_frame)
+{
+	int cam_idx;
+	struct sta_info	*psta;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	WLAN_BSSID_EX *pbss;
+	int ret = _SUCCESS;
+	u8 *p = NULL;
+	u32 ielen = 0;
+
+#ifdef CONFIG_ATTEMPT_TO_FIX_AP_BEACON_ERROR
+	p = rtw_get_ie(pframe + sizeof(struct rtw_ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ielen, precv_frame->u.hdr.len -sizeof(struct rtw_ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_);
+	if ((p != NULL) && (ielen > 0))
+	{
+		if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D))
+		{
+			/* Invalid value 0x2D is detected in Extended Supported Rates (ESR) IE. Try to fix the IE length to avoid failed Beacon parsing. */	
+		       	DBG_871X("[WIFIDBG] Error in ESR IE is detected in Beacon of BSSID:"MAC_FMT". Fix the length of ESR IE to avoid failed Beacon parsing.\n", MAC_ARG(GetAddr3Ptr(pframe)));
+		       	*(p + 1) = ielen - 1;
+		}
+	}
+#endif
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
+	{
+		report_survey_event(padapter, precv_frame);
+#ifdef CONFIG_CONCURRENT_MODE
+		report_survey_event(padapter->pbuddy_adapter, precv_frame);	
+#endif
+
+#ifdef CONFIG_DUALMAC_CONCURRENT
+		dc_report_survey_event(padapter, precv_frame);
+#endif
+
+		return _SUCCESS;
+	}
+
+	if (_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
+	{
+		if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
+		{
+			//we should update current network before auth, or some IE is wrong
+			pbss = (WLAN_BSSID_EX*)rtw_malloc(sizeof(WLAN_BSSID_EX));
+			if (pbss) {
+				if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
+					update_network(&(pmlmepriv->cur_network.network), pbss, padapter, _TRUE);
+					rtw_get_bcn_info(&(pmlmepriv->cur_network));
+				}
+				rtw_mfree((u8*)pbss, sizeof(WLAN_BSSID_EX));
+			}
+
+			//check the vendor of the assoc AP
+			pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct rtw_ieee80211_hdr_3addr), len-sizeof(struct rtw_ieee80211_hdr_3addr));				
+
+			//update TSF Value
+			update_TSF(pmlmeext, pframe, len);
+
+			//start auth
+			start_clnt_auth(padapter);
+
+			return _SUCCESS;
+		}
+
+		if(((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+		{
+			if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
+			{
+				#ifdef CONFIG_PATCH_JOIN_WRONG_CHANNEL        
+				//Merge from 8712 FW code
+				if (cmp_pkt_chnl_diff(padapter,pframe,len) != 0)        
+				{            // join wrong channel, deauth and reconnect           
+					issue_deauth(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
+
+					report_del_sta_event(padapter,(&(pmlmeinfo->network))->MacAddress, WLAN_REASON_JOIN_WRONG_CHANNEL);    		
+					pmlmeinfo->state &= (~WIFI_FW_ASSOC_SUCCESS);    		
+					return _SUCCESS;
+				}        
+				#endif //CONFIG_PATCH_JOIN_WRONG_CHANNEL
+
+				ret = rtw_check_bcn_info(padapter, pframe, len);
+				if (!ret) {
+						DBG_871X_LEVEL(_drv_always_, "ap has changed, disconnect now\n ");
+						receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 65535);
+						return _SUCCESS;
+				}
+				//update WMM, ERP in the beacon
+				//todo: the timer is used instead of the number of the beacon received
+				if ((sta_rx_pkts(psta) & 0xf) == 0)
+				{
+					//DBG_871X("update_bcn_info\n");
+					update_beacon_info(padapter, pframe, len, psta);
+				}
+				
+#ifdef CONFIG_DFS
+				process_csa_ie(padapter, pframe, len);	//channel switch announcement
+#endif //CONFIG_DFS
+
+#ifdef CONFIG_P2P_PS
+				process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
+#endif //CONFIG_P2P_PS
+
+				#if 0 //move to validate_recv_mgnt_frame
+				psta->sta_stats.rx_mgnt_pkts++;
+				#endif
+			}
+		}
+		else if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
+		{
+			if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
+			{
+				//update WMM, ERP in the beacon
+				//todo: the timer is used instead of the number of the beacon received
+				if ((sta_rx_pkts(psta) & 0xf) == 0)
+				{
+					//DBG_871X("update_bcn_info\n");
+					update_beacon_info(padapter, pframe, len, psta);
+				}
+
+				#if 0 //move to validate_recv_mgnt_frame
+				psta->sta_stats.rx_mgnt_pkts++;
+				#endif
+			}
+			else
+			{
+				//allocate a new CAM entry for IBSS station
+				if ((cam_idx = allocate_fw_sta_entry(padapter)) == NUM_STA)
+				{
+					goto _END_ONBEACON_;
+				}
+
+				//get supported rate
+				if (update_sta_support_rate(padapter, (pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_), (len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_), cam_idx) == _FAIL)
+				{
+					pmlmeinfo->FW_sta_info[cam_idx].status = 0;
+					goto _END_ONBEACON_;
+				}
+
+				//update TSF Value
+				update_TSF(pmlmeext, pframe, len);			
+
+				//report sta add event
+				report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
+			}
+		}
+	}
+
+_END_ONBEACON_:
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnAuth(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_AP_MODE
+	_irqL irqL;
+	unsigned int	auth_mode, seq, ie_len;	
+	unsigned char	*sa, *p;	
+	u16	algorithm;
+	int	status;
+	static struct sta_info stat;	
+	struct	sta_info	*pstat=NULL;	
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data; 
+	uint len = precv_frame->u.hdr.len;
+
+	
+#ifdef CONFIG_CONCURRENT_MODE	
+	if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
+		check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
+	{
+		//don't process auth request;
+		return _SUCCESS;
+	}
+#endif //CONFIG_CONCURRENT_MODE
+
+	if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	DBG_871X("+OnAuth\n");
+	
+	sa = GetAddr2Ptr(pframe);
+	
+	auth_mode = psecuritypriv->dot11AuthAlgrthm;
+	seq = cpu_to_le16(*(u16*)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + 2));
+	algorithm = cpu_to_le16(*(u16*)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN));
+
+	if (GetPrivacy(pframe))
+	{	
+#if 0 //TODO: SW rtw_wep_decrypt
+		if (SWCRYPTO)
+		{
+			status = rtw_wep_decrypt(priv, pframe, pfrinfo->pktlen,
+				priv->pmib->dot1180211AuthEntry.dot11PrivacyAlgrthm);
+			if (status == FALSE)
+			{
+				SAVE_INT_AND_CLI(flags);
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,"wep-decrypt a Auth frame error!\n");
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+		}
+
+		seq = cpu_to_le16(*(unsigned short *)((unsigned int)pframe + WLAN_HDR_A3_LEN + 4 + 2));
+		algorithm = cpu_to_le16(*(unsigned short *)((unsigned int)pframe + WLAN_HDR_A3_LEN + 4));
+#endif
+	}
+
+
+	DBG_871X("auth alg=%x, seq=%X\n", algorithm, seq);
+
+	if (auth_mode == 2 &&
+			psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
+			psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
+		auth_mode = 0;
+
+	if ((algorithm > 0 && auth_mode == 0) ||	// rx a shared-key auth but shared not enabled
+		(algorithm == 0 && auth_mode == 1) )	// rx a open-system auth but shared-key is enabled
+	{		
+		DBG_871X("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n",
+			algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
+		
+		status = _STATS_NO_SUPP_ALG_;
+		
+		goto auth_fail;
+	}
+	
+#if 0 //ACL control	
+	phead = &priv->wlan_acl_list;
+	plist = phead->next;
+	//check sa
+	if (acl_mode == 1)		// 1: positive check, only those on acl_list can be connected.
+		res = FAIL;
+	else
+		res = SUCCESS;
+
+	while(plist != phead)
+	{
+		paclnode = list_entry(plist, struct rtw_wlan_acl_node, list);
+		plist = plist->next;
+		if (!memcmp((void *)sa, paclnode->addr, 6)) {
+			if (paclnode->mode & 2) { // deny
+				res = FAIL;
+				break;
+			}
+			else {
+				res = SUCCESS;
+				break;
+			}
+		}
+	}
+
+	if (res != SUCCESS) {
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,"auth abort because ACL!\n");
+		return FAIL;
+	}
+#else
+	if(rtw_access_ctrl(padapter, sa) == _FALSE)
+	{
+		status = _STATS_UNABLE_HANDLE_STA_;
+		goto auth_fail;
+	}	
+#endif
+
+	pstat = rtw_get_stainfo(pstapriv, sa);
+	if (pstat == NULL)
+	{
+		// allocate a new one
+		DBG_871X("going to alloc stainfo for sa="MAC_FMT"\n",  MAC_ARG(sa));
+		pstat = rtw_alloc_stainfo(pstapriv, sa);
+		if (pstat == NULL)
+		{
+			DBG_871X(" Exceed the upper limit of supported clients...\n");
+			status = _STATS_UNABLE_HANDLE_STA_;
+			goto auth_fail;
+		}
+		
+		pstat->state = WIFI_FW_AUTH_NULL;
+		pstat->auth_seq = 0;
+		
+		//pstat->flags = 0;
+		//pstat->capability = 0;
+	}
+	else
+	{		
+		_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+		if(rtw_is_list_empty(&pstat->asoc_list)==_FALSE)
+		{			
+			rtw_list_delete(&pstat->asoc_list);
+			pstapriv->asoc_list_cnt--;
+			if (pstat->expire_to > 0)
+			{
+				//TODO: STA re_auth within expire_to
+			}
+		}
+		_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+		
+		if (seq==1) {
+			//TODO: STA re_auth and auth timeout 
+		}
+	}
+
+	_enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
+	if (rtw_is_list_empty(&pstat->auth_list))
+	{		
+		rtw_list_insert_tail(&pstat->auth_list, &pstapriv->auth_list);
+		pstapriv->auth_list_cnt++;
+	}	
+	_exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
+
+	if (pstat->auth_seq == 0)
+		pstat->expire_to = pstapriv->auth_to;
+
+	if ((pstat->auth_seq + 1) != seq)
+	{
+		DBG_871X("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+			seq, pstat->auth_seq+1);
+		status = _STATS_OUT_OF_AUTH_SEQ_;
+		goto auth_fail;
+	}
+
+	if (algorithm==0 && (auth_mode == 0 || auth_mode == 2))
+	{
+		if (seq == 1)
+		{
+			pstat->state &= ~WIFI_FW_AUTH_NULL;
+			pstat->state |= WIFI_FW_AUTH_SUCCESS;
+			pstat->expire_to = pstapriv->assoc_to;
+			pstat->authalg = algorithm;
+		}
+		else
+		{
+			DBG_871X("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+				seq, pstat->auth_seq+1);
+			status = _STATS_OUT_OF_AUTH_SEQ_;
+			goto auth_fail;
+		}
+	}
+	else // shared system or auto authentication
+	{
+		if (seq == 1)
+		{
+			//prepare for the challenging txt...
+			
+			//get_random_bytes((void *)pstat->chg_txt, 128);//TODO:
+			
+			pstat->state &= ~WIFI_FW_AUTH_NULL;
+			pstat->state |= WIFI_FW_AUTH_STATE;
+			pstat->authalg = algorithm;
+			pstat->auth_seq = 2;
+		}
+		else if (seq == 3)
+		{
+			//checking for challenging txt...
+			DBG_871X("checking for challenging txt...\n");
+			
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len,
+					len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
+
+			if((p==NULL) || (ie_len<=0))
+			{
+				DBG_871X("auth rejected because challenge failure!(1)\n");
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+			
+			if (_rtw_memcmp((void *)(p + 2), pstat->chg_txt, 128))
+			{
+				pstat->state &= (~WIFI_FW_AUTH_STATE);
+				pstat->state |= WIFI_FW_AUTH_SUCCESS;
+				// challenging txt is correct...
+				pstat->expire_to =  pstapriv->assoc_to;
+			}
+			else
+			{
+				DBG_871X("auth rejected because challenge failure!\n");
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+		}
+		else
+		{
+			DBG_871X("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+				seq, pstat->auth_seq+1);
+			status = _STATS_OUT_OF_AUTH_SEQ_;
+			goto auth_fail;
+		}
+	}
+
+
+	// Now, we are going to issue_auth...
+	pstat->auth_seq = seq + 1;	
+	
+#ifdef CONFIG_NATIVEAP_MLME
+	issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
+#endif
+
+	if (pstat->state & WIFI_FW_AUTH_SUCCESS)
+		pstat->auth_seq = 0;
+
+		
+	return _SUCCESS;
+
+auth_fail:
+
+	if(pstat)
+		rtw_free_stainfo(padapter , pstat);
+	
+	pstat = &stat;
+	_rtw_memset((char *)pstat, '\0', sizeof(stat));
+	pstat->auth_seq = 2;
+	_rtw_memcpy(pstat->hwaddr, sa, 6);	
+	
+#ifdef CONFIG_NATIVEAP_MLME
+	issue_auth(padapter, pstat, (unsigned short)status);	
+#endif
+
+#endif
+	return _FAIL;
+
+}
+
+unsigned int OnAuthClient(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int	seq, len, status, algthm, offset;
+	unsigned char	*p;
+	unsigned int	go2asoc = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+	//check A1 matches or not
+	if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
+		return _SUCCESS;
+
+	offset = (GetPrivacy(pframe))? 4: 0;
+
+	algthm 	= le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset));
+	seq 	= le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2));
+	status 	= le16_to_cpu(*(unsigned short *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 4));
+
+	if (status != 0)
+	{
+		DBG_871X("clnt auth fail, status: %d\n", status);
+		if(status == 13)//&& pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto)
+		{
+			if(pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+			else
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
+			//pmlmeinfo->reauth_count = 0;
+		}
+		
+		set_link_timer(pmlmeext, 1);
+		goto authclnt_fail;
+	}
+
+	if (seq == 2)
+	{
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+		{
+			 // legendary shared system
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
+				pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
+
+			if (p == NULL)
+			{
+				//DBG_871X("marc: no challenge text?\n");
+				goto authclnt_fail;
+			}
+
+			_rtw_memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
+			pmlmeinfo->auth_seq = 3;
+			issue_auth(padapter, NULL, 0);
+			set_link_timer(pmlmeext, REAUTH_TO);
+
+			return _SUCCESS;
+		}
+		else
+		{
+			// open system
+			go2asoc = 1;
+		}
+	}
+	else if (seq == 4)
+	{
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+		{
+			go2asoc = 1;
+		}
+		else
+		{
+			goto authclnt_fail;
+		}
+	}
+	else
+	{
+		// this is also illegal
+		//DBG_871X("marc: clnt auth failed due to illegal seq=%x\n", seq);
+		goto authclnt_fail;
+	}
+
+	if (go2asoc)
+	{
+		DBG_871X_LEVEL(_drv_always_, "auth success, start assoc\n");
+		start_clnt_assoc(padapter);
+		return _SUCCESS;
+	}
+
+authclnt_fail:
+
+	//pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE);
+
+	return _FAIL;
+
+}
+
+unsigned int OnAssocReq(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_AP_MODE
+	_irqL irqL;
+	u16 capab_info, listen_interval;
+	struct rtw_ieee802_11_elems elems;	
+	struct sta_info	*pstat;
+	unsigned char		reassoc, *p, *pos, *wpa_ie;
+	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+	int		i, ie_len, wpa_ie_len, left;
+	unsigned char		supportRate[16];
+	int					supportRateNum;
+	unsigned short		status = _STATS_SUCCESSFUL_;
+	unsigned short		frame_type, ie_offset=0;	
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);	
+	WLAN_BSSID_EX 	*cur = &(pmlmeinfo->network);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8 p2p_status_code = P2P_STATUS_SUCCESS;
+	u8 *p2pie;
+	u32 p2pielen = 0;
+#ifdef CONFIG_WFD
+	u8	wfd_ie[ 128 ] = { 0x00 };
+	u32	wfd_ielen = 0;
+#endif // CONFIG_WFD
+#endif //CONFIG_P2P
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
+		check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
+	{
+		//don't process assoc request;
+		return _SUCCESS;
+	}
+#endif //CONFIG_CONCURRENT_MODE
+
+	if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+	
+	frame_type = GetFrameSubType(pframe);
+	if (frame_type == WIFI_ASSOCREQ)
+	{
+		reassoc = 0;
+		ie_offset = _ASOCREQ_IE_OFFSET_;
+	}	
+	else // WIFI_REASSOCREQ
+	{
+		reassoc = 1;
+		ie_offset = _REASOCREQ_IE_OFFSET_;
+	}
+	
+
+	if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
+		DBG_871X("handle_assoc(reassoc=%d) - too short payload (len=%lu)"
+		       "\n", reassoc, (unsigned long)pkt_len);
+		return _FAIL;
+	}
+	
+	pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+	if (pstat == (struct sta_info *)NULL)
+	{
+		status = _RSON_CLS2_;
+		goto asoc_class2_error;
+	}
+
+	capab_info = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN);
+	//capab_info = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));	
+	//listen_interval = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN+2));
+	listen_interval = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN+2);
+
+	left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
+	pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
+	
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+	// check if this stat has been successfully authenticated/assocated
+	if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS))
+	{
+		if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS))
+		{
+			status = _RSON_CLS2_;
+			goto asoc_class2_error;
+		}
+		else
+		{
+			pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
+			pstat->state |= WIFI_FW_ASSOC_STATE;				
+		}
+	}
+	else
+	{
+		pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
+		pstat->state |= WIFI_FW_ASSOC_STATE;
+	}
+
+
+#if 0// todo:tkip_countermeasures
+	if (hapd->tkip_countermeasures) {
+		resp = WLAN_REASON_MICHAEL_MIC_FAILURE;
+		goto fail;
+	}
+#endif
+
+	pstat->capability = capab_info;
+
+#if 0//todo:
+	//check listen_interval
+	if (listen_interval > hapd->conf->max_listen_interval) {
+		hostapd_logger(hapd, mgmt->sa, HOSTAPD_MODULE_IEEE80211,
+			       HOSTAPD_LEVEL_DEBUG,
+			       "Too large Listen Interval (%d)",
+			       listen_interval);
+		resp = WLAN_STATUS_ASSOC_DENIED_LISTEN_INT_TOO_LARGE;
+		goto fail;
+	}
+	
+	pstat->listen_interval = listen_interval;
+#endif
+
+	//now parse all ieee802_11 ie to point to elems
+	if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
+	    !elems.ssid) {
+		DBG_871X("STA " MAC_FMT " sent invalid association request\n",
+		       MAC_ARG(pstat->hwaddr));
+		status = _STATS_FAILURE_;		
+		goto OnAssocReqFail;
+	}
+
+
+	// now we should check all the fields...
+	// checking SSID
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
+		pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+	if (p == NULL)
+	{
+		status = _STATS_FAILURE_;		
+	}
+
+	if (ie_len == 0) // broadcast ssid, however it is not allowed in assocreq
+		status = _STATS_FAILURE_;
+	else
+	{
+		// check if ssid match
+		if (!_rtw_memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
+			status = _STATS_FAILURE_;
+
+		if (ie_len != cur->Ssid.SsidLength)
+			status = _STATS_FAILURE_;
+	}
+
+	if(_STATS_SUCCESSFUL_ != status)
+		goto OnAssocReqFail;
+
+	// check if the supported rate is ok
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+	if (p == NULL) {
+		DBG_871X("Rx a sta assoc-req which supported rate is empty!\n");
+		// use our own rate set as statoin used
+		//_rtw_memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN);
+		//supportRateNum = AP_BSSRATE_LEN;
+		
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+	else {
+		_rtw_memcpy(supportRate, p+2, ie_len);
+		supportRateNum = ie_len;
+
+		p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_ , &ie_len,
+				pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+		if (p !=  NULL) {
+			
+			if(supportRateNum<=sizeof(supportRate))
+			{
+				_rtw_memcpy(supportRate+supportRateNum, p+2, ie_len);
+				supportRateNum += ie_len;
+			}			
+		}
+	}
+
+	//todo: mask supportRate between AP & STA -> move to update raid
+	//get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0);
+
+	//update station supportRate	
+	pstat->bssratelen = supportRateNum;
+	_rtw_memcpy(pstat->bssrateset, supportRate, supportRateNum);
+	UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
+
+	//check RSN/WPA/WPS
+	pstat->dot8021xalg = 0;
+      	pstat->wpa_psk = 0;
+	pstat->wpa_group_cipher = 0;
+	pstat->wpa2_group_cipher = 0;
+	pstat->wpa_pairwise_cipher = 0;
+	pstat->wpa2_pairwise_cipher = 0;
+	_rtw_memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
+	if((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
+
+		int group_cipher=0, pairwise_cipher=0;	
+		
+		wpa_ie = elems.rsn_ie;
+		wpa_ie_len = elems.rsn_ie_len;
+
+		if(rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS)
+		{
+			pstat->dot8021xalg = 1;//psk,  todo:802.1x						
+			pstat->wpa_psk |= BIT(1);
+
+			pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;				
+			pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
+			
+			if(!pstat->wpa2_group_cipher)
+				status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
+
+			if(!pstat->wpa2_pairwise_cipher)
+				status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
+		}
+		else
+		{
+			status = WLAN_STATUS_INVALID_IE;
+		}	
+			
+	} else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
+
+		int group_cipher=0, pairwise_cipher=0;	
+		
+		wpa_ie = elems.wpa_ie;
+		wpa_ie_len = elems.wpa_ie_len;
+
+		if(rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS)
+		{
+			pstat->dot8021xalg = 1;//psk,  todo:802.1x						
+			pstat->wpa_psk |= BIT(0);
+
+			pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;				
+			pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
+			
+			if(!pstat->wpa_group_cipher)
+				status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
+
+			if(!pstat->wpa_pairwise_cipher)
+				status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
+		
+		}
+		else
+		{
+			status = WLAN_STATUS_INVALID_IE;
+		}
+		
+	} else {
+		wpa_ie = NULL;
+		wpa_ie_len = 0;
+	}
+
+	if(_STATS_SUCCESSFUL_ != status)
+		goto OnAssocReqFail;
+
+	pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
+	//if (hapd->conf->wps_state && wpa_ie == NULL) { //todo: to check ap if supporting WPS
+	if(wpa_ie == NULL) {
+		if (elems.wps_ie) {
+			DBG_871X("STA included WPS IE in "
+				   "(Re)Association Request - assume WPS is "
+				   "used\n");
+			pstat->flags |= WLAN_STA_WPS;
+			//wpabuf_free(sta->wps_ie);
+			//sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4,
+			//				elems.wps_ie_len - 4);
+		} else {
+			DBG_871X("STA did not include WPA/RSN IE "
+				   "in (Re)Association Request - possible WPS "
+				   "use\n");
+			pstat->flags |= WLAN_STA_MAYBE_WPS;
+		}
+
+
+		// AP support WPA/RSN, and sta is going to do WPS, but AP is not ready
+		// that the selected registrar of AP is _FLASE
+		if((psecuritypriv->wpa_psk >0)  
+			&& (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS)))
+		{
+			if(pmlmepriv->wps_beacon_ie)
+			{	
+				u8 selected_registrar = 0;
+				
+				rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR , &selected_registrar, NULL);
+
+				if(!selected_registrar)
+				{						
+					DBG_871X("selected_registrar is _FALSE , or AP is not ready to do WPS\n");
+						
+					status = _STATS_UNABLE_HANDLE_STA_;
+			
+					goto OnAssocReqFail;
+				}						
+			}			
+		}
+			
+	}
+	else
+	{
+		int copy_len;
+
+		if(psecuritypriv->wpa_psk == 0)
+		{
+			DBG_871X("STA " MAC_FMT ": WPA/RSN IE in association "
+		       	"request, but AP don't support WPA/RSN\n", MAC_ARG(pstat->hwaddr));
+			
+			status = WLAN_STATUS_INVALID_IE;
+			
+			goto OnAssocReqFail;
+
+		}
+
+		if (elems.wps_ie) {
+			DBG_871X("STA included WPS IE in "
+				   "(Re)Association Request - WPS is "
+				   "used\n");
+			pstat->flags |= WLAN_STA_WPS;
+			copy_len=0;
+		}
+		else
+		{
+			copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)):(wpa_ie_len+2);
+		}
+
+		
+		if(copy_len>0)
+			_rtw_memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
+		
+	}
+
+
+	// check if there is WMM IE & support WWM-PS
+	pstat->flags &= ~WLAN_STA_WME;
+	pstat->qos_option = 0;
+	pstat->qos_info = 0;
+	pstat->has_legacy_ac = _TRUE;
+	pstat->uapsd_vo = 0;
+	pstat->uapsd_vi = 0;
+	pstat->uapsd_be = 0;
+	pstat->uapsd_bk = 0;
+	if (pmlmepriv->qospriv.qos_option) 
+	{
+		p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
+		for (;;) 
+		{
+			p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+			if (p != NULL) {
+				if (_rtw_memcmp(p+2, WMM_IE, 6)) {
+
+					pstat->flags |= WLAN_STA_WME;
+					
+					pstat->qos_option = 1;				
+					pstat->qos_info = *(p+8);
+					
+					pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
+
+					if((pstat->qos_info&0xf) !=0xf)
+						pstat->has_legacy_ac = _TRUE;
+					else
+						pstat->has_legacy_ac = _FALSE;
+					
+					if(pstat->qos_info&0xf)
+					{
+						if(pstat->qos_info&BIT(0))
+							pstat->uapsd_vo = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_vo = 0;
+		
+						if(pstat->qos_info&BIT(1))
+							pstat->uapsd_vi = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_vi = 0;
+			
+						if(pstat->qos_info&BIT(2))
+							pstat->uapsd_bk = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_bk = 0;
+			
+						if(pstat->qos_info&BIT(3))			
+							pstat->uapsd_be = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_be = 0;
+		
+					}
+	
+					break;
+				}
+			}
+			else {
+				break;
+			}
+			p = p + ie_len + 2;
+		}
+	}
+
+
+#ifdef CONFIG_80211N_HT
+	/* save HT capabilities in the sta object */
+	_rtw_memset(&pstat->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));
+	if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap)) 
+	{
+		pstat->flags |= WLAN_STA_HT;
+		
+		pstat->flags |= WLAN_STA_WME;
+		
+		_rtw_memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap));			
+		
+	} else
+		pstat->flags &= ~WLAN_STA_HT;
+
+	
+	if((pmlmepriv->htpriv.ht_option == _FALSE) && (pstat->flags&WLAN_STA_HT))
+	{
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+		
+
+	if ((pstat->flags & WLAN_STA_HT) &&
+		    ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
+		      (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP)))
+	{		    
+		DBG_871X("HT: " MAC_FMT " tried to "
+				   "use TKIP with HT association\n", MAC_ARG(pstat->hwaddr));
+		
+		//status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY;
+		//goto OnAssocReqFail;
+	}
+#endif /* CONFIG_80211N_HT */
+
+       //
+       //if (hapd->iface->current_mode->mode == HOSTAPD_MODE_IEEE80211G)//?
+	pstat->flags |= WLAN_STA_NONERP;	
+	for (i = 0; i < pstat->bssratelen; i++) {
+		if ((pstat->bssrateset[i] & 0x7f) > 22) {
+			pstat->flags &= ~WLAN_STA_NONERP;
+			break;
+		}
+	}
+
+	if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
+	else
+		pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
+
+	
+	
+	if (status != _STATS_SUCCESSFUL_)
+		goto OnAssocReqFail;
+
+#ifdef CONFIG_P2P
+	pstat->is_p2p_device = _FALSE;
+	if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+	{		
+		if( (p2pie=rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , NULL, &p2pielen)))
+		{
+			pstat->is_p2p_device = _TRUE;
+			if((p2p_status_code=(u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat))>0)
+			{
+				pstat->p2p_status_code = p2p_status_code;
+				status = _STATS_CAP_FAIL_;
+				goto OnAssocReqFail;
+			}
+		}
+#ifdef CONFIG_WFD
+		if(rtw_get_wfd_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , wfd_ie, &wfd_ielen ))
+		{
+			u8	attr_content[ 10 ] = { 0x00 };
+			u32	attr_contentlen = 0;
+
+			DBG_8192C( "[%s] WFD IE Found!!\n", __FUNCTION__ );
+			rtw_get_wfd_attr_content( wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
+			if ( attr_contentlen )
+			{
+				pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16( attr_content + 2 );
+				DBG_8192C( "[%s] Peer PORT NUM = %d\n", __FUNCTION__, pwdinfo->wfd_info->peer_rtsp_ctrlport );
+			}
+		}
+#endif
+	}
+	pstat->p2p_status_code = p2p_status_code;
+#endif //CONFIG_P2P
+
+	//TODO: identify_proprietary_vendor_ie();
+	// Realtek proprietary IE
+	// identify if this is Broadcom sta
+	// identify if this is ralink sta
+	// Customer proprietary IE
+
+	
+
+	/* get a unique AID */
+	if (pstat->aid > 0) {
+		DBG_871X("  old AID %d\n", pstat->aid);
+	} else {
+		for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
+			if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
+				break;
+				
+		//if (pstat->aid > NUM_STA) {
+		if (pstat->aid > pstapriv->max_num_sta) {
+				
+			pstat->aid = 0;
+				
+			DBG_871X("  no room for more AIDs\n");
+
+			status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
+				
+			goto OnAssocReqFail;
+				
+			
+		} else {
+			pstapriv->sta_aid[pstat->aid - 1] = pstat;
+			DBG_871X("allocate new AID = (%d)\n", pstat->aid);
+		}	
+	}
+
+
+	pstat->state &= (~WIFI_FW_ASSOC_STATE);	
+	pstat->state |= WIFI_FW_ASSOC_SUCCESS;
+	
+	_enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
+	if (!rtw_is_list_empty(&pstat->auth_list))
+	{
+		rtw_list_delete(&pstat->auth_list);
+		pstapriv->auth_list_cnt--;
+	}
+	_exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
+
+	_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);	
+	if (rtw_is_list_empty(&pstat->asoc_list))
+	{
+		pstat->expire_to = pstapriv->expire_to;
+		rtw_list_insert_tail(&pstat->asoc_list, &pstapriv->asoc_list);
+		pstapriv->asoc_list_cnt++;
+	}
+	_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+	// now the station is qualified to join our BSS...	
+	if(pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_==status))
+	{
+#ifdef CONFIG_NATIVEAP_MLME
+		//.1 bss_cap_update & sta_info_update
+		bss_cap_update_on_sta_join(padapter, pstat);
+		sta_info_update(padapter, pstat);
+
+		//issue assoc rsp before notify station join event.
+		if (frame_type == WIFI_ASSOCREQ)
+			issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+		else
+			issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+
+		//.2 - report to upper layer 
+		DBG_871X("indicate_sta_join_event to upper layer - hostapd\n");
+		#ifdef CONFIG_IOCTL_CFG80211
+		if (1) {
+			#ifdef COMPAT_KERNEL_RELEASE
+			rtw_cfg80211_indicate_sta_assoc(padapter, pframe, pkt_len);
+			#elif (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)
+			rtw_cfg80211_indicate_sta_assoc(padapter, pframe, pkt_len);
+			#else //(LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)
+			_enter_critical_bh(&pstat->lock, &irqL);
+			if(pstat->passoc_req)
+			{
+				rtw_mfree(pstat->passoc_req, pstat->assoc_req_len);
+				pstat->passoc_req = NULL;
+				pstat->assoc_req_len = 0;
+			}
+
+			pstat->passoc_req =  rtw_zmalloc(pkt_len);
+			if(pstat->passoc_req)
+			{
+				_rtw_memcpy(pstat->passoc_req, pframe, pkt_len);
+				pstat->assoc_req_len = pkt_len;
+			}
+			_exit_critical_bh(&pstat->lock, &irqL);		
+			#endif //(LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)
+		}
+		else
+		#endif //CONFIG_IOCTL_CFG80211
+		{
+			rtw_indicate_sta_assoc_event(padapter, pstat);
+		}
+
+		//.3-(1) report sta add event
+		report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
+		
+/*		
+		//issue assoc rsp before notify station join event.
+		if (frame_type == WIFI_ASSOCREQ)
+			issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+		else
+			issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+*/			
+	
+#endif
+	}
+
+	return _SUCCESS;
+
+asoc_class2_error:
+
+#ifdef CONFIG_NATIVEAP_MLME
+	issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
+#endif
+
+	return _FAIL;		
+
+OnAssocReqFail:
+
+
+#ifdef CONFIG_NATIVEAP_MLME
+	pstat->aid = 0;
+	if (frame_type == WIFI_ASSOCREQ)
+		issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+	else
+		issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+#endif
+
+
+#endif /* CONFIG_AP_MODE */
+
+	return _FAIL;		
+
+}
+
+unsigned int OnAssocRsp(_adapter *padapter, union recv_frame *precv_frame)
+{
+	uint i;
+	int res;
+	unsigned short	status;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	//WLAN_BSSID_EX 		*cur_network = &(pmlmeinfo->network);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+	PNDIS_802_11_VARIABLE_IEs	pWapiIE = NULL;
+
+	DBG_871X("%s\n", __FUNCTION__);
+	
+	//check A1 matches or not
+	if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
+		return _SUCCESS;
+
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+		return _SUCCESS;
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	//status
+	if ((status = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 2))) > 0)
+	{
+		DBG_871X("assoc reject, status code: %d\n", status);
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		res = -4;
+		goto report_assoc_result;
+	}
+
+	//get capabilities
+	pmlmeinfo->capability = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
+
+	//set slot time
+	pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10))? 9: 20;
+
+	//AID
+	res = pmlmeinfo->aid = (int)(le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
+
+	//following are moved to join event callback function
+	//to handle HT, WMM, rate adaptive, update MAC reg
+	//for not to handle the synchronous IO in the tasklet
+	for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;)
+	{
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + i);
+
+		switch (pIE->ElementID)
+		{
+			case _VENDOR_SPECIFIC_IE_:
+				if (_rtw_memcmp(pIE->data, WMM_PARA_OUI, 6))	//WMM
+				{
+					WMM_param_handler(padapter, pIE);
+				}
+#if defined(CONFIG_P2P) && defined(CONFIG_WFD)
+				else if ( _rtw_memcmp(pIE->data, WFD_OUI, 4))		//WFD
+				{
+					DBG_871X( "[%s] Found WFD IE\n", __FUNCTION__ );
+					WFD_info_handler( padapter, pIE );
+				}
+#endif				
+				break;
+
+#ifdef CONFIG_WAPI_SUPPORT
+			case _WAPI_IE_:
+				pWapiIE = pIE;
+				break;
+#endif
+
+			case _HT_CAPABILITY_IE_:	//HT caps
+				HT_caps_handler(padapter, pIE);
+				break;
+
+			case _HT_EXTRA_INFO_IE_:	//HT info
+				HT_info_handler(padapter, pIE);
+				break;
+
+			case _ERPINFO_IE_:
+				ERP_IE_handler(padapter, pIE);
+
+			default:
+				break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+#ifdef CONFIG_WAPI_SUPPORT
+	rtw_wapi_on_assoc_ok(padapter, pIE);
+#endif
+
+	pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
+	pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+
+	//Update Basic Rate Table for spec, 2010-12-28 , by thomas
+	UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);
+
+report_assoc_result:
+	if (res > 0) {
+		rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
+	} else {
+		rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
+	}
+
+	report_join_res(padapter, res);
+
+	return _SUCCESS;
+}
+
+unsigned int OnDeAuth(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned short	reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
+#endif //CONFIG_P2P
+
+	//check A3
+	if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+#ifdef CONFIG_P2P
+	if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
+	{
+		_cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
+		_set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
+	}
+#endif //CONFIG_P2P
+
+	reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
+
+	DBG_871X("%s Reason code(%d)\n", __FUNCTION__,reason);
+
+#ifdef CONFIG_AP_MODE
+	if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+	{		
+		_irqL irqL;
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+		
+		//_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		
+		//rtw_free_stainfo(padapter, psta);
+		//_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		
+
+		DBG_871X_LEVEL(_drv_always_, "ap recv deauth reason code(%d) sta:%pM\n",
+			       	reason, GetAddr2Ptr(pframe));
+
+		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));	
+		if(psta)
+		{
+			u8 updated;
+		
+			_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+			if(rtw_is_list_empty(&psta->asoc_list)==_FALSE)
+			{			
+				rtw_list_delete(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, _FALSE, reason);
+
+			}
+			_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+			associated_clients_update(padapter, updated);
+		}
+		
+
+		return _SUCCESS;
+	}
+	else
+#endif
+	{
+		DBG_871X_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM\n",
+			       	reason, GetAddr3Ptr(pframe));
+		
+		receive_disconnect(padapter, GetAddr3Ptr(pframe) ,reason);
+	}	
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE;
+	return _SUCCESS;
+
+}
+
+unsigned int OnDisassoc(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned short	reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
+#endif //CONFIG_P2P
+
+	//check A3
+	if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+#ifdef CONFIG_P2P
+	if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
+	{
+		_cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
+		_set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
+	}
+#endif //CONFIG_P2P
+
+	reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
+
+        DBG_871X("%s Reason code(%d)\n", __FUNCTION__,reason);
+
+#ifdef CONFIG_AP_MODE
+	if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+	{	
+		_irqL irqL;
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+		
+		//_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);	
+		//rtw_free_stainfo(padapter, psta);
+		//_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		
+
+		DBG_871X_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
+				reason, GetAddr2Ptr(pframe));
+
+		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));	
+		if(psta)
+		{
+			u8 updated;
+			
+			_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+			if(rtw_is_list_empty(&psta->asoc_list)==_FALSE)
+			{
+				rtw_list_delete(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, _FALSE, reason);
+			
+			}
+			_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+			associated_clients_update(padapter, updated);
+		}
+
+		return _SUCCESS;
+	}
+	else
+#endif
+	{
+		DBG_871X_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
+				reason, GetAddr3Ptr(pframe));
+		
+		receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
+	}	
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE;
+	return _SUCCESS;
+
+}
+
+unsigned int OnAtim(_adapter *padapter, union recv_frame *precv_frame)
+{
+	DBG_871X("%s\n", __FUNCTION__);
+	return _SUCCESS;
+}
+
+unsigned int on_action_spct_ch_switch(_adapter *padapter, struct sta_info *psta, u8 *ies, uint ies_len)
+{
+	unsigned int ret = _FAIL;
+	struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(mlmeext->mlmext_info);
+
+	if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+		ret = _SUCCESS;	
+		goto exit;
+	}
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
+		
+		int ch_switch_mode = -1, ch = -1, ch_switch_cnt = -1;
+		int ch_offset = -1;
+		u8 bwmode;
+		struct ieee80211_info_element *ie;
+
+		DBG_871X(FUNC_NDEV_FMT" from "MAC_FMT"\n",
+			FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(psta->hwaddr));
+
+		for_each_ie(ie, ies, ies_len) {
+			if (ie->id == WLAN_EID_CHANNEL_SWITCH) {
+				ch_switch_mode = ie->data[0];
+				ch = ie->data[1];
+				ch_switch_cnt = ie->data[2];
+				DBG_871X("ch_switch_mode:%d, ch:%d, ch_switch_cnt:%d\n",
+					ch_switch_mode, ch, ch_switch_cnt);
+			}
+			else if (ie->id == WLAN_EID_SECONDARY_CHANNEL_OFFSET) {
+				ch_offset = secondary_ch_offset_to_hal_ch_offset(ie->data[0]);
+				DBG_871X("ch_offset:%d\n", ch_offset);
+			}
+		}
+
+		if (ch == -1)
+			return _SUCCESS;
+
+		if (ch_offset == -1)
+			bwmode = mlmeext->cur_bwmode;
+		else
+			bwmode = (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) ?
+				HT_CHANNEL_WIDTH_20 : HT_CHANNEL_WIDTH_40;
+
+		ch_offset = (ch_offset == -1) ? mlmeext->cur_ch_offset : ch_offset;
+
+		/* todo:
+		 * 1. the decision of channel switching
+		 * 2. things after channel switching
+		 */
+
+		ret = rtw_set_ch_cmd(padapter, ch, bwmode, ch_offset, _TRUE);
+	}
+
+exit:
+	return ret;
+}
+
+unsigned int on_action_spct(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint frame_len = precv_frame->u.hdr.len;
+	u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+	u8 category;
+	u8 action;
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+
+	psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+
+	if (!psta)
+		goto exit;
+
+	category = frame_body[0];
+	if(category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case RTW_WLAN_ACTION_SPCT_MSR_REQ:
+	case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
+	case RTW_WLAN_ACTION_SPCT_TPC_REQ:
+	case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
+		break;
+	case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
+		#ifdef CONFIG_SPCT_CH_SWITCH
+		ret = on_action_spct_ch_switch(padapter, psta, &frame_body[2],
+			frame_len-(frame_body-pframe)-2);
+		#endif
+		break;
+	default:
+		break;
+	}
+
+exit:
+	return ret;
+}
+
+unsigned int OnAction_qos(_adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int OnAction_dls(_adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int OnAction_back(_adapter *padapter, union recv_frame *precv_frame)
+{
+	u8 *addr;
+	struct sta_info *psta=NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	unsigned char		*frame_body;
+	unsigned char		category, action;
+	unsigned short	tid, status, reason_code = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+#ifdef CONFIG_80211N_HT
+	//check RA matches or not	
+	if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))//for if1, sta/ap mode
+		return _SUCCESS;
+
+/*
+	//check A1 matches or not
+	if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+*/
+	DBG_871X("%s\n", __FUNCTION__);
+
+	if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)	
+		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+			return _SUCCESS;
+
+	addr = GetAddr2Ptr(pframe);
+	psta = rtw_get_stainfo(pstapriv, addr);
+
+	if(psta==NULL)
+		return _SUCCESS;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+	if (category == RTW_WLAN_CATEGORY_BACK)// representing Block Ack
+	{
+#ifdef CONFIG_TDLS
+		if((psta->tdls_sta_state & TDLS_LINKED_STATE) && 
+			(psta->htpriv.ht_option==_TRUE) &&
+			(psta->htpriv.ampdu_enable==_TRUE) )
+		{
+			//do nothing; just don't want to return _SUCCESS;
+		}
+		else
+#endif //CONFIG_TDLS
+		if (!pmlmeinfo->HT_enable)
+		{
+			return _SUCCESS;
+		}
+
+		action = frame_body[1];
+		DBG_871X("%s, action=%d\n", __FUNCTION__, action);
+		switch (action)
+		{
+			case RTW_WLAN_ACTION_ADDBA_REQ: //ADDBA request
+
+				_rtw_memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
+				//process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), GetAddr3Ptr(pframe));
+				process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), addr);
+				
+				if(pmlmeinfo->bAcceptAddbaReq == _TRUE)
+				{
+					issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0);
+				}
+				else
+				{
+					issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);//reject ADDBA Req
+				}
+								
+				break;
+
+			case RTW_WLAN_ACTION_ADDBA_RESP: //ADDBA response
+
+				//status = frame_body[3] | (frame_body[4] << 8); //endian issue
+				status = RTW_GET_LE16(&frame_body[3]);
+				tid = ((frame_body[5] >> 2) & 0x7);
+
+				if (status == 0)
+				{	//successful					
+					DBG_871X("agg_enable for TID=%d\n", tid);
+					psta->htpriv.agg_enable_bitmap |= 1 << tid;					
+					psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);				
+				}
+				else
+				{					
+					psta->htpriv.agg_enable_bitmap &= ~BIT(tid);					
+				}
+
+				//DBG_871X("marc: ADDBA RSP: %x\n", pmlmeinfo->agg_enable_bitmap);
+				break;
+
+			case RTW_WLAN_ACTION_DELBA: //DELBA
+				if ((frame_body[3] & BIT(3)) == 0)
+				{
+					psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+					psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+					
+					//reason_code = frame_body[4] | (frame_body[5] << 8);
+					reason_code = RTW_GET_LE16(&frame_body[4]);
+				}
+				else if((frame_body[3] & BIT(3)) == BIT(3))
+				{						
+					tid = (frame_body[3] >> 4) & 0x0F;
+				
+					preorder_ctrl =  &psta->recvreorder_ctrl[tid];
+					preorder_ctrl->enable = _FALSE;
+					preorder_ctrl->indicate_seq = 0xffff;
+					#ifdef DBG_RX_SEQ
+					DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
+						preorder_ctrl->indicate_seq);
+					#endif
+				}
+				
+				DBG_871X("%s(): DELBA: %x(%x)\n", __FUNCTION__,pmlmeinfo->agg_enable_bitmap, reason_code);
+				//todo: how to notify the host while receiving DELETE BA
+				break;
+
+			default:
+				break;
+		}
+	}
+#endif //CONFIG_80211N_HT
+	return _SUCCESS;
+}
+
+#ifdef CONFIG_P2P
+
+static int get_reg_classes_full_count(struct p2p_channels channel_list) {
+	int cnt = 0;
+	int i;
+
+	for (i = 0; i < channel_list.reg_classes; i++) {
+		cnt += channel_list.reg_class[i].channels;
+	}
+
+	return cnt;
+}
+
+static void get_channel_cnt_24g_5gl_5gh(  struct mlme_ext_priv *pmlmeext, u8* p24g_cnt, u8* p5gl_cnt, u8* p5gh_cnt )
+{
+	int	i = 0;
+
+	*p24g_cnt = 0;
+	*p5gl_cnt = 0;
+	*p5gh_cnt = 0;	
+	
+	for( i = 0; i < pmlmeext->max_chan_nums; i++ )
+	{
+		if ( pmlmeext->channel_set[ i ].ChannelNum <= 14 )
+		{
+			(*p24g_cnt)++;
+		}
+		else if ( ( pmlmeext->channel_set[ i ].ChannelNum > 14 ) && ( pmlmeext->channel_set[ i ].ChannelNum <= 48 ) )
+		{
+			//	Just include the channel 36, 40, 44, 48 channels for 5G low
+			(*p5gl_cnt)++;
+		}
+		else if ( ( pmlmeext->channel_set[ i ].ChannelNum >= 149 ) && ( pmlmeext->channel_set[ i ].ChannelNum <= 161 ) )
+		{
+			//	Just include the channel 149, 153, 157, 161 channels for 5G high
+			(*p5gh_cnt)++;
+		}
+	}
+}
+
+void issue_p2p_GO_request(_adapter *padapter, u8* raddr)
+{
+
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	u32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_GO_NEGO_REQ;
+	u8			wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
+	u8			wpsielen = 0, p2pielen = 0, i;
+	u8			channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
+	u16			len_channellist_attr = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD		
+	
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &( padapter->wdinfo);
+
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	DBG_871X( "[%s] In\n", __FUNCTION__ );
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));	
+	pwdinfo->negotiation_dialog_token = 1;	//	Initialize the dialog value
+	pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &(pattrib->pktlen));
+
+	
+
+	//	WPS Section
+	wpsielen = 0;
+	//	WPS OUI
+	*(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
+	wpsielen += 4;
+
+	//	WPS version
+	//	Type:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
+	wpsielen += 2;
+
+	//	Length:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
+	wpsielen += 2;
+
+	//	Value:
+	wpsie[wpsielen++] = WPS_VERSION_1;	//	Version 1.0
+
+	//	Device Password ID
+	//	Type:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
+	wpsielen += 2;
+
+	//	Length:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
+	wpsielen += 2;
+
+	//	Value:
+
+	if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN )
+	{
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_USER_SPEC );
+	}
+	else if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN )
+	{
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );
+	}
+	else if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC )
+	{
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_PBC );
+	}
+
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
+
+
+	//	P2P IE Section.
+
+	//	P2P OUI
+	p2pielen = 0;
+	p2pie[ p2pielen++ ] = 0x50;
+	p2pie[ p2pielen++ ] = 0x6F;
+	p2pie[ p2pielen++ ] = 0x9A;
+	p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+	//	Commented by Albert 20110306
+	//	According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes
+	//	1. P2P Capability
+	//	2. Group Owner Intent
+	//	3. Configuration Timeout
+	//	4. Listen Channel
+	//	5. Extended Listen Timing
+	//	6. Intended P2P Interface Address
+	//	7. Channel List
+	//	8. P2P Device Info
+	//	9. Operating Channel
+
+
+	//	P2P Capability
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
+	p2pielen += 2;
+
+	//	Value:
+	//	Device Capability Bitmap, 1 byte
+	p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
+
+	//	Group Capability Bitmap, 1 byte
+	if ( pwdinfo->persistent_supported )
+	{
+		p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+	}
+	else
+	{
+		p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
+	}
+
+
+	//	Group Owner Intent
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_GO_INTENT;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
+	p2pielen += 2;
+
+	//	Value:
+	//	Todo the tie breaker bit.
+	p2pie[ p2pielen++ ] = ( ( pwdinfo->intent << 1 ) | BIT(0) );
+
+	//	Configuration Timeout
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
+	p2pielen += 2;
+
+	//	Value:
+	p2pie[ p2pielen++ ] = 200;	//	2 seconds needed to be the P2P GO
+	p2pie[ p2pielen++ ] = 200;	//	2 seconds needed to be the P2P Client
+
+
+	//	Listen Channel
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_LISTEN_CH;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
+	p2pielen += 2;
+
+	//	Value:
+	//	Country String
+	p2pie[ p2pielen++ ] = 'X';
+	p2pie[ p2pielen++ ] = 'X';
+	
+	//	The third byte should be set to 0x04.
+	//	Described in the "Operating Channel Attribute" section.
+	p2pie[ p2pielen++ ] = 0x04;
+
+	//	Operating Class
+	p2pie[ p2pielen++ ] = 0x51;	//	Copy from SD7
+	
+	//	Channel Number
+	p2pie[ p2pielen++ ] = pwdinfo->listen_channel;	//	listening channel number
+	
+
+	//	Extended Listen Timing ATTR
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
+	p2pielen += 2;
+
+	//	Value:
+	//	Availability Period
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
+	p2pielen += 2;
+
+	//	Availability Interval
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
+	p2pielen += 2;
+
+
+	//	Intended P2P Interface Address
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_INTENTED_IF_ADDR;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
+	p2pielen += 2;
+
+	//	Value:
+	_rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
+	p2pielen += ETH_ALEN;
+
+
+	//	Channel List
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
+
+	// Length:
+	// Country String(3)
+	// + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
+	// + number of channels in all classes
+	len_channellist_attr = 3
+	   + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
+	   + get_reg_classes_full_count(pmlmeext->channel_list);
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+	{
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
+	}
+	else
+	{
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
+	}
+#else
+
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
+
+#endif
+	p2pielen += 2;
+
+	//	Value:
+	//	Country String
+	p2pie[ p2pielen++ ] = 'X';
+	p2pie[ p2pielen++ ] = 'X';
+	
+	//	The third byte should be set to 0x04.
+	//	Described in the "Operating Channel Attribute" section.
+	p2pie[ p2pielen++ ] = 0x04;
+
+	//	Channel Entry List
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+	{
+		_adapter *pbuddy_adapter = padapter->pbuddy_adapter;	
+		struct mlme_ext_priv	*pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+
+		//	Operating Class
+		if ( pbuddy_mlmeext->cur_channel > 14 )
+		{
+			if ( pbuddy_mlmeext->cur_channel >= 149 )
+			{
+				p2pie[ p2pielen++ ] = 0x7c;
+			}
+			else
+			{
+				p2pie[ p2pielen++ ] = 0x73;
+			}
+		}
+		else
+		{
+			p2pie[ p2pielen++ ] = 0x51;
+		}
+
+		//	Number of Channels
+		//	Just support 1 channel and this channel is AP's channel
+		p2pie[ p2pielen++ ] = 1;
+
+		//	Channel List
+		p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
+	}
+	else
+	{
+		int i,j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			//	Operating Class
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			//	Number of Channels
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			//	Channel List
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+			}
+		}
+	}
+#else // CONFIG_CONCURRENT_MODE
+	{
+		int i,j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			//	Operating Class
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			//	Number of Channels
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			//	Channel List
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+			}
+		}
+	}
+#endif // CONFIG_CONCURRENT_MODE
+
+	//	Device Info
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) 
+	//	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
+	p2pielen += 2;
+
+	//	Value:
+	//	P2P Device Address
+	_rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
+	p2pielen += ETH_ALEN;
+
+	//	Config Method
+	//	This field should be big endian. Noted by P2P specification.
+
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
+
+	p2pielen += 2;
+
+	//	Primary Device Type
+	//	Category ID
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
+	p2pielen += 2;
+
+	//	OUI
+	*(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
+	p2pielen += 4;
+
+	//	Sub Category ID
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
+	p2pielen += 2;
+
+	//	Number of Secondary Device Types
+	p2pie[ p2pielen++ ] = 0x00;	//	No Secondary Device Type List
+
+	//	Device Name
+	//	Type:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
+	p2pielen += 2;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
+	p2pielen += 2;
+
+	//	Value:
+	_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len );
+	p2pielen += pwdinfo->device_name_len;	
+	
+
+	//	Operating Channel
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
+	p2pielen += 2;
+
+	//	Value:
+	//	Country String
+	p2pie[ p2pielen++ ] = 'X';
+	p2pie[ p2pielen++ ] = 'X';
+	
+	//	The third byte should be set to 0x04.
+	//	Described in the "Operating Channel Attribute" section.
+	p2pie[ p2pielen++ ] = 0x04;
+
+	//	Operating Class
+	if ( pwdinfo->operating_channel <= 14 )
+	{
+		//	Operating Class
+		p2pie[ p2pielen++ ] = 0x51;
+	}
+	else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
+	{
+		//	Operating Class
+		p2pie[ p2pielen++ ] = 0x73;
+	}
+	else
+	{
+		//	Operating Class
+		p2pie[ p2pielen++ ] = 0x7c;
+	}
+
+	//	Channel Number
+	p2pie[ p2pielen++ ] = pwdinfo->operating_channel;	//	operating channel number
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );		
+
+#ifdef CONFIG_WFD
+	wfdielen = build_nego_req_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif //CONFIG_WFD
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+
+void issue_p2p_GO_response(_adapter *padapter, u8* raddr, u8* frame_body,uint len, u8 result)
+{
+
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	u32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_GO_NEGO_RESP;
+	u8			wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
+	u8			p2pielen = 0, i;
+	uint			wpsielen = 0;
+	u16			wps_devicepassword_id = 0x0000;
+	uint			wps_devicepassword_id_len = 0;
+	u8			channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh;
+	u16			len_channellist_attr = 0;
+	
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &( padapter->wdinfo);
+
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	DBG_871X( "[%s] In, result = %d\n", __FUNCTION__,  result );
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));	
+	pwdinfo->negotiation_dialog_token = frame_body[7];	//	The Dialog Token of provisioning discovery request frame.
+	pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
+
+	//	Commented by Albert 20110328
+	//	Try to get the device password ID from the WPS IE of group negotiation request frame
+	//	WiFi Direct test plan 5.1.15
+	rtw_get_wps_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
+	rtw_get_wps_attr_content( wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len);
+	wps_devicepassword_id = be16_to_cpu( wps_devicepassword_id );
+
+	_rtw_memset( wpsie, 0x00, 255 );
+	wpsielen = 0;
+
+	//	WPS Section
+	wpsielen = 0;
+	//	WPS OUI
+	*(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
+	wpsielen += 4;
+
+	//	WPS version
+	//	Type:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
+	wpsielen += 2;
+
+	//	Length:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
+	wpsielen += 2;
+
+	//	Value:
+	wpsie[wpsielen++] = WPS_VERSION_1;	//	Version 1.0
+
+	//	Device Password ID
+	//	Type:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
+	wpsielen += 2;
+
+	//	Length:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
+	wpsielen += 2;
+
+	//	Value:
+	if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
+	{
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );
+	}
+	else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
+	{
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_USER_SPEC );
+	}
+	else
+	{
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_PBC );
+	}
+	wpsielen += 2;
+
+	//	Commented by Kurt 20120113
+	//	If some device wants to do p2p handshake without sending prov_disc_req
+	//	We have to get peer_req_cm from here.
+	if(_rtw_memcmp( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3) )
+	{
+		if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
+		{
+			_rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3 );
+		}
+		else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
+		{
+			_rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3 );	
+		}
+		else
+		{
+			_rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3 );	
+		}
+	}
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
+
+
+	//	P2P IE Section.
+
+	//	P2P OUI
+	p2pielen = 0;
+	p2pie[ p2pielen++ ] = 0x50;
+	p2pie[ p2pielen++ ] = 0x6F;
+	p2pie[ p2pielen++ ] = 0x9A;
+	p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+	//	Commented by Albert 20100908
+	//	According to the P2P Specification, the group negoitation response frame should contain 9 P2P attributes
+	//	1. Status
+	//	2. P2P Capability
+	//	3. Group Owner Intent
+	//	4. Configuration Timeout
+	//	5. Operating Channel
+	//	6. Intended P2P Interface Address
+	//	7. Channel List
+	//	8. Device Info
+	//	9. Group ID	( Only GO )
+
+
+	//	ToDo:
+
+	//	P2P Status
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
+	p2pielen += 2;
+
+	//	Value:
+	p2pie[ p2pielen++ ] = result;
+	
+	//	P2P Capability
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
+	p2pielen += 2;
+
+	//	Value:
+	//	Device Capability Bitmap, 1 byte
+
+	if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
+	{
+		//	Commented by Albert 2011/03/08
+		//	According to the P2P specification
+		//	if the sending device will be client, the P2P Capability should be reserved of group negotation response frame
+		p2pie[ p2pielen++ ] = 0;
+	}
+	else
+	{
+		//	Be group owner or meet the error case
+		p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
+	}
+	
+	//	Group Capability Bitmap, 1 byte
+	if ( pwdinfo->persistent_supported )
+	{
+		p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+	}
+	else
+	{
+		p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
+	}
+
+	//	Group Owner Intent
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_GO_INTENT;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
+	p2pielen += 2;
+
+	//	Value:
+	if ( pwdinfo->peer_intent & 0x01 )
+	{
+		//	Peer's tie breaker bit is 1, our tie breaker bit should be 0
+		p2pie[ p2pielen++ ] = ( pwdinfo->intent << 1 );
+	}
+	else
+	{
+		//	Peer's tie breaker bit is 0, our tie breaker bit should be 1
+		p2pie[ p2pielen++ ] = ( ( pwdinfo->intent << 1 ) | BIT(0) );
+	}
+
+
+	//	Configuration Timeout
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
+	p2pielen += 2;
+
+	//	Value:
+	p2pie[ p2pielen++ ] = 200;	//	2 seconds needed to be the P2P GO
+	p2pie[ p2pielen++ ] = 200;	//	2 seconds needed to be the P2P Client
+
+	//	Operating Channel
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
+	p2pielen += 2;
+
+	//	Value:
+	//	Country String
+	p2pie[ p2pielen++ ] = 'X';
+	p2pie[ p2pielen++ ] = 'X';
+	
+	//	The third byte should be set to 0x04.
+	//	Described in the "Operating Channel Attribute" section.
+	p2pie[ p2pielen++ ] = 0x04;
+
+	//	Operating Class
+	if ( pwdinfo->operating_channel <= 14 )
+	{
+		//	Operating Class
+		p2pie[ p2pielen++ ] = 0x51;
+	}
+	else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
+	{
+		//	Operating Class
+		p2pie[ p2pielen++ ] = 0x73;
+	}
+	else
+	{
+		//	Operating Class
+		p2pie[ p2pielen++ ] = 0x7c;
+	}
+	
+	//	Channel Number
+	p2pie[ p2pielen++ ] = pwdinfo->operating_channel;	//	operating channel number
+
+	//	Intended P2P Interface Address	
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_INTENTED_IF_ADDR;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
+	p2pielen += 2;
+
+	//	Value:
+	_rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
+	p2pielen += ETH_ALEN;
+
+	//	Channel List
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
+
+	// Country String(3)
+	// + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
+	// + number of channels in all classes
+	len_channellist_attr = 3
+	   + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+	   + get_reg_classes_full_count(pmlmeext->channel_list);
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+	{
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
+	}
+	else
+	{
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
+	}
+#else
+
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
+
+ #endif
+	p2pielen += 2;
+
+	//	Value:
+	//	Country String
+	p2pie[ p2pielen++ ] = 'X';
+	p2pie[ p2pielen++ ] = 'X';
+	
+	//	The third byte should be set to 0x04.
+	//	Described in the "Operating Channel Attribute" section.
+	p2pie[ p2pielen++ ] = 0x04;
+
+	//	Channel Entry List
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+	{
+		_adapter *pbuddy_adapter = padapter->pbuddy_adapter;	
+		struct mlme_ext_priv	*pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+
+		//	Operating Class
+		if ( pbuddy_mlmeext->cur_channel > 14 )
+		{
+			if ( pbuddy_mlmeext->cur_channel >= 149 )
+			{
+				p2pie[ p2pielen++ ] = 0x7c;
+			}
+			else
+			{
+				p2pie[ p2pielen++ ] = 0x73;
+			}
+		}
+		else
+		{
+			p2pie[ p2pielen++ ] = 0x51;
+		}
+
+		//	Number of Channels
+		//	Just support 1 channel and this channel is AP's channel
+		p2pie[ p2pielen++ ] = 1;
+
+		//	Channel List
+		p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
+	}
+	else
+	{
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			//	Operating Class
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			//	Number of Channels
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			//	Channel List
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+			}
+		}
+	}
+#else // CONFIG_CONCURRENT_MODE
+	{
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			//	Operating Class
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			//	Number of Channels
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			//	Channel List
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+			}
+		}
+	}
+#endif // CONFIG_CONCURRENT_MODE
+
+	
+	//	Device Info
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) 
+	//	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
+	p2pielen += 2;
+
+	//	Value:
+	//	P2P Device Address
+	_rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
+	p2pielen += ETH_ALEN;
+
+	//	Config Method
+	//	This field should be big endian. Noted by P2P specification.
+
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
+
+	p2pielen += 2;
+
+	//	Primary Device Type
+	//	Category ID
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
+	p2pielen += 2;
+
+	//	OUI
+	*(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
+	p2pielen += 4;
+
+	//	Sub Category ID
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
+	p2pielen += 2;
+
+	//	Number of Secondary Device Types
+	p2pie[ p2pielen++ ] = 0x00;	//	No Secondary Device Type List
+
+	//	Device Name
+	//	Type:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
+	p2pielen += 2;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
+	p2pielen += 2;
+
+	//	Value:
+	_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len );
+	p2pielen += pwdinfo->device_name_len;	
+	
+	if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
+	{
+		//	Group ID Attribute
+		//	Type:
+		p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
+
+		//	Length:
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + pwdinfo->nego_ssidlen );
+		p2pielen += 2;
+
+		//	Value:
+		//	p2P Device Address
+		_rtw_memcpy( p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN );
+		p2pielen += ETH_ALEN;
+
+		//	SSID
+		_rtw_memcpy( p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
+		p2pielen += pwdinfo->nego_ssidlen;
+		
+	}
+	
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );	
+	
+#ifdef CONFIG_WFD
+	wfdielen = build_nego_resp_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif //CONFIG_WFD
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+void issue_p2p_GO_confirm(_adapter *padapter, u8* raddr, u8 result)
+{
+
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	u32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_GO_NEGO_CONF;
+	u8			wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
+	u8			wpsielen = 0, p2pielen = 0;
+	
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &( padapter->wdinfo);
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	DBG_871X( "[%s] In\n", __FUNCTION__ );
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));	
+	pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
+
+	
+
+	//	P2P IE Section.
+
+	//	P2P OUI
+	p2pielen = 0;
+	p2pie[ p2pielen++ ] = 0x50;
+	p2pie[ p2pielen++ ] = 0x6F;
+	p2pie[ p2pielen++ ] = 0x9A;
+	p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+	//	Commented by Albert 20110306
+	//	According to the P2P Specification, the group negoitation request frame should contain 5 P2P attributes
+	//	1. Status
+	//	2. P2P Capability
+	//	3. Operating Channel
+	//	4. Channel List
+	//	5. Group ID	( if this WiFi is GO )
+
+	//	P2P Status
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
+	p2pielen += 2;
+
+	//	Value:
+	p2pie[ p2pielen++ ] = result;
+
+	//	P2P Capability
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
+	p2pielen += 2;
+
+	//	Value:
+	//	Device Capability Bitmap, 1 byte
+	p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
+	
+	//	Group Capability Bitmap, 1 byte
+	if ( pwdinfo->persistent_supported )
+	{
+		p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+	}
+	else
+	{
+		p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
+	}
+
+
+	//	Operating Channel
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
+	p2pielen += 2;
+
+	//	Value:
+	//	Country String
+	p2pie[ p2pielen++ ] = 'X';
+	p2pie[ p2pielen++ ] = 'X';
+	
+	//	The third byte should be set to 0x04.
+	//	Described in the "Operating Channel Attribute" section.
+	p2pie[ p2pielen++ ] = 0x04;
+
+
+	if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
+	{
+		if ( pwdinfo->peer_operating_ch <= 14 )
+		{
+			//	Operating Class
+			p2pie[ p2pielen++ ] = 0x51;
+		}
+		else if ( ( pwdinfo->peer_operating_ch >= 36 ) && ( pwdinfo->peer_operating_ch <= 48 ) )
+		{
+			//	Operating Class
+			p2pie[ p2pielen++ ] = 0x73;
+		}
+		else
+		{
+			//	Operating Class
+			p2pie[ p2pielen++ ] = 0x7c;
+		}
+		
+		p2pie[ p2pielen++ ] = pwdinfo->peer_operating_ch;
+	}
+	else
+	{
+		if ( pwdinfo->operating_channel <= 14 )
+		{
+			//	Operating Class
+			p2pie[ p2pielen++ ] = 0x51;
+		}
+		else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
+		{
+			//	Operating Class
+			p2pie[ p2pielen++ ] = 0x73;
+		}
+		else
+		{
+			//	Operating Class
+			p2pie[ p2pielen++ ] = 0x7c;
+		}
+		
+		//	Channel Number
+		p2pie[ p2pielen++ ] = pwdinfo->operating_channel;		//	Use the listen channel as the operating channel
+	}
+
+
+	//	Channel List
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( pwdinfo->channel_list_attr_len );
+	p2pielen += 2;
+
+	//	Value:
+	_rtw_memcpy( p2pie + p2pielen, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len );
+	p2pielen += pwdinfo->channel_list_attr_len;
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
+	{
+		//	Group ID Attribute
+		//	Type:
+		p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
+
+		//	Length:
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + pwdinfo->nego_ssidlen );
+		p2pielen += 2;
+
+		//	Value:
+		//	p2P Device Address
+		_rtw_memcpy( p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN );
+		p2pielen += ETH_ALEN;
+
+		//	SSID
+		_rtw_memcpy( p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
+		p2pielen += pwdinfo->nego_ssidlen;
+	}
+	
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
+	
+#ifdef CONFIG_WFD
+	wfdielen = build_nego_confirm_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif //CONFIG_WFD
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+void issue_p2p_invitation_request(_adapter *padapter, u8* raddr )
+{
+
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	u32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_INVIT_REQ;
+	u8			p2pie[ 255 ] = { 0x00 };
+	u8			p2pielen = 0, i;
+	u8			dialogToken = 3;
+	u8			channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
+	u16			len_channellist_attr = 0;	
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD
+#ifdef CONFIG_CONCURRENT_MODE
+	_adapter				*pbuddy_adapter = padapter->pbuddy_adapter;
+	struct wifidirect_info	*pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
+	struct mlme_priv		*pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
+	struct mlme_ext_priv	*pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+#endif
+
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &( padapter->wdinfo);
+
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));	
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+	//	P2P IE Section.
+
+	//	P2P OUI
+	p2pielen = 0;
+	p2pie[ p2pielen++ ] = 0x50;
+	p2pie[ p2pielen++ ] = 0x6F;
+	p2pie[ p2pielen++ ] = 0x9A;
+	p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+	//	Commented by Albert 20101011
+	//	According to the P2P Specification, the P2P Invitation request frame should contain 7 P2P attributes
+	//	1. Configuration Timeout
+	//	2. Invitation Flags
+	//	3. Operating Channel	( Only GO )
+	//	4. P2P Group BSSID	( Should be included if I am the GO )
+	//	5. Channel List
+	//	6. P2P Group ID
+	//	7. P2P Device Info
+
+	//	Configuration Timeout
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
+	p2pielen += 2;
+
+	//	Value:
+	p2pie[ p2pielen++ ] = 200;	//	2 seconds needed to be the P2P GO
+	p2pie[ p2pielen++ ] = 200;	//	2 seconds needed to be the P2P Client
+
+	//	Invitation Flags
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_INVITATION_FLAGS;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
+	p2pielen += 2;
+
+	//	Value:
+	p2pie[ p2pielen++ ] = P2P_INVITATION_FLAGS_PERSISTENT;
+
+
+	//	Operating Channel
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
+	p2pielen += 2;
+
+	//	Value:
+	//	Country String
+	p2pie[ p2pielen++ ] = 'X';
+	p2pie[ p2pielen++ ] = 'X';
+	
+	//	The third byte should be set to 0x04.
+	//	Described in the "Operating Channel Attribute" section.
+	p2pie[ p2pielen++ ] = 0x04;
+
+	//	Operating Class
+	if ( pwdinfo->invitereq_info.operating_ch <= 14 )
+		p2pie[ p2pielen++ ] = 0x51;
+	else if ( ( pwdinfo->invitereq_info.operating_ch >= 36 ) && ( pwdinfo->invitereq_info.operating_ch <= 48 ) )
+		p2pie[ p2pielen++ ] = 0x73;
+	else
+		p2pie[ p2pielen++ ] = 0x7c;
+	
+	//	Channel Number
+	p2pie[ p2pielen++ ] = pwdinfo->invitereq_info.operating_ch;	//	operating channel number
+
+	if ( _rtw_memcmp( myid( &padapter->eeprompriv ), pwdinfo->invitereq_info.go_bssid, ETH_ALEN ) )
+	{
+		//	P2P Group BSSID
+		//	Type:
+		p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_BSSID;
+
+		//	Length:
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
+		p2pielen += 2;
+
+		//	Value:
+		//	P2P Device Address for GO
+		_rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN );
+		p2pielen += ETH_ALEN;
+	}
+
+	//	Channel List
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
+
+	
+	//	Length:
+	// Country String(3)
+	// + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
+	// + number of channels in all classes
+	len_channellist_attr = 3
+	   + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+	   + get_reg_classes_full_count(pmlmeext->channel_list);
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+	{
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
+	}
+	else
+	{
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
+	}
+#else
+
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
+
+ #endif
+	p2pielen += 2;
+
+	//	Value:
+	//	Country String
+	p2pie[ p2pielen++ ] = 'X';
+	p2pie[ p2pielen++ ] = 'X';
+	
+	//	The third byte should be set to 0x04.
+	//	Described in the "Operating Channel Attribute" section.
+	p2pie[ p2pielen++ ] = 0x04;
+
+	//	Channel Entry List
+#ifdef CONFIG_CONCURRENT_MODE
+	if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+	{
+		_adapter *pbuddy_adapter = padapter->pbuddy_adapter;	
+		struct mlme_ext_priv	*pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+
+		//	Operating Class
+		if ( pbuddy_mlmeext->cur_channel > 14 )
+		{
+			if ( pbuddy_mlmeext->cur_channel >= 149 )
+			{
+				p2pie[ p2pielen++ ] = 0x7c;
+			}
+			else
+			{
+				p2pie[ p2pielen++ ] = 0x73;
+			}
+		}
+		else
+		{
+			p2pie[ p2pielen++ ] = 0x51;
+		}
+
+		//	Number of Channels
+		//	Just support 1 channel and this channel is AP's channel
+		p2pie[ p2pielen++ ] = 1;
+
+		//	Channel List
+		p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
+	}
+	else
+	{
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			//	Operating Class
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			//	Number of Channels
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			//	Channel List
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+			}
+		}
+	}
+#else // CONFIG_CONCURRENT_MODE
+	{
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			//	Operating Class
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			//	Number of Channels
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			//	Channel List
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+			}
+		}
+	}
+#endif // CONFIG_CONCURRENT_MODE
+
+
+	//	P2P Group ID
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 6 + pwdinfo->invitereq_info.ssidlen );
+	p2pielen += 2;
+
+	//	Value:
+	//	P2P Device Address for GO
+	_rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN );
+	p2pielen += ETH_ALEN;
+
+	//	SSID
+	_rtw_memcpy( p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid, pwdinfo->invitereq_info.ssidlen );
+	p2pielen += pwdinfo->invitereq_info.ssidlen;
+	
+
+	//	Device Info
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) 
+	//	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
+	p2pielen += 2;
+	
+	//	Value:
+	//	P2P Device Address
+	_rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
+	p2pielen += ETH_ALEN;
+
+	//	Config Method
+	//	This field should be big endian. Noted by P2P specification.
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
+	p2pielen += 2;
+
+	//	Primary Device Type
+	//	Category ID
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
+	p2pielen += 2;
+
+	//	OUI
+	*(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
+	p2pielen += 4;
+
+	//	Sub Category ID
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
+	p2pielen += 2;
+
+	//	Number of Secondary Device Types
+	p2pie[ p2pielen++ ] = 0x00;	//	No Secondary Device Type List
+
+	//	Device Name
+	//	Type:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
+	p2pielen += 2;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
+	p2pielen += 2;
+
+	//	Value:
+	_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
+	p2pielen += pwdinfo->device_name_len;
+		
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );	
+
+#ifdef CONFIG_WFD
+	wfdielen = build_invitation_req_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif //CONFIG_WFD	
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+void issue_p2p_invitation_response(_adapter *padapter, u8* raddr, u8 dialogToken, u8 status_code)
+{
+
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	u32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_INVIT_RESP;
+	u8			p2pie[ 255 ] = { 0x00 };
+	u8			p2pielen = 0, i;
+	u8			channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
+	u16			len_channellist_attr = 0;
+#ifdef CONFIG_CONCURRENT_MODE
+	_adapter				*pbuddy_adapter = padapter->pbuddy_adapter;
+	struct wifidirect_info	*pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
+	struct mlme_priv		*pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
+	struct mlme_ext_priv	*pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+#endif	
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD
+	
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &( padapter->wdinfo);
+
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));	
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+	//	P2P IE Section.
+
+	//	P2P OUI
+	p2pielen = 0;
+	p2pie[ p2pielen++ ] = 0x50;
+	p2pie[ p2pielen++ ] = 0x6F;
+	p2pie[ p2pielen++ ] = 0x9A;
+	p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+	//	Commented by Albert 20101005
+	//	According to the P2P Specification, the P2P Invitation response frame should contain 5 P2P attributes
+	//	1. Status
+	//	2. Configuration Timeout
+	//	3. Operating Channel	( Only GO )
+	//	4. P2P Group BSSID	( Only GO )
+	//	5. Channel List
+
+	//	P2P Status
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
+	p2pielen += 2;
+
+	//	Value:
+	//	When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE.
+	//	Sent the event receiving the P2P Invitation Req frame to DMP UI.
+	//	DMP had to compare the MAC address to find out the profile.
+	//	So, the WiFi driver will send the P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB.
+	//	If the UI found the corresponding profile, the WiFi driver sends the P2P Invitation Req
+	//	to NB to rebuild the persistent group.
+	p2pie[ p2pielen++ ] = status_code;
+	
+	//	Configuration Timeout
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
+
+	//	Length:
+	*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
+	p2pielen += 2;
+
+	//	Value:
+	p2pie[ p2pielen++ ] = 200;	//	2 seconds needed to be the P2P GO
+	p2pie[ p2pielen++ ] = 200;	//	2 seconds needed to be the P2P Client
+
+	if( status_code == P2P_STATUS_SUCCESS )
+	{
+		if( rtw_p2p_chk_role( pwdinfo, P2P_ROLE_GO ) )
+		{
+			//	The P2P Invitation request frame asks this Wi-Fi device to be the P2P GO
+			//	In this case, the P2P Invitation response frame should carry the two more P2P attributes.
+			//	First one is operating channel attribute.
+			//	Second one is P2P Group BSSID attribute.
+
+			//	Operating Channel
+			//	Type:
+			p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
+
+			//	Length:
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
+			p2pielen += 2;
+
+			//	Value:
+			//	Country String
+			p2pie[ p2pielen++ ] = 'X';
+			p2pie[ p2pielen++ ] = 'X';
+		
+			//	The third byte should be set to 0x04.
+			//	Described in the "Operating Channel Attribute" section.
+			p2pie[ p2pielen++ ] = 0x04;
+
+			//	Operating Class
+			p2pie[ p2pielen++ ] = 0x51;	//	Copy from SD7
+		
+			//	Channel Number
+			p2pie[ p2pielen++ ] = pwdinfo->operating_channel;	//	operating channel number
+			
+
+			//	P2P Group BSSID
+			//	Type:
+			p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_BSSID;
+
+			//	Length:
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
+			p2pielen += 2;
+
+			//	Value:
+			//	P2P Device Address for GO
+			_rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
+			p2pielen += ETH_ALEN;
+
+		}
+
+		//	Channel List
+		//	Type:
+		p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
+
+		//	Length:
+		// Country String(3)
+		// + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?)
+		// + number of channels in all classes
+		len_channellist_attr = 3
+			+ (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+			+ get_reg_classes_full_count(pmlmeext->channel_list);
+
+#ifdef CONFIG_CONCURRENT_MODE
+		if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+		{
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
+		}
+		else
+		{
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
+		}
+#else
+
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
+
+#endif
+		p2pielen += 2;
+
+		//	Value:
+		//	Country String
+		p2pie[ p2pielen++ ] = 'X';
+		p2pie[ p2pielen++ ] = 'X';
+
+		//	The third byte should be set to 0x04.
+		//	Described in the "Operating Channel Attribute" section.
+		p2pie[ p2pielen++ ] = 0x04;
+
+		//	Channel Entry List
+#ifdef CONFIG_CONCURRENT_MODE
+		if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+		{
+			_adapter *pbuddy_adapter = padapter->pbuddy_adapter;	
+			struct mlme_ext_priv	*pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+
+			//	Operating Class
+			if ( pbuddy_mlmeext->cur_channel > 14 )
+			{
+				if ( pbuddy_mlmeext->cur_channel >= 149 )
+				{
+					p2pie[ p2pielen++ ] = 0x7c;
+				}
+				else
+				{
+					p2pie[ p2pielen++ ] = 0x73;
+				}
+			}
+			else
+			{
+				p2pie[ p2pielen++ ] = 0x51;
+			}
+
+			//	Number of Channels
+			//	Just support 1 channel and this channel is AP's channel
+			p2pie[ p2pielen++ ] = 1;
+
+			//	Channel List
+			p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
+		}
+		else
+		{
+			int i, j;
+			for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+				//	Operating Class
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+				//	Number of Channels
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+				//	Channel List
+				for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+					p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+				}
+			}
+		}
+#else // CONFIG_CONCURRENT_MODE
+		{
+			int i, j;
+			for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+				//	Operating Class
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+				//	Number of Channels
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+				//	Channel List
+				for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
+					p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+				}
+			}
+		}
+#endif // CONFIG_CONCURRENT_MODE
+	}
+		
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );	
+	
+#ifdef CONFIG_WFD
+	wfdielen = build_invitation_resp_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif //CONFIG_WFD
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+void issue_p2p_provision_request(_adapter *padapter, u8* pssid, u8 ussidlen, u8* pdev_raddr )
+{
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	u8			dialogToken = 1;
+	u32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_PROVISION_DISC_REQ;
+	u8			wpsie[ 100 ] = { 0x00 };
+	u8			wpsielen = 0;
+	u32			p2pielen = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD		
+	
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	DBG_871X( "[%s] In\n", __FUNCTION__ );
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, pdev_raddr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, pdev_raddr, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));	
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));		
+
+	p2pielen = build_prov_disc_request_p2p_ie( pwdinfo, pframe, pssid, ussidlen, pdev_raddr );
+
+	pframe += p2pielen;
+	pattrib->pktlen += p2pielen;
+
+	wpsielen = 0;
+	//	WPS OUI
+	*(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
+	wpsielen += 4;
+
+	//	WPS version
+	//	Type:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
+	wpsielen += 2;
+
+	//	Length:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
+	wpsielen += 2;
+
+	//	Value:
+	wpsie[wpsielen++] = WPS_VERSION_1;	//	Version 1.0
+
+	//	Config Method
+	//	Type:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
+	wpsielen += 2;
+
+	//	Length:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
+	wpsielen += 2;
+
+	//	Value:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->tx_prov_disc_info.wps_config_method_request );
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
+
+
+#ifdef CONFIG_WFD
+	wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif //CONFIG_WFD
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+
+u8 is_matched_in_profilelist( u8* peermacaddr, struct profile_info* profileinfo )
+{
+	u8 i, match_result = 0;
+
+	DBG_871X( "[%s] peermac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
+	  	    peermacaddr[0], peermacaddr[1],peermacaddr[2],peermacaddr[3],peermacaddr[4],peermacaddr[5]);
+	
+	for( i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++ )
+	{
+	       DBG_871X( "[%s] profileinfo_mac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
+		   	    profileinfo->peermac[0], profileinfo->peermac[1],profileinfo->peermac[2],profileinfo->peermac[3],profileinfo->peermac[4],profileinfo->peermac[5]);		   
+		if ( _rtw_memcmp( peermacaddr, profileinfo->peermac, ETH_ALEN ) )
+		{
+			match_result = 1;
+			DBG_871X( "[%s] Match!\n", __FUNCTION__ );
+			break;
+		}
+	}
+	
+	return (match_result );
+}
+
+void issue_probersp_p2p(_adapter *padapter, unsigned char *da)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;	
+	unsigned char					*mac;
+	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	//WLAN_BSSID_EX 		*cur_network = &(pmlmeinfo->network);
+	u16					beacon_interval = 100;
+	u16					capInfo = 0;
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8					wpsie[255] = { 0x00 };
+	u32					wpsielen = 0, p2pielen = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD
+#ifdef CONFIG_IOCTL_CFG80211
+	struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
+	struct ieee80211_channel *ieee_ch = &pcfg80211_wdinfo->remain_on_ch_channel;
+	u8 listen_channel = (u8) ieee80211_frequency_to_channel(ieee_ch->center_freq);
+#endif //CONFIG_IOCTL_CFG80211
+#ifdef CONFIG_INTEL_WIDI
+	u8 zero_array_check[L2SDTA_SERVICE_VE_LEN] = { 0x00 };
+#endif //CONFIG_INTEL_WIDI
+
+	//DBG_871X("%s\n", __FUNCTION__);
+	
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+	
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);	
+	
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+		
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;	
+	
+	mac = myid(&(padapter->eeprompriv));
+	
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+	_rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+	
+	//	Use the device address for BSSID field.	
+	_rtw_memcpy(pwlanhdr->addr3, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(fctrl, WIFI_PROBERSP);
+	
+	pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+	//timestamp will be inserted by hardware
+	pframe += 8;
+	pattrib->pktlen += 8;
+
+	// beacon interval: 2 bytes
+	_rtw_memcpy(pframe, (unsigned char *) &beacon_interval, 2); 
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	//	capability info: 2 bytes
+	//	ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of WiFi Direct Spec)
+	capInfo |= cap_ShortPremble;
+	capInfo |= cap_ShortSlot;
+	
+	_rtw_memcpy(pframe, (unsigned char *) &capInfo, 2);
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+
+	// SSID
+	pframe = rtw_set_ie(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid, &pattrib->pktlen);
+
+	// supported rates...
+	//	Use the OFDM rate in the P2P probe response frame. ( 6(B), 9(B), 12, 18, 24, 36, 48, 54 )
+	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
+
+	// DS parameter set
+#ifdef CONFIG_IOCTL_CFG80211
+	if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled && listen_channel !=0 )
+	{
+		pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&listen_channel, &pattrib->pktlen);
+	}
+	else
+#endif //CONFIG_IOCTL_CFG80211
+	{
+		pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&pwdinfo->listen_channel, &pattrib->pktlen);
+	}
+
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
+	{
+		if( pmlmepriv->wps_probe_resp_ie != NULL && pmlmepriv->p2p_probe_resp_ie != NULL )
+		{
+			//WPS IE
+			_rtw_memcpy(pframe, pmlmepriv->wps_probe_resp_ie, pmlmepriv->wps_probe_resp_ie_len);
+			pattrib->pktlen += pmlmepriv->wps_probe_resp_ie_len;
+			pframe += pmlmepriv->wps_probe_resp_ie_len;
+
+			//P2P IE
+			_rtw_memcpy(pframe, pmlmepriv->p2p_probe_resp_ie, pmlmepriv->p2p_probe_resp_ie_len);
+			pattrib->pktlen += pmlmepriv->p2p_probe_resp_ie_len;
+			pframe += pmlmepriv->p2p_probe_resp_ie_len;
+		}
+	}
+	else
+#endif //CONFIG_IOCTL_CFG80211		
+	{
+
+		//	Todo: WPS IE
+		//	Noted by Albert 20100907
+		//	According to the WPS specification, all the WPS attribute is presented by Big Endian.
+
+		wpsielen = 0;
+		//	WPS OUI
+		*(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
+		wpsielen += 4;
+
+		//	WPS version
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
+		wpsielen += 2;
+
+		//	Value:
+		wpsie[wpsielen++] = WPS_VERSION_1;	//	Version 1.0
+
+#ifdef CONFIG_INTEL_WIDI
+		//	Commented by Kurt
+		//	Appended WiDi info. only if we did issued_probereq_widi(), and then we saved ven. ext. in pmlmepriv->sa_ext.
+		if(  _rtw_memcmp(pmlmepriv->sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN) == _FALSE )
+		{
+			//Sec dev type
+			*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SEC_DEV_TYPE_LIST );
+			wpsielen += 2;
+
+			//	Length:
+			*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
+			wpsielen += 2;
+
+			//	Value:
+			//	Category ID
+			*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_DISPLAYS );
+			wpsielen += 2;
+
+			//	OUI
+			*(u32*) ( wpsie + wpsielen ) = cpu_to_be32( INTEL_DEV_TYPE_OUI );
+			wpsielen += 4;
+
+			*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_WIDI_CONSUMER_SINK );
+			wpsielen += 2;
+
+			//	Vendor Extension
+			_rtw_memcpy( wpsie + wpsielen, pmlmepriv->sa_ext, L2SDTA_SERVICE_VE_LEN );
+			wpsielen += L2SDTA_SERVICE_VE_LEN;
+		}
+#endif //CONFIG_INTEL_WIDI
+
+		//	WiFi Simple Config State
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SIMPLE_CONF_STATE );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
+		wpsielen += 2;
+
+		//	Value:
+		wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG;	//	Not Configured.
+
+		//	Response Type
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_RESP_TYPE );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
+		wpsielen += 2;
+
+		//	Value:
+		wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
+
+		//	UUID-E
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_UUID_E );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0010 );
+		wpsielen += 2;
+
+		//	Value:
+		_rtw_memcpy( wpsie + wpsielen, myid( &padapter->eeprompriv ), ETH_ALEN );
+		wpsielen += 0x10;
+
+		//	Manufacturer
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MANUFACTURER );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0007 );
+		wpsielen += 2;
+
+		//	Value:
+		_rtw_memcpy( wpsie + wpsielen, "Realtek", 7 );
+		wpsielen += 7;
+
+		//	Model Name
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MODEL_NAME );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0006 );
+		wpsielen += 2;	
+
+		//	Value:
+		_rtw_memcpy( wpsie + wpsielen, "8192CU", 6 );
+		wpsielen += 6;
+
+		//	Model Number
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MODEL_NUMBER );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
+		wpsielen += 2;
+
+		//	Value:
+		wpsie[ wpsielen++ ] = 0x31;		//	character 1
+
+		//	Serial Number
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SERIAL_NUMBER );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( ETH_ALEN );
+		wpsielen += 2;
+
+		//	Value:
+		_rtw_memcpy( wpsie + wpsielen, "123456" , ETH_ALEN );
+		wpsielen += ETH_ALEN;
+
+		//	Primary Device Type
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
+		wpsielen += 2;
+
+		//	Value:
+		//	Category ID
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
+		wpsielen += 2;
+
+		//	OUI
+		*(u32*) ( wpsie + wpsielen ) = cpu_to_be32( WPSOUI );
+		wpsielen += 4;
+
+		//	Sub Category ID
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
+		wpsielen += 2;
+
+		//	Device Name
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->device_name_len );
+		wpsielen += 2;
+
+		//	Value:
+		if (pwdinfo->device_name_len)
+		{
+			_rtw_memcpy( wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len );
+			wpsielen += pwdinfo->device_name_len;
+		}
+
+		//	Config Method
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
+		wpsielen += 2;
+
+		//	Value:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
+		wpsielen += 2;
+		
+
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
+		
+
+		p2pielen = build_probe_resp_p2p_ie(pwdinfo, pframe);
+		pframe += p2pielen;
+		pattrib->pktlen += p2pielen;
+	}
+
+#ifdef CONFIG_WFD
+#ifdef CONFIG_IOCTL_CFG80211
+	if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
+#endif //CONFIG_IOCTL_CFG80211
+	{
+		wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
+		pframe += wfdielen;
+		pattrib->pktlen += wfdielen;
+	}
+#ifdef CONFIG_IOCTL_CFG80211
+	else if (pmlmepriv->wfd_probe_resp_ie != NULL && pmlmepriv->wfd_probe_resp_ie_len>0)		
+	{
+		//WFD IE
+		_rtw_memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, pmlmepriv->wfd_probe_resp_ie_len);
+		pattrib->pktlen += pmlmepriv->wfd_probe_resp_ie_len;
+		pframe += pmlmepriv->wfd_probe_resp_ie_len;		
+	}
+#endif //CONFIG_IOCTL_CFG80211
+#endif //CONFIG_WFD	
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	
+
+	dump_mgntframe(padapter, pmgntframe);
+	
+	return;
+
+}
+
+int _issue_probereq_p2p(_adapter *padapter, u8 *da, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	unsigned char			*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short		*fctrl;
+	unsigned char			*mac;
+	unsigned char			bssrate[NumRates];
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	int	bssrate_len = 0;
+	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);	
+	u8					wpsie[255] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
+	u16					wpsielen = 0, p2pielen = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD
+ 
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+ 
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		goto exit;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	mac = myid(&(padapter->eeprompriv));
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	if (da) {
+		_rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+		_rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN);
+	} else {
+		if ( ( pwdinfo->p2p_info.scan_op_ch_only ) || ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) )
+		{
+			//	This two flags will be set when this is only the P2P client mode.
+			_rtw_memcpy(pwlanhdr->addr1, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
+			_rtw_memcpy(pwlanhdr->addr3, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
+		}
+		else
+		{
+			//	broadcast probe request frame
+			_rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+			_rtw_memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
+		}
+	}
+	_rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_PROBEREQ);
+
+	pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
+
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
+	{
+		pframe = rtw_set_ie(pframe, _SSID_IE_, pwdinfo->tx_prov_disc_info.ssid.SsidLength, pwdinfo->tx_prov_disc_info.ssid.Ssid, &(pattrib->pktlen));
+	}
+	else
+	{
+		pframe = rtw_set_ie(pframe, _SSID_IE_, P2P_WILDCARD_SSID_LEN, pwdinfo->p2p_wildcard_ssid, &(pattrib->pktlen));
+	}
+	//	Use the OFDM rate in the P2P probe request frame. ( 6(B), 9(B), 12(B), 24(B), 36, 48, 54 )
+	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
+	{
+		if( pmlmepriv->wps_probe_req_ie != NULL && pmlmepriv->p2p_probe_req_ie != NULL )
+		{
+			//WPS IE
+			_rtw_memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
+			pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
+			pframe += pmlmepriv->wps_probe_req_ie_len;
+
+			//P2P IE
+			_rtw_memcpy(pframe, pmlmepriv->p2p_probe_req_ie, pmlmepriv->p2p_probe_req_ie_len);
+			pattrib->pktlen += pmlmepriv->p2p_probe_req_ie_len;
+			pframe += pmlmepriv->p2p_probe_req_ie_len;
+		}
+	}
+	else
+#endif //CONFIG_IOCTL_CFG80211
+	{
+
+		//	WPS IE
+		//	Noted by Albert 20110221
+		//	According to the WPS specification, all the WPS attribute is presented by Big Endian.
+
+		wpsielen = 0;
+		//	WPS OUI
+		*(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
+		wpsielen += 4;
+
+		//	WPS version
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
+		wpsielen += 2;
+
+		//	Value:
+		wpsie[wpsielen++] = WPS_VERSION_1;	//	Version 1.0
+
+		if( pmlmepriv->wps_probe_req_ie == NULL )
+		{
+			//	UUID-E
+			//	Type:
+			*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_UUID_E );
+			wpsielen += 2;
+
+			//	Length:
+			*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0010 );
+			wpsielen += 2;
+
+			//	Value:
+			_rtw_memcpy( wpsie + wpsielen, myid( &padapter->eeprompriv ), ETH_ALEN );
+			wpsielen += 0x10;
+
+			//	Config Method
+			//	Type:
+			*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
+			wpsielen += 2;
+
+			//	Length:
+			*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
+			wpsielen += 2;
+
+			//	Value:
+			*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
+			wpsielen += 2;
+		}
+
+		//	Device Name
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->device_name_len );
+		wpsielen += 2;
+
+		//	Value:
+		_rtw_memcpy( wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len );
+		wpsielen += pwdinfo->device_name_len;
+
+		//	Primary Device Type
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
+		wpsielen += 2;
+
+		//	Value:
+		//	Category ID
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_RTK_WIDI );
+		wpsielen += 2;
+
+		//	OUI
+		*(u32*) ( wpsie + wpsielen ) = cpu_to_be32( WPSOUI );
+		wpsielen += 4;
+
+		//	Sub Category ID
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_RTK_DMP );
+		wpsielen += 2;
+
+		//	Device Password ID
+		//	Type:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
+		wpsielen += 2;
+
+		//	Length:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
+		wpsielen += 2;
+
+		//	Value:
+		*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );	//	Registrar-specified
+		wpsielen += 2;	
+
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
+		
+		//	P2P OUI
+		p2pielen = 0;
+		p2pie[ p2pielen++ ] = 0x50;
+		p2pie[ p2pielen++ ] = 0x6F;
+		p2pie[ p2pielen++ ] = 0x9A;
+		p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+		//	Commented by Albert 20110221
+		//	According to the P2P Specification, the probe request frame should contain 5 P2P attributes
+		//	1. P2P Capability
+		//	2. P2P Device ID if this probe request wants to find the specific P2P device
+		//	3. Listen Channel
+		//	4. Extended Listen Timing
+		//	5. Operating Channel if this WiFi is working as the group owner now
+
+		//	P2P Capability
+		//	Type:
+		p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
+
+		//	Length:
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
+		p2pielen += 2;
+
+		//	Value:
+		//	Device Capability Bitmap, 1 byte
+		p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
+		
+		//	Group Capability Bitmap, 1 byte
+		if ( pwdinfo->persistent_supported )
+			p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+		else
+			p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
+
+		//	Listen Channel
+		//	Type:
+		p2pie[ p2pielen++ ] = P2P_ATTR_LISTEN_CH;
+
+		//	Length:
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
+		p2pielen += 2;
+
+		//	Value:
+		//	Country String
+		p2pie[ p2pielen++ ] = 'X';
+		p2pie[ p2pielen++ ] = 'X';
+		
+		//	The third byte should be set to 0x04.
+		//	Described in the "Operating Channel Attribute" section.
+		p2pie[ p2pielen++ ] = 0x04;
+
+		//	Operating Class
+		p2pie[ p2pielen++ ] = 0x51;	//	Copy from SD7
+		
+		//	Channel Number
+		p2pie[ p2pielen++ ] = pwdinfo->listen_channel;	//	listen channel
+		
+
+		//	Extended Listen Timing
+		//	Type:
+		p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
+
+		//	Length:
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
+		p2pielen += 2;
+
+		//	Value:
+		//	Availability Period
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
+		p2pielen += 2;
+
+		//	Availability Interval
+		*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
+		p2pielen += 2;
+
+		if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
+		{
+			//	Operating Channel (if this WiFi is working as the group owner now)
+			//	Type:
+			p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
+
+			//	Length:
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
+			p2pielen += 2;
+
+			//	Value:
+			//	Country String
+			p2pie[ p2pielen++ ] = 'X';
+			p2pie[ p2pielen++ ] = 'X';
+		
+			//	The third byte should be set to 0x04.
+			//	Described in the "Operating Channel Attribute" section.
+			p2pie[ p2pielen++ ] = 0x04;
+
+			//	Operating Class
+			p2pie[ p2pielen++ ] = 0x51;	//	Copy from SD7
+		
+			//	Channel Number
+			p2pie[ p2pielen++ ] = pwdinfo->operating_channel;	//	operating channel number
+			
+		}	
+
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );	
+
+		if( pmlmepriv->wps_probe_req_ie != NULL )
+		{
+			//WPS IE
+			_rtw_memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
+			pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
+			pframe += pmlmepriv->wps_probe_req_ie_len;
+		}
+	}
+
+#ifdef CONFIG_WFD
+#ifdef CONFIG_IOCTL_CFG80211
+	if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
+#endif
+	{
+		wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe);
+		pframe += wfdielen;
+		pattrib->pktlen += wfdielen;
+	}
+#ifdef CONFIG_IOCTL_CFG80211
+	else if (pmlmepriv->wfd_probe_req_ie != NULL && pmlmepriv->wfd_probe_req_ie_len>0)		
+	{
+		//WFD IE
+		_rtw_memcpy(pframe, pmlmepriv->wfd_probe_req_ie, pmlmepriv->wfd_probe_req_ie_len);
+		pattrib->pktlen += pmlmepriv->wfd_probe_req_ie_len;
+		pframe += pmlmepriv->wfd_probe_req_ie_len;		
+	}
+#endif //CONFIG_IOCTL_CFG80211
+#endif //CONFIG_WFD
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+inline void issue_probereq_p2p(_adapter *adapter, u8 *da)
+{
+	_issue_probereq_p2p(adapter, da, _FALSE);
+}
+
+int issue_probereq_p2p_ex(_adapter *adapter, u8 *da, int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	u32 start = rtw_get_current_time();
+
+	do
+	{
+		ret = _issue_probereq_p2p(adapter, da, wait_ms>0?_TRUE:_FALSE);
+
+		i++;
+
+		if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+			break;
+
+		if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
+			rtw_msleep_os(wait_ms);
+
+	}while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(adapter), MAC_ARG(da), rtw_get_oper_ch(adapter),
+				ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(adapter), rtw_get_oper_ch(adapter),
+				ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+#endif //CONFIG_P2P
+
+s32 rtw_action_public_decache(union recv_frame *recv_frame, s32 token)
+{
+	_adapter *adapter = recv_frame->u.hdr.adapter;
+	struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
+	u8 *frame = recv_frame->u.hdr.rx_data;
+	u16 seq_ctrl = ( (recv_frame->u.hdr.attrib.seq_num&0xffff) << 4) |
+		(recv_frame->u.hdr.attrib.frag_num & 0xf);
+	
+	if (GetRetry(frame)) {
+		if (token >= 0) {
+			if ((seq_ctrl == mlmeext->action_public_rxseq)
+				&& (token == mlmeext->action_public_dialog_token))
+			{
+				DBG_871X(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x, token:%d\n",
+					FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
+				return _FAIL;
+			}
+		} else {
+			if (seq_ctrl == mlmeext->action_public_rxseq) {
+				DBG_871X(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x\n",
+					FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq);
+				return _FAIL;
+			}
+		}
+	}
+	
+	mlmeext->action_public_rxseq = seq_ctrl;
+	
+	if (token >= 0)
+		mlmeext->action_public_dialog_token = token;
+
+	return _SUCCESS;
+}
+
+unsigned int on_action_public_p2p(union recv_frame *precv_frame)
+{
+	_adapter *padapter = precv_frame->u.hdr.adapter;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	u8 *frame_body;
+	u8 dialogToken=0;
+#ifdef CONFIG_P2P
+	u8 *p2p_ie;
+	u32	p2p_ielen, wps_ielen;
+	struct	wifidirect_info	*pwdinfo = &( padapter->wdinfo );
+	u8	result = P2P_STATUS_SUCCESS;
+	u8	empty_addr[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+#endif //CONFIG_P2P
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	dialogToken = frame_body[7];
+
+	if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
+		return _FAIL;
+	
+#ifdef CONFIG_P2P
+	_cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
+#ifdef CONFIG_IOCTL_CFG80211
+	if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
+	{
+		rtw_cfg80211_rx_p2p_action_public(padapter, pframe, len);
+	}
+	else
+#endif //CONFIG_IOCTL_CFG80211
+	{
+		//	Do nothing if the driver doesn't enable the P2P function.
+		if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
+			return _SUCCESS;
+
+		len -= sizeof(struct rtw_ieee80211_hdr_3addr);
+
+		switch( frame_body[ 6 ] )//OUI Subtype
+		{
+			case P2P_GO_NEGO_REQ:
+			{
+				DBG_871X( "[%s] Got GO Nego Req Frame\n", __FUNCTION__);
+				_rtw_memset( &pwdinfo->groupid_info, 0x00, sizeof( struct group_id_info ) );
+
+				if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
+				{
+					rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+				}
+
+				if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
+				{
+					//	Commented by Albert 20110526
+					//	In this case, this means the previous nego fail doesn't be reset yet.
+					_cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
+					//	Restore the previous p2p state
+					rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+					DBG_871X( "[%s] Restore the previous p2p state to %d\n", __FUNCTION__, rtw_p2p_state(pwdinfo) );						
+				}					
+#ifdef CONFIG_CONCURRENT_MODE
+				if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+				{
+					_cancel_timer_ex( &pwdinfo->ap_p2p_switch_timer );
+				}
+#endif // CONFIG_CONCURRENT_MODE
+
+				//	Commented by Kurt 20110902
+				//Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered.
+				if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
+					rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+
+				//	Commented by Kurt 20120113
+				//	Get peer_dev_addr here if peer doesn't issue prov_disc frame.
+				if( _rtw_memcmp(pwdinfo->rx_prov_disc_info.peerDevAddr, empty_addr, ETH_ALEN) );
+					_rtw_memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
+
+				result = process_p2p_group_negotation_req( pwdinfo, frame_body, len );
+				issue_p2p_GO_response( padapter, GetAddr2Ptr(pframe), frame_body, len, result );
+#ifdef CONFIG_INTEL_WIDI
+				if( (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) && (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_WFD_CONNECTION) )
+				{
+					padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
+					intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL);
+				}
+#endif //CONFIG_INTEL_WIDI
+
+				//	Commented by Albert 20110718
+				//	No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer.
+#ifdef CONFIG_CONCURRENT_MODE
+				//	Commented by Albert 20120107
+				_set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
+#else // CONFIG_CONCURRENT_MODE
+				_set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
+#endif // CONFIG_CONCURRENT_MODE
+				break;					
+			}
+			case P2P_GO_NEGO_RESP:
+			{
+				DBG_871X( "[%s] Got GO Nego Resp Frame\n", __FUNCTION__);
+
+				if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
+				{
+					//	Commented by Albert 20110425
+					//	The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function.
+					_cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
+					pwdinfo->nego_req_info.benable = _FALSE;
+					result = process_p2p_group_negotation_resp( pwdinfo, frame_body, len);
+					issue_p2p_GO_confirm( pwdinfo->padapter, GetAddr2Ptr(pframe), result);
+					if ( P2P_STATUS_SUCCESS == result )
+					{
+						if ( rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT )
+						{
+							pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
+							pwdinfo->p2p_info.scan_op_ch_only = 1;
+							_set_timer( &pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH );
+						}
+					}
+
+					//	Reset the dialog token for group negotiation frames.
+					pwdinfo->negotiation_dialog_token = 1;
+
+					if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
+					{
+						_set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
+					}
+				}
+				else
+				{
+					DBG_871X( "[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __FUNCTION__);
+				}
+				
+				break;
+			}
+			case P2P_GO_NEGO_CONF:
+			{
+				DBG_871X( "[%s] Got GO Nego Confirm Frame\n", __FUNCTION__);
+				result = process_p2p_group_negotation_confirm( pwdinfo, frame_body, len);
+				if ( P2P_STATUS_SUCCESS == result )
+				{
+					if ( rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT )
+					{
+						pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
+						pwdinfo->p2p_info.scan_op_ch_only = 1;
+						_set_timer( &pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH );
+					}
+				}
+				break;
+			}
+			case P2P_INVIT_REQ:
+			{
+				//	Added by Albert 2010/10/05
+				//	Received the P2P Invite Request frame.
+				
+				DBG_871X( "[%s] Got invite request frame!\n", __FUNCTION__ );
+				if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
+				{
+					//	Parse the necessary information from the P2P Invitation Request frame.
+					//	For example: The MAC address of sending this P2P Invitation Request frame.
+					u32	attr_contentlen = 0;
+					u8	status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+					struct group_id_info group_id;
+					u8	invitation_flag = 0;
+
+					rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen);
+					if ( attr_contentlen )
+					{
+
+						rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen);
+						//	Commented by Albert 20120510
+						//	Copy to the pwdinfo->p2p_peer_interface_addr.
+						//	So that the WFD UI ( or Sigma ) can get the peer interface address by using the following command.
+						//	#> iwpriv wlan0 p2p_get peer_ifa
+						//	After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant.
+
+						if ( attr_contentlen )
+						{
+							DBG_871X( "[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
+									pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1],
+									pwdinfo->p2p_peer_interface_addr[2], pwdinfo->p2p_peer_interface_addr[3],
+									pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5] );
+						}								
+
+						if ( invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT )
+						{
+							//	Re-invoke the persistent group.
+							
+							_rtw_memset( &group_id, 0x00, sizeof( struct group_id_info ) );
+							rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, ( u8* ) &group_id, &attr_contentlen);
+							if ( attr_contentlen )
+							{
+								if ( _rtw_memcmp( group_id.go_device_addr, myid( &padapter->eeprompriv ), ETH_ALEN ) )
+								{
+									//	The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO.
+									rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO );
+									rtw_p2p_set_role( pwdinfo, P2P_ROLE_GO );
+									status_code = P2P_STATUS_SUCCESS;
+								}
+								else
+								{
+									//	The p2p device sending this p2p invitation request wants to be the persistent GO.
+									if ( is_matched_in_profilelist( pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[ 0 ] ) )
+									{
+										u8 operatingch_info[5] = { 0x00 };
+										if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen) )
+										{
+											if( rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, (u32)operatingch_info[4] ) )
+											{
+												//	The operating channel is acceptable for this device.
+												pwdinfo->rx_invitereq_info.operation_ch[0]= operatingch_info[4];
+												pwdinfo->rx_invitereq_info.scan_op_ch_only = 1;
+												_set_timer( &pwdinfo->reset_ch_sitesurvey, P2P_RESET_SCAN_CH );
+												rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH );
+												rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
+												status_code = P2P_STATUS_SUCCESS;
+												}
+											else
+											{
+												//	The operating channel isn't supported by this device.
+												rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
+												rtw_p2p_set_role( pwdinfo, P2P_ROLE_DEVICE );
+												status_code = P2P_STATUS_FAIL_NO_COMMON_CH;
+												_set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
+											}
+										}
+										else
+										{
+											//	Commented by Albert 20121130
+											//	Intel will use the different P2P IE to store the operating channel information
+											//	Workaround for Intel WiDi 3.5
+											rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH );
+											rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
+											status_code = P2P_STATUS_SUCCESS;
+										}								
+									}
+									else
+									{
+										rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
+										#ifdef CONFIG_INTEL_WIDI
+										_rtw_memcpy( pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN );
+										rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
+										#endif //CONFIG_INTEL_WIDI
+
+										status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
+									}
+								}
+							}
+							else
+							{
+								DBG_871X( "[%s] P2P Group ID Attribute NOT FOUND!\n", __FUNCTION__ );
+								status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+							}									
+						}
+						else
+						{
+							//	Received the invitation to join a P2P group.
+
+							_rtw_memset( &group_id, 0x00, sizeof( struct group_id_info ) );
+							rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, ( u8* ) &group_id, &attr_contentlen);
+							if ( attr_contentlen )
+							{
+								if ( _rtw_memcmp( group_id.go_device_addr, myid( &padapter->eeprompriv ), ETH_ALEN ) )
+								{
+									//	In this case, the GO can't be myself.
+									rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
+									status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+								}
+								else
+								{
+									//	The p2p device sending this p2p invitation request wants to join an existing P2P group
+									//	Commented by Albert 2012/06/28
+									//	In this case, this Wi-Fi device should use the iwpriv command to get the peer device address.
+									//	The peer device address should be the destination address for the provisioning discovery request.
+									//	Then, this Wi-Fi device should use the iwpriv command to get the peer interface address.
+									//	The peer interface address should be the address for WPS mac address
+									_rtw_memcpy( pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN );											
+									rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
+									rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN );
+									status_code = P2P_STATUS_SUCCESS;
+								}
+							}
+							else
+							{
+								DBG_871X( "[%s] P2P Group ID Attribute NOT FOUND!\n", __FUNCTION__ );
+								status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+							}
+						}
+					}
+					else
+					{
+						DBG_871X( "[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __FUNCTION__ );
+						status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+					}							
+
+					DBG_871X( "[%s] status_code = %d\n", __FUNCTION__, status_code );
+
+					pwdinfo->inviteresp_info.token = frame_body[ 7 ];
+					issue_p2p_invitation_response( padapter, GetAddr2Ptr(pframe), pwdinfo->inviteresp_info.token, status_code );
+				}
+#ifdef CONFIG_INTEL_WIDI
+				if( (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) && (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_WFD_CONNECTION) )
+				{
+					padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
+					intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL);
+				}
+#endif //CONFIG_INTEL_WIDI
+				break;
+			}
+			case P2P_INVIT_RESP:
+			{
+				u8	attr_content = 0x00;
+				u32	attr_contentlen = 0;
+				
+				DBG_871X( "[%s] Got invite response frame!\n", __FUNCTION__ );
+				_cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
+				if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
+				{
+					rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+									
+					if ( attr_contentlen == 1 )
+					{
+						DBG_871X( "[%s] Status = %d\n", __FUNCTION__, attr_content );
+						pwdinfo->invitereq_info.benable = _FALSE;
+
+						if ( attr_content == P2P_STATUS_SUCCESS )
+						{
+							if ( _rtw_memcmp( pwdinfo->invitereq_info.go_bssid, myid( &padapter->eeprompriv ), ETH_ALEN ))
+							{
+								rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO );
+							}
+							else
+							{
+								rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+							}
+							rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_OK );
+						}
+						else
+						{
+							rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+							rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
+						}
+					}
+					else
+					{
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+						rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
+					}
+				}
+				else
+				{
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+					rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
+				}
+
+				if ( rtw_p2p_chk_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL ) )
+				{
+					_set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
+				}
+				break;
+			}
+			case P2P_DEVDISC_REQ:
+
+				process_p2p_devdisc_req(pwdinfo, pframe, len);
+
+				break;
+
+			case P2P_DEVDISC_RESP:
+
+				process_p2p_devdisc_resp(pwdinfo, pframe, len);
+
+				break;
+
+			case P2P_PROVISION_DISC_REQ:
+				DBG_871X( "[%s] Got Provisioning Discovery Request Frame\n", __FUNCTION__ );
+				process_p2p_provdisc_req(pwdinfo, pframe, len);
+				_rtw_memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
+
+				//20110902 Kurt
+				//Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered.
+				if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
+					rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+				
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ);
+				_set_timer( &pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT );
+#ifdef CONFIG_INTEL_WIDI
+				if( (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) && (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_WFD_CONNECTION) )
+				{
+					padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
+					intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL);
+				}
+#endif //CONFIG_INTEL_WIDI
+				break;
+
+			case P2P_PROVISION_DISC_RESP:
+				//	Commented by Albert 20110707
+				//	Should we check the pwdinfo->tx_prov_disc_info.bsent flag here??
+				DBG_871X( "[%s] Got Provisioning Discovery Response Frame\n", __FUNCTION__ );
+				//	Commented by Albert 20110426
+				//	The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function.
+				_cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP);
+				process_p2p_provdisc_resp(pwdinfo, pframe);
+				_set_timer( &pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT );
+				break;
+
+		}
+	}
+#endif //CONFIG_P2P
+	
+	return _SUCCESS;
+}
+
+unsigned int on_action_public_vendor(union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint frame_len = precv_frame->u.hdr.len;
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if (_rtw_memcmp(frame_body + 2, P2P_OUI, 4) == _TRUE) {
+		ret = on_action_public_p2p(precv_frame);
+	}
+
+	return ret;
+}
+
+unsigned int on_action_public_default(union recv_frame *precv_frame, u8 action)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint frame_len = precv_frame->u.hdr.len;
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 token;
+	_adapter *adapter = precv_frame->u.hdr.adapter;
+	int cnt = 0;
+	char msg[64];
+
+	token = frame_body[2];
+
+	if (rtw_action_public_decache(precv_frame, token) == _FAIL)
+		goto exit;
+
+	#ifdef CONFIG_IOCTL_CFG80211
+	cnt += sprintf((msg+cnt), "%s(token:%u)", action_public_str(action), token);
+	rtw_cfg80211_rx_action(adapter, pframe, frame_len, msg);
+	#endif
+
+	ret = _SUCCESS;
+	
+exit:
+	return ret;
+}
+
+unsigned int on_action_public(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint frame_len = precv_frame->u.hdr.len;
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 category, action;
+
+	/* check RA matches or not */
+	if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
+		goto exit;
+
+	category = frame_body[0];
+	if(category != RTW_WLAN_CATEGORY_PUBLIC)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case ACT_PUBLIC_VENDOR:
+		ret = on_action_public_vendor(precv_frame);
+		break;
+	default:
+		ret = on_action_public_default(precv_frame, action);
+		break;
+	}
+
+exit:
+	return ret;
+}
+
+unsigned int OnAction_ht(_adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int OnAction_wmm(_adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int OnAction_p2p(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_P2P
+	u8 *frame_body;
+	u8 category, OUI_Subtype, dialogToken=0;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	struct	wifidirect_info	*pwdinfo = &( padapter->wdinfo );
+	
+
+	DBG_871X("%s\n", __FUNCTION__);
+	
+	//check RA matches or not
+	if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))//for if1, sta/ap mode
+		return _SUCCESS;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+	if(category != RTW_WLAN_CATEGORY_P2P)
+		return _SUCCESS;
+
+	if ( cpu_to_be32( *( ( u32* ) ( frame_body + 1 ) ) ) != P2POUI )
+		return _SUCCESS;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
+	{
+		rtw_cfg80211_rx_action_p2p(padapter, pframe, len);
+		return _SUCCESS;
+	}
+	else
+#endif //CONFIG_IOCTL_CFG80211
+	{
+		len -= sizeof(struct rtw_ieee80211_hdr_3addr);
+		OUI_Subtype = frame_body[5];
+		dialogToken = frame_body[6];
+
+		switch(OUI_Subtype)
+		{
+			case P2P_NOTICE_OF_ABSENCE:
+				
+				break;
+				
+			case P2P_PRESENCE_REQUEST:
+
+				process_p2p_presence_req(pwdinfo, pframe, len);			
+				
+				break;
+				
+			case P2P_PRESENCE_RESPONSE:
+				
+				break;
+				
+			case P2P_GO_DISC_REQUEST:
+				
+				break;
+				
+			default:
+				break;
+				
+		}
+	}
+#endif //CONFIG_P2P
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnAction(_adapter *padapter, union recv_frame *precv_frame)
+{
+	int i;
+	unsigned char	category;
+	struct action_handler *ptable;
+	unsigned char	*frame_body;
+	u8 *pframe = precv_frame->u.hdr.rx_data; 
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+	
+	category = frame_body[0];
+	
+	for(i = 0; i < sizeof(OnAction_tbl)/sizeof(struct action_handler); i++)	
+	{
+		ptable = &OnAction_tbl[i];
+		
+		if(category == ptable->num)
+			ptable->func(padapter, precv_frame);
+	
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int DoReserved(_adapter *padapter, union recv_frame *precv_frame)
+{
+
+	//DBG_871X("rcvd mgt frame(%x, %x)\n", (GetFrameSubType(pframe) >> 4), *(unsigned int *)GetAddr1Ptr(pframe));
+	return _SUCCESS;
+}
+
+struct xmit_frame *_alloc_mgtxmitframe(struct xmit_priv *pxmitpriv, bool once)
+{
+	struct xmit_frame *pmgntframe;
+	struct xmit_buf *pxmitbuf;
+
+	if (once)
+		pmgntframe = rtw_alloc_xmitframe_once(pxmitpriv);
+	else
+		pmgntframe = rtw_alloc_xmitframe_ext(pxmitpriv);
+
+	if (pmgntframe == NULL) {
+		DBG_871X(FUNC_ADPT_FMT" alloc xmitframe fail, once:%d\n", FUNC_ADPT_ARG(pxmitpriv->adapter), once);
+		goto exit;
+	}
+
+	if ((pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv)) == NULL) {
+		DBG_871X(FUNC_ADPT_FMT" alloc xmitbuf fail\n", FUNC_ADPT_ARG(pxmitpriv->adapter));
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		pmgntframe = NULL;
+		goto exit;
+	}
+
+	pmgntframe->frame_tag = MGNT_FRAMETAG;
+	pmgntframe->pxmitbuf = pxmitbuf;
+	pmgntframe->buf_addr = pxmitbuf->pbuf;
+	pxmitbuf->priv_data = pmgntframe;
+
+exit:
+	return pmgntframe;
+
+}
+
+inline struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
+{
+	return _alloc_mgtxmitframe(pxmitpriv, _FALSE);
+}
+
+inline struct xmit_frame *alloc_mgtxmitframe_once(struct xmit_priv *pxmitpriv)
+{
+	return _alloc_mgtxmitframe(pxmitpriv, _TRUE);
+}
+
+
+/****************************************************************************
+
+Following are some TX fuctions for WiFi MLME
+
+*****************************************************************************/
+
+void update_mgnt_tx_rate(_adapter *padapter, u8 rate)
+{
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+
+	pmlmeext->tx_rate = rate;
+	DBG_871X("%s(): rate = %x\n",__FUNCTION__, rate);
+}
+
+void update_mgntframe_attrib(_adapter *padapter, struct pkt_attrib *pattrib)
+{
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+
+	_rtw_memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib));
+
+	pattrib->hdrlen = 24;
+	pattrib->nr_frags = 1;
+	pattrib->priority = 7;
+	pattrib->mac_id = 0;
+	pattrib->qsel = 0x12;
+
+	pattrib->pktlen = 0;
+
+	if(pmlmeext->cur_wireless_mode & WIRELESS_11B)
+		pattrib->raid = 6;//b mode
+	else
+		pattrib->raid = 5;//a/g mode
+
+	pattrib->encrypt = _NO_PRIVACY_;
+	pattrib->bswenc = _FALSE;	
+
+	pattrib->qos_en = _FALSE;
+	pattrib->ht_en = _FALSE;
+	pattrib->bwmode = HT_CHANNEL_WIDTH_20;
+	pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	pattrib->sgi = _FALSE;
+
+	pattrib->seqnum = pmlmeext->mgnt_seq;
+
+	pattrib->retry_ctrl = _TRUE;
+
+}
+
+void dump_mgntframe(_adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	if(padapter->bSurpriseRemoved == _TRUE ||
+		padapter->bDriverStopped == _TRUE)
+		return;
+
+	rtw_hal_mgnt_xmit(padapter, pmgntframe);
+}
+
+s32 dump_mgntframe_and_wait(_adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
+{
+	s32 ret = _FAIL;
+	_irqL irqL;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;	
+	struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
+	struct submit_ctx sctx;
+
+	if(padapter->bSurpriseRemoved == _TRUE ||
+		padapter->bDriverStopped == _TRUE)
+		return ret;
+
+	rtw_sctx_init(&sctx, timeout_ms);
+	pxmitbuf->sctx = &sctx;
+
+	ret = rtw_hal_mgnt_xmit(padapter, pmgntframe);
+
+	if (ret == _SUCCESS)
+		ret = rtw_sctx_wait(&sctx);
+
+	_enter_critical(&pxmitpriv->lock_sctx, &irqL);
+	pxmitbuf->sctx = NULL;
+	_exit_critical(&pxmitpriv->lock_sctx, &irqL);
+
+	 return ret;
+}
+
+s32 dump_mgntframe_and_wait_ack(_adapter *padapter, struct xmit_frame *pmgntframe)
+{
+#ifdef CONFIG_XMIT_ACK
+	s32 ret = _FAIL;
+	u32 timeout_ms = 500;//  500ms
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	#ifdef CONFIG_CONCURRENT_MODE
+	if (padapter->pbuddy_adapter && !padapter->isprimary)
+		pxmitpriv = &(padapter->pbuddy_adapter->xmitpriv);
+	#endif
+
+	if(padapter->bSurpriseRemoved == _TRUE ||
+		padapter->bDriverStopped == _TRUE)
+		return -1;
+
+	_enter_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
+	pxmitpriv->ack_tx = _TRUE;
+
+	pmgntframe->ack_report = 1;
+	if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS) {
+		ret = rtw_ack_tx_wait(pxmitpriv, timeout_ms);
+	}
+
+	pxmitpriv->ack_tx = _FALSE;
+	_exit_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
+
+	 return ret;
+#else //!CONFIG_XMIT_ACK
+	dump_mgntframe(padapter, pmgntframe);
+	rtw_msleep_os(50);
+	return _SUCCESS;
+#endif //!CONFIG_XMIT_ACK	 
+}
+
+int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
+{
+	u8 *ssid_ie;
+	sint ssid_len_ori;
+	int len_diff = 0;
+	
+	ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
+
+	//DBG_871X("%s hidden_ssid_mode:%u, ssid_ie:%p, ssid_len_ori:%d\n", __FUNCTION__, hidden_ssid_mode, ssid_ie, ssid_len_ori);
+	
+	if(ssid_ie && ssid_len_ori>0)
+	{
+		switch(hidden_ssid_mode)
+		{
+			case 1:
+			{
+				u8 *next_ie = ssid_ie + 2 + ssid_len_ori;
+				u32 remain_len = 0;
+				
+				remain_len = ies_len -(next_ie-ies);
+				
+				ssid_ie[1] = 0;				
+				_rtw_memcpy(ssid_ie+2, next_ie, remain_len);
+				len_diff -= ssid_len_ori;
+				
+				break;
+			}		
+			case 2:
+				_rtw_memset(&ssid_ie[2], 0, ssid_len_ori);
+				break;
+			default:
+				break;
+		}
+	}
+
+	return len_diff;
+}
+
+void issue_beacon(_adapter *padapter, int timeout_ms)
+{
+	struct xmit_frame	*pmgntframe;
+	struct pkt_attrib	*pattrib;
+	unsigned char	*pframe;
+	struct rtw_ieee80211_hdr *pwlanhdr;
+	unsigned short *fctrl;
+	unsigned int	rate_len;
+	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
+#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	_irqL irqL;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+#endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX 		*cur_network = &(pmlmeinfo->network);
+	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#endif //CONFIG_P2P
+
+
+	//DBG_871X("%s\n", __FUNCTION__);
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		DBG_871X("%s, alloc mgnt frame fail\n", __FUNCTION__);
+		return;
+	}
+#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	_enter_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
+#endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->qsel = 0x10;
+	
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+		
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;	
+	
+	
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+	
+	_rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
+	//pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_BEACON);
+	
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);	
+	pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
+	
+	if( (pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+	{
+		//DBG_871X("ie len=%d\n", cur_network->IELength);
+#ifdef CONFIG_P2P
+		// for P2P : Primary Device Type & Device Name
+		u32 wpsielen=0, insert_len=0;
+		u8 *wpsie=NULL;		
+		wpsie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wpsielen);
+		
+		if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wpsie && wpsielen>0)
+		{
+			uint wps_offset, remainder_ielen;
+			u8 *premainder_ie, *pframe_wscie;
+	
+			wps_offset = (uint)(wpsie - cur_network->IEs);
+
+			premainder_ie = wpsie + wpsielen;
+
+			remainder_ielen = cur_network->IELength - wps_offset - wpsielen;
+
+#ifdef CONFIG_IOCTL_CFG80211
+			if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
+			{
+				if(pmlmepriv->wps_beacon_ie && pmlmepriv->wps_beacon_ie_len>0)
+				{
+					_rtw_memcpy(pframe, cur_network->IEs, wps_offset);
+					pframe += wps_offset;
+					pattrib->pktlen += wps_offset;
+
+					_rtw_memcpy(pframe, pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len);
+					pframe += pmlmepriv->wps_beacon_ie_len;
+					pattrib->pktlen += pmlmepriv->wps_beacon_ie_len;
+
+					//copy remainder_ie to pframe
+					_rtw_memcpy(pframe, premainder_ie, remainder_ielen);
+					pframe += remainder_ielen;		
+					pattrib->pktlen += remainder_ielen;
+				}
+				else
+				{
+					_rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
+					pframe += cur_network->IELength;
+					pattrib->pktlen += cur_network->IELength;
+				}
+			}
+			else
+#endif //CONFIG_IOCTL_CFG80211
+			{
+				pframe_wscie = pframe + wps_offset;
+				_rtw_memcpy(pframe, cur_network->IEs, wps_offset+wpsielen);			
+				pframe += (wps_offset + wpsielen);		
+				pattrib->pktlen += (wps_offset + wpsielen);
+
+				//now pframe is end of wsc ie, insert Primary Device Type & Device Name
+				//	Primary Device Type
+				//	Type:
+				*(u16*) ( pframe + insert_len) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
+				insert_len += 2;
+				
+				//	Length:
+				*(u16*) ( pframe + insert_len ) = cpu_to_be16( 0x0008 );
+				insert_len += 2;
+				
+				//	Value:
+				//	Category ID
+				*(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
+				insert_len += 2;
+
+				//	OUI
+				*(u32*) ( pframe + insert_len ) = cpu_to_be32( WPSOUI );
+				insert_len += 4;
+
+				//	Sub Category ID
+				*(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
+				insert_len += 2;
+
+
+				//	Device Name
+				//	Type:
+				*(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
+				insert_len += 2;
+
+				//	Length:
+				*(u16*) ( pframe + insert_len ) = cpu_to_be16( pwdinfo->device_name_len );
+				insert_len += 2;
+
+				//	Value:
+				_rtw_memcpy( pframe + insert_len, pwdinfo->device_name, pwdinfo->device_name_len );
+				insert_len += pwdinfo->device_name_len;
+
+
+				//update wsc ie length
+				*(pframe_wscie+1) = (wpsielen -2) + insert_len;
+
+				//pframe move to end
+				pframe+=insert_len;
+				pattrib->pktlen += insert_len;
+
+				//copy remainder_ie to pframe
+				_rtw_memcpy(pframe, premainder_ie, remainder_ielen);
+				pframe += remainder_ielen;		
+				pattrib->pktlen += remainder_ielen;
+			}
+		}
+		else
+#endif //CONFIG_P2P
+		{
+			int len_diff;
+			_rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
+			len_diff = update_hidden_ssid(
+				pframe+_BEACON_IE_OFFSET_
+				, cur_network->IELength-_BEACON_IE_OFFSET_
+				, pmlmeinfo->hidden_ssid_mode
+			);
+			pframe += (cur_network->IELength+len_diff);
+			pattrib->pktlen += (cur_network->IELength+len_diff);
+		}
+
+		{
+			u8 *wps_ie;
+			uint wps_ielen;
+			u8 sr = 0;
+			wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr+TXDESC_OFFSET+sizeof (struct rtw_ieee80211_hdr_3addr)+_BEACON_IE_OFFSET_,
+				pattrib->pktlen-sizeof (struct rtw_ieee80211_hdr_3addr)-_BEACON_IE_OFFSET_, NULL, &wps_ielen);
+			if (wps_ie && wps_ielen>0) {
+				rtw_get_wps_attr_content(wps_ie,  wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8*)(&sr), NULL);
+			}
+			if (sr != 0)
+				set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
+			else
+				_clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
+		}
+
+#ifdef CONFIG_P2P
+		if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+		{
+			u32 len;
+#ifdef CONFIG_IOCTL_CFG80211
+			if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
+			{
+				len = pmlmepriv->p2p_beacon_ie_len;
+				if(pmlmepriv->p2p_beacon_ie && len>0)				
+					_rtw_memcpy(pframe, pmlmepriv->p2p_beacon_ie, len);
+			}
+			else
+#endif //CONFIG_IOCTL_CFG80211
+			{
+				len = build_beacon_p2p_ie(pwdinfo, pframe);
+			}
+
+			pframe += len;
+			pattrib->pktlen += len;
+#ifdef CONFIG_WFD
+#ifdef CONFIG_IOCTL_CFG80211
+			if(_TRUE == pwdinfo->wfd_info->wfd_enable)
+#endif //CONFIG_IOCTL_CFG80211
+			{
+				len = build_beacon_wfd_ie( pwdinfo, pframe );
+			}
+#ifdef CONFIG_IOCTL_CFG80211
+			else
+			{	
+				len = 0;
+				if(pmlmepriv->wfd_beacon_ie && pmlmepriv->wfd_beacon_ie_len>0)
+				{
+					len = pmlmepriv->wfd_beacon_ie_len;
+					_rtw_memcpy(pframe, pmlmepriv->wfd_beacon_ie, len);	
+			}
+			}		
+#endif //CONFIG_IOCTL_CFG80211
+			pframe += len;
+			pattrib->pktlen += len;
+#endif //CONFIG_WFD
+		}
+#endif //CONFIG_P2P
+
+		goto _issue_bcn;
+
+	}
+
+	//below for ad-hoc mode
+
+	//timestamp will be inserted by hardware
+	pframe += 8;
+	pattrib->pktlen += 8;
+
+	// beacon interval: 2 bytes
+
+	_rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2); 
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	// capability info: 2 bytes
+
+	_rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	// SSID
+	pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
+
+	// supported rates...
+	rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
+	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8)? 8: rate_len), cur_network->SupportedRates, &pattrib->pktlen);
+
+	// DS parameter set
+	pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
+
+	//if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
+	{
+		u8 erpinfo=0;
+		u32 ATIMWindow;
+		// IBSS Parameter Set...
+		//ATIMWindow = cur->Configuration.ATIMWindow;
+		ATIMWindow = 0;
+		pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
+
+		//ERP IE
+		pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
+	}	
+
+
+	// EXTERNDED SUPPORTED RATE
+	if (rate_len > 8)
+	{
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
+	}
+
+
+	//todo:HT for adhoc
+
+_issue_bcn:
+
+#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	pmlmepriv->update_bcn = _FALSE;
+	
+	_exit_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);	
+#endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+
+	if ((pattrib->pktlen + TXDESC_SIZE) > 512)
+	{
+		DBG_871X("beacon frame too large\n");
+		return;
+	}
+	
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	//DBG_871X("issue bcn_sz=%d\n", pattrib->last_txcmdsz);
+	if(timeout_ms > 0)
+		dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
+	else
+		dump_mgntframe(padapter, pmgntframe);
+
+}
+
+void issue_probersp(_adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;	
+	unsigned char					*mac, *bssid;
+	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
+#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	u8 *pwps_ie;
+	uint wps_ielen;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+#endif //#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX 		*cur_network = &(pmlmeinfo->network);
+	unsigned int	rate_len;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD
+#endif //CONFIG_P2P
+
+	//DBG_871X("%s\n", __FUNCTION__);
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		DBG_871X("%s, alloc mgnt frame fail\n", __FUNCTION__);
+		return;
+	}
+
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);	
+	
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+		
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;	
+	
+	mac = myid(&(padapter->eeprompriv));
+	bssid = cur_network->MacAddress;
+	
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+	_rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(fctrl, WIFI_PROBERSP);
+	
+	pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+
+	if(cur_network->IELength>MAX_IE_SZ)
+		return;
+	
+#if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	if( (pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+	{
+		pwps_ie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
+	
+		//inerset & update wps_probe_resp_ie
+		if((pmlmepriv->wps_probe_resp_ie!=NULL) && pwps_ie && (wps_ielen>0))
+		{
+			uint wps_offset, remainder_ielen;
+			u8 *premainder_ie;		
+	
+			wps_offset = (uint)(pwps_ie - cur_network->IEs);
+
+			premainder_ie = pwps_ie + wps_ielen;
+
+			remainder_ielen = cur_network->IELength - wps_offset - wps_ielen;
+
+			_rtw_memcpy(pframe, cur_network->IEs, wps_offset);		
+			pframe += wps_offset;		
+			pattrib->pktlen += wps_offset;		
+
+			wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];//to get ie data len
+			if((wps_offset+wps_ielen+2)<=MAX_IE_SZ)
+			{
+				_rtw_memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen+2);
+				pframe += wps_ielen+2;		
+				pattrib->pktlen += wps_ielen+2;	
+			}
+
+			if((wps_offset+wps_ielen+2+remainder_ielen)<=MAX_IE_SZ)
+			{
+				_rtw_memcpy(pframe, premainder_ie, remainder_ielen);
+				pframe += remainder_ielen;		
+				pattrib->pktlen += remainder_ielen;	
+			}
+		}
+		else
+		{
+			_rtw_memcpy(pframe, cur_network->IEs, cur_network->IELength);
+			pframe += cur_network->IELength;
+			pattrib->pktlen += cur_network->IELength;
+		}
+
+		/* retrieve SSID IE from cur_network->Ssid */
+		{
+			u8 *ssid_ie;
+			sint ssid_ielen;
+			sint ssid_ielen_diff;
+			u8 buf[MAX_IE_SZ];
+			u8 *ies = pmgntframe->buf_addr+TXDESC_OFFSET+sizeof(struct rtw_ieee80211_hdr_3addr);
+
+			ssid_ie = rtw_get_ie(ies+_FIXED_IE_LENGTH_, _SSID_IE_, &ssid_ielen,
+				(pframe-ies)-_FIXED_IE_LENGTH_);
+
+			ssid_ielen_diff = cur_network->Ssid.SsidLength - ssid_ielen;
+
+			if (ssid_ie &&  cur_network->Ssid.SsidLength) {
+				uint remainder_ielen;
+				u8 *remainder_ie;
+				remainder_ie = ssid_ie+2;
+				remainder_ielen = (pframe-remainder_ie);
+
+				DBG_871X_LEVEL(_drv_warning_, FUNC_ADPT_FMT" remainder_ielen > MAX_IE_SZ\n", FUNC_ADPT_ARG(padapter));
+				if (remainder_ielen > MAX_IE_SZ) {
+					remainder_ielen = MAX_IE_SZ;
+				}
+
+				_rtw_memcpy(buf, remainder_ie, remainder_ielen);
+				_rtw_memcpy(remainder_ie+ssid_ielen_diff, buf, remainder_ielen);
+				*(ssid_ie+1) = cur_network->Ssid.SsidLength;
+				_rtw_memcpy(ssid_ie+2, cur_network->Ssid.Ssid, cur_network->Ssid.SsidLength);
+
+				pframe += ssid_ielen_diff;
+				pattrib->pktlen += ssid_ielen_diff;
+			}
+		}
+	}	
+	else		
+#endif		
+	{
+	
+		//timestamp will be inserted by hardware
+		pframe += 8;
+		pattrib->pktlen += 8;
+
+		// beacon interval: 2 bytes
+
+		_rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2); 
+
+		pframe += 2;
+		pattrib->pktlen += 2;
+
+		// capability info: 2 bytes
+
+		_rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
+
+		pframe += 2;
+		pattrib->pktlen += 2;
+
+		//below for ad-hoc mode
+
+		// SSID
+		pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
+
+		// supported rates...
+		rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8)? 8: rate_len), cur_network->SupportedRates, &pattrib->pktlen);
+
+		// DS parameter set
+		pframe =rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
+
+		if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
+		{
+			u8 erpinfo=0;
+			u32 ATIMWindow;
+			// IBSS Parameter Set...
+			//ATIMWindow = cur->Configuration.ATIMWindow;
+			ATIMWindow = 0;
+			pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
+
+			//ERP IE
+			pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
+		}
+
+		
+		// EXTERNDED SUPPORTED RATE
+		if (rate_len > 8)
+		{
+			pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
+		}
+
+
+		//todo:HT for adhoc
+
+	}	
+
+#ifdef CONFIG_P2P
+	if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && is_valid_p2p_probereq)
+	{
+		u32 len;
+#ifdef CONFIG_IOCTL_CFG80211
+		if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
+		{
+			//if pwdinfo->role == P2P_ROLE_DEVICE will call issue_probersp_p2p()
+			len = pmlmepriv->p2p_go_probe_resp_ie_len;
+			if(pmlmepriv->p2p_go_probe_resp_ie && len>0)
+				_rtw_memcpy(pframe, pmlmepriv->p2p_go_probe_resp_ie, len);
+		}
+		else
+#endif //CONFIG_IOCTL_CFG80211
+		{
+			len = build_probe_resp_p2p_ie(pwdinfo, pframe);
+		}
+
+		pframe += len;
+		pattrib->pktlen += len;
+		
+#ifdef CONFIG_WFD
+#ifdef CONFIG_IOCTL_CFG80211
+		if(_TRUE == pwdinfo->wfd_info->wfd_enable)
+#endif //CONFIG_IOCTL_CFG80211
+		{
+			len = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
+		}
+#ifdef CONFIG_IOCTL_CFG80211
+		else
+		{	
+			len = 0;
+			if(pmlmepriv->wfd_probe_resp_ie && pmlmepriv->wfd_probe_resp_ie_len>0)
+			{
+				len = pmlmepriv->wfd_probe_resp_ie_len;
+				_rtw_memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, len);	
+			}	
+		}
+#endif //CONFIG_IOCTL_CFG80211		
+		pframe += len;
+		pattrib->pktlen += len;
+#endif //CONFIG_WFD
+
+	}
+#endif //CONFIG_P2P
+
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	
+
+	dump_mgntframe(padapter, pmgntframe);
+	
+	return;
+
+}
+
+int _issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	unsigned char			*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short		*fctrl;
+	unsigned char			*mac;
+	unsigned char			bssrate[NumRates];
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	int	bssrate_len = 0;
+	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_notice_,("+issue_probereq\n"));
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		goto exit;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	mac = myid(&(padapter->eeprompriv));
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	if (da)
+	{
+		//	unicast probe request frame
+		_rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+		_rtw_memcpy(pwlanhdr->addr3, da, ETH_ALEN);
+	}
+	else
+	{
+		//	broadcast probe request frame
+		_rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+		_rtw_memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
+	}
+
+	_rtw_memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_PROBEREQ);
+
+	pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
+
+	if(pssid)
+		pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &(pattrib->pktlen));
+	else
+		pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &(pattrib->pktlen));
+
+	get_rate_set(padapter, bssrate, &bssrate_len);
+
+	if (bssrate_len > 8)
+	{
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
+	}
+	else
+	{
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
+	}
+
+#if 0
+	//add wps_ie for wps2.0
+	if(pmlmepriv->probereq_wpsie_len>0 && pmlmepriv->probereq_wpsie_len<MAX_WPS_IE_LEN)
+	{
+		_rtw_memcpy(pframe, pmlmepriv->probereq_wpsie, pmlmepriv->probereq_wpsie_len);
+		pframe += pmlmepriv->probereq_wpsie_len;
+		pattrib->pktlen += pmlmepriv->probereq_wpsie_len;
+		//pmlmepriv->probereq_wpsie_len = 0 ;//reset to zero		
+	}	
+#else
+	//add wps_ie for wps2.0
+	if(pmlmepriv->wps_probe_req_ie_len>0 && pmlmepriv->wps_probe_req_ie)
+	{
+		_rtw_memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
+		pframe += pmlmepriv->wps_probe_req_ie_len;
+		pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
+		//pmlmepriv->wps_probe_req_ie_len = 0 ;//reset to zero		
+	}	
+#endif
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_notice_,("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+inline void issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da)
+{
+	_issue_probereq(padapter, pssid, da, _FALSE);
+}
+
+int issue_probereq_ex(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da,
+	int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	u32 start = rtw_get_current_time();
+
+	do
+	{
+		ret = _issue_probereq(padapter, pssid, da, wait_ms>0?_TRUE:_FALSE);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
+			rtw_msleep_os(wait_ms);
+
+	}while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+// if psta == NULL, indiate we are station(client) now...
+void issue_auth(_adapter *padapter, struct sta_info *psta, unsigned short status)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	unsigned int					val32;
+	unsigned short				val16;
+	int use_shared_key = 0;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_AUTH);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+
+	if(psta)// for AP mode
+	{
+#ifdef CONFIG_NATIVEAP_MLME
+
+		_rtw_memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);		
+		_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		_rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+	
+		// setting auth algo number	
+		val16 = (u16)psta->authalg;
+
+		if(status != _STATS_SUCCESSFUL_)
+			val16 = 0;
+
+		if (val16)	{
+			val16 = cpu_to_le16(val16);	
+			use_shared_key = 1;
+		}
+
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
+
+		// setting auth seq number
+		val16 =(u16)psta->auth_seq;
+		val16 = cpu_to_le16(val16);	
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
+
+		// setting status code...
+		val16 = status;
+		val16 = cpu_to_le16(val16);	
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen));
+
+		// added challenging text...
+		if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
+		{
+			pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, psta->chg_txt, &(pattrib->pktlen));			
+		}
+#endif
+	}
+	else
+	{		
+		_rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+		_rtw_memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+		_rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+	
+		// setting auth algo number		
+		val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)? 1: 0;// 0:OPEN System, 1:Shared key
+		if (val16)	{
+			val16 = cpu_to_le16(val16);	
+			use_shared_key = 1;
+		}	
+		//DBG_871X("%s auth_algo= %s auth_seq=%d\n",__FUNCTION__,(pmlmeinfo->auth_algo==0)?"OPEN":"SHARED",pmlmeinfo->auth_seq);
+		
+		//setting IV for auth seq #3
+		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
+		{
+			//DBG_871X("==> iv(%d),key_index(%d)\n",pmlmeinfo->iv,pmlmeinfo->key_index);
+			val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
+			val32 = cpu_to_le32(val32);
+			pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&val32, &(pattrib->pktlen));
+
+			pattrib->iv_len = 4;
+		}
+
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
+		
+		// setting auth seq number
+		val16 = pmlmeinfo->auth_seq;
+		val16 = cpu_to_le16(val16);	
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
+
+		
+		// setting status code...
+		val16 = status;
+		val16 = cpu_to_le16(val16);	
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen));
+
+		// then checking to see if sending challenging text...
+		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
+		{
+			pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen));
+
+			SetPrivacy(fctrl);
+			
+			pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);			
+			
+			pattrib->encrypt = _WEP40_;
+
+			pattrib->icv_len = 4;
+			
+			pattrib->pktlen += pattrib->icv_len;			
+			
+		}
+		
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	rtw_wep_encrypt(padapter, (u8 *)pmgntframe);
+	DBG_871X("%s\n", __FUNCTION__);
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+
+void issue_asocrsp(_adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
+{
+#ifdef CONFIG_AP_MODE
+	struct xmit_frame	*pmgntframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	struct pkt_attrib *pattrib;
+	unsigned char	*pbuf, *pframe;
+	unsigned short val;		
+	unsigned short *fctrl;
+	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);	
+	WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);
+	u8 *ie = pnetwork->IEs; 
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD
+
+#endif //CONFIG_P2P
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
+	_rtw_memcpy((void *)GetAddr2Ptr(pwlanhdr), myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
+		SetFrameSubType(pwlanhdr, pkt_type);		
+	else
+		return;
+
+	pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen += pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+	//capability
+	val = *(unsigned short *)rtw_get_capability_from_ie(ie);
+
+	pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_ , (unsigned char *)&val, &(pattrib->pktlen));
+
+	status = cpu_to_le16(status);
+	pframe = rtw_set_fixed_ie(pframe , _STATUS_CODE_ , (unsigned char *)&status, &(pattrib->pktlen));
+	
+	val = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
+	pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_ , (unsigned char *)&val, &(pattrib->pktlen));
+
+	if (pstat->bssratelen <= 8)
+	{
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen));
+	}	
+	else 
+	{
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &(pattrib->pktlen));
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (pstat->bssratelen-8), pstat->bssrateset+8, &(pattrib->pktlen));
+	}
+
+#ifdef CONFIG_80211N_HT
+	if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option))
+	{
+		uint ie_len=0;
+		
+		//FILL HT CAP INFO IE
+		//p = hostapd_eid_ht_capabilities_info(hapd, p);
+		pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+		if(pbuf && ie_len>0)
+		{
+			_rtw_memcpy(pframe, pbuf, ie_len+2);
+			pframe += (ie_len+2);
+			pattrib->pktlen +=(ie_len+2);
+		}
+
+		//FILL HT ADD INFO IE
+		//p = hostapd_eid_ht_operation(hapd, p);
+		pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+		if(pbuf && ie_len>0)
+		{
+			_rtw_memcpy(pframe, pbuf, ie_len+2);
+			pframe += (ie_len+2);
+			pattrib->pktlen +=(ie_len+2);
+		}
+		
+	}	
+#endif
+
+	//FILL WMM IE
+	if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option))
+	{
+		uint ie_len=0;
+		unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};	
+		
+		for (pbuf = ie + _BEACON_IE_OFFSET_; ;pbuf+= (ie_len + 2))
+		{			
+			pbuf = rtw_get_ie(pbuf, _VENDOR_SPECIFIC_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));	
+			if(pbuf && _rtw_memcmp(pbuf+2, WMM_PARA_IE, 6)) 
+			{				
+				_rtw_memcpy(pframe, pbuf, ie_len+2);
+				pframe += (ie_len+2);
+				pattrib->pktlen +=(ie_len+2);
+				
+				break;				
+			}
+			
+			if ((pbuf == NULL) || (ie_len == 0))
+			{
+				break;
+			}			
+		}
+		
+	}
+
+
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
+	{
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
+	}
+
+	//add WPS IE ie for wps 2.0
+	if(pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len>0)
+	{
+		_rtw_memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len);
+		
+		pframe += pmlmepriv->wps_assoc_resp_ie_len;
+		pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
+	}
+
+#ifdef CONFIG_P2P
+#ifndef CONFIG_IOCTL_CFG80211
+	if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && (pstat->is_p2p_device == _TRUE))
+	{
+		u32 len;
+
+		len = build_assoc_resp_p2p_ie(pwdinfo, pframe, pstat->p2p_status_code);
+
+		pframe += len;
+		pattrib->pktlen += len;
+	}
+#endif //CONFIG_IOCTL_CFG80211	
+#ifdef CONFIG_WFD
+	if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)
+#ifdef CONFIG_IOCTL_CFG80211		
+		&& (_TRUE == pwdinfo->wfd_info->wfd_enable)
+#endif //CONFIG_IOCTL_CFG80211	
+	)
+	{
+		wfdielen = build_assoc_resp_wfd_ie(pwdinfo, pframe);
+		pframe += wfdielen;
+		pattrib->pktlen += wfdielen;
+	}
+#endif //CONFIG_WFD
+#endif //CONFIG_P2P
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	
+	dump_mgntframe(padapter, pmgntframe);
+	
+#endif
+}
+
+void issue_assocreq(_adapter *padapter)
+{
+	int ret = _FAIL;
+	struct xmit_frame				*pmgntframe;
+	struct pkt_attrib				*pattrib;
+	unsigned char					*pframe, *p;
+	struct rtw_ieee80211_hdr			*pwlanhdr;
+	unsigned short				*fctrl;
+	unsigned short				val16;
+	unsigned int					i, j, ie_len, index=0;
+	unsigned char					rf_type, bssrate[NumRates], sta_bssrate[NumRates];
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	int	bssrate_len = 0, sta_bssrate_len = 0;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8					p2pie[ 255 ] = { 0x00 };
+	u16					p2pielen = 0;	
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD
+#endif //CONFIG_P2P
+
+#ifdef CONFIG_DFS
+	u16	cap;
+#endif //CONFIG_DFS
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+		goto exit;
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+	_rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ASSOCREQ);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	//caps
+
+#ifdef CONFIG_DFS
+	_rtw_memcpy(&cap, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
+	cap |= BIT(8);
+	_rtw_memcpy(pframe, &cap, 2);
+#else
+	_rtw_memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
+#endif //CONFIG_DFS
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	//listen interval
+	//todo: listen interval for power saving
+	val16 = cpu_to_le16(3);
+	_rtw_memcpy(pframe ,(unsigned char *)&val16, 2);
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	//SSID
+	pframe = rtw_set_ie(pframe, _SSID_IE_,  pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &(pattrib->pktlen));
+
+	//supported rate & extended supported rate
+
+#if 1	// Check if the AP's supported rates are also supported by STA.
+	get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);
+	//DBG_871X("sta_bssrate_len=%d\n", sta_bssrate_len);
+	
+	if(pmlmeext->cur_channel == 14)// for JAPAN, channel 14 can only uses B Mode(CCK) 
+	{
+		sta_bssrate_len = 4;
+	}
+
+	
+	//for (i = 0; i < sta_bssrate_len; i++) {
+	//	DBG_871X("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]);
+	//}
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		if (pmlmeinfo->network.SupportedRates[i] == 0) break;
+		DBG_871X("network.SupportedRates[%d]=%02X\n", i, pmlmeinfo->network.SupportedRates[i]);
+	}
+	
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		if (pmlmeinfo->network.SupportedRates[i] == 0) break;
+
+		
+		// Check if the AP's supported rates are also supported by STA.
+		for (j=0; j < sta_bssrate_len; j++) {
+			 // Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP
+			if ( (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK) 
+					== (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)) {
+				//DBG_871X("match i = %d, j=%d\n", i, j);
+				break;
+			} else {
+				//DBG_871X("not match: %02X != %02X\n", (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK), (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK));
+			}
+		}
+		
+		if (j == sta_bssrate_len) {
+			// the rate is not supported by STA
+			DBG_871X("%s(): the rate[%d]=%02X is not supported by STA!\n",__FUNCTION__, i, pmlmeinfo->network.SupportedRates[i]);
+		} else {
+			// the rate is supported by STA
+			bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
+		}
+	}
+	
+	bssrate_len = index;
+	DBG_871X("bssrate_len = %d\n", bssrate_len);
+
+#else	// Check if the AP's supported rates are also supported by STA.
+#if 0
+	get_rate_set(padapter, bssrate, &bssrate_len);
+#else
+	for (bssrate_len = 0; bssrate_len < NumRates; bssrate_len++) {
+		if (pmlmeinfo->network.SupportedRates[bssrate_len] == 0) break;
+
+		if (pmlmeinfo->network.SupportedRates[bssrate_len] == 0x2C) // Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP
+			break;
+
+		bssrate[bssrate_len] = pmlmeinfo->network.SupportedRates[bssrate_len];
+	}
+#endif
+#endif	// Check if the AP's supported rates are also supported by STA.
+
+	if (bssrate_len == 0) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit; //don't connect to AP if no joint supported rate
+	}
+
+
+	if (bssrate_len > 8)
+	{
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
+	}
+	else
+	{
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
+	}
+
+	//RSN
+	p = rtw_get_ie((pmlmeinfo->network.IEs + sizeof(NDIS_802_11_FIXED_IEs)), _RSN_IE_2_, &ie_len, (pmlmeinfo->network.IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+	if (p != NULL)
+	{
+		pframe = rtw_set_ie(pframe, _RSN_IE_2_, ie_len, (p + 2), &(pattrib->pktlen));
+	}
+
+#ifdef CONFIG_80211N_HT
+	//HT caps
+	if(padapter->mlmepriv.htpriv.ht_option==_TRUE)
+	{
+		p = rtw_get_ie((pmlmeinfo->network.IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_CAPABILITY_IE_, &ie_len, (pmlmeinfo->network.IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+		if ((p != NULL) && (!(is_ap_in_tkip(padapter))))
+		{
+			_rtw_memcpy(&(pmlmeinfo->HT_caps), (p + 2), sizeof(struct HT_caps_element));
+
+			//to disable 40M Hz support while gd_bw_40MHz_en = 0
+			if (pregpriv->cbw40_enable == 0)
+			{
+				pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info &= (~(BIT(6) | BIT(1)));
+			}
+			else
+			{
+				pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= BIT(1);
+			}
+
+			//todo: disable SM power save mode
+			pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= 0x000c;
+
+			rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+			//switch (pregpriv->rf_config)
+			switch(rf_type)
+			{
+				case RF_1T1R:
+				
+					if(pregpriv->rx_stbc)
+						pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0100);//RX STBC One spatial stream
+						
+					_rtw_memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_1R, 16);
+					break;
+
+				case RF_2T2R:
+				case RF_1T2R:
+				default:
+				
+					if((pregpriv->rx_stbc == 0x3) ||//enable for 2.4/5 GHz	
+						((pmlmeext->cur_wireless_mode & WIRELESS_11_24N) && (pregpriv->rx_stbc == 0x1)) || //enable for 2.4GHz
+						((pmlmeext->cur_wireless_mode & WIRELESS_11_5N) && (pregpriv->rx_stbc == 0x2)) || //enable for 5GHz
+						(pregpriv->wifi_spec==1))
+					{
+						DBG_871X("declare supporting RX STBC\n");
+						pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0200);//RX STBC two spatial stream
+					}
+					#ifdef CONFIG_DISABLE_MCS13TO15
+					if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40 && (pregpriv->wifi_spec!=1))
+						_rtw_memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_2R_MCS13TO15_OFF, 16);
+					else
+					_rtw_memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_2R, 16);
+					#else //CONFIG_DISABLE_MCS13TO15
+					_rtw_memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_2R, 16);
+					#endif //CONFIG_DISABLE_MCS13TO15
+					break;
+			}
+#ifdef RTL8192C_RECONFIG_TO_1T1R
+			{
+				if(pregpriv->rx_stbc)
+					pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0100);//RX STBC One spatial stream
+			
+				_rtw_memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_1R, 16);
+			}
+#endif
+			pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = cpu_to_le16(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info);
+
+#ifdef CONFIG_BT_COEXIST
+			if (BT_1Ant(padapter) == _TRUE)
+			{
+				// set to 8K
+				pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para &= (u8)~IEEE80211_HT_CAP_AMPDU_FACTOR;
+//				pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para |= MAX_AMPDU_FACTOR_8K
+			}
+#endif
+
+			pframe = rtw_set_ie(pframe, _HT_CAPABILITY_IE_, ie_len , (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
+
+		}
+	}
+#endif
+
+	//vendor specific IE, such as WPA, WMM, WPS
+	for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;)
+	{
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i);
+
+		switch (pIE->ElementID)
+		{
+			case _VENDOR_SPECIFIC_IE_:
+				if ((_rtw_memcmp(pIE->data, RTW_WPA_OUI, 4)) ||
+						(_rtw_memcmp(pIE->data, WMM_OUI, 4)) ||
+						(_rtw_memcmp(pIE->data, WPS_OUI, 4)))
+				{
+					if(!padapter->registrypriv.wifi_spec)
+					{
+						//Commented by Kurt 20110629
+						//In some older APs, WPS handshake
+						//would be fail if we append vender extensions informations to AP
+						if(_rtw_memcmp(pIE->data, WPS_OUI, 4)){
+							pIE->Length=14;
+						}
+					}
+					pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, pIE->Length, pIE->data, &(pattrib->pktlen));
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
+	{
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
+	}
+
+
+#ifdef CONFIG_WAPI_SUPPORT
+	rtw_build_assoc_req_wapi_ie(padapter, pframe, pattrib);
+#endif
+
+
+#ifdef CONFIG_P2P
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
+	{
+		if(pmlmepriv->p2p_assoc_req_ie && pmlmepriv->p2p_assoc_req_ie_len>0)
+		{
+			_rtw_memcpy(pframe, pmlmepriv->p2p_assoc_req_ie, pmlmepriv->p2p_assoc_req_ie_len);
+			pframe += pmlmepriv->p2p_assoc_req_ie_len;
+			pattrib->pktlen += pmlmepriv->p2p_assoc_req_ie_len;
+		}
+	}
+	else
+#endif //CONFIG_IOCTL_CFG80211
+	{
+		if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
+		{
+			//	Should add the P2P IE in the association request frame.	
+			//	P2P OUI
+			
+			p2pielen = 0;
+			p2pie[ p2pielen++ ] = 0x50;
+			p2pie[ p2pielen++ ] = 0x6F;
+			p2pie[ p2pielen++ ] = 0x9A;
+			p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+			//	Commented by Albert 20101109
+			//	According to the P2P Specification, the association request frame should contain 3 P2P attributes
+			//	1. P2P Capability
+			//	2. Extended Listen Timing
+			//	3. Device Info
+			//	Commented by Albert 20110516
+			//	4. P2P Interface
+
+			//	P2P Capability
+			//	Type:
+			p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
+
+			//	Length:
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
+			p2pielen += 2;
+
+			//	Value:
+			//	Device Capability Bitmap, 1 byte
+			p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
+
+			//	Group Capability Bitmap, 1 byte
+			if ( pwdinfo->persistent_supported )
+				p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+			else
+				p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
+
+			//	Extended Listen Timing
+			//	Type:
+			p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
+
+			//	Length:
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
+			p2pielen += 2;
+
+			//	Value:
+			//	Availability Period
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
+			p2pielen += 2;
+
+			//	Availability Interval
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
+			p2pielen += 2;
+
+			//	Device Info
+			//	Type:
+			p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
+
+			//	Length:
+			//	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) 
+			//	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
+			p2pielen += 2;
+
+			//	Value:
+			//	P2P Device Address
+			_rtw_memcpy( p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
+			p2pielen += ETH_ALEN;
+
+			//	Config Method
+			//	This field should be big endian. Noted by P2P specification.
+			if ( ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN ) ||
+				( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN ) )
+			{
+				*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
+			}
+			else
+			{
+				*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_PBC );
+			}
+
+			p2pielen += 2;
+
+			//	Primary Device Type
+			//	Category ID
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
+			p2pielen += 2;
+
+			//	OUI
+			*(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
+			p2pielen += 4;
+
+			//	Sub Category ID
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
+			p2pielen += 2;
+
+			//	Number of Secondary Device Types
+			p2pie[ p2pielen++ ] = 0x00;	//	No Secondary Device Type List
+
+			//	Device Name
+			//	Type:
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
+			p2pielen += 2;
+
+			//	Length:
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
+			p2pielen += 2;
+
+			//	Value:
+			_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
+			p2pielen += pwdinfo->device_name_len;
+		
+			//	P2P Interface
+			//	Type:
+			p2pie[ p2pielen++ ] = P2P_ATTR_INTERFACE;
+			
+			//	Length:
+			*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x000D );
+			p2pielen += 2;
+			
+			//	Value:
+			_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );	//	P2P Device Address
+			p2pielen += ETH_ALEN;
+
+			p2pie[ p2pielen++ ] = 1;	//	P2P Interface Address Count
+			
+			_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );	//	P2P Interface Address List
+			p2pielen += ETH_ALEN;
+		
+			pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
+
+#ifdef CONFIG_WFD
+			//wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);
+			//pframe += wfdielen;
+			//pattrib->pktlen += wfdielen;
+#endif //CONFIG_WFD
+		}
+	}
+
+#endif //CONFIG_P2P
+
+#ifdef CONFIG_WFD
+#ifdef CONFIG_IOCTL_CFG80211
+	if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
+#endif //CONFIG_IOCTL_CFG80211
+	{
+		wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);
+		pframe += wfdielen;
+		pattrib->pktlen += wfdielen;
+	}
+#ifdef CONFIG_IOCTL_CFG80211
+	else if (pmlmepriv->wfd_assoc_req_ie != NULL && pmlmepriv->wfd_assoc_req_ie_len>0)		
+	{
+		//WFD IE
+		_rtw_memcpy(pframe, pmlmepriv->wfd_assoc_req_ie, pmlmepriv->wfd_assoc_req_ie_len);
+		pattrib->pktlen += pmlmepriv->wfd_assoc_req_ie_len;
+		pframe += pmlmepriv->wfd_assoc_req_ie_len;		
+	}
+#endif //CONFIG_IOCTL_CFG80211
+#endif //CONFIG_WFD	
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	dump_mgntframe(padapter, pmgntframe);
+
+	ret = _SUCCESS;
+
+exit:
+	if (ret == _SUCCESS)
+		rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen);
+	else
+		rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
+
+	return;
+}
+
+//when wait_ack is ture, this function shoule be called at process context
+static int _issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	struct xmit_priv	*pxmitpriv;
+	struct mlme_ext_priv	*pmlmeext;
+	struct mlme_ext_info	*pmlmeinfo;
+
+	//DBG_871X("%s:%d\n", __FUNCTION__, power_mode);
+
+	if(!padapter)
+		goto exit;
+
+	pxmitpriv = &(padapter->xmitpriv);
+	pmlmeext = &(padapter->mlmeextpriv);
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		goto exit;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->retry_ctrl = _FALSE;
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+	{
+		SetFrDs(fctrl);
+	}
+	else if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+	{
+		SetToDs(fctrl);
+	}
+	
+	if (power_mode)
+	{
+		SetPwrMgt(fctrl);
+	}
+
+	_rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_DATA_NULL);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if(wait_ack)
+	{
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	}
+	else
+	{
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+
+//when wait_ms >0 , this function shoule be called at process context
+//da == NULL for station mode
+int issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	u32 start = rtw_get_current_time();
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* da == NULL, assum it's null data for sta to ap*/
+	if (da == NULL)
+		da = get_my_bssid(&(pmlmeinfo->network));
+
+	do
+	{
+		ret = _issue_nulldata(padapter, da, power_mode, wait_ms>0?_TRUE:_FALSE);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
+			rtw_msleep_os(wait_ms);
+
+	}while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+//when wait_ack is ture, this function shoule be called at process context
+static int _issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl, *qc;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		goto exit;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	pattrib->hdrlen +=2;
+	pattrib->qos_en = _TRUE;
+	pattrib->eosp = 1;
+	pattrib->ack_policy = 0;
+	pattrib->mdata = 0;
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+	{
+		SetFrDs(fctrl);
+	}
+	else if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+	{
+		SetToDs(fctrl);
+	}
+
+	if(pattrib->mdata)
+		SetMData(fctrl);
+
+	qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
+	
+	SetPriority(qc, tid);
+
+	SetEOSP(qc, pattrib->eosp);
+
+	SetAckpolicy(qc, pattrib->ack_policy);
+
+	_rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	
+	if(wait_ack)
+	{
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	}
+	else
+	{
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+//when wait_ms >0 , this function shoule be called at process context
+//da == NULL for station mode
+int issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	u32 start = rtw_get_current_time();
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* da == NULL, assum it's null data for sta to ap*/
+	if (da == NULL)
+		da = get_my_bssid(&(pmlmeinfo->network));
+	
+	do
+	{
+		ret = _issue_qos_nulldata(padapter, da, tid, wait_ms>0?_TRUE:_FALSE);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
+			rtw_msleep_os(wait_ms);
+
+	}while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+static int _issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	int ret = _FAIL;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
+#endif //CONFIG_P2P
+
+	//DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
+
+#ifdef CONFIG_P2P
+	if ( !( rtw_p2p_chk_state( pwdinfo, P2P_STATE_NONE ) ) && ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) )
+	{
+		_cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
+		_set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
+	}
+#endif //CONFIG_P2P
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		goto exit;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->retry_ctrl = _FALSE;
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_DEAUTH);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	reason = cpu_to_le16(reason);
+	pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_ , (unsigned char *)&reason, &(pattrib->pktlen));
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+
+	if(wait_ack)
+	{
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	}
+	else
+	{
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+int issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason)
+{
+	DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
+	return _issue_deauth(padapter, da, reason, _FALSE);
+}
+
+int issue_deauth_ex(_adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
+	int wait_ms)
+{
+	int ret;
+	int i = 0;
+	u32 start = rtw_get_current_time();
+
+	do
+	{
+		ret = _issue_deauth(padapter, da, reason, wait_ms>0?_TRUE:_FALSE);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
+			rtw_msleep_os(wait_ms);
+
+	}while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+void issue_action_spct_ch_switch(_adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset)
+{	
+	_irqL	irqL;
+	_list		*plist, *phead;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char				*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short			*fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+
+	DBG_871X(FUNC_NDEV_FMT" ra="MAC_FMT", ch:%u, offset:%u\n",
+		FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(ra), new_ch, ch_offset);
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+		return;
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN); /* RA */
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); /* TA */
+	_rtw_memcpy(pwlanhdr->addr3, ra, ETH_ALEN); /* DA = RA */
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	/* category, action */
+	{
+		u8 category, action;
+		category = RTW_WLAN_CATEGORY_SPECTRUM_MGMT;
+		action = RTW_WLAN_ACTION_SPCT_CHL_SWITCH;
+
+		pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+		pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	}
+
+	pframe = rtw_set_ie_ch_switch(pframe, &(pattrib->pktlen), 0, new_ch, 0);
+	pframe = rtw_set_ie_secondary_ch_offset(pframe, &(pattrib->pktlen),
+		hal_ch_offset_to_secondary_ch_offset(ch_offset));
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+}
+
+void issue_action_BA(_adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short status)
+{
+	u8	category = RTW_WLAN_CATEGORY_BACK;
+	u16	start_seq;
+	u16	BA_para_set;
+	u16	reason_code;
+	u16	BA_timeout_value;
+	u16	BA_starting_seqctrl;
+	HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	u8					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	u16					*fctrl;
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_info		*psta;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct registry_priv	 	*pregpriv = &padapter->registrypriv;
+#ifdef CONFIG_BT_COEXIST
+	u8 tendaAPMac[] = {0xC8, 0x3A, 0x35};
+#endif
+
+#ifdef CONFIG_80211N_HT
+	DBG_871X("%s, category=%d, action=%d, status=%d\n", __FUNCTION__, category, action, status);
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	//_rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+
+      status = cpu_to_le16(status);
+	
+
+	if (category == 3)
+	{
+		switch (action)
+		{
+			case 0: //ADDBA req
+				do {
+					pmlmeinfo->dialogToken++;
+				} while (pmlmeinfo->dialogToken == 0);
+				pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen));
+
+#ifdef CONFIG_BT_COEXIST
+				if ((BT_1Ant(padapter) == _TRUE) &&
+					((pmlmeinfo->assoc_AP_vendor != broadcomAP) ||
+					 (_rtw_memcmp(raddr, tendaAPMac, 3) == _FALSE)))
+				{
+					// A-MSDU NOT Supported
+					BA_para_set = 0;
+					// immediate Block Ack
+					BA_para_set |= (1 << 1) & IEEE80211_ADDBA_PARAM_POLICY_MASK;
+					// TID
+					BA_para_set |= (status << 2) & IEEE80211_ADDBA_PARAM_TID_MASK;
+					// max buffer size is 8 MSDU
+					BA_para_set |= (8 << 6) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+				}
+				else
+#endif
+				{
+					#if defined(CONFIG_RTL8188E) && defined(CONFIG_SDIO_HCI)
+					BA_para_set = (0x0802 | ((status & 0xf) << 2)); //immediate ack & 16 buffer size
+					#else
+					BA_para_set = (0x1002 | ((status & 0xf) << 2)); //immediate ack & 64 buffer size
+					#endif
+				}
+				//sys_mib.BA_para_set = 0x0802; //immediate ack & 32 buffer size
+				BA_para_set = cpu_to_le16(BA_para_set);
+				pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
+
+				//BA_timeout_value = 0xffff;//max: 65535 TUs(~ 65 ms)
+				BA_timeout_value = 5000;//~ 5ms
+				BA_timeout_value = cpu_to_le16(BA_timeout_value);
+				pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_timeout_value)), &(pattrib->pktlen));
+
+				//if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
+				if ((psta = rtw_get_stainfo(pstapriv, raddr)) != NULL)
+				{
+					start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07]&0xfff) + 1;
+
+					DBG_871X("BA_starting_seqctrl = %d for TID=%d\n", start_seq, status & 0x07);
+					
+					psta->BA_starting_seqctrl[status & 0x07] = start_seq;
+					
+					BA_starting_seqctrl = start_seq << 4;
+				}
+				
+				BA_starting_seqctrl = cpu_to_le16(BA_starting_seqctrl);
+				pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_starting_seqctrl)), &(pattrib->pktlen));
+				break;
+
+			case 1: //ADDBA rsp
+				pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen));
+				pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&status), &(pattrib->pktlen));
+				/*
+				//BA_para_set = cpu_to_le16((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); //64 buffer size
+				#if defined(CONFIG_RTL8188E )&& defined (CONFIG_SDIO_HCI)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0800); //32buffer size
+				#else
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); //64 buffer size
+				#endif
+				*/
+				rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
+				if(MAX_AMPDU_FACTOR_64K == max_rx_ampdu_factor)
+					BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); //64 buffer size
+				else if(MAX_AMPDU_FACTOR_32K == max_rx_ampdu_factor)
+					BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0800); //32 buffer size
+				else if(MAX_AMPDU_FACTOR_16K == max_rx_ampdu_factor)
+					BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0400); //16 buffer size
+				else if(MAX_AMPDU_FACTOR_8K == max_rx_ampdu_factor)
+					BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0200); //8 buffer size
+				else
+					BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); //64 buffer size	
+
+#ifdef CONFIG_BT_COEXIST
+				if ((BT_1Ant(padapter) == _TRUE) &&
+					((pmlmeinfo->assoc_AP_vendor != broadcomAP) ||
+					 (_rtw_memcmp(raddr, tendaAPMac, 3) == _FALSE)))
+				{
+					// max buffer size is 8 MSDU
+					BA_para_set &= ~RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+					BA_para_set |= (8 << 6) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+				}
+#endif
+
+				if(pregpriv->ampdu_amsdu==0)//disabled
+					BA_para_set = cpu_to_le16(BA_para_set & ~BIT(0));
+				else if(pregpriv->ampdu_amsdu==1)//enabled
+					BA_para_set = cpu_to_le16(BA_para_set | BIT(0));
+				else //auto
+					BA_para_set = cpu_to_le16(BA_para_set);
+				
+				pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
+				pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen));
+				break;
+			case 2://DELBA
+				BA_para_set = (status & 0x1F) << 3;
+				BA_para_set = cpu_to_le16(BA_para_set);				
+				pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
+
+				reason_code = 37;//Requested from peer STA as it does not want to use the mechanism
+				reason_code = cpu_to_le16(reason_code);
+				pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(reason_code)), &(pattrib->pktlen));
+				break;
+			default:
+				break;
+		}
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+#endif //CONFIG_80211N_HT
+}
+
+static void issue_action_BSSCoexistPacket(_adapter *padapter)
+{	
+	_irqL	irqL;
+	_list		*plist, *phead;
+	unsigned char category, action;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char				*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short			*fctrl;
+	struct	wlan_network	*pnetwork = NULL;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	_queue		*queue	= &(pmlmepriv->scanned_queue);
+	u8 InfoContent[16] = {0};
+	u8 ICS[8][15];
+#ifdef CONFIG_80211N_HT	
+	if((pmlmepriv->num_FortyMHzIntolerant==0) || (pmlmepriv->num_sta_no_ht==0))
+		return;
+
+	if(_TRUE == pmlmeinfo->bwmode_updated)
+		return;
+	
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+
+	category = RTW_WLAN_CATEGORY_PUBLIC;
+	action = ACT_PUBLIC_BSSCOEXIST;
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+
+
+	//
+	if(pmlmepriv->num_FortyMHzIntolerant>0)
+	{
+		u8 iedata=0;
+		
+		iedata |= BIT(2);//20 MHz BSS Width Request
+
+		pframe = rtw_set_ie(pframe, EID_BSSCoexistence,  1, &iedata, &(pattrib->pktlen));
+		
+	}
+	
+
+	//
+	_rtw_memset(ICS, 0, sizeof(ICS));
+	if(pmlmepriv->num_sta_no_ht>0)
+	{	
+		int i;
+	
+		_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+		phead = get_list_head(queue);
+		plist = get_next(phead);
+       
+		while(1)
+		{
+			int len;
+			u8 *p;
+			WLAN_BSSID_EX *pbss_network;
+	
+			if (rtw_end_of_queue_search(phead,plist)== _TRUE)
+				break;		
+
+			pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);      
+		
+			plist = get_next(plist);
+
+			pbss_network = (WLAN_BSSID_EX *)&pnetwork->network;
+
+			p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
+			if((p==NULL) || (len==0))//non-HT
+			{
+				if((pbss_network->Configuration.DSConfig<=0) || (pbss_network->Configuration.DSConfig>14))
+					continue;
+				
+				ICS[0][pbss_network->Configuration.DSConfig]=1;
+				
+				if(ICS[0][0] == 0)
+					ICS[0][0] = 1;		
+			}		
+	
+		}        
+
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+
+		for(i= 0;i<8;i++)
+		{
+			if(ICS[i][0] == 1)
+			{
+				int j, k = 0;
+				
+				InfoContent[k] = i;				
+				//SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent,i);
+				k++;
+				
+				for(j=1;j<=14;j++)
+				{
+					if(ICS[i][j]==1)
+					{
+						if(k<16)
+						{
+							InfoContent[k] = j; //channel number
+							//SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j);
+							k++;
+						}	
+					}	
+				}	
+
+				pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &(pattrib->pktlen));
+				
+			}
+			
+		}
+		
+
+	}
+		
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+#endif //CONFIG_80211N_HT
+}
+
+unsigned int send_delba(_adapter *padapter, u8 initiator, u8 *addr)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+	//struct recv_reorder_ctrl *preorder_ctrl;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u16 tid;
+
+	if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)	
+		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+			return _SUCCESS;
+	
+	psta = rtw_get_stainfo(pstapriv, addr);
+	if(psta==NULL)
+		return _SUCCESS;
+
+	//DBG_871X("%s:%s\n", __FUNCTION__, (initiator==0)?"RX_DIR":"TX_DIR");
+	
+	if(initiator==0) // recipient
+	{
+		for(tid = 0;tid<MAXTID;tid++)
+		{
+			if(psta->recvreorder_ctrl[tid].enable == _TRUE)
+			{
+				DBG_871X("rx agg disable tid(%d)\n",tid);
+				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F));
+				psta->recvreorder_ctrl[tid].enable = _FALSE;
+				psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
+				#ifdef DBG_RX_SEQ
+				DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
+					psta->recvreorder_ctrl[tid].indicate_seq);
+				#endif
+			}		
+		}
+	}
+	else if(initiator == 1)// originator
+	{
+#ifdef CONFIG_80211N_HT
+		//DBG_871X("tx agg_enable_bitmap(0x%08x)\n", psta->htpriv.agg_enable_bitmap);
+		for(tid = 0;tid<MAXTID;tid++)
+		{
+			if(psta->htpriv.agg_enable_bitmap & BIT(tid))
+			{
+				DBG_871X("tx agg disable tid(%d)\n",tid);
+				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F) );
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+				
+			}			
+		}
+#endif //CONFIG_80211N_HT
+	}
+	
+	return _SUCCESS;
+	
+}
+
+unsigned int send_beacon(_adapter *padapter)
+{
+	u8	bxmitok = _FALSE;
+	int	issue=0;
+	int poll = 0;
+//#ifdef CONFIG_CONCURRENT_MODE
+	//struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	//struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	//_adapter *pbuddy_adapter = padapter->pbuddy_adapter;
+	//struct mlme_priv *pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
+//#endif		
+
+#ifdef CONFIG_PCI_HCI
+
+	//DBG_871X("%s\n", __FUNCTION__);
+
+	issue_beacon(padapter, 0);
+
+	return _SUCCESS;
+
+#endif
+
+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+	u32 start = rtw_get_current_time();
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
+	do{
+		issue_beacon(padapter, 100);
+		issue++;
+		do {
+			rtw_yield_os();
+			rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
+			poll++;
+		}while((poll%10)!=0 && _FALSE == bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+	}while(_FALSE == bxmitok && issue<100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+	if(padapter->bSurpriseRemoved || padapter->bDriverStopped)
+	{
+		return _FAIL;
+	}
+
+	
+	if(_FALSE == bxmitok)
+	{
+		DBG_871X("%s fail! %u ms\n", __FUNCTION__, rtw_get_passing_time_ms(start));
+		return _FAIL;
+	}
+	else
+	{
+		u32 passing_time = rtw_get_passing_time_ms(start);
+
+		if(passing_time > 100 || issue > 3)
+			DBG_871X("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start));
+		//else
+		//	DBG_871X("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start));
+		
+		return _SUCCESS;
+	}
+
+#endif
+
+}
+
+/****************************************************************************
+
+Following are some utitity fuctions for WiFi MLME
+
+*****************************************************************************/
+
+BOOLEAN IsLegal5GChannel(
+	IN PADAPTER			Adapter,
+	IN u8			channel)
+{
+	
+	int i=0;
+	u8 Channel_5G[45] = {36,38,40,42,44,46,48,50,52,54,56,58,
+		60,62,64,100,102,104,106,108,110,112,114,116,118,120,122,
+		124,126,128,130,132,134,136,138,140,149,151,153,155,157,159,
+		161,163,165};
+	for(i=0;i<sizeof(Channel_5G);i++)
+		if(channel == Channel_5G[i])
+			return _TRUE;
+	return _FALSE;
+}
+
+void site_survey(_adapter *padapter)
+{
+	unsigned char		survey_channel = 0, val8;
+	RT_SCAN_TYPE	ScanType = SCAN_PASSIVE;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u32 initialgain = 0;
+
+#ifdef CONFIG_P2P
+
+#ifdef CONFIG_CONCURRENT_MODE
+
+#ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+	u8 stay_buddy_ch = 0;
+#endif //CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;			
+	struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+
+#endif //CONFIG_CONCURRENT_MODE
+	struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
+	static unsigned char  prev_survey_channel = 0;
+	static unsigned int p2p_scan_count = 0;	
+
+	if ( ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) || ( pwdinfo->p2p_info.scan_op_ch_only ) )
+	{
+		if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
+		{
+			survey_channel = pwdinfo->rx_invitereq_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
+		}
+		else
+		{
+			survey_channel = pwdinfo->p2p_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
+		}
+		ScanType = SCAN_ACTIVE;
+	}
+	else if(rtw_p2p_findphase_ex_is_social(pwdinfo))
+	{
+		//	Commented by Albert 2011/06/03
+		//	The driver is in the find phase, it should go through the social channel.
+		int ch_set_idx;
+		survey_channel = pwdinfo->social_chan[pmlmeext->sitesurvey_res.channel_idx];
+		ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, survey_channel);
+		if (ch_set_idx >= 0)
+			ScanType = pmlmeext->channel_set[ch_set_idx].ScanType;
+		else
+			ScanType = SCAN_ACTIVE;
+	}
+	else
+#endif //CONFIG_P2P
+	{
+		struct rtw_ieee80211_channel *ch;
+		if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
+			ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
+			survey_channel = ch->hw_value;
+			ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
+		}
+	}
+
+	if (0){
+		DBG_871X(FUNC_ADPT_FMT" ch:%u (cnt:%u,idx:%d) at %dms, %c%c%c\n"
+		, FUNC_ADPT_ARG(padapter)
+		, survey_channel
+		, pwdinfo->find_phase_state_exchange_cnt, pmlmeext->sitesurvey_res.channel_idx
+		, rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time)
+		, ScanType?'A':'P', pmlmeext->sitesurvey_res.scan_mode?'A':'P'
+		, pmlmeext->sitesurvey_res.ssid[0].SsidLength?'S':' ' 
+		);
+		#ifdef DBG_FIXED_CHAN
+		DBG_871X(FUNC_ADPT_FMT" fixed_chan:%u\n", pmlmeext->fixed_chan);
+		#endif
+	}
+
+	if(survey_channel != 0)
+	{
+		//PAUSE 4-AC Queue when site_survey
+		//rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
+		//val8 |= 0x0f;
+		//rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
+#ifdef CONFIG_CONCURRENT_MODE
+#ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+		if((padapter->pbuddy_adapter->mlmeextpriv.mlmext_info.state&0x03) == WIFI_FW_AP_STATE)
+		{
+			if( pmlmeinfo->scan_cnt == RTW_SCAN_NUM_OF_CH )
+			{
+				pmlmeinfo->scan_cnt = 0;
+				survey_channel = pbuddy_mlmeext->cur_channel;
+				ScanType = SCAN_ACTIVE;
+				stay_buddy_ch = 1;
+			}
+			else 
+			{
+				if( pmlmeinfo->scan_cnt == 0 )
+					stay_buddy_ch = 2;
+				pmlmeinfo->scan_cnt++;
+			}
+		}
+#endif //CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+#endif //CONFIG_CONCURRENT_MODE
+		if(pmlmeext->sitesurvey_res.channel_idx == 0)
+		{
+#ifdef DBG_FIXED_CHAN
+			if(pmlmeext->fixed_chan !=0xff)
+				set_channel_bwmode(padapter, pmlmeext->fixed_chan, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+			else	
+#endif
+				set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+		}
+		else
+		{
+#ifdef DBG_FIXED_CHAN
+			if(pmlmeext->fixed_chan!=0xff)
+				SelectChannel(padapter, pmlmeext->fixed_chan);
+			else	
+#endif
+				SelectChannel(padapter, survey_channel);
+		}
+
+#ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+		if( stay_buddy_ch == 1 )
+		{
+			val8 = 0; //survey done
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+			if(check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) &&
+				check_buddy_fwstate(padapter, _FW_LINKED))
+			{
+				update_beacon(padapter->pbuddy_adapter, 0, NULL, _TRUE);
+			}
+		}
+		else if( stay_buddy_ch == 2 )
+		{
+			val8 = 1; //under site survey
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+		}
+#endif //CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+
+		if(ScanType == SCAN_ACTIVE) //obey the channel plan setting...
+		{
+			#ifdef CONFIG_P2P
+			if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || 
+				rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)
+			)
+			{
+				issue_probereq_p2p(padapter, NULL);
+				issue_probereq_p2p(padapter, NULL);
+				issue_probereq_p2p(padapter, NULL);
+			}
+			else
+			#endif //CONFIG_P2P
+			{
+				int i;
+				for(i=0;i<RTW_SSID_SCAN_AMOUNT;i++){
+					if(pmlmeext->sitesurvey_res.ssid[i].SsidLength) {
+						//todo: to issue two probe req???
+						issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
+						//rtw_msleep_os(SURVEY_TO>>1);
+						issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
+					}
+				}
+
+				if(pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
+					//todo: to issue two probe req???
+					issue_probereq(padapter, NULL, NULL);
+					//rtw_msleep_os(SURVEY_TO>>1);
+					issue_probereq(padapter, NULL, NULL);
+				}
+			}
+		}
+
+#ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+		if( stay_buddy_ch == 1 )
+			set_survey_timer(pmlmeext, pmlmeext->chan_scan_time * RTW_STAY_AP_CH_MILLISECOND );
+		else
+#endif //CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+			set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
+
+	}
+	else
+	{
+
+		//	channel number is 0 or this channel is not valid.
+
+#ifdef CONFIG_CONCURRENT_MODE
+		u8 cur_channel;
+		u8 cur_bwmode;
+		u8 cur_ch_offset;
+
+		if(check_fwstate(pmlmepriv, _FW_LINKED))
+		{
+			cur_channel = pmlmeext->cur_channel;
+			cur_bwmode = pmlmeext->cur_bwmode;
+			cur_ch_offset = pmlmeext->cur_ch_offset;
+		}
+		//else if((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+		else if(check_buddy_fwstate(padapter, _FW_LINKED)) // for AP or STA 	
+		{
+			cur_channel = pbuddy_mlmeext->cur_channel;
+			cur_bwmode = pbuddy_mlmeext->cur_bwmode;
+			cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
+		}
+		else
+		{
+			cur_channel = pmlmeext->cur_channel;
+			cur_bwmode = pmlmeext->cur_bwmode;
+			cur_ch_offset = pmlmeext->cur_ch_offset;
+		}		
+#endif
+
+	
+#ifdef CONFIG_P2P
+		if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
+		{
+			if( ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) || ( pwdinfo->p2p_info.scan_op_ch_only ) )
+			{
+				//	Set the find_phase_state_exchange_cnt to P2P_FINDPHASE_EX_CNT.
+				//	This will let the following flow to run the scanning end.
+				rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
+			}
+			#ifdef CONFIG_DBG_P2P
+			DBG_871X( "[%s] find phase exchange cnt = %d\n", __FUNCTION__, pwdinfo->find_phase_state_exchange_cnt );
+			#endif
+		}
+
+		if(rtw_p2p_findphase_ex_is_needed(pwdinfo))
+		{
+			//	Set the P2P State to the listen state of find phase and set the current channel to the listen channel
+			set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN);
+			pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+
+			initialgain = 0xff; //restore RX GAIN
+			rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));	
+			//turn on dynamic functions
+			Restore_DM_Func_Flag(padapter);
+			//Switch_DM_Func(padapter, DYNAMIC_FUNC_DIG|DYNAMIC_FUNC_HP|DYNAMIC_FUNC_SS, _TRUE);
+			
+			_set_timer( &pwdinfo->find_phase_timer, ( u32 ) ( ( u32 ) ( pwdinfo->listen_dwell ) * 100 ) );
+		}
+		else
+#endif //CONFIG_P2P
+		{
+
+#ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+			pmlmeinfo->scan_cnt = 0;
+#endif //CONFIG_DMP_STA_NODE_SCAN_UNDER_AP_MODE
+
+#ifdef CONFIG_ANTENNA_DIVERSITY
+			// 20100721:Interrupt scan operation here.
+			// For SW antenna diversity before link, it needs to switch to another antenna and scan again.
+			// It compares the scan result and select beter one to do connection.
+			if(rtw_hal_antdiv_before_linked(padapter))
+			{				
+				pmlmeext->sitesurvey_res.bss_cnt = 0;
+				pmlmeext->sitesurvey_res.channel_idx = -1;
+				pmlmeext->chan_scan_time = SURVEY_TO /2;			
+				set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
+				return;
+			}
+#endif
+
+#ifdef CONFIG_P2P
+			if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
+			{
+			#ifdef CONFIG_CONCURRENT_MODE
+				#ifndef CONFIG_IOCTL_CFG80211
+				if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+				{
+					_set_timer( &pwdinfo->ap_p2p_switch_timer, 500 );
+				}
+				#endif //CONFIG_IOCTL_CFG80211			
+				rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+			#else
+				rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+			#endif
+			}
+			rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
+#endif //CONFIG_P2P
+			
+			pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
+
+			//switch back to the original channel
+			//SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset);
+
+			{
+#ifdef CONFIG_DUALMAC_CONCURRENT
+				dc_set_channel_bwmode_survey_done(padapter);
+#else
+#ifndef CONFIG_IOCTL_CFG80211
+				if( rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN) )
+				{
+					set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+				}
+				else
+#endif //CONFIG_IOCTL_CFG80211
+#ifdef CONFIG_CONCURRENT_MODE
+					set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
+#else
+					set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+#endif //CONFIG_DUALMAC_CONCURRENT
+#endif //CONFIG_CONCURRENT_MODE
+			}
+
+			//flush 4-AC Queue after site_survey
+			//val8 = 0;
+			//rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8));
+
+			//config MSR
+			Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+			initialgain = 0xff; //restore RX GAIN
+			rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));	
+			//turn on dynamic functions
+			Restore_DM_Func_Flag(padapter);
+			//Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE);
+
+			if (is_client_associated_to_ap(padapter) == _TRUE)
+			{
+				issue_nulldata(padapter, NULL, 0, 3, 500);
+				
+#ifdef CONFIG_CONCURRENT_MODE
+				if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
+				{
+					DBG_871X("adapter is surveydone(buddy_adapter is linked), issue nulldata(pwrbit=0)\n");
+					
+					issue_nulldata(padapter->pbuddy_adapter, NULL, 0, 3, 500);
+				}
+#endif	
+			}
+#ifdef CONFIG_CONCURRENT_MODE
+			else if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
+			{
+				issue_nulldata(padapter->pbuddy_adapter, NULL, 0, 3, 500);
+			}
+#endif	
+
+			val8 = 0; //survey done
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+			report_surveydone_event(padapter);
+
+			pmlmeext->chan_scan_time = SURVEY_TO;
+			pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+
+			issue_action_BSSCoexistPacket(padapter);
+			issue_action_BSSCoexistPacket(padapter);
+			issue_action_BSSCoexistPacket(padapter);
+
+		}
+
+#ifdef CONFIG_CONCURRENT_MODE
+		if(check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) &&
+			check_buddy_fwstate(padapter, _FW_LINKED))
+		{
+
+			DBG_871X("survey done, current CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
+
+			DBG_871X("restart pbuddy_adapter's beacon\n");
+		
+			update_beacon(padapter->pbuddy_adapter, 0, NULL, _TRUE);
+		}
+#endif
+
+	}
+
+	return;
+
+}
+
+//collect bss info from Beacon and Probe request/response frames.
+u8 collect_bss_info(_adapter *padapter, union recv_frame *precv_frame, WLAN_BSSID_EX *bssid)
+{
+	int	i;
+	u32	len;
+	u8	*p;
+	u16	val16, subtype;
+	u8	*pframe = precv_frame->u.hdr.rx_data;
+	u32	packet_len = precv_frame->u.hdr.len;
+	u8 ie_offset;
+	struct registry_priv 	*pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ)
+	{
+		//DBG_871X("IE too long for survey event\n");
+		return _FAIL;
+	}
+
+	_rtw_memset(bssid, 0, sizeof(WLAN_BSSID_EX));
+
+	subtype = GetFrameSubType(pframe);
+
+	if(subtype==WIFI_BEACON) {
+		bssid->Reserved[0] = 1;
+		ie_offset = _BEACON_IE_OFFSET_;
+	} else {
+		// FIXME : more type
+		if (subtype == WIFI_PROBEREQ) {
+			ie_offset = _PROBEREQ_IE_OFFSET_;
+			bssid->Reserved[0] = 2;
+		} else if (subtype == WIFI_PROBERSP) {
+			ie_offset = _PROBERSP_IE_OFFSET_;
+			bssid->Reserved[0] = 3;
+		} else {
+			bssid->Reserved[0] = 0;
+			ie_offset = _FIXED_IE_LENGTH_;
+		}
+	}
+		
+	bssid->Length = sizeof(WLAN_BSSID_EX) - MAX_IE_SZ + len;
+
+	//below is to copy the information element
+	bssid->IELength = len;
+	_rtw_memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength);
+
+	//get the signal strength
+	//bssid->Rssi = precv_frame->u.hdr.attrib.SignalStrength; // 0-100 index.
+	bssid->Rssi = precv_frame->u.hdr.attrib.phy_info.RecvSignalPower; // in dBM.raw data	
+	bssid->PhyInfo.SignalQuality = precv_frame->u.hdr.attrib.phy_info.SignalQuality;//in percentage 
+	bssid->PhyInfo.SignalStrength = precv_frame->u.hdr.attrib.phy_info.SignalStrength;//in percentage
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	//rtw_hal_get_hwreg(padapter, HW_VAR_CURRENT_ANTENNA, (u8 *)(&bssid->PhyInfo.Optimum_antenna));
+	rtw_hal_get_def_var(padapter, HAL_DEF_CURRENT_ANTENNA,  &bssid->PhyInfo.Optimum_antenna);
+#endif
+
+	// checking SSID
+	if ((p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset)) == NULL)
+	{
+		DBG_871X("marc: cannot find SSID for survey event\n");
+		return _FAIL;
+	}
+
+	if (*(p + 1))
+	{
+		if (len > NDIS_802_11_LENGTH_SSID)
+		{
+			DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
+			return _FAIL;
+		}
+		_rtw_memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
+		bssid->Ssid.SsidLength = *(p + 1);
+	}
+	else
+	{
+		bssid->Ssid.SsidLength = 0;
+	}
+
+	_rtw_memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+	//checking rate info...
+	i = 0;
+	p = rtw_get_ie(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
+	if (p != NULL)
+	{
+		if (len > NDIS_802_11_LENGTH_RATES_EX)
+		{
+			DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
+			return _FAIL;
+		}
+		_rtw_memcpy(bssid->SupportedRates, (p + 2), len);
+		i = len;
+	}
+
+	p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
+	if (p != NULL)
+	{
+		if (len > (NDIS_802_11_LENGTH_RATES_EX-i))
+		{
+			DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
+			return _FAIL;
+		}
+		_rtw_memcpy(bssid->SupportedRates + i, (p + 2), len);
+	}
+
+	//todo:
+#if 0
+	if (judge_network_type(bssid->SupportedRates, (len + i)) == WIRELESS_11B)
+	{
+		bssid->NetworkTypeInUse = Ndis802_11DS;
+	}
+	else
+#endif
+	{
+		bssid->NetworkTypeInUse = Ndis802_11OFDM24;
+	}
+
+	if (bssid->IELength < 12)
+		return _FAIL;
+
+	// Checking for DSConfig
+	p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
+
+	bssid->Configuration.DSConfig = 0;
+	bssid->Configuration.Length = 0;
+
+	if (p)
+	{
+		bssid->Configuration.DSConfig = *(p + 2);
+	}
+	else
+	{// In 5G, some ap do not have DSSET IE
+		// checking HT info for channel
+		p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
+		if(p)
+		{
+			struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
+			bssid->Configuration.DSConfig = HT_info->primary_channel;
+		}
+		else
+		{ // use current channel
+			bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter);
+		}
+	}
+
+	if (subtype==WIFI_PROBEREQ)
+	{
+		// FIXME
+		bssid->InfrastructureMode = Ndis802_11Infrastructure;
+		_rtw_memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
+		bssid->Privacy = 1;
+		return _SUCCESS;
+	}
+
+	_rtw_memcpy(&bssid->Configuration.BeaconPeriod, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
+	bssid->Configuration.BeaconPeriod = le32_to_cpu(bssid->Configuration.BeaconPeriod);
+
+	val16 = rtw_get_capability((WLAN_BSSID_EX *)bssid);
+
+	if (val16 & BIT(0))
+	{
+		bssid->InfrastructureMode = Ndis802_11Infrastructure;
+		_rtw_memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
+	}
+	else
+	{
+		bssid->InfrastructureMode = Ndis802_11IBSS;
+		_rtw_memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN);
+	}
+
+	if (val16 & BIT(4))
+		bssid->Privacy = 1;
+	else
+		bssid->Privacy = 0;
+
+	bssid->Configuration.ATIMWindow = 0;
+
+	//20/40 BSS Coexistence check
+	if((pregistrypriv->wifi_spec==1) && (_FALSE == pmlmeinfo->bwmode_updated))
+	{	
+		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+#ifdef CONFIG_80211N_HT
+		p = rtw_get_ie(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset);
+		if(p && len>0)
+		{
+			struct HT_caps_element	*pHT_caps;
+			pHT_caps = (struct HT_caps_element	*)(p + 2);
+			
+			if(pHT_caps->u.HT_cap_element.HT_caps_info&BIT(14))
+			{				
+				pmlmepriv->num_FortyMHzIntolerant++;
+			}
+		}
+		else
+		{
+			pmlmepriv->num_sta_no_ht++;
+		}
+#endif //CONFIG_80211N_HT
+		
+	}
+
+#ifdef CONFIG_INTEL_WIDI
+	//process_intel_widi_query_or_tigger(padapter, bssid);
+	if(process_intel_widi_query_or_tigger(padapter, bssid))
+	{
+		return _FAIL;
+	}
+#endif // CONFIG_INTEL_WIDI
+
+	#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) & 1
+	if(strcmp(bssid->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
+		DBG_871X("Receiving %s("MAC_FMT", DSConfig:%u) from ch%u with ss:%3u, sq:%3u, RawRSSI:%3ld\n"
+			, bssid->Ssid.Ssid, MAC_ARG(bssid->MacAddress), bssid->Configuration.DSConfig
+			, rtw_get_oper_ch(padapter)
+			, bssid->PhyInfo.SignalStrength, bssid->PhyInfo.SignalQuality, bssid->Rssi
+		);
+	}
+	#endif
+
+	// mark bss info receving from nearby channel as SignalQuality 101
+	if(bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
+	{
+		bssid->PhyInfo.SignalQuality= 101;
+	}
+
+	return _SUCCESS;
+}
+
+void start_create_ibss(_adapter* padapter)
+{
+	unsigned short	caps;
+	u8	val8;
+	u8	join_type;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
+	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
+
+	//update wireless mode
+	update_wireless_mode(padapter);
+
+	//udpate capability
+	caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
+	update_capinfo(padapter, caps);
+	if(caps&cap_IBSS)//adhoc master
+	{
+		//set_opmode_cmd(padapter, adhoc);//removed
+
+		val8 = 0xcf;
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+		//switch channel
+		//SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE);
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+
+		beacon_timing_control(padapter);
+
+		//set msr to WIFI_FW_ADHOC_STATE
+		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+		Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+		//issue beacon
+		if(send_beacon(padapter)==_FAIL)
+		{
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("issuing beacon frame fail....\n"));
+
+			report_join_res(padapter, -1);
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		}
+		else
+		{
+			rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
+			join_type = 0;
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+			report_join_res(padapter, 1);
+			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+		}
+	}
+	else
+	{
+		DBG_871X("start_create_ibss, invalid cap:%x\n", caps);
+		return;
+	}
+
+}
+
+void start_clnt_join(_adapter* padapter)
+{
+	unsigned short	caps;
+	u8	val8;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
+#ifdef CONFIG_DUALMAC_CONCURRENT
+	u8	dc_join_status;
+#endif
+	int beacon_timeout;
+
+	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
+
+	//update wireless mode
+	update_wireless_mode(padapter);
+
+	//udpate capability
+	caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
+	update_capinfo(padapter, caps);
+	if (caps&cap_ESS)
+	{
+#ifdef CONFIG_DUALMAC_CONCURRENT
+		dc_join_status = dc_handle_join_request(padapter);
+		if(dc_join_status == _FAIL)
+		{
+			DBG_871X("dc_handle_join_request for STA fail !!!\n");
+			return;
+		}
+#endif
+#ifdef CONFIG_CONCURRENT_MODE
+		if(concurrent_chk_start_clnt_join(padapter) == _FAIL)
+			return;	
+#endif
+
+		Set_MSR(padapter, WIFI_FW_STATION_STATE);
+
+		val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf;
+
+#ifdef CONFIG_WAPI_SUPPORT
+		if (padapter->wapiInfo.bWapiEnable && pmlmeinfo->auth_algo == dot11AuthAlgrthm_WAPI)
+		{
+			//Disable TxUseDefaultKey, RxUseDefaultKey, RxBroadcastUseDefaultKey.
+			val8 = 0x4c;
+		}
+#endif
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+		//switch channel
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+		//here wait for receiving the beacon to start auth
+		//and enable a timer
+		beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
+		set_link_timer(pmlmeext, beacon_timeout);	
+		_set_timer( &padapter->mlmepriv.assoc_timer, 
+			(REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) +beacon_timeout);
+		
+		pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
+	}
+	else if (caps&cap_IBSS) //adhoc client
+	{
+#ifdef CONFIG_DUALMAC_CONCURRENT
+		dc_join_status = dc_handle_join_request(padapter);
+		if(dc_join_status == _FAIL)
+		{
+			DBG_871X("dc_handle_join_request for Ad-hoc fail !!!\n");
+			return;
+		}
+#endif
+
+		Set_MSR(padapter, WIFI_FW_ADHOC_STATE);
+
+		val8 = 0xcf;
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+		//switch channel
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+		beacon_timing_control(padapter);
+
+		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+
+		report_join_res(padapter, 1);
+	}
+	else
+	{
+		//DBG_871X("marc: invalid cap:%x\n", caps);
+		return;
+	}
+
+}
+
+void start_clnt_auth(_adapter* padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
+	pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
+
+	pmlmeinfo->auth_seq = 1;
+	pmlmeinfo->reauth_count = 0;
+	pmlmeinfo->reassoc_count = 0;
+	pmlmeinfo->link_count = 0;
+	pmlmeext->retry = 0;
+
+
+	// Because of AP's not receiving deauth before
+	// AP may: 1)not response auth or 2)deauth us after link is complete
+	// issue deauth before issuing auth to deal with the situation
+#ifndef CONFIG_PLATFORM_RTK_DMP
+	//	Commented by Albert 2012/07/21
+	//	For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it.
+	issue_deauth(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
+#endif
+
+	DBG_871X_LEVEL(_drv_always_, "start auth\n");
+	issue_auth(padapter, NULL, 0);
+
+	set_link_timer(pmlmeext, REAUTH_TO);
+
+}
+
+
+void start_clnt_assoc(_adapter* padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
+	pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
+
+	issue_assocreq(padapter);
+
+	set_link_timer(pmlmeext, REASSOC_TO);
+}
+
+unsigned int receive_disconnect(_adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	//check A3
+	if (!(_rtw_memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+	if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+	{
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+		{
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_del_sta_event(padapter, MacAddr, reason);
+
+		}
+		else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE)
+		{
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_join_res(padapter, -2);
+		}
+	}
+
+	return _SUCCESS;
+}
+
+#ifdef CONFIG_80211D
+static void process_80211d(PADAPTER padapter, WLAN_BSSID_EX *bssid)
+{
+	struct registry_priv *pregistrypriv;
+	struct mlme_ext_priv *pmlmeext;
+	RT_CHANNEL_INFO *chplan_new;
+	u8 channel;
+	u8 i;
+
+
+	pregistrypriv = &padapter->registrypriv;
+	pmlmeext = &padapter->mlmeextpriv;
+
+	// Adjust channel plan by AP Country IE
+	if (pregistrypriv->enable80211d &&
+		(!pmlmeext->update_channel_plan_by_ap_done))
+	{
+		u8 *ie, *p;
+		u32 len;
+		RT_CHANNEL_PLAN chplan_ap;
+		RT_CHANNEL_INFO chplan_sta[MAX_CHANNEL_NUM];
+		u8 country[4];
+		u8 fcn; // first channel number
+		u8 noc; // number of channel
+		u8 j, k;
+
+		ie = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+		if (!ie) return;
+		if (len < 6) return;
+
+		ie += 2;
+		p = ie;
+		ie += len;
+
+		_rtw_memset(country, 0, 4);
+		_rtw_memcpy(country, p, 3);
+		p += 3;
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+				("%s: 802.11d country=%s\n", __FUNCTION__, country));
+
+		i = 0;
+		while ((ie - p) >= 3)
+		{
+			fcn = *(p++);
+			noc = *(p++);
+			p++;
+
+			for (j = 0; j < noc; j++)
+			{
+				if (fcn <= 14) channel = fcn + j; // 2.4 GHz
+				else channel = fcn + j*4; // 5 GHz
+
+				chplan_ap.Channel[i++] = channel;
+			}
+		}
+		chplan_ap.Len = i;
+
+#ifdef CONFIG_DEBUG_RTL871X
+#ifdef PLATFORM_LINUX
+		i = 0;
+		DBG_871X("%s: AP[%s] channel plan {", __func__, bssid->Ssid.Ssid);
+		while ((i < chplan_ap.Len) && (chplan_ap.Channel[i] != 0))
+		{
+			DBG_8192C("%02d,", chplan_ap.Channel[i]);
+			i++;
+		}
+		DBG_871X("}\n");
+#endif
+#endif
+
+		_rtw_memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
+#ifdef CONFIG_DEBUG_RTL871X
+#ifdef PLATFORM_LINUX
+		i = 0;
+		DBG_871X("%s: STA channel plan {", __func__);
+		while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
+		{
+			DBG_871X("%02d(%c),", chplan_sta[i].ChannelNum, chplan_sta[i].ScanType==SCAN_PASSIVE?'p':'a');
+			i++;
+		}
+		DBG_871X("}\n");
+#endif
+#endif
+
+		_rtw_memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
+		chplan_new = pmlmeext->channel_set;
+
+		i = j = k = 0;
+		if (pregistrypriv->wireless_mode & WIRELESS_11G)
+		{
+			do {
+				if ((i == MAX_CHANNEL_NUM) ||
+					(chplan_sta[i].ChannelNum == 0) ||
+					(chplan_sta[i].ChannelNum > 14))
+					break;
+
+				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
+					break;
+
+				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j])
+				{
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					i++;
+					j++;
+					k++;
+				}
+				else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j])
+				{
+					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+//					chplan_new[k].ScanType = chplan_sta[i].ScanType;
+					chplan_new[k].ScanType = SCAN_PASSIVE;
+					i++;
+					k++;
+				}
+				else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j])
+				{
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					j++;
+					k++;
+				}
+			} while (1);
+
+			// change AP not support channel to Passive scan
+			while ((i < MAX_CHANNEL_NUM) &&
+				(chplan_sta[i].ChannelNum != 0) &&
+				(chplan_sta[i].ChannelNum <= 14))
+			{
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+//				chplan_new[k].ScanType = chplan_sta[i].ScanType;
+				chplan_new[k].ScanType = SCAN_PASSIVE;
+				i++;
+				k++;
+			}
+
+			// add channel AP supported
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
+			{
+				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				chplan_new[k].ScanType = SCAN_ACTIVE;
+				j++;
+				k++;
+			}
+		}
+		else
+		{
+			// keep original STA 2.4G channel plan
+			while ((i < MAX_CHANNEL_NUM) &&
+				(chplan_sta[i].ChannelNum != 0) &&
+				(chplan_sta[i].ChannelNum <= 14))
+			{
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+				chplan_new[k].ScanType = chplan_sta[i].ScanType;
+				i++;
+				k++;
+			}
+
+			// skip AP 2.4G channel plan
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
+			{
+				j++;
+			}
+		}
+
+		if (pregistrypriv->wireless_mode & WIRELESS_11A)
+		{
+			do {
+				if ((i == MAX_CHANNEL_NUM) ||
+					(chplan_sta[i].ChannelNum == 0))
+					break;
+
+				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0))
+					break;
+
+				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j])
+				{
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					i++;
+					j++;
+					k++;
+				}
+				else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j])
+				{
+					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+//					chplan_new[k].ScanType = chplan_sta[i].ScanType;
+					chplan_new[k].ScanType = SCAN_PASSIVE;
+					i++;
+					k++;
+				}
+				else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j])
+				{
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					j++;
+					k++;
+				}
+			} while (1);
+
+			// change AP not support channel to Passive scan
+			while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
+			{
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+//				chplan_new[k].ScanType = chplan_sta[i].ScanType;
+				chplan_new[k].ScanType = SCAN_PASSIVE;
+				i++;
+				k++;
+			}
+
+			// add channel AP supported
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] != 0))
+			{
+				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				chplan_new[k].ScanType = SCAN_ACTIVE;
+				j++;
+				k++;
+			}
+		}
+		else
+		{
+			// keep original STA 5G channel plan
+			while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
+			{
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+				chplan_new[k].ScanType = chplan_sta[i].ScanType;
+				i++;
+				k++;
+			}
+		}
+
+		pmlmeext->update_channel_plan_by_ap_done = 1;
+
+#ifdef CONFIG_DEBUG_RTL871X
+#ifdef PLATFORM_LINUX
+		k = 0;
+		DBG_871X("%s: new STA channel plan {", __func__);
+		while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0))
+		{
+			DBG_871X("%02d(%c),", chplan_new[k].ChannelNum, chplan_new[k].ScanType==SCAN_PASSIVE?'p':'c');
+			k++;
+		}
+		DBG_871X("}\n");
+#endif
+#endif
+
+#if 0
+		// recover the right channel index
+		channel = chplan_sta[pmlmeext->sitesurvey_res.channel_idx].ChannelNum;
+		k = 0;
+		while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0))
+		{
+			if (chplan_new[k].ChannelNum == channel) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+						 ("%s: change mlme_ext sitesurvey channel index from %d to %d\n",
+						  __FUNCTION__, pmlmeext->sitesurvey_res.channel_idx, k));
+				pmlmeext->sitesurvey_res.channel_idx = k;
+				break;
+			}
+			k++;
+		}
+#endif
+	}
+
+	// If channel is used by AP, set channel scan type to active
+	channel = bssid->Configuration.DSConfig;
+	chplan_new = pmlmeext->channel_set;
+	i = 0;
+	while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0))
+	{
+		if (chplan_new[i].ChannelNum == channel)
+		{
+			if (chplan_new[i].ScanType == SCAN_PASSIVE)
+			{
+				//5G Bnad 2, 3 (DFS) doesn't change to active scan
+				if(channel >= 52 && channel <= 144)
+					break;
+				
+				chplan_new[i].ScanType = SCAN_ACTIVE;
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+						 ("%s: change channel %d scan type from passive to active\n",
+						  __FUNCTION__, channel));
+			}
+			break;
+		}
+		i++;
+	}
+}
+#endif
+
+/****************************************************************************
+
+Following are the functions to report events
+
+*****************************************************************************/
+
+void report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct survey_event	*psurvey_evt;
+	struct C2HEvent_Header *pc2h_evt_hdr;
+	struct mlme_ext_priv *pmlmeext;
+	struct cmd_priv *pcmdpriv;
+	//u8 *pframe = precv_frame->u.hdr.rx_data;
+	//uint len = precv_frame->u.hdr.len;
+
+	if(!padapter)
+		return;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pcmdpriv = &padapter->cmdpriv;
+	
+
+	if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
+	{
+		return;
+	}
+
+	cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
+	if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
+	{
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		return;
+	}
+
+	_rtw_init_listhead(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct survey_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	psurvey_evt = (struct survey_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
+
+	if (collect_bss_info(padapter, precv_frame, (WLAN_BSSID_EX *)&psurvey_evt->bss) == _FAIL)
+	{
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		rtw_mfree((u8 *)pevtcmd, cmdsz);
+		return;
+	}
+
+#ifdef CONFIG_80211D
+	process_80211d(padapter, &psurvey_evt->bss);
+#endif
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	pmlmeext->sitesurvey_res.bss_cnt++;
+
+	return;
+
+}
+
+void report_surveydone_event(_adapter *padapter)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct surveydone_event *psurveydone_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
+	{
+		return;
+	}
+
+	cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
+	if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
+	{
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		return;
+	}
+
+	_rtw_init_listhead(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct surveydone_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	psurveydone_evt = (struct surveydone_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
+	psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
+
+	DBG_871X("survey done event(%x)\n", psurveydone_evt->bss_cnt);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+
+}
+
+void report_join_res(_adapter *padapter, int res)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct joinbss_event		*pjoinbss_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
+	{
+		return;
+	}
+
+	cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
+	if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
+	{
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		return;
+	}
+
+	_rtw_init_listhead(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct joinbss_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	pjoinbss_evt = (struct joinbss_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
+	_rtw_memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
+	pjoinbss_evt->network.join_res 	= pjoinbss_evt->network.aid = res;
+
+	DBG_871X("report_join_res(%d)\n", res);
+	
+	
+	rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network);
+	
+	
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+
+}
+
+void report_del_sta_event(_adapter *padapter, unsigned char* MacAddr, unsigned short reason)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct sta_info *psta;
+	int	mac_id;
+	struct stadel_event			*pdel_sta_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
+	{
+		return;
+	}
+
+	cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
+	if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
+	{
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		return;
+	}
+
+	_rtw_init_listhead(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct stadel_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	pdel_sta_evt = (struct stadel_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
+	_rtw_memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+	_rtw_memcpy((unsigned char *)(pdel_sta_evt->rsvd),(unsigned char *)(&reason),2);
+
+
+	psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
+	if(psta)
+		mac_id = (int)psta->mac_id;	
+	else
+		mac_id = (-1);
+
+	pdel_sta_evt->mac_id = mac_id;
+
+	DBG_871X("report_del_sta_event: delete STA, mac_id=%d\n", mac_id);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+}
+
+void report_add_sta_event(_adapter *padapter, unsigned char* MacAddr, int cam_idx)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct stassoc_event		*padd_sta_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
+	{
+		return;
+	}
+
+	cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
+	if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
+	{
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		return;
+	}
+
+	_rtw_init_listhead(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct stassoc_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	padd_sta_evt = (struct stassoc_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
+	_rtw_memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+	padd_sta_evt->cam_id = cam_idx;
+
+	DBG_871X("report_add_sta_event: add STA\n");
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+}
+
+
+/****************************************************************************
+
+Following are the event callback functions
+
+*****************************************************************************/
+
+//for sta/adhoc mode
+void update_sta_info(_adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	//ERP
+	VCS_update(padapter, psta);
+
+#ifdef CONFIG_80211N_HT
+	//HT
+	if(pmlmepriv->htpriv.ht_option)
+	{
+		psta->htpriv.ht_option = _TRUE;
+
+		psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
+
+		if (support_short_GI(padapter, &(pmlmeinfo->HT_caps)))
+			psta->htpriv.sgi = _TRUE;
+
+		psta->qos_option = _TRUE;
+		
+	}
+	else
+#endif //CONFIG_80211N_HT
+	{
+#ifdef CONFIG_80211N_HT
+		psta->htpriv.ht_option = _FALSE;
+
+		psta->htpriv.ampdu_enable = _FALSE;
+		
+		psta->htpriv.sgi = _FALSE;
+#endif //CONFIG_80211N_HT
+		psta->qos_option = _FALSE;
+
+	}
+#ifdef CONFIG_80211N_HT
+	psta->htpriv.bwmode = pmlmeext->cur_bwmode;
+	psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
+	
+	psta->htpriv.agg_enable_bitmap = 0x0;//reset
+	psta->htpriv.candidate_tid_bitmap = 0x0;//reset
+#endif //CONFIG_80211N_HT
+
+	//QoS
+	if(pmlmepriv->qospriv.qos_option)
+		psta->qos_option = _TRUE;
+	
+
+	psta->state = _FW_LINKED;
+
+}
+
+void mlmeext_joinbss_event_callback(_adapter *padapter, int join_res)
+{
+	struct sta_info		*psta, *psta_bmc;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX 		*cur_network = &(pmlmeinfo->network);
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	u8	join_type;
+	u16 media_status;
+
+	if(join_res < 0)
+	{
+		join_type = 1;
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+		rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
+
+		//restore to initial setting.
+		update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+
+		goto exit_mlmeext_joinbss_event_callback;
+	}
+
+	if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
+	{
+		//for bc/mc
+		psta_bmc = rtw_get_bcmc_stainfo(padapter);
+		if(psta_bmc)
+		{
+			pmlmeinfo->FW_sta_info[psta_bmc->mac_id].psta = psta_bmc;
+			update_bmc_sta_support_rate(padapter, psta_bmc->mac_id);
+			Update_RA_Entry(padapter, psta_bmc);
+		}
+	}
+
+
+	//turn on dynamic functions
+	Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE);
+
+	// update IOT-releated issue
+	update_IOT_info(padapter);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates);
+
+	//BCN interval
+	rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
+
+	//udpate capability
+	update_capinfo(padapter, pmlmeinfo->capability);
+
+	//WMM, Update EDCA param
+	WMMOnAssocRsp(padapter);
+
+	//HT
+	HTOnAssocRsp(padapter);
+
+#ifndef CONFIG_CONCURRENT_MODE
+	//	Call set_channel_bwmode when the CONFIG_CONCURRENT_MODE doesn't be defined.
+	//Set cur_channel&cur_bwmode&cur_ch_offset
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+#endif
+
+	psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
+	if (psta) //only for infra. mode
+	{
+		pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+		//DBG_871X("set_sta_rate\n");
+
+		psta->wireless_mode = pmlmeext->cur_wireless_mode;
+	
+		//set per sta rate after updating HT cap.
+		set_sta_rate(padapter, psta);
+		
+		#if (RATE_ADAPTIVE_SUPPORT==1)	//for 88E RA	
+		rtw_hal_set_hwreg(padapter,HW_VAR_TX_RPT_MAX_MACID, (u8*)&psta->mac_id);		
+		#endif
+		media_status = (psta->mac_id<<8)|1; //  MACID|OPMODE: 1 means connect
+		rtw_hal_set_hwreg(padapter,HW_VAR_H2C_MEDIA_STATUS_RPT,(u8 *)&media_status);
+	}
+
+	join_type = 2;
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+	if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+	{
+		// correcting TSF
+		correct_TSF(padapter, pmlmeext);
+	
+		//set_link_timer(pmlmeext, DISCONNECT_TO);
+	}
+
+#ifdef CONFIG_LPS
+	rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
+#endif
+
+exit_mlmeext_joinbss_event_callback:
+
+#ifdef CONFIG_DUALMAC_CONCURRENT
+	dc_handle_join_done(padapter, join_res);
+#endif
+#ifdef CONFIG_CONCURRENT_MODE
+	concurrent_chk_joinbss_done(padapter, join_res);
+#endif
+
+	DBG_871X("=>%s\n", __FUNCTION__);
+
+}
+
+void mlmeext_sta_add_event_callback(_adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8	join_type;
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+	if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
+	{
+		if(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)//adhoc master or sta_count>1
+		{
+			//nothing to do
+		}
+		else//adhoc client
+		{
+			//update TSF Value
+			//update_TSF(pmlmeext, pframe, len);			
+
+			// correcting TSF
+			correct_TSF(padapter, pmlmeext);
+
+			//start beacon
+			if(send_beacon(padapter)==_FAIL)
+			{
+				pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
+
+				pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
+
+				return;
+			}
+
+			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+				
+		}
+
+		join_type = 2;
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	}
+
+	pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+	//rate radaptive
+	Update_RA_Entry(padapter, psta);
+
+	//update adhoc sta_info
+	update_sta_info(padapter, psta);
+
+}
+
+void mlmeext_sta_del_event_callback(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter))
+	{
+		//set_opmode_cmd(padapter, infra_client_with_mlme);
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
+		rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
+
+		//restore to initial setting.
+		update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+
+#ifdef CONFIG_DUALMAC_CONCURRENT
+		dc_set_channel_bwmode_disconnect(padapter);
+#else
+#ifdef CONFIG_CONCURRENT_MODE
+		if((check_buddy_fwstate(padapter, _FW_LINKED)) != _TRUE)
+		{
+#endif //CONFIG_CONCURRENT_MODE
+
+		//switch to the 20M Hz mode after disconnect
+		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+		pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+		//SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset);
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	
+#ifdef CONFIG_CONCURRENT_MODE
+		}
+#endif //CONFIG_CONCURRENT_MODE
+#endif //CONFIG_DUALMAC_CONCURRENT
+
+		flush_all_cam_entry(padapter);
+
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+		//set MSR to no link state -> infra. mode
+		Set_MSR(padapter, _HW_STATE_STATION_);
+
+		_cancel_timer_ex(&pmlmeext->link_timer);
+
+	}
+
+}
+
+/****************************************************************************
+
+Following are the functions for the timer handlers
+
+*****************************************************************************/
+void _linked_rx_signal_strehgth_display(_adapter *padapter);
+void _linked_rx_signal_strehgth_display(_adapter *padapter)
+{
+	struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
+      struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 mac_id;
+	int UndecoratedSmoothedPWDB;
+	#if 0
+	DBG_871X("============ linked status check ===================\n");
+	DBG_871X("pathA Rx SNRdb:%d, pathB Rx SNRdb:%d\n",padapter->recvpriv.RxSNRdB[0], padapter->recvpriv.RxSNRdB[1]);
+	DBG_871X("pathA Rx PWDB:%d\n",padapter->recvpriv.rxpwdb);		
+	rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
+	DBG_871X("UndecoratedSmoothedPWDB:%d\n",UndecoratedSmoothedPWDB);
+	DBG_871X("Rx RSSI:%d\n",padapter->recvpriv.rssi);
+	DBG_871X("Rx Signal_strength:%d\n",padapter->recvpriv.signal_strength);
+	DBG_871X("Rx Signal_qual:%d \n",padapter->recvpriv.signal_qual);
+	if ( check_fwstate( &padapter->mlmepriv,  _FW_LINKED ) )
+	{
+		DBG_871X("bw mode: %d, channel: %d\n", padapter->mlmeextpriv.cur_bwmode, padapter->mlmeextpriv.cur_channel );
+		DBG_871X("received bytes = %d\n", (u32) (padapter->recvpriv.rx_bytes - padapter->recvpriv.last_rx_bytes ) );
+	}	
+	DBG_871X("============ linked status check ===================\n");
+	#endif
+	if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+	{
+		mac_id=0;
+	}
+	else if((pmlmeinfo->state&0x03) == _HW_STATE_AP_)
+	{
+		mac_id=2;		
+	}
+	
+	rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP,&mac_id);	
+	#if 0
+	DBG_871X("============ RX GAIN / FALSE ALARM  ===================\n");
+	DBG_871X(" DIG PATH-A(0x%02x), PATH-B(0x%02x)\n",rtw_read8(padapter,0xc50),rtw_read8(padapter,0xc58));
+	DBG_871X(" OFDM -Alarm DA2(0x%04x),DA4(0x%04x),DA6(0x%04x),DA8(0x%04x)\n",
+		rtw_read16(padapter,0xDA2),rtw_read16(padapter,0xDA4),rtw_read16(padapter,0xDA6),rtw_read16(padapter,0xDA8));
+
+	DBG_871X(" CCK -Alarm A5B(0x%02x),A5C(0x%02x)\n",rtw_read8(padapter,0xA5B),rtw_read8(padapter,0xA5C));
+	#endif
+
+
+	rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);	
+	DBG_871X("UndecoratedSmoothedPWDB:%d\n",UndecoratedSmoothedPWDB);
+
+}
+
+u8 chk_ap_is_alive(_adapter *padapter, struct sta_info *psta)
+{
+	u8 ret = _FALSE;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	#ifdef DBG_EXPIRATION_CHK
+	DBG_871X(FUNC_ADPT_FMT" rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu"
+				/*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/
+				", retry:%u\n"
+		, FUNC_ADPT_ARG(padapter)
+		, STA_RX_PKTS_DIFF_ARG(psta)
+		, psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts
+		, psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts
+		/*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts
+		, psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts
+		, psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts
+		, pmlmeinfo->bcn_interval*/
+		, pmlmeext->retry
+	);
+
+	DBG_871X(FUNC_ADPT_FMT" tx_pkts:%llu, link_count:%u\n", FUNC_ADPT_ARG(padapter)
+		, padapter->xmitpriv.tx_pkts
+		, pmlmeinfo->link_count
+	);
+	#endif
+
+	if((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta))
+		&& sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta)
+		&& sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta)
+	)
+	{
+		ret = _FALSE;
+	}
+	else
+	{
+		ret = _TRUE;
+	}
+
+	sta_update_last_rx_pkts(psta);
+
+	return ret;
+}
+
+void linked_status_chk(_adapter *padapter)
+{
+	u32	i;
+	struct sta_info		*psta;
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+
+	if(padapter->bRxRSSIDisplay)
+		 _linked_rx_signal_strehgth_display(padapter);
+
+	#ifdef DBG_CONFIG_ERROR_DETECT	
+	rtw_hal_sreset_linked_status_check(padapter);
+	#endif
+
+	if (is_client_associated_to_ap(padapter))
+	{
+		//linked infrastructure client mode
+
+		int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
+		int rx_chk_limit;
+
+		#if defined(DBG_ROAMING_TEST)
+		rx_chk_limit = 1;
+		#elif defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK)
+		rx_chk_limit = 4;
+		#else
+		rx_chk_limit = 8;
+		#endif
+
+		#ifdef CONFIG_INTEL_WIDI
+		if (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_NONE)
+			rx_chk_limit = 1;
+		#endif
+
+		if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
+		{
+			bool is_p2p_enable = _FALSE;
+			#ifdef CONFIG_P2P
+			is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
+			#endif
+			
+			if (chk_ap_is_alive(padapter, psta) == _FALSE)
+				rx_chk = _FAIL;
+
+			if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
+				tx_chk = _FAIL;
+
+			#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+			if (pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL)) {
+				u8 backup_oper_channel=0;
+
+				/* switch to correct channel of current network  before issue keep-alive frames */
+				if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
+					backup_oper_channel = rtw_get_oper_ch(padapter);
+					SelectChannel(padapter, pmlmeext->cur_channel);
+				}
+
+				if (rx_chk != _SUCCESS)
+					issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
+
+				if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) || rx_chk != _SUCCESS) {
+					tx_chk = issue_nulldata(padapter, psta->hwaddr, 0, 3, 1);
+					/* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */
+					if (tx_chk == _SUCCESS && !is_p2p_enable)
+						rx_chk = _SUCCESS;
+				}
+
+				/* back to the original operation channel */
+				if(backup_oper_channel>0)
+					SelectChannel(padapter, backup_oper_channel);
+
+			}
+			else
+			#endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */
+			{
+				if (rx_chk != _SUCCESS) {
+					if (pmlmeext->retry == 0) {
+						#ifdef DBG_EXPIRATION_CHK
+						DBG_871X("issue_probereq to trigger probersp, retry=%d\n", pmlmeext->retry);
+						#endif
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+					}
+				}
+
+				if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) {
+					#ifdef DBG_EXPIRATION_CHK
+					DBG_871X("%s issue_nulldata 0\n", __FUNCTION__);
+					#endif
+					tx_chk = issue_nulldata(padapter, NULL, 0, 1, 0);
+				}
+			}
+
+			if (rx_chk == _FAIL) {
+				pmlmeext->retry++;
+				if (pmlmeext->retry > rx_chk_limit) {
+					DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n",
+						FUNC_ADPT_ARG(padapter));
+					receive_disconnect(padapter, pmlmeinfo->network.MacAddress
+						, WLAN_REASON_EXPIRATION_CHK);
+					return;
+				}
+			} else {
+				pmlmeext->retry = 0;
+			}
+
+			if (tx_chk == _FAIL) {
+				pmlmeinfo->link_count &= 0xf;
+			} else {
+				pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
+				pmlmeinfo->link_count = 0;
+			}
+
+		} //end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL)
+	}
+	else if (is_client_associated_to_ibss(padapter))
+	{
+		//linked IBSS mode
+		//for each assoc list entry to check the rx pkt counter
+		for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
+		{
+			if (pmlmeinfo->FW_sta_info[i].status == 1)
+			{
+				psta = pmlmeinfo->FW_sta_info[i].psta;
+
+				if(NULL==psta) continue;
+
+				if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta))
+				{
+
+					if(pmlmeinfo->FW_sta_info[i].retry<3)
+					{
+						pmlmeinfo->FW_sta_info[i].retry++;
+					}
+					else
+					{
+						pmlmeinfo->FW_sta_info[i].retry = 0;
+						pmlmeinfo->FW_sta_info[i].status = 0;
+						report_del_sta_event(padapter, psta->hwaddr
+							, 65535// indicate disconnect caused by no rx
+						);
+					}	
+				}
+				else
+				{
+					pmlmeinfo->FW_sta_info[i].retry = 0;
+					pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
+				}
+			}
+		}
+
+		//set_link_timer(pmlmeext, DISCONNECT_TO);
+
+	}
+
+}
+
+void survey_timer_hdl(_adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct sitesurvey_parm	*psurveyPara;
+	struct cmd_priv					*pcmdpriv=&padapter->cmdpriv;
+	struct mlme_ext_priv 		*pmlmeext = &padapter->mlmeextpriv;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
+#endif
+
+	//DBG_871X("marc: survey timer\n");
+#ifdef PLATFORM_FREEBSD
+	rtw_mtx_lock(NULL);
+	 if (callout_pending(&padapter->mlmeextpriv.survey_timer.callout)) {
+		 /* callout was reset */
+		 //mtx_unlock(&sc->sc_mtx);
+		 rtw_mtx_unlock(NULL);
+		 return;
+	 }
+	 if (!callout_active(&padapter->mlmeextpriv.survey_timer.callout)) {
+		 /* callout was stopped */
+		 //mtx_unlock(&sc->sc_mtx);
+		 rtw_mtx_unlock(NULL);
+		 return;
+	 }
+	 callout_deactivate(&padapter->mlmeextpriv.survey_timer.callout);
+
+
+#endif
+
+	//issue rtw_sitesurvey_cmd
+	if (pmlmeext->sitesurvey_res.state > SCAN_START)
+	{
+		if(pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
+		{
+#ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+			if( padapter->mlmeextpriv.mlmext_info.scan_cnt != RTW_SCAN_NUM_OF_CH )
+#endif //CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
+				pmlmeext->sitesurvey_res.channel_idx++;
+		}
+
+		if(pmlmeext->scan_abort == _TRUE)
+		{
+			#ifdef CONFIG_P2P
+			if(!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE))
+			{
+				rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
+				pmlmeext->sitesurvey_res.channel_idx = 3;
+				DBG_871X("%s idx:%d, cnt:%u\n", __FUNCTION__
+					, pmlmeext->sitesurvey_res.channel_idx
+					, pwdinfo->find_phase_state_exchange_cnt
+				);
+			}
+			else
+			#endif
+			{
+				pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
+				DBG_871X("%s idx:%d\n", __FUNCTION__
+					, pmlmeext->sitesurvey_res.channel_idx
+				);
+			}
+
+			pmlmeext->scan_abort = _FALSE;//reset
+		}
+
+		if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
+		{
+			goto exit_survey_timer_hdl;
+		}
+
+		if ((psurveyPara = (struct sitesurvey_parm*)rtw_zmalloc(sizeof(struct sitesurvey_parm))) == NULL)
+		{
+			rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+			goto exit_survey_timer_hdl;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+		rtw_enqueue_cmd(pcmdpriv, ph2c);
+	}
+
+
+exit_survey_timer_hdl:
+#ifdef PLATFORM_FREEBSD
+		rtw_mtx_unlock(NULL);
+#endif
+
+	return;
+}
+
+void link_timer_hdl(_adapter *padapter)
+{
+	//static unsigned int		rx_pkt = 0;
+	//static u64				tx_cnt = 0;
+	//struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	//struct sta_priv		*pstapriv = &padapter->stapriv;
+
+#ifdef PLATFORM_FREEBSD
+	rtw_mtx_lock(NULL);
+	 if (callout_pending(&padapter->mlmeextpriv.survey_timer.callout)) {
+		 /* callout was reset */
+		 //mtx_unlock(&sc->sc_mtx);
+		 rtw_mtx_unlock(NULL);
+		 return;
+	 }
+	 if (!callout_active(&padapter->mlmeextpriv.survey_timer.callout)) {
+		 /* callout was stopped */
+		 //mtx_unlock(&sc->sc_mtx);
+		 rtw_mtx_unlock(NULL);
+		 return;
+	 }
+	 callout_deactivate(&padapter->mlmeextpriv.survey_timer.callout);
+
+	
+#endif
+
+	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
+	{
+		DBG_871X("link_timer_hdl:no beacon while connecting\n");
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		report_join_res(padapter, -3);
+	}
+	else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE)
+	{
+		//re-auth timer
+		if (++pmlmeinfo->reauth_count > REAUTH_LIMIT)
+		{
+			//if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto)
+			//{
+				pmlmeinfo->state = 0;
+				report_join_res(padapter, -1);
+				return;
+			//}
+			//else
+			//{
+			//	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
+			//	pmlmeinfo->reauth_count = 0;
+			//}
+		}
+		
+		DBG_871X("link_timer_hdl: auth timeout and try again\n");
+		pmlmeinfo->auth_seq = 1;
+		issue_auth(padapter, NULL, 0);
+		set_link_timer(pmlmeext, REAUTH_TO);
+	}
+	else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)
+	{
+		//re-assoc timer
+		if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT)
+		{
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_join_res(padapter, -2);
+			return;
+		}
+
+		DBG_871X("link_timer_hdl: assoc timeout and try again\n");
+		issue_assocreq(padapter);
+		set_link_timer(pmlmeext, REASSOC_TO);
+	}
+#if 0
+	else if (is_client_associated_to_ap(padapter))
+	{
+		//linked infrastructure client mode
+		if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
+		{
+			/*to monitor whether the AP is alive or not*/
+			if (rx_pkt == psta->sta_stats.rx_pkts)
+			{
+				receive_disconnect(padapter, pmlmeinfo->network.MacAddress);
+				return;
+			}
+			else
+			{
+				rx_pkt = psta->sta_stats.rx_pkts;
+				set_link_timer(pmlmeext, DISCONNECT_TO);
+			}
+
+			//update the EDCA paramter according to the Tx/RX mode
+			update_EDCA_param(padapter);
+
+			/*to send the AP a nulldata if no frame is xmitted in order to keep alive*/
+			if (pmlmeinfo->link_count++ == 0)
+			{
+				tx_cnt = pxmitpriv->tx_pkts;
+			}
+			else if ((pmlmeinfo->link_count & 0xf) == 0)
+			{
+				if (tx_cnt == pxmitpriv->tx_pkts)
+				{
+					issue_nulldata(padapter, NULL, 0, 0, 0);
+				}
+
+				tx_cnt = pxmitpriv->tx_pkts;
+			}
+		} //end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL)
+	}
+	else if (is_client_associated_to_ibss(padapter))
+	{
+		//linked IBSS mode
+		//for each assoc list entry to check the rx pkt counter
+		for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
+		{
+			if (pmlmeinfo->FW_sta_info[i].status == 1)
+			{
+				psta = pmlmeinfo->FW_sta_info[i].psta;
+
+				if (pmlmeinfo->FW_sta_info[i].rx_pkt == psta->sta_stats.rx_pkts)
+				{
+					pmlmeinfo->FW_sta_info[i].status = 0;
+					report_del_sta_event(padapter, psta->hwaddr);
+				}
+				else
+				{
+					pmlmeinfo->FW_sta_info[i].rx_pkt = psta->sta_stats.rx_pkts;
+				}
+			}
+		}
+
+		set_link_timer(pmlmeext, DISCONNECT_TO);
+	}
+#endif
+
+#ifdef PLATFORM_FREEBSD
+	rtw_mtx_unlock(NULL);
+#endif
+
+	return;
+}
+
+void addba_timer_hdl(struct sta_info *psta)
+{
+#ifdef CONFIG_80211N_HT
+	struct ht_priv	*phtpriv;
+
+	if(!psta)
+		return;
+	
+	phtpriv = &psta->htpriv;
+
+	if((phtpriv->ht_option==_TRUE) && (phtpriv->ampdu_enable==_TRUE)) 
+	{
+		if(phtpriv->candidate_tid_bitmap)
+			phtpriv->candidate_tid_bitmap=0x0;
+		
+	}
+#endif //CONFIG_80211N_HT
+}
+
+u8 NULL_hdl(_adapter *padapter, u8 *pbuf)
+{
+	return H2C_SUCCESS;
+}
+
+u8 setopmode_hdl(_adapter *padapter, u8 *pbuf)
+{
+	u8	type;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
+
+	if(psetop->mode == Ndis802_11APMode)
+	{
+		pmlmeinfo->state = WIFI_FW_AP_STATE;
+		type = _HW_STATE_AP_;
+#ifdef CONFIG_NATIVEAP_MLME
+		//start_ap_mode(padapter);
+#endif
+	}
+	else if(psetop->mode == Ndis802_11Infrastructure)
+	{
+		pmlmeinfo->state &= ~(BIT(0)|BIT(1));// clear state
+		pmlmeinfo->state |= WIFI_FW_STATION_STATE;//set to 	STATION_STATE
+		type = _HW_STATE_STATION_;
+	}
+	else if(psetop->mode == Ndis802_11IBSS)
+	{
+		type = _HW_STATE_ADHOC_;
+	}
+	else
+	{
+		type = _HW_STATE_NOLINK_;
+	}
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
+	//Set_NETYPE0_MSR(padapter, type);
+
+	return H2C_SUCCESS;
+	
+}
+
+u8 createbss_hdl(_adapter *padapter, u8 *pbuf)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX	*pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
+	struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
+	//u32	initialgain;
+
+	
+	if(pparm->network.InfrastructureMode == Ndis802_11APMode)
+	{
+#ifdef CONFIG_AP_MODE
+	
+		if(pmlmeinfo->state == WIFI_FW_AP_STATE)
+		{		
+			//todo:
+			return H2C_SUCCESS;		
+		}		
+#endif
+	}
+
+	//below is for ad-hoc master
+	if(pparm->network.InfrastructureMode == Ndis802_11IBSS)
+	{
+		rtw_joinbss_reset(padapter);
+	
+		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+		pmlmeext->cur_ch_offset= HAL_PRIME_CHNL_OFFSET_DONT_CARE;	
+		pmlmeinfo->ERP_enable = 0;
+		pmlmeinfo->WMM_enable = 0;
+		pmlmeinfo->HT_enable = 0;
+		pmlmeinfo->HT_caps_enable = 0;
+		pmlmeinfo->HT_info_enable = 0;
+		pmlmeinfo->agg_enable_bitmap = 0;
+		pmlmeinfo->candidate_tid_bitmap = 0;
+
+		//disable dynamic functions, such as high power, DIG
+		Save_DM_Func_Flag(padapter);
+		Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
+
+		//config the initial gain under linking, need to write the BB registers
+		//initialgain = 0x1E;
+		//rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
+
+		//cancel link timer 
+		_cancel_timer_ex(&pmlmeext->link_timer);
+
+		//clear CAM
+		flush_all_cam_entry(padapter);	
+
+		_rtw_memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength)); 
+		pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength;
+
+		if(pnetwork->IELength>MAX_IE_SZ)//Check pbuf->IELength
+			return H2C_PARAMETERS_ERROR;
+
+		_rtw_memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength);
+	
+		start_create_ibss(padapter);
+
+	}	
+
+	return H2C_SUCCESS;
+
+}
+
+u8 join_cmd_hdl(_adapter *padapter, u8 *pbuf)
+{
+	u8	join_type;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	struct joinbss_parm	*pparm = (struct joinbss_parm *)pbuf;
+#endif //CONFIG_ANTENNA_DIVERSITY
+	u32 i;
+        //u32	initialgain;
+	//u32	acparm;
+
+	//check already connecting to AP or not
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+	{
+		if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
+		{
+			issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 5, 100);
+		}
+
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		
+		//clear CAM
+		flush_all_cam_entry(padapter);		
+		
+		_cancel_timer_ex(&pmlmeext->link_timer);
+		
+		//set MSR to nolink -> infra. mode		
+		//Set_MSR(padapter, _HW_STATE_NOLINK_);
+		Set_MSR(padapter, _HW_STATE_STATION_);	
+		
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
+	}
+
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	rtw_antenna_select_cmd(padapter, pparm->network.PhyInfo.Optimum_antenna, _FALSE);
+#endif
+
+#ifdef CONFIG_WAPI_SUPPORT
+	rtw_wapi_clear_all_cam_entry(padapter);
+#endif
+
+	rtw_joinbss_reset(padapter);
+	
+	pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+	pmlmeext->cur_ch_offset= HAL_PRIME_CHNL_OFFSET_DONT_CARE;	
+	pmlmeinfo->ERP_enable = 0;
+	pmlmeinfo->WMM_enable = 0;
+	pmlmeinfo->HT_enable = 0;
+	pmlmeinfo->HT_caps_enable = 0;
+	pmlmeinfo->HT_info_enable = 0;
+	pmlmeinfo->agg_enable_bitmap = 0;
+	pmlmeinfo->candidate_tid_bitmap = 0;
+	pmlmeinfo->bwmode_updated = _FALSE;
+	//pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX;
+
+	_rtw_memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength)); 
+	pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength;
+	
+	if(pnetwork->IELength>MAX_IE_SZ)//Check pbuf->IELength
+		return H2C_PARAMETERS_ERROR;	
+		
+	_rtw_memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength); 
+
+	//Check AP vendor to move rtw_joinbss_cmd()
+	//pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->IEs, pnetwork->IELength);
+
+	for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pnetwork->IELength;)
+	{
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pnetwork->IEs + i);
+
+		switch (pIE->ElementID)
+		{
+			case _VENDOR_SPECIFIC_IE_://Get WMM IE.
+				if ( _rtw_memcmp(pIE->data, WMM_OUI, 4) )
+				{
+					pmlmeinfo->WMM_enable = 1;
+				}
+				break;
+
+			case _HT_CAPABILITY_IE_:	//Get HT Cap IE.
+				pmlmeinfo->HT_caps_enable = 1;
+				break;
+
+			case _HT_EXTRA_INFO_IE_:	//Get HT Info IE.
+#ifdef CONFIG_80211N_HT
+				pmlmeinfo->HT_info_enable = 1;
+
+				//spec case only for cisco's ap because cisco's ap issue assoc rsp using mcs rate @40MHz or @20MHz	
+//#if !defined(CONFIG_CONCURRENT_MODE) && !defined(CONFIG_DUALMAC_CONCURRENT)
+//				if(pmlmeinfo->assoc_AP_vendor == ciscoAP)
+//#endif
+				{				
+					struct HT_info_element *pht_info = (struct HT_info_element *)(pIE->data);
+							
+					if ((pregpriv->cbw40_enable) &&	 (pht_info->infos[0] & BIT(2)))
+					{
+						//switch to the 40M Hz mode according to the AP
+						pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+						switch (pht_info->infos[0] & 0x3)
+						{
+							case 1:
+								pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+								break;
+			
+							case 3:
+								pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+								break;
+				
+							default:
+								pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+								break;
+						}
+
+						DBG_871X("set ch/bw before connected\n");
+					}
+				}
+#endif //CONFIG_80211N_HT
+				break;
+
+			default:
+				break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+#if 0
+	if (padapter->registrypriv.wifi_spec) {
+		// for WiFi test, follow WMM test plan spec
+		acparm = 0x002F431C; // VO
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
+		acparm = 0x005E541C; // VI
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
+		acparm = 0x0000A525; // BE
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
+		acparm = 0x0000A549; // BK
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
+	
+		// for WiFi test, mixed mode with intel STA under bg mode throughput issue
+		if (padapter->mlmepriv.htpriv.ht_option == _FALSE){
+			acparm = 0x00004320;
+			rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
+		}
+	}
+	else {
+		acparm = 0x002F3217; // VO
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
+		acparm = 0x005E4317; // VI
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
+		acparm = 0x00105320; // BE
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
+		acparm = 0x0000A444; // BK
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
+	}
+#endif
+	//disable dynamic functions, such as high power, DIG
+	//Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
+
+	//config the initial gain under linking, need to write the BB registers
+	//initialgain = 0x1E;
+	//rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
+	join_type = 0;
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+	//cancel link timer 
+	_cancel_timer_ex(&pmlmeext->link_timer);
+	
+	start_clnt_join(padapter);
+	
+	return H2C_SUCCESS;
+	
+}
+
+u8 disconnect_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
+	u8	val8;
+
+	if (is_client_associated_to_ap(padapter))
+	{
+		issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100);
+	}
+
+	//set_opmode_cmd(padapter, infra_client_with_mlme);
+
+	//pmlmeinfo->state = WIFI_FW_NULL_STATE;
+	
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
+
+	//restore to initial setting.
+	update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+
+	if(((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
+	{
+		//Stop BCN
+		val8 = 0;
+		rtw_hal_set_hwreg(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8));
+	}
+
+
+	//set MSR to no link state -> infra. mode
+	Set_MSR(padapter, _HW_STATE_STATION_);
+
+	pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+#ifdef CONFIG_DUALMAC_CONCURRENT
+	dc_set_channel_bwmode_disconnect(padapter);
+#else
+#ifdef CONFIG_CONCURRENT_MODE	
+	if((check_buddy_fwstate(padapter, _FW_LINKED)) != _TRUE)
+	{
+#endif //CONFIG_CONCURRENT_MODE
+		//switch to the 20M Hz mode after disconnect
+		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+		pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+#ifdef CONFIG_CONCURRENT_MODE
+	}
+#endif //CONFIG_CONCURRENT_MODE
+#endif //CONFIG_DUALMAC_CONCURRENT
+
+	flush_all_cam_entry(padapter);
+		
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	rtw_free_uc_swdec_pending_queue(padapter);
+	
+	return 	H2C_SUCCESS;
+}
+
+int rtw_scan_ch_decision(_adapter *padapter, struct rtw_ieee80211_channel *out,
+	u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
+{
+	int i, j;
+	int scan_ch_num = 0;
+	int set_idx;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	/* clear out first */
+	_rtw_memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num);
+
+	/* acquire channels from in */
+	j = 0;
+	for (i=0;i<in_num;i++) {
+		if (0)
+		DBG_871X(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[i]));
+		if(in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED)
+			&& (set_idx=rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value)) >=0
+		)
+		{
+			_rtw_memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel));
+			
+			if(pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
+				out[j].flags &= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
+				
+			j++;
+		}
+		if(j>=out_num)
+			break;
+	}
+	
+	/* if out is empty, use channel_set as default */
+	if(j == 0) {
+		for (i=0;i<pmlmeext->max_chan_nums;i++) {
+			out[i].hw_value = pmlmeext->channel_set[i].ChannelNum;
+			
+			if(pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
+				out[i].flags &= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
+
+			j++;
+		}
+	}
+
+	if (padapter->setband == GHZ_24) {				// 2.4G
+		for (i=0; i < j ; i++) {
+			if (out[i].hw_value > 35) 
+				_rtw_memset(&out[i], 0 , sizeof(struct rtw_ieee80211_channel));
+			else
+				scan_ch_num++;
+		}
+		j = scan_ch_num;
+	} else if  (padapter->setband == GHZ_50) {			// 5G
+		for (i=0; i < j ; i++) {
+			if (out[i].hw_value > 35) {
+				_rtw_memcpy(&out[scan_ch_num++], &out[i], sizeof(struct rtw_ieee80211_channel));
+			}
+		}
+		j = scan_ch_num;
+	} else
+		{}
+
+	return j;
+}
+
+u8 sitesurvey_cmd_hdl(_adapter *padapter, u8 *pbuf)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct sitesurvey_parm	*pparm = (struct sitesurvey_parm *)pbuf;
+	u8	bdelayscan = _FALSE;
+	u8	val8;
+	u32	initialgain;
+	u32	i;
+
+#ifdef CONFIG_P2P
+	struct wifidirect_info*	pwdinfo = &padapter->wdinfo;
+#endif
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE)
+	{
+		//for first time sitesurvey_cmd
+		rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);	
+		
+		pmlmeext->sitesurvey_res.state = SCAN_START;
+		pmlmeext->sitesurvey_res.bss_cnt = 0;
+		pmlmeext->sitesurvey_res.channel_idx = 0;
+
+		for(i=0;i<RTW_SSID_SCAN_AMOUNT;i++){
+			if(pparm->ssid[i].SsidLength) {
+				_rtw_memcpy(pmlmeext->sitesurvey_res.ssid[i].Ssid, pparm->ssid[i].Ssid, IW_ESSID_MAX_SIZE);
+				pmlmeext->sitesurvey_res.ssid[i].SsidLength= pparm->ssid[i].SsidLength;
+			} else {
+				pmlmeext->sitesurvey_res.ssid[i].SsidLength= 0;
+			}
+		}
+
+		pmlmeext->sitesurvey_res.ch_num = rtw_scan_ch_decision(padapter
+			, pmlmeext->sitesurvey_res.ch, RTW_CHANNEL_SCAN_AMOUNT
+			, pparm->ch, pparm->ch_num
+		);
+
+		pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
+
+#ifdef CONFIG_DUALMAC_CONCURRENT
+		bdelayscan = dc_handle_site_survey(padapter);
+#endif
+
+		//issue null data if associating to the AP
+		if (is_client_associated_to_ap(padapter) == _TRUE)
+		{
+			pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
+
+			/* switch to correct channel of current network  before issue keep-alive frames */
+			if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
+				SelectChannel(padapter, pmlmeext->cur_channel);
+			}
+
+			issue_nulldata(padapter, NULL, 1, 3, 500);
+
+#ifdef CONFIG_CONCURRENT_MODE
+			if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
+			{
+				DBG_871X("adapter is scanning(buddy_adapter is linked), issue nulldata(pwrbit=1)\n");
+				
+				issue_nulldata(padapter->pbuddy_adapter, NULL, 1, 3, 500);
+			}
+#endif
+			bdelayscan = _TRUE;
+		}
+#ifdef CONFIG_CONCURRENT_MODE
+		else if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
+		{
+			#ifdef CONFIG_TDLS
+			if(padapter->pbuddy_adapter->wdinfo.wfd_tdls_enable == 1)
+			{
+				issue_tunneled_probe_req(padapter->pbuddy_adapter);
+			}
+			#endif //CONFIG_TDLS
+
+			pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
+
+			issue_nulldata(padapter->pbuddy_adapter, NULL, 1, 3, 500);
+
+			bdelayscan = _TRUE;			
+		}
+#endif		
+		if(bdelayscan)
+		{
+			//delay 50ms to protect nulldata(1).
+			set_survey_timer(pmlmeext, 50);
+			return H2C_SUCCESS;
+		}
+	}
+
+	if ((pmlmeext->sitesurvey_res.state == SCAN_START) || (pmlmeext->sitesurvey_res.state == SCAN_TXNULL))
+	{
+#ifdef CONFIG_FIND_BEST_CHANNEL
+#if 0
+		for (i=0; pmlmeext->channel_set[i].ChannelNum !=0; i++) {
+			pmlmeext->channel_set[i].rx_count = 0;				
+		}
+#endif
+#endif /* CONFIG_FIND_BEST_CHANNEL */
+
+		//disable dynamic functions, such as high power, DIG
+		Save_DM_Func_Flag(padapter);
+		Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
+
+		//config the initial gain under scaning, need to write the BB registers
+#ifdef CONFIG_IOCTL_CFG80211
+		if((wdev_to_priv(padapter->rtw_wdev))->p2p_enabled == _TRUE)
+		{
+			initialgain = 0x30;
+		}
+		else
+			initialgain = 0x1E;
+#else	//	go through the WEXT interface //CONFIG_IOCTL_CFG80211
+#ifdef CONFIG_P2P
+		if ( rtw_p2p_chk_state( pwdinfo, P2P_STATE_NONE ) )
+			initialgain = 0x1E;
+		else
+			initialgain = 0x28;
+#else	// CONFIG_P2P
+		initialgain = 0x1E;
+#endif // CONFIG_P2P
+#endif // CONFIG_IOCTL_CFG80211
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
+		
+		//set MSR to no link state
+		Set_MSR(padapter, _HW_STATE_NOLINK_);
+
+		val8 = 1; //under site survey
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+		pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
+	}
+
+	site_survey(padapter);
+
+	return H2C_SUCCESS;
+	
+}
+
+u8 setauth_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct setauth_parm		*pparm = (struct setauth_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	
+	if (pparm->mode < 4)
+	{
+		pmlmeinfo->auth_algo = pparm->mode;
+	}
+
+	return 	H2C_SUCCESS;
+}
+
+u8 setkey_hdl(_adapter *padapter, u8 *pbuf)
+{
+	unsigned short				ctrl;
+	struct setkey_parm		*pparm = (struct setkey_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	unsigned char					null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+	//main tx key for wep.
+	if(pparm->set_tx)
+		pmlmeinfo->key_index = pparm->keyid;
+	
+	//write cam
+	ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;	
+
+	DBG_871X_LEVEL(_drv_always_, "set group key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) "
+			"keyid:%d\n", pparm->algorithm, pparm->keyid);
+	write_cam(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
+	
+	//allow multicast packets to driver
+        padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_ON_RCR_AM, null_addr);
+
+	return H2C_SUCCESS;
+}
+
+u8 set_stakey_hdl(_adapter *padapter, u8 *pbuf)
+{
+	u16 ctrl=0;
+	u8 cam_id;//cam_entry
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct set_stakey_parm	*pparm = (struct set_stakey_parm *)pbuf;
+#ifdef CONFIG_TDLS
+	struct tdls_info	*ptdlsinfo = &padapter->tdlsinfo;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	struct sta_info	*psta;
+#endif //CONFIG_TDLS
+
+	//cam_entry:
+	//0~3 for default key
+	
+	//for concurrent mode (ap+sta):
+	//default key is disable, using sw encrypt/decrypt
+	//cam_entry = 4 //for sta mode (macid=0)
+	//cam_entry(macid+3) = 5 ~ N//for ap mode (aid=1~N, macid=2 ~N)
+
+	//for concurrent mode (sta+sta):
+	//default key is disable, using sw encrypt/decrypt
+	//cam_entry = 4 //mapping to macid=0
+	//cam_entry = 5 //mapping to macid=2
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+	{
+		struct sta_priv	*pstapriv = &padapter->stapriv;
+		struct sta_info	*psta;
+		
+		psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
+
+		if(psta && psta->mac_id==2)
+		{
+			cam_id = 5;
+		}
+		else
+		{
+			cam_id = 4;
+		}
+/*		
+		if(padapter->iface_type > PRIMARY_IFACE)
+		{
+			cam_id = 5;
+		}
+		else
+		{
+			cam_id = 4;
+		}
+*/		
+	}	
+#else
+	cam_id = 4;
+#endif
+
+	DBG_871X_LEVEL(_drv_always_, "set pairwise key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) camid:%d\n",
+		       	pparm->algorithm, cam_id);
+	if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+	{
+	
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		if(pparm->algorithm == _NO_PRIVACY_)	// clear cam entry
+		{
+			clear_cam_entry(padapter, pparm->id);
+			return H2C_SUCCESS_RSP;
+		}
+		
+		psta = rtw_get_stainfo(pstapriv, pparm->addr);
+		if(psta)
+		{			
+			ctrl = (BIT(15) | ((pparm->algorithm) << 2));
+
+			DBG_871X("r871x_set_stakey_hdl(): enc_algorithm=%d\n", pparm->algorithm);
+
+			if((psta->mac_id<1) || (psta->mac_id>(NUM_STA-4)))
+			{
+				DBG_871X("r871x_set_stakey_hdl():set_stakey failed, mac_id(aid)=%d\n", psta->mac_id);
+				return H2C_REJECTED;
+			}	
+				 
+			cam_id = (psta->mac_id + 3);//0~3 for default key, cmd_id=macid + 3, macid=aid+1;
+
+			DBG_871X("Write CAM, mac_addr=%x:%x:%x:%x:%x:%x, cam_entry=%d\n", pparm->addr[0], 
+						pparm->addr[1], pparm->addr[2], pparm->addr[3], pparm->addr[4],
+						pparm->addr[5], cam_id);
+
+			write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+	
+			return H2C_SUCCESS_RSP;
+		
+		}
+		else
+		{
+			DBG_871X("r871x_set_stakey_hdl(): sta has been free\n");
+			return H2C_REJECTED;
+		}
+		
+	}
+
+	//below for sta mode
+	
+	if(pparm->algorithm == _NO_PRIVACY_)	// clear cam entry
+	{
+		clear_cam_entry(padapter, pparm->id);
+		return H2C_SUCCESS;
+	}
+	
+	ctrl = BIT(15) | ((pparm->algorithm) << 2);	
+	
+#ifdef CONFIG_TDLS
+	if(ptdlsinfo->clear_cam!=0){
+		clear_cam_entry(padapter, ptdlsinfo->clear_cam);
+		ptdlsinfo->clear_cam=0;
+
+		return H2C_SUCCESS;
+	}
+
+	psta = rtw_get_stainfo(pstapriv, pparm->addr);//Get TDLS Peer STA
+	if( psta->tdls_sta_state&TDLS_LINKED_STATE ){
+		write_cam(padapter, psta->mac_id, ctrl, pparm->addr, pparm->key);
+	}
+	else
+#endif //CONFIG_TDLS	
+	write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+
+	pmlmeinfo->enc_algo = pparm->algorithm;
+	
+	return H2C_SUCCESS;
+}
+
+u8 add_ba_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct addBaReq_parm 	*pparm = (struct addBaReq_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr);
+	
+	if(!psta)
+		return 	H2C_SUCCESS;
+		
+#ifdef CONFIG_80211N_HT
+	if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
+		((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
+	{
+		//pmlmeinfo->ADDBA_retry_count = 0;
+		//pmlmeinfo->candidate_tid_bitmap |= (0x1 << pparm->tid);		
+		//psta->htpriv.candidate_tid_bitmap |= BIT(pparm->tid);
+		issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);		
+		//_set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO);
+		_set_timer(&psta->addba_retry_timer, ADDBA_TO);
+	}
+#ifdef CONFIG_TDLS
+	else if((psta->tdls_sta_state & TDLS_LINKED_STATE)&& 
+		(psta->htpriv.ht_option==_TRUE) && 
+		(psta->htpriv.ampdu_enable==_TRUE) )
+	{
+		issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);		
+		//_set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO);
+		_set_timer(&psta->addba_retry_timer, ADDBA_TO);
+	}
+#endif //CONFIG
+	else
+	{		
+		psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);		
+	}
+#endif //CONFIG_80211N_HT
+	return 	H2C_SUCCESS;
+}
+
+u8 set_tx_beacon_cmd(_adapter* padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct Tx_Beacon_param 	*ptxBeacon_parm;	
+	struct cmd_priv	*pcmdpriv = &(padapter->cmdpriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8	res = _SUCCESS;
+	int len_diff = 0;
+	
+_func_enter_;	
+	
+	if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
+	{
+		res= _FAIL;
+		goto exit;
+	}
+	
+	if ((ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param))) == NULL)
+	{
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct	cmd_obj));
+		res= _FAIL;
+		goto exit;
+	}
+
+	_rtw_memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
+	
+	len_diff = update_hidden_ssid(
+		ptxBeacon_parm->network.IEs+_BEACON_IE_OFFSET_
+		, ptxBeacon_parm->network.IELength-_BEACON_IE_OFFSET_
+		, pmlmeinfo->hidden_ssid_mode
+	);
+	ptxBeacon_parm->network.IELength += len_diff;
+		
+	init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+	
+exit:
+	
+_func_exit_;
+
+	return res;
+}
+
+
+u8 mlme_evt_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	u8 evt_code, evt_seq;
+	u16 evt_sz;
+	uint 	*peventbuf;
+	void (*event_callback)(_adapter *dev, u8 *pbuf);
+	struct evt_priv *pevt_priv = &(padapter->evtpriv);
+
+	peventbuf = (uint*)pbuf;
+	evt_sz = (u16)(*peventbuf&0xffff);
+	evt_seq = (u8)((*peventbuf>>24)&0x7f);
+	evt_code = (u8)((*peventbuf>>16)&0xff);
+	
+		
+	#ifdef CHECK_EVENT_SEQ
+	// checking event sequence...		
+	if (evt_seq != (ATOMIC_READ(&pevt_priv->event_seq) & 0x7f) )
+	{
+		RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("Evetn Seq Error! %d vs %d\n", (evt_seq & 0x7f), (ATOMIC_READ(&pevt_priv->event_seq) & 0x7f)));
+	
+		pevt_priv->event_seq = (evt_seq+1)&0x7f;
+
+		goto _abort_event_;
+	}
+	#endif
+
+	// checking if event code is valid
+	if (evt_code >= MAX_C2HEVT)
+	{
+		RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nEvent Code(%d) mismatch!\n", evt_code));
+		goto _abort_event_;
+	}
+
+	// checking if event size match the event parm size	
+	if ((wlanevents[evt_code].parmsize != 0) && 
+			(wlanevents[evt_code].parmsize != evt_sz))
+	{
+			
+		RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n", 
+			evt_code, wlanevents[evt_code].parmsize, evt_sz));
+		goto _abort_event_;	
+			
+	}
+
+	ATOMIC_INC(&pevt_priv->event_seq);
+
+	peventbuf += 2;
+				
+	if(peventbuf)
+	{
+		event_callback = wlanevents[evt_code].event_callback;
+		event_callback(padapter, (u8*)peventbuf);
+
+		pevt_priv->evt_done_cnt++;
+	}
+
+
+_abort_event_:
+
+
+	return H2C_SUCCESS;
+		
+}
+
+u8 h2c_msg_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	if(!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	return H2C_SUCCESS;
+}
+
+u8 tx_beacon_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	if(send_beacon(padapter)==_FAIL)
+	{
+		DBG_871X("issue_beacon, fail!\n");
+		return H2C_PARAMETERS_ERROR;
+	}
+#ifdef CONFIG_AP_MODE
+	else //tx bc/mc frames after update TIM 
+	{	
+		_irqL irqL;
+		struct sta_info *psta_bmc;
+		_list	*xmitframe_plist, *xmitframe_phead;
+		struct xmit_frame *pxmitframe=NULL;
+		struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+		struct sta_priv  *pstapriv = &padapter->stapriv;
+		
+		//for BC/MC Frames
+		psta_bmc = rtw_get_bcmc_stainfo(padapter);
+		if(!psta_bmc)
+			return H2C_SUCCESS;
+	
+		if((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len>0))
+		{				
+#ifndef CONFIG_PCI_HCI
+			rtw_msleep_os(10);// 10ms, ATIM(HIQ) Windows
+#endif
+			//_enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
+			_enter_critical_bh(&pxmitpriv->lock, &irqL);
+
+			xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+			xmitframe_plist = get_next(xmitframe_phead);
+
+			while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
+			{			
+				pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+				xmitframe_plist = get_next(xmitframe_plist);
+
+				rtw_list_delete(&pxmitframe->list);
+
+				psta_bmc->sleepq_len--;
+				if(psta_bmc->sleepq_len>0)
+					pxmitframe->attrib.mdata = 1;
+				else
+					pxmitframe->attrib.mdata = 0;
+
+				pxmitframe->attrib.triggered=1;
+
+				pxmitframe->attrib.qsel = 0x11;//HIQ
+
+#if 0
+				_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
+				if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE)
+				{		
+					rtw_os_xmit_complete(padapter, pxmitframe);
+				}
+				_enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
+
+#endif
+				rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+				//pstapriv->tim_bitmap &= ~BIT(0);				
+		
+			}	
+	
+			//_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);	
+			_exit_critical_bh(&pxmitpriv->lock, &irqL);
+
+//#if defined(CONFIG_PCI_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+			rtw_chk_hi_queue_cmd(padapter);
+#endif
+
+		}
+
+	}
+#endif
+
+	return H2C_SUCCESS;
+	
+}
+
+#ifdef CONFIG_DUALMAC_CONCURRENT
+void dc_SelectChannel(_adapter *padapter, unsigned char channel)
+{
+	PADAPTER ptarget_adapter;
+
+	if( (padapter->pbuddy_adapter != NULL) && 
+		(padapter->DualMacConcurrent == _TRUE) &&
+		(padapter->adapter_type == SECONDARY_ADAPTER))
+	{
+		// only mac0 could control BB&RF
+		ptarget_adapter = padapter->pbuddy_adapter;
+	}
+	else
+	{
+		ptarget_adapter = padapter;
+	}
+
+	_enter_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setch_mutex), NULL);
+
+	rtw_hal_set_chan(ptarget_adapter, channel);
+
+	_exit_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setch_mutex), NULL);
+}
+
+void dc_SetBWMode(_adapter *padapter, unsigned short bwmode, unsigned char channel_offset)
+{
+	PADAPTER ptarget_adapter;
+
+	if( (padapter->pbuddy_adapter != NULL) && 
+		(padapter->DualMacConcurrent == _TRUE) &&
+		(padapter->adapter_type == SECONDARY_ADAPTER))
+	{
+		// only mac0 could control BB&RF
+		ptarget_adapter = padapter->pbuddy_adapter;
+	}
+	else
+	{
+		ptarget_adapter = padapter;
+	}
+
+	_enter_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setbw_mutex), NULL);
+
+	rtw_hal_set_bwmode(ptarget_adapter, (HT_CHANNEL_WIDTH)bwmode, channel_offset);
+
+	_exit_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setbw_mutex), NULL);
+}
+
+static void dc_change_band(_adapter *padapter, WLAN_BSSID_EX *pnetwork)
+{
+	u8	network_type,rate_len, total_rate_len,remainder_rate_len;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8	erpinfo=0x4;
+
+	//DBG_871X("%s\n", __FUNCTION__);
+
+	if(pmlmeext->cur_channel >= 36)
+	{
+		network_type = WIRELESS_11A;
+		total_rate_len = IEEE80211_NUM_OFDM_RATESLEN;
+		DBG_871X("%s(): change to 5G Band\n",__FUNCTION__);
+		rtw_remove_bcn_ie(padapter, pnetwork, _ERPINFO_IE_);
+	}
+	else
+	{
+		network_type = WIRELESS_11BG;
+		total_rate_len = IEEE80211_CCK_RATE_LEN+IEEE80211_NUM_OFDM_RATESLEN;
+		DBG_871X("%s(): change to 2.4G Band\n",__FUNCTION__);
+		rtw_add_bcn_ie(padapter, pnetwork, _ERPINFO_IE_, &erpinfo, 1);
+	}
+
+	rtw_set_supported_rate(pnetwork->SupportedRates, network_type);
+
+	UpdateBrateTbl(padapter, pnetwork->SupportedRates);
+	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
+
+	if(total_rate_len > 8)
+	{
+		rate_len = 8;
+		remainder_rate_len = total_rate_len - 8;
+	}
+	else
+	{
+		rate_len = total_rate_len;
+		remainder_rate_len = 0;
+	}
+
+	rtw_add_bcn_ie(padapter, pnetwork, _SUPPORTEDRATES_IE_, pnetwork->SupportedRates, rate_len);
+
+	if(remainder_rate_len)
+	{
+		rtw_add_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_, (pnetwork->SupportedRates+8), remainder_rate_len);
+	}
+	else
+	{
+		rtw_remove_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_);
+	}
+}
+
+void dc_set_channel_bwmode_disconnect(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
+	struct mlme_priv *pbuddy_mlmepriv = NULL;
+
+	if(pbuddy_adapter != NULL && 
+		padapter->DualMacConcurrent == _TRUE)
+	{
+		pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
+		if((check_fwstate(pbuddy_mlmepriv, _FW_LINKED)) != _TRUE)
+		{
+			//switch to the 20M Hz mode after disconnect
+			pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+			set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+		}
+	}
+	else
+	{
+		//switch to the 20M Hz mode after disconnect
+		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+		pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	}
+}
+
+u8 dc_handle_join_request(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
+	PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;		
+	struct mlme_ext_priv *pbuddy_mlmeext = NULL;
+	struct mlme_priv	*pbuddy_mlmepriv = NULL;
+	u8	ret = _SUCCESS;
+
+	if(pbuddy_adapter != NULL && 
+		padapter->DualMacConcurrent == _TRUE)
+	{
+		pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+		pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
+
+		if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel ||
+			pmlmeext->cur_bwmode != pbuddy_mlmeext->cur_bwmode ||
+			pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
+		{
+			if((check_fwstate(pbuddy_mlmepriv, WIFI_AP_STATE)) == _TRUE)
+			{
+				//issue deauth to all stas if if2 is at ap mode
+				rtw_sta_flush(pbuddy_adapter);
+
+				//rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
+				rtw_hal_set_hwreg(pbuddy_adapter, HW_VAR_CHECK_TXBUF, 0);
+			}
+			else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED) == _TRUE)
+			{
+				if(pmlmeext->cur_channel == pbuddy_mlmeext->cur_channel)
+				{
+					// HT_CHANNEL_WIDTH_40 or HT_CHANNEL_WIDTH_20 but channel offset is different
+					if((pmlmeext->cur_bwmode == pbuddy_mlmeext->cur_bwmode) &&
+						(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset) )
+					{
+						report_join_res(padapter, -4);
+						ret = _FAIL;
+					}
+				}
+				else
+				{
+					report_join_res(padapter, -4);
+					ret = _FAIL;
+				}
+			}
+		}
+		else	 if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
+		{
+			issue_nulldata(pbuddy_adapter, NULL, 1, 0, 0);
+		}
+	}
+
+	return ret;
+}
+
+void dc_handle_join_done(_adapter *padapter, u8 join_res)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
+	struct mlme_priv *pbuddy_mlmepriv = NULL;
+	struct mlme_ext_priv *pbuddy_mlmeext = NULL;
+	struct mlme_ext_info *pbuddy_mlmeinfo = NULL;
+	WLAN_BSSID_EX *pbuddy_network_mlmeext = NULL;
+	u8	change_band = _FALSE;
+
+
+	if(pbuddy_adapter != NULL && 
+		padapter->DualMacConcurrent == _TRUE)
+	{
+		pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
+		pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+		pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
+		pbuddy_network_mlmeext = &(pbuddy_mlmeinfo->network);
+	
+		if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
+				check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
+		{
+			//restart and update beacon
+			DBG_871X("after join, current adapter, CH=%d, BW=%d, offset=%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+
+			if(join_res >= 0)
+			{
+				u8 *p;
+				int	ie_len;
+				struct HT_info_element *pht_info=NULL;
+
+				if((pbuddy_mlmeext->cur_channel <= 14 && pmlmeext->cur_channel >= 36) ||
+					(pbuddy_mlmeext->cur_channel >= 36 && pmlmeext->cur_channel <= 14))
+				{
+					change_band = _TRUE;
+				}
+
+				//sync channel/bwmode/ch_offset with another adapter
+				pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
+				
+				if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
+				{
+					p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+					if( p && ie_len)
+					{
+						pht_info = (struct HT_info_element *)(p+2);
+						pht_info->infos[0] &= ~(BIT(0)|BIT(1)); //no secondary channel is present
+					}	
+				
+					if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
+					{
+						pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
+
+						//to update cur_ch_offset value in beacon
+						if( pht_info )
+						{
+							switch(pmlmeext->cur_ch_offset)
+							{
+								case HAL_PRIME_CHNL_OFFSET_LOWER:
+									pht_info->infos[0] |= 0x1;
+									break;
+								case HAL_PRIME_CHNL_OFFSET_UPPER:
+									pht_info->infos[0] |= 0x3;
+									break;
+								case HAL_PRIME_CHNL_OFFSET_DONT_CARE:							
+								default:							
+									break;					
+							}
+						}
+					}
+					else if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_20)
+					{
+						pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+						pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+						if(pmlmeext->cur_channel>0 && pmlmeext->cur_channel<5)
+						{
+							if(pht_info)
+								pht_info->infos[0] |= 0x1;
+
+							pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+							pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+						}
+
+						if(pmlmeext->cur_channel>7 && pmlmeext->cur_channel<(14+1))
+						{
+							if(pht_info)
+								pht_info->infos[0] |= 0x3;
+
+							pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+							pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+						}
+
+						set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
+					}
+				}
+
+				// to update channel value in beacon
+				pbuddy_network_mlmeext->Configuration.DSConfig = pmlmeext->cur_channel;
+				p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+				if(p && ie_len>0)
+					*(p + 2) = pmlmeext->cur_channel;
+
+				p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+				if( p && ie_len)
+				{
+					pht_info = (struct HT_info_element *)(p+2);
+					pht_info->primary_channel = pmlmeext->cur_channel;
+				}
+
+				// update mlmepriv's cur_network
+				_rtw_memcpy(&pbuddy_mlmepriv->cur_network.network, pbuddy_network_mlmeext, pbuddy_network_mlmeext->Length);
+			}
+			else
+			{
+				// switch back to original channel/bwmode/ch_offset;
+				set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
+			}
+
+			DBG_871X("after join, another adapter, CH=%d, BW=%d, offset=%d\n", pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
+
+			if(change_band == _TRUE)
+				dc_change_band(pbuddy_adapter, pbuddy_network_mlmeext);
+
+			DBG_871X("update pbuddy_adapter's beacon\n");
+
+			update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
+		}
+		else	 if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
+		{
+			if((pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40) &&
+				(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_20))
+			{
+				set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
+			}
+		
+			issue_nulldata(pbuddy_adapter, NULL, 0, 0, 0);
+		}
+	}
+}
+
+sint	dc_check_fwstate(_adapter *padapter, sint fw_state)
+{
+	PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
+	struct mlme_priv *pbuddy_mlmepriv = NULL;
+
+	if(padapter->pbuddy_adapter != NULL && 
+		padapter->DualMacConcurrent == _TRUE)
+
+	{
+		pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
+
+		return check_fwstate(pbuddy_mlmepriv, fw_state);
+	}
+
+	return _FALSE;
+}
+
+u8 dc_handle_site_survey(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;		
+
+	// only mac0 can do scan request, help issue nulldata(1) for mac1
+	if(pbuddy_adapter != NULL && 
+		padapter->DualMacConcurrent == _TRUE)
+	{
+		if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
+		{
+			pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
+		
+			issue_nulldata(pbuddy_adapter, NULL, 1, 2, 0);			
+
+			return _TRUE;
+		}
+	}
+
+	return _FALSE;
+}
+
+void	dc_report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
+{
+	if(padapter->pbuddy_adapter != NULL && 
+		padapter->DualMacConcurrent == _TRUE)
+	{
+		report_survey_event(padapter->pbuddy_adapter, precv_frame);	
+	}
+}
+
+void dc_set_channel_bwmode_survey_done(_adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
+	struct mlme_priv *pbuddy_mlmepriv = NULL;
+	struct mlme_ext_priv *pbuddy_mlmeext = NULL;
+	struct mlme_ext_info *pbuddy_mlmeinfo = NULL;
+	u8 cur_channel;
+	u8 cur_bwmode;
+	u8 cur_ch_offset;
+
+	if(pbuddy_adapter != NULL && 
+		padapter->DualMacConcurrent == _TRUE)
+	{
+		pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
+		pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+		pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
+
+		if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
+		{
+			if(check_fwstate(pmlmepriv, _FW_LINKED) &&
+				(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40))
+			{
+				cur_channel = pmlmeext->cur_channel;
+				cur_bwmode = pmlmeext->cur_bwmode;
+				cur_ch_offset = pmlmeext->cur_ch_offset;
+			}
+			else
+			{
+				cur_channel = pbuddy_mlmeext->cur_channel;
+				cur_bwmode = pbuddy_mlmeext->cur_bwmode;
+				cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
+			}
+		}
+		else
+		{
+			cur_channel = pmlmeext->cur_channel;
+			cur_bwmode = pmlmeext->cur_bwmode;
+			cur_ch_offset = pmlmeext->cur_ch_offset;
+		}
+
+		set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
+
+		if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
+		{
+			//issue null data 
+			issue_nulldata(pbuddy_adapter, NULL, 0, 0, 0);
+		}
+
+		if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
+			check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
+		{
+
+			DBG_871X("survey done, current CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
+
+			DBG_871X("restart pbuddy_adapter's beacon\n");
+		
+			update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
+		}
+	}
+	else
+	{
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	}
+}
+
+void dc_set_ap_channel_bandwidth(_adapter *padapter, u8 channel, u8 channel_offset, u8 bwmode)
+{
+	u8	*p;
+	u8	val8, cur_channel, cur_bwmode, cur_ch_offset, change_band;
+	int	ie_len;	
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_priv		*pmlmepriv = &(padapter->mlmepriv);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct HT_info_element	*pht_info=NULL;
+	_adapter	*pbuddy_adapter = padapter->pbuddy_adapter;
+	struct mlme_priv	*pbuddy_mlmepriv = NULL;
+	struct mlme_ext_priv	*pbuddy_mlmeext = NULL;
+
+	DBG_871X("dualmac_concurrent_ap_set_channel_bwmode ==>\n");
+
+	cur_channel = channel;
+	cur_bwmode = bwmode;
+	cur_ch_offset = channel_offset;
+	change_band = _FALSE;
+
+	p = rtw_get_ie((pnetwork->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+	if( p && ie_len)
+	{
+		pht_info = (struct HT_info_element *)(p+2);
+	}
+
+	if(pbuddy_adapter != NULL && 
+		padapter->DualMacConcurrent == _TRUE)
+	{
+		pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
+		pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+		
+		if(!check_fwstate(pbuddy_mlmepriv, _FW_LINKED|_FW_UNDER_LINKING|_FW_UNDER_SURVEY))
+		{
+			set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
+		}
+		else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED)==_TRUE)
+		{
+			//To sync cur_channel/cur_bwmode/cur_ch_offset with another adapter
+			DBG_871X("Another iface is at linked state, sync cur_channel/cur_bwmode/cur_ch_offset\n");
+			DBG_871X("Another adapter, CH=%d, BW=%d, offset=%d\n", pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
+			DBG_871X("Current adapter, CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
+			
+			cur_channel = pbuddy_mlmeext->cur_channel;
+			if(cur_bwmode == HT_CHANNEL_WIDTH_40)
+			{
+				if(pht_info)
+					pht_info->infos[0] &= ~(BIT(0)|BIT(1));
+
+				if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
+				{
+					cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
+
+					//to update cur_ch_offset value in beacon
+					if(pht_info)
+					{				
+						switch(cur_ch_offset)
+						{
+							case HAL_PRIME_CHNL_OFFSET_LOWER:
+								pht_info->infos[0] |= 0x1;
+								break;
+							case HAL_PRIME_CHNL_OFFSET_UPPER:
+								pht_info->infos[0] |= 0x3;
+								break;
+							case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
+							default:							
+								break;					
+						}
+					}
+				}
+				else if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_20)
+				{
+					cur_bwmode = HT_CHANNEL_WIDTH_20;
+					cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+					if(cur_channel>0 && cur_channel<5)
+					{
+						if(pht_info)
+							pht_info->infos[0] |= 0x1;		
+
+						cur_bwmode = HT_CHANNEL_WIDTH_40;
+						cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+					}
+
+					if(cur_channel>7 && cur_channel<(14+1))
+					{
+						if(pht_info)
+							pht_info->infos[0] |= 0x3;
+
+						cur_bwmode = HT_CHANNEL_WIDTH_40;
+						cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+					}
+
+					set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
+				}
+			}
+
+			// to update channel value in beacon
+			pnetwork->Configuration.DSConfig = cur_channel;		
+			p = rtw_get_ie((pnetwork->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (pnetwork->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+			if(p && ie_len>0)
+				*(p + 2) = cur_channel;
+
+			if(pht_info)
+				pht_info->primary_channel = cur_channel;
+		}
+	}
+	else
+	{
+		set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
+	}
+
+	DBG_871X("CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
+
+	if((channel <= 14 && cur_channel >= 36) ||
+		(channel >= 36 && cur_channel <= 14))
+	{
+		change_band = _TRUE;
+	}
+
+	pmlmeext->cur_channel = cur_channel;
+	pmlmeext->cur_bwmode = cur_bwmode;
+	pmlmeext->cur_ch_offset = cur_ch_offset;
+
+	if(change_band == _TRUE)
+		dc_change_band(padapter, pnetwork);
+
+	DBG_871X("dualmac_concurrent_ap_set_channel_bwmode <==\n");
+}
+
+void dc_resume_xmit(_adapter *padapter)
+{
+	PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
+	
+	if(pbuddy_adapter != NULL && 
+		padapter->DualMacConcurrent == _TRUE)
+	{
+		DBG_871X("dc_resume_xmit,  resume pbuddy_adapter Tx\n");
+		rtw_os_xmit_schedule(pbuddy_adapter);
+	}
+}
+
+u8	dc_check_xmit(_adapter *padapter)
+{
+	PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
+	struct mlme_priv *pbuddy_mlmepriv = NULL;
+
+	if(pbuddy_adapter != NULL && 
+		padapter->DualMacConcurrent == _TRUE)
+	{
+		pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
+		if (check_fwstate(pbuddy_mlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == _TRUE)
+		{
+			DBG_871X("dc_check_xmit  pbuddy_adapter is under survey or under linking\n");
+			return _FALSE;
+		}
+	}
+
+	return _TRUE;
+}
+#endif
+
+#ifdef CONFIG_CONCURRENT_MODE
+sint check_buddy_mlmeinfo_state(_adapter *padapter, u32 state)
+{
+	PADAPTER pbuddy_adapter;
+	struct mlme_ext_priv *pbuddy_mlmeext;
+	struct mlme_ext_info *pbuddy_mlmeinfo;
+
+	if(padapter == NULL)
+		return _FALSE;	
+	
+	pbuddy_adapter = padapter->pbuddy_adapter;
+
+	if(pbuddy_adapter == NULL)
+		return _FALSE;	
+
+
+	pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+	pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
+		
+	if((pbuddy_mlmeinfo->state&0x03) == state)
+		return _TRUE;		
+
+	return _FALSE;
+	
+}
+
+int concurrent_chk_start_clnt_join(_adapter *padapter)
+{
+	int ret = _FAIL;
+	struct mlme_ext_priv	*pmlmeext;
+	struct mlme_ext_info	*pmlmeinfo;
+	PADAPTER pbuddy_adapter;	
+	struct mlme_ext_priv *pbuddy_mlmeext;
+	struct mlme_ext_info	*pbuddy_pmlmeinfo;
+	struct mlme_priv *pbuddy_mlmepriv;
+
+	if(!rtw_buddy_adapter_up(padapter))
+		return _SUCCESS;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &(pmlmeext->mlmext_info);	
+
+	pbuddy_adapter = padapter->pbuddy_adapter;		
+	pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+	pbuddy_pmlmeinfo = &(pbuddy_mlmeext->mlmext_info);
+	pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
+
+	if((pbuddy_pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)//for AP MODE
+	{
+		bool inform_ch_switch = _FALSE;
+		if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel)
+		{
+			inform_ch_switch = _TRUE;
+		}
+		else if((pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40) &&
+			(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40) &&
+			(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset))
+		{
+			inform_ch_switch = _TRUE;
+		}
+
+		if (inform_ch_switch) {
+			#ifdef CONFIG_SPCT_CH_SWITCH
+			if (1) {
+				rtw_ap_inform_ch_switch(pbuddy_adapter, pmlmeext->cur_channel , pmlmeext->cur_ch_offset);
+			} else
+			#endif
+			{
+				//issue deauth to all stas if if2 is at ap mode
+				rtw_sta_flush(pbuddy_adapter);
+			}
+			rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
+		}
+	}
+	else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED) == _TRUE &&
+		check_fwstate(pbuddy_mlmepriv, WIFI_STATION_STATE) == _TRUE) //for Client Mode/p2p client
+	{
+#if defined(CONFIG_P2P) && defined(CONFIG_IOCTL_CFG80211) 	
+		struct wifidirect_info *pbuddy_wdinfo = &(pbuddy_adapter->wdinfo);
+		if(!rtw_p2p_chk_state(pbuddy_wdinfo, P2P_STATE_NONE))
+			return _SUCCESS; //wlan0-sta mode has higher priority than p2p0-p2p client
+#endif //CONFIG_P2P && CONFIG_IOCTL_CFG80211	
+	
+		if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel)
+		{
+			DBG_871X("start_clnt_join(ch=%d), but channel mismatch with buddy(ch=%d) interface\n", 
+				pmlmeext->cur_channel, pbuddy_mlmeext->cur_channel);
+				
+			report_join_res(padapter, (-4));
+
+			return ret;
+		}
+
+		if((pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40) &&
+			(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40) && 
+			(pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset))
+		{
+			DBG_871X("start_clnt_join(bwmode=%d, ch_offset=%d), but bwmode & ch_offset mismatch with buddy(bwmode=%d, ch_offset=%d) interface\n", 
+				pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
+			
+			report_join_res(padapter, (-4));
+
+			return ret;
+		}			
+
+	}	
+	
+	return _SUCCESS;
+	
+}
+
+void concurrent_chk_joinbss_done(_adapter *padapter, int join_res)
+{
+	struct mlme_ext_priv	*pmlmeext;
+	struct mlme_ext_info	*pmlmeinfo;	
+	PADAPTER pbuddy_adapter;
+	struct mlme_priv *pbuddy_mlmepriv;
+	struct mlme_ext_priv *pbuddy_mlmeext;
+	struct mlme_ext_info *pbuddy_mlmeinfo;
+	WLAN_BSSID_EX *pbuddy_network_mlmeext;
+
+	if(!rtw_buddy_adapter_up(padapter))
+		return;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &(pmlmeext->mlmext_info);	
+	
+	pbuddy_adapter = padapter->pbuddy_adapter;
+	pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
+	pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+	pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
+	pbuddy_network_mlmeext = &(pbuddy_mlmeinfo->network);
+
+	if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
+			check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
+	{
+		//restart and update beacon
+
+		DBG_871X("after join,primary adapter, CH=%d, BW=%d, offset=%d\n"
+			, pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+		
+
+		if(join_res >= 0)
+		{
+			u8 *p;
+			int	ie_len;
+			struct HT_info_element *pht_info=NULL;
+			
+			//sync channel/bwmode/ch_offset with primary adapter
+			pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
+			if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
+			{
+				p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+				if( p && ie_len)
+				{
+					pht_info = (struct HT_info_element *)(p+2);
+					pht_info->infos[0] &= ~(BIT(0)|BIT(1)); //no secondary channel is present
+				}
+			
+				if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
+				{
+					pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
+
+					//to update cur_ch_offset value in beacon
+					if( pht_info )
+					{
+						switch(pmlmeext->cur_ch_offset)
+						{
+							case HAL_PRIME_CHNL_OFFSET_LOWER:
+								pht_info->infos[0] |= 0x1;
+								break;
+							case HAL_PRIME_CHNL_OFFSET_UPPER:
+								pht_info->infos[0] |= 0x3;
+								break;
+							case HAL_PRIME_CHNL_OFFSET_DONT_CARE:							
+							default:							
+								break;					
+						}
+						
+					}					
+				
+				}
+				else if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_20)
+				{
+					if(pmlmeext->cur_channel>=1 && pmlmeext->cur_channel<=4)
+					{
+						if(pht_info)
+							pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;		
+
+						pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+						pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+					}
+					else if(pmlmeext->cur_channel>=5 && pmlmeext->cur_channel<=14)
+					{
+						if(pht_info)
+							pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
+						
+						pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+						pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;						
+					}
+					else
+					{
+						switch(pmlmeext->cur_channel)
+						{
+							case 36:
+							case 44:
+							case 52:
+							case 60:
+							case 100:
+							case 108:
+							case 116:
+							case 124:
+							case 132:
+							case 149:
+							case 157:
+							{
+								if(pht_info)
+									pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;		
+								pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+								pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+								break;
+							}
+							case 40:
+							case 48:
+							case 56:
+							case 64:
+							case 104:
+							case 112:
+							case 120:
+							case 128:
+							case 136:
+							case 153:
+							case 161:
+							{
+								if(pht_info)
+									pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
+								
+								pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+								pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+								break;
+							}
+							default:
+								if(pht_info)
+									pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
+								pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
+								pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+								break;								
+
+						}
+						
+					}
+
+				}
+
+				set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
+				
+			}
+
+			// to update channel value in beacon
+			pbuddy_network_mlmeext->Configuration.DSConfig = pmlmeext->cur_channel;		
+			p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+			if(p && ie_len>0)
+				*(p + 2) = pmlmeext->cur_channel;
+
+			p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+			if( p && ie_len)
+			{
+				pht_info = (struct HT_info_element *)(p+2);
+				pht_info->primary_channel = pmlmeext->cur_channel;
+			}
+			
+		}
+		else
+		{
+			// switch back to original channel/bwmode/ch_offset;
+			set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
+		}
+
+		DBG_871X("after join, second adapter, CH=%d, BW=%d, offset=%d\n", pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
+
+		DBG_871X("update pbuddy_adapter's beacon\n");
+		
+		update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
+
+	}
+	else if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) &&
+			check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
+	{		
+		if(join_res >= 0)
+		{	
+			pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
+			if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
+				set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);		
+			else if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
+				set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);		
+			else
+				set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+		}
+		else
+		{
+			// switch back to original channel/bwmode/ch_offset;
+			set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
+		}			
+	}
+	
+}
+#endif //CONFIG_CONCURRENT_MODE
+
+u8 set_ch_hdl(_adapter *padapter, u8 *pbuf)
+{
+	struct set_ch_parm *set_ch_parm;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	if(!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	set_ch_parm = (struct set_ch_parm *)pbuf;
+
+	DBG_871X(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
+		FUNC_NDEV_ARG(padapter->pnetdev),
+		set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
+
+	pmlmeext->cur_channel = set_ch_parm->ch;
+	pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
+	pmlmeext->cur_bwmode = set_ch_parm->bw;
+
+	set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
+
+	return 	H2C_SUCCESS;
+}
+
+u8 set_chplan_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct SetChannelPlan_param *setChannelPlan_param;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	if(!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
+
+	pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);	
+
+	return 	H2C_SUCCESS;
+}
+
+u8 led_blink_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct LedBlink_param *ledBlink_param;
+
+	if(!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	ledBlink_param = (struct LedBlink_param *)pbuf;
+
+	#ifdef CONFIG_LED_HANDLED_BY_CMD_THREAD
+	BlinkHandler(ledBlink_param->pLed);
+	#endif
+
+	return 	H2C_SUCCESS;
+}
+
+u8 set_csa_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+#ifdef CONFIG_DFS
+	struct SetChannelSwitch_param *setChannelSwitch_param;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	u8 new_ch_no;
+	u8 gval8 = 0x00, sval8 = 0xff;
+
+	if(!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	setChannelSwitch_param = (struct SetChannelSwitch_param *)pbuf;
+	new_ch_no = setChannelSwitch_param->new_ch_no;
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &sval8);
+
+	DBG_871X("DFS detected! Swiching channel to %d!\n", new_ch_no);
+	SelectChannel(padapter, new_ch_no);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
+
+	rtw_free_network_queue(padapter, _TRUE);
+	rtw_indicate_disconnect(padapter);
+
+	if ( ((new_ch_no >= 52) && (new_ch_no <= 64)) ||((new_ch_no >= 100) && (new_ch_no <= 140)) ) {
+		DBG_871X("Switched to DFS band (ch %02x) again!!\n", new_ch_no);
+	}
+
+	return 	H2C_SUCCESS;
+#else
+	return	H2C_REJECTED;
+#endif //CONFIG_DFS
+
+}
+
+// TDLS_WRCR		: write RCR DATA BIT
+// TDLS_SD_PTI		: issue peer traffic indication
+// TDLS_CS_OFF		: go back to the channel linked with AP, terminating channel switch procedure
+// TDLS_INIT_CH_SEN	: init channel sensing, receive all data and mgnt frame
+// TDLS_DONE_CH_SEN: channel sensing and report candidate channel
+// TDLS_OFF_CH		: first time set channel to off channel
+// TDLS_BASE_CH		: go back tp the channel linked with AP when set base channel as target channel
+// TDLS_P_OFF_CH	: periodically go to off channel
+// TDLS_P_BASE_CH	: periodically go back to base channel
+// TDLS_RS_RCR		: restore RCR
+// TDLS_CKALV_PH1	: check alive timer phase1
+// TDLS_CKALV_PH2	: check alive timer phase2
+// TDLS_FREE_STA	: free tdls sta
+u8 tdls_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+#ifdef CONFIG_TDLS
+	_irqL irqL;
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	struct TDLSoption_param *TDLSoption;
+	struct sta_info *ptdls_sta;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	u8 survey_channel, i, min, option;
+
+	if(!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	TDLSoption = (struct TDLSoption_param *)pbuf;
+
+	ptdls_sta = rtw_get_stainfo( &(padapter->stapriv), TDLSoption->addr );
+	option = TDLSoption->option;
+
+	if( ptdls_sta == NULL )
+	{
+		if( option != TDLS_RS_RCR )
+			return H2C_REJECTED;
+	}
+
+	//_enter_critical_bh(&(ptdlsinfo->hdl_lock), &irqL);
+	DBG_871X("[%s] option:%d\n", __FUNCTION__, option);
+		
+	switch(option){
+		case TDLS_WRCR:
+			//As long as TDLS handshake success, we should set RCR_CBSSID_DATA bit to 0
+			//such we can receive all kinds of data frames.
+			rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_WRCR, 0);
+			DBG_871X("TDLS with "MAC_FMT"\n", MAC_ARG(ptdls_sta->hwaddr));
+
+			pmlmeinfo->FW_sta_info[ptdls_sta->mac_id].psta = ptdls_sta;
+			//set TDLS sta rate.
+			set_sta_rate(padapter, ptdls_sta);
+			break;
+		case TDLS_SD_PTI:
+			issue_tdls_peer_traffic_indication(padapter, ptdls_sta);
+			break;
+		case TDLS_CS_OFF:
+			_cancel_timer_ex(&ptdls_sta->base_ch_timer);
+			_cancel_timer_ex(&ptdls_sta->off_ch_timer);
+			SelectChannel(padapter, pmlmeext->cur_channel);
+			ptdls_sta->tdls_sta_state &= ~(TDLS_CH_SWITCH_ON_STATE | 
+								TDLS_PEER_AT_OFF_STATE | 
+								TDLS_AT_OFF_CH_STATE);
+			DBG_871X("go back to base channel\n ");
+			issue_nulldata(padapter, NULL, 0, 0, 0);
+			break;
+		case TDLS_INIT_CH_SEN:
+			rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_INIT_CH_SEN, 0);
+			pmlmeext->sitesurvey_res.channel_idx = 0;
+			ptdls_sta->option = TDLS_DONE_CH_SEN;
+			rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_DONE_CH_SEN);
+			break;
+		case TDLS_DONE_CH_SEN:
+			survey_channel = pmlmeext->channel_set[pmlmeext->sitesurvey_res.channel_idx].ChannelNum;
+			if(survey_channel){
+				SelectChannel(padapter, survey_channel);
+				ptdlsinfo->cur_channel = survey_channel;
+				pmlmeext->sitesurvey_res.channel_idx++;
+				_set_timer(&ptdls_sta->option_timer, SURVEY_TO);
+			}else{
+				SelectChannel(padapter, pmlmeext->cur_channel);
+
+				rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_DONE_CH_SEN, 0);
+
+				if(ptdlsinfo->ch_sensing==1){
+					ptdlsinfo->ch_sensing=0;
+					ptdlsinfo->cur_channel=1;
+					min=ptdlsinfo->collect_pkt_num[0];
+					for(i=1; i<MAX_CHANNEL_NUM-1; i++){
+						if(min > ptdlsinfo->collect_pkt_num[i]){
+							ptdlsinfo->cur_channel=i+1;
+							min=ptdlsinfo->collect_pkt_num[i];
+						}
+						ptdlsinfo->collect_pkt_num[i]=0;
+					}
+					ptdlsinfo->collect_pkt_num[0]=0;
+					ptdlsinfo->candidate_ch=ptdlsinfo->cur_channel;
+					DBG_871X("TDLS channel sensing done, candidate channel: %02x\n", ptdlsinfo->candidate_ch);
+					ptdlsinfo->cur_channel=0;
+
+				}
+
+				if(ptdls_sta->tdls_sta_state & TDLS_PEER_SLEEP_STATE){
+					ptdls_sta->tdls_sta_state |= TDLS_APSD_CHSW_STATE;
+				}else{
+					//send null data with pwrbit==1 before send ch_switching_req to peer STA.
+					issue_nulldata(padapter, NULL, 1, 0, 0);
+
+					ptdls_sta->tdls_sta_state |= TDLS_CH_SW_INITIATOR_STATE;
+
+					issue_tdls_ch_switch_req(padapter, ptdls_sta->hwaddr);
+					DBG_871X("issue tdls ch switch req\n");
+				}
+			}
+			break;
+		case TDLS_OFF_CH:
+			issue_nulldata(padapter, NULL, 1, 0, 0);
+			SelectChannel(padapter, ptdls_sta->off_ch);
+
+			DBG_871X("change channel to tar ch:%02x\n", ptdls_sta->off_ch);
+			ptdls_sta->tdls_sta_state |= TDLS_AT_OFF_CH_STATE;
+			ptdls_sta->tdls_sta_state &= ~(TDLS_PEER_AT_OFF_STATE);
+			_set_timer(&ptdls_sta->option_timer, (u32)ptdls_sta->ch_switch_time);
+			break;
+		case TDLS_BASE_CH:
+			_cancel_timer_ex(&ptdls_sta->base_ch_timer);
+			_cancel_timer_ex(&ptdls_sta->off_ch_timer);
+			SelectChannel(padapter, pmlmeext->cur_channel);
+			ptdls_sta->tdls_sta_state &= ~(TDLS_CH_SWITCH_ON_STATE | 
+								TDLS_PEER_AT_OFF_STATE | 
+								TDLS_AT_OFF_CH_STATE);
+			DBG_871X("go back to base channel\n ");
+			issue_nulldata(padapter, NULL, 0, 0, 0);
+			_set_timer(&ptdls_sta->option_timer, (u32)ptdls_sta->ch_switch_time);
+			break;
+		case TDLS_P_OFF_CH:
+			SelectChannel(padapter, pmlmeext->cur_channel);
+			issue_nulldata(padapter, NULL, 0, 0, 0);
+			DBG_871X("change channel to base ch:%02x\n", pmlmeext->cur_channel);
+			ptdls_sta->tdls_sta_state &= ~(TDLS_PEER_AT_OFF_STATE| TDLS_AT_OFF_CH_STATE);
+			_set_timer(&ptdls_sta->off_ch_timer, TDLS_STAY_TIME);
+			break;
+		case TDLS_P_BASE_CH:
+			issue_nulldata(ptdls_sta->padapter, NULL, 1, 0, 0);
+			SelectChannel(padapter, ptdls_sta->off_ch);
+			DBG_871X("change channel to off ch:%02x\n", ptdls_sta->off_ch);
+			ptdls_sta->tdls_sta_state |= TDLS_AT_OFF_CH_STATE;
+			if((ptdls_sta->tdls_sta_state & TDLS_PEER_AT_OFF_STATE) != TDLS_PEER_AT_OFF_STATE){
+				issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta, 0);
+			}
+			_set_timer(&ptdls_sta->base_ch_timer, TDLS_STAY_TIME);	
+			break;
+		case TDLS_RS_RCR:
+			rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_RS_RCR, 0);
+			DBG_871X("wirte REG_RCR, set bit6 on\n");
+			break;
+		case TDLS_CKALV_PH1:
+			_set_timer(&ptdls_sta->alive_timer2, TDLS_ALIVE_TIMER_PH2);
+			break;
+		case TDLS_CKALV_PH2:
+			_set_timer(&ptdls_sta->alive_timer1, TDLS_ALIVE_TIMER_PH1);
+			break;
+		case TDLS_FREE_STA:
+			free_tdls_sta(padapter, ptdls_sta);
+			break;
+			
+	}
+
+	//_exit_critical_bh(&(ptdlsinfo->hdl_lock), &irqL);
+
+	return H2C_SUCCESS;
+#else
+	return H2C_REJECTED;
+#endif //CONFIG_TDLS
+
+}
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_mp.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_mp.c
new file mode 100755
index 00000000..a63604fc
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_mp.c
@@ -0,0 +1,1604 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_MP_C_
+
+#include <drv_types.h>
+
+#ifdef PLATFORM_FREEBSD
+#include <sys/unistd.h>		/* for RFHIGHPID */
+#endif
+
+#ifdef CONFIG_RTL8712
+#include <rtw_mp_phy_regdef.h>
+#endif
+#ifdef CONFIG_RTL8192C
+#include <rtl8192c_hal.h>
+#endif
+#ifdef CONFIG_RTL8192D
+#include <rtl8192d_hal.h>
+#endif
+#ifdef CONFIG_RTL8723A
+#include <rtl8723a_hal.h>
+#include "rtw_bt_mp.h"
+#endif
+#ifdef CONFIG_RTL8188E
+#include "../hal/OUTSRC/odm_precomp.h"		
+#include "rtl8188e_hal.h"  
+#endif
+
+
+#ifdef CONFIG_MP_INCLUDED
+
+u32 read_macreg(_adapter *padapter, u32 addr, u32 sz)
+{
+	u32 val = 0;
+
+	switch(sz)
+	{
+		case 1:
+			val = rtw_read8(padapter, addr);
+			break;
+		case 2:
+			val = rtw_read16(padapter, addr);
+			break;
+		case 4:
+			val = rtw_read32(padapter, addr);
+			break;
+		default:
+			val = 0xffffffff;
+			break;
+	}
+
+	return val;
+	
+}
+
+void write_macreg(_adapter *padapter, u32 addr, u32 val, u32 sz)
+{
+	switch(sz)
+	{
+		case 1:
+			rtw_write8(padapter, addr, (u8)val);
+			break;
+		case 2:
+			rtw_write16(padapter, addr, (u16)val);
+			break;
+		case 4:
+			rtw_write32(padapter, addr, val);
+			break;
+		default:
+			break;
+	}
+
+}
+
+u32 read_bbreg(_adapter *padapter, u32 addr, u32 bitmask)
+{
+	return rtw_hal_read_bbreg(padapter, addr, bitmask);
+}
+
+void write_bbreg(_adapter *padapter, u32 addr, u32 bitmask, u32 val)
+{
+	rtw_hal_write_bbreg(padapter, addr, bitmask, val);
+}
+
+u32 _read_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 bitmask)
+{
+	return rtw_hal_read_rfreg(padapter, (RF_RADIO_PATH_E)rfpath, addr, bitmask);
+}
+
+void _write_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 bitmask, u32 val)
+{
+	rtw_hal_write_rfreg(padapter, (RF_RADIO_PATH_E)rfpath, addr, bitmask, val);
+}
+
+u32 read_rfreg(PADAPTER padapter, u8 rfpath, u32 addr)
+{
+	return _read_rfreg(padapter, (RF_RADIO_PATH_E)rfpath, addr, bRFRegOffsetMask);
+}
+
+void write_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 val)
+{
+	_write_rfreg(padapter, (RF_RADIO_PATH_E)rfpath, addr, bRFRegOffsetMask, val);
+}
+
+static void _init_mp_priv_(struct mp_priv *pmp_priv)
+{
+	WLAN_BSSID_EX *pnetwork;
+
+	_rtw_memset(pmp_priv, 0, sizeof(struct mp_priv));
+
+	pmp_priv->mode = MP_OFF;
+
+	pmp_priv->channel = 1;
+	pmp_priv->bandwidth = HT_CHANNEL_WIDTH_20;
+	pmp_priv->prime_channel_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	pmp_priv->rateidx = MPT_RATE_1M;
+	pmp_priv->txpoweridx = 0x2A;
+
+	pmp_priv->antenna_tx = ANTENNA_A;
+	pmp_priv->antenna_rx = ANTENNA_AB;
+
+	pmp_priv->check_mp_pkt = 0;
+
+	pmp_priv->tx_pktcount = 0;
+
+	pmp_priv->rx_pktcount = 0;
+	pmp_priv->rx_crcerrpktcount = 0;
+
+	pmp_priv->network_macaddr[0] = 0x00;
+	pmp_priv->network_macaddr[1] = 0xE0;
+	pmp_priv->network_macaddr[2] = 0x4C;
+	pmp_priv->network_macaddr[3] = 0x87;
+	pmp_priv->network_macaddr[4] = 0x66;
+	pmp_priv->network_macaddr[5] = 0x55;
+
+	pnetwork = &pmp_priv->mp_network.network;
+	_rtw_memcpy(pnetwork->MacAddress, pmp_priv->network_macaddr, ETH_ALEN);
+
+	pnetwork->Ssid.SsidLength = 8;
+	_rtw_memcpy(pnetwork->Ssid.Ssid, "mp_871x", pnetwork->Ssid.SsidLength);
+}
+
+#ifdef PLATFORM_WINDOWS
+/*
+void mp_wi_callback(
+	IN NDIS_WORK_ITEM*	pwk_item,
+	IN PVOID			cntx
+	)
+{
+	_adapter* padapter =(_adapter *)cntx;
+	struct mp_priv *pmppriv=&padapter->mppriv;
+	struct mp_wi_cntx	*pmp_wi_cntx=&pmppriv->wi_cntx;
+
+	// Execute specified action.
+	if(pmp_wi_cntx->curractfunc != NULL)
+	{
+		LARGE_INTEGER	cur_time;
+		ULONGLONG start_time, end_time;
+		NdisGetCurrentSystemTime(&cur_time);	// driver version
+		start_time = cur_time.QuadPart/10; // The return value is in microsecond
+
+		pmp_wi_cntx->curractfunc(padapter);
+
+		NdisGetCurrentSystemTime(&cur_time);	// driver version
+		end_time = cur_time.QuadPart/10; // The return value is in microsecond
+
+		RT_TRACE(_module_mp_, _drv_info_,
+			 ("WorkItemActType: %d, time spent: %I64d us\n",
+			  pmp_wi_cntx->param.act_type, (end_time-start_time)));
+	}
+
+	NdisAcquireSpinLock(&(pmp_wi_cntx->mp_wi_lock));
+	pmp_wi_cntx->bmp_wi_progress= _FALSE;
+	NdisReleaseSpinLock(&(pmp_wi_cntx->mp_wi_lock));
+
+	if (pmp_wi_cntx->bmpdrv_unload)
+	{
+		NdisSetEvent(&(pmp_wi_cntx->mp_wi_evt));
+	}
+
+}
+*/
+
+static int init_mp_priv_by_os(struct mp_priv *pmp_priv)
+{
+	struct mp_wi_cntx *pmp_wi_cntx;
+
+	if (pmp_priv == NULL) return _FAIL;
+
+	pmp_priv->rx_testcnt = 0;
+	pmp_priv->rx_testcnt1 = 0;
+	pmp_priv->rx_testcnt2 = 0;
+
+	pmp_priv->tx_testcnt = 0;
+	pmp_priv->tx_testcnt1 = 0;
+
+	pmp_wi_cntx = &pmp_priv->wi_cntx
+	pmp_wi_cntx->bmpdrv_unload = _FALSE;
+	pmp_wi_cntx->bmp_wi_progress = _FALSE;
+	pmp_wi_cntx->curractfunc = NULL;
+
+	return _SUCCESS;
+}
+#endif
+
+#ifdef PLATFORM_LINUX
+static int init_mp_priv_by_os(struct mp_priv *pmp_priv)
+{
+	int i, res;
+	struct mp_xmit_frame *pmp_xmitframe;
+
+	if (pmp_priv == NULL) return _FAIL;
+
+	_rtw_init_queue(&pmp_priv->free_mp_xmitqueue);
+
+	pmp_priv->pallocated_mp_xmitframe_buf = NULL;
+	pmp_priv->pallocated_mp_xmitframe_buf = rtw_zmalloc(NR_MP_XMITFRAME * sizeof(struct mp_xmit_frame) + 4);
+	if (pmp_priv->pallocated_mp_xmitframe_buf == NULL) {
+		res = _FAIL;
+		goto _exit_init_mp_priv;
+	}
+
+	pmp_priv->pmp_xmtframe_buf = pmp_priv->pallocated_mp_xmitframe_buf + 4 - ((SIZE_PTR) (pmp_priv->pallocated_mp_xmitframe_buf) & 3);
+
+	pmp_xmitframe = (struct mp_xmit_frame*)pmp_priv->pmp_xmtframe_buf;
+
+	for (i = 0; i < NR_MP_XMITFRAME; i++)
+	{
+		_rtw_init_listhead(&pmp_xmitframe->list);
+		rtw_list_insert_tail(&pmp_xmitframe->list, &pmp_priv->free_mp_xmitqueue.queue);
+
+		pmp_xmitframe->pkt = NULL;
+		pmp_xmitframe->frame_tag = MP_FRAMETAG;
+		pmp_xmitframe->padapter = pmp_priv->papdater;
+
+		pmp_xmitframe++;
+	}
+
+	pmp_priv->free_mp_xmitframe_cnt = NR_MP_XMITFRAME;
+
+	res = _SUCCESS;
+
+_exit_init_mp_priv:
+
+	return res;
+}
+#endif
+
+static void mp_init_xmit_attrib(struct mp_tx *pmptx, PADAPTER padapter)
+{
+	struct pkt_attrib *pattrib;
+	struct tx_desc *desc;
+
+	// init xmitframe attribute
+	pattrib = &pmptx->attrib;
+	_rtw_memset(pattrib, 0, sizeof(struct pkt_attrib));
+	desc = &pmptx->desc;
+	_rtw_memset(desc, 0, TXDESC_SIZE);
+
+	pattrib->ether_type = 0x8712;
+	//_rtw_memcpy(pattrib->src, padapter->eeprompriv.mac_addr, ETH_ALEN);
+//	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+	_rtw_memset(pattrib->dst, 0xFF, ETH_ALEN);
+//	pattrib->pctrl = 0;
+//	pattrib->dhcp_pkt = 0;
+//	pattrib->pktlen = 0;
+	pattrib->ack_policy = 0;
+//	pattrib->pkt_hdrlen = ETH_HLEN;
+	pattrib->hdrlen = WLAN_HDR_A3_LEN;
+	pattrib->subtype = WIFI_DATA;
+	pattrib->priority = 0;
+	pattrib->qsel = pattrib->priority;
+//	do_queue_select(padapter, pattrib);
+	pattrib->nr_frags = 1;
+	pattrib->encrypt = 0;
+	pattrib->bswenc = _FALSE;
+	pattrib->qos_en = _FALSE;
+}
+
+s32 init_mp_priv(PADAPTER padapter)
+{
+	struct mp_priv *pmppriv = &padapter->mppriv;
+
+	_init_mp_priv_(pmppriv);
+	pmppriv->papdater = padapter;
+
+	pmppriv->tx.stop = 1;
+	mp_init_xmit_attrib(&pmppriv->tx, padapter);
+
+	switch (padapter->registrypriv.rf_config) {
+		case RF_1T1R:
+			pmppriv->antenna_tx = ANTENNA_A;
+			pmppriv->antenna_rx = ANTENNA_A;
+			break;
+		case RF_1T2R:
+		default:
+			pmppriv->antenna_tx = ANTENNA_A;
+			pmppriv->antenna_rx = ANTENNA_AB;
+			break;
+		case RF_2T2R:
+		case RF_2T2R_GREEN:
+			pmppriv->antenna_tx = ANTENNA_AB;
+			pmppriv->antenna_rx = ANTENNA_AB;
+			break;
+		case RF_2T4R:
+			pmppriv->antenna_tx = ANTENNA_AB;
+			pmppriv->antenna_rx = ANTENNA_ABCD;
+			break;
+	}
+
+	return _SUCCESS;
+}
+
+void free_mp_priv(struct mp_priv *pmp_priv)
+{
+	if (pmp_priv->pallocated_mp_xmitframe_buf) {
+		rtw_mfree(pmp_priv->pallocated_mp_xmitframe_buf, 0);
+		pmp_priv->pallocated_mp_xmitframe_buf = NULL;
+	}
+	pmp_priv->pmp_xmtframe_buf = NULL;
+}
+
+#if defined (CONFIG_RTL8192C) || defined (CONFIG_RTL8723A)
+#define PHY_IQCalibrate(a,b)	rtl8192c_PHY_IQCalibrate(a,b)
+#define PHY_LCCalibrate(a)	rtl8192c_PHY_LCCalibrate(a)
+//#define dm_CheckTXPowerTracking(a)	rtl8192c_odm_CheckTXPowerTracking(a)
+#define PHY_SetRFPathSwitch(a,b)	rtl8192c_PHY_SetRFPathSwitch(a,b)
+#endif
+
+#ifdef CONFIG_RTL8192D
+#define PHY_IQCalibrate(a,b)	rtl8192d_PHY_IQCalibrate(a)
+#define PHY_LCCalibrate(a)	rtl8192d_PHY_LCCalibrate(a)
+//#define dm_CheckTXPowerTracking(a)	rtl8192d_odm_CheckTXPowerTracking(a)
+#define PHY_SetRFPathSwitch(a,b)	rtl8192d_PHY_SetRFPathSwitch(a,b)
+#endif
+
+#ifdef CONFIG_RTL8188E
+#define PHY_IQCalibrate(a,b)	PHY_IQCalibrate_8188E(a,b)
+#define PHY_LCCalibrate(a)	PHY_LCCalibrate_8188E(a)
+#define PHY_SetRFPathSwitch(a,b) PHY_SetRFPathSwitch_8188E(a,b)
+#endif
+
+s32
+MPT_InitializeAdapter(
+	IN	PADAPTER			pAdapter,
+	IN	u8				Channel
+	)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(pAdapter);
+	s32		rtStatus = _SUCCESS;
+	PMPT_CONTEXT	pMptCtx = &pAdapter->mppriv.MptCtx;
+	u32		ledsetting;
+	struct mlme_priv *pmlmepriv = &pAdapter->mlmepriv;
+
+	//-------------------------------------------------------------------------
+	// HW Initialization for 8190 MPT.
+	//-------------------------------------------------------------------------
+	//-------------------------------------------------------------------------
+	// SW Initialization for 8190 MP.
+	//-------------------------------------------------------------------------
+	pMptCtx->bMptDrvUnload = _FALSE;
+	pMptCtx->bMassProdTest = _FALSE;
+	pMptCtx->bMptIndexEven = _TRUE;	//default gain index is -6.0db
+	pMptCtx->h2cReqNum = 0x0;
+	/* Init mpt event. */
+#if 0 // for Windows
+	NdisInitializeEvent( &(pMptCtx->MptWorkItemEvent) );
+	NdisAllocateSpinLock( &(pMptCtx->MptWorkItemSpinLock) );
+
+	PlatformInitializeWorkItem(
+		Adapter,
+		&(pMptCtx->MptWorkItem),
+		(RT_WORKITEM_CALL_BACK)MPT_WorkItemCallback,
+		(PVOID)Adapter,
+		"MptWorkItem");
+#endif
+	//init for BT MP
+#ifdef CONFIG_RTL8723A
+	pMptCtx->bMPh2c_timeout = _FALSE;
+	pMptCtx->MptH2cRspEvent = _FALSE;
+	pMptCtx->MptBtC2hEvent = _FALSE;
+	
+	_rtw_init_sema(&pMptCtx->MPh2c_Sema, 0);
+	_init_timer( &pMptCtx->MPh2c_timeout_timer, pAdapter->pnetdev, MPh2c_timeout_handle, pAdapter );
+
+	//before the reset bt patch command,set the wifi page 0's IO to BT mac reboot.
+#endif
+
+	pMptCtx->bMptWorkItemInProgress = _FALSE;
+	pMptCtx->CurrMptAct = NULL;
+	//-------------------------------------------------------------------------
+
+#if 1
+	// Don't accept any packets
+	rtw_write32(pAdapter, REG_RCR, 0);
+#else
+	// Accept CRC error and destination address
+	//pHalData->ReceiveConfig |= (RCR_ACRC32|RCR_AAP);
+	//rtw_write32(pAdapter, REG_RCR, pHalData->ReceiveConfig);
+	rtw_write32(pAdapter, REG_RCR, 0x70000101);
+#endif
+
+#if 0
+	// If EEPROM or EFUSE is empty,we assign as RF 2T2R for MP.
+	if (pHalData->AutoloadFailFlag == TRUE)
+	{
+		pHalData->RF_Type = RF_2T2R;
+	}
+#endif
+
+	//ledsetting = rtw_read32(pAdapter, REG_LEDCFG0);
+	//rtw_write32(pAdapter, REG_LEDCFG0, ledsetting & ~LED0DIS);
+	
+	if(IS_HARDWARE_TYPE_8192DU(pAdapter))
+	{
+		rtw_write32(pAdapter, REG_LEDCFG0, 0x8888);
+	}
+	else
+	{
+		//rtw_write32(pAdapter, REG_LEDCFG0, 0x08080);
+		ledsetting = rtw_read32(pAdapter, REG_LEDCFG0);
+		
+	#if defined (CONFIG_RTL8192C) || defined( CONFIG_RTL8192D )
+			rtw_write32(pAdapter, REG_LEDCFG0, ledsetting & ~LED0DIS);
+	#endif
+	}
+	
+	PHY_IQCalibrate(pAdapter, _FALSE);
+	dm_CheckTXPowerTracking(&pHalData->odmpriv);	//trigger thermal meter
+	PHY_LCCalibrate(pAdapter);
+
+#ifdef CONFIG_PCI_HCI
+	PHY_SetRFPathSwitch(pAdapter, 1/*pHalData->bDefaultAntenna*/);	//Wifi default use Main
+#else
+
+#ifdef CONFIG_RTL8192C
+	if (pHalData->BoardType == BOARD_MINICARD)
+		PHY_SetRFPathSwitch(pAdapter, 1/*pHalData->bDefaultAntenna*/); //default use Main
+#endif
+
+#endif
+
+	pMptCtx->backup0xc50 = (u1Byte)PHY_QueryBBReg(pAdapter, rOFDM0_XAAGCCore1, bMaskByte0);
+	pMptCtx->backup0xc58 = (u1Byte)PHY_QueryBBReg(pAdapter, rOFDM0_XBAGCCore1, bMaskByte0);
+	pMptCtx->backup0xc30 = (u1Byte)PHY_QueryBBReg(pAdapter, rOFDM0_RxDetector1, bMaskByte0);
+#ifdef CONFIG_RTL8188E
+	pMptCtx->backup0x52_RF_A = (u1Byte)PHY_QueryRFReg(pAdapter, RF_PATH_A, RF_0x52, 0x000F0);
+	pMptCtx->backup0x52_RF_B = (u1Byte)PHY_QueryRFReg(pAdapter, RF_PATH_A, RF_0x52, 0x000F0);
+#endif
+		//set ant to wifi side in mp mode
+#ifdef CONFIG_RTL8723A
+		rtl8723a_InitAntenna_Selection(pAdapter);	
+#endif //CONFIG_RTL8723A
+
+	//set ant to wifi side in mp mode
+	rtw_write16(pAdapter, 0x870, 0x300);
+	rtw_write16(pAdapter, 0x860, 0x110);
+
+	if (pAdapter->registrypriv.mp_mode == 1)
+		pmlmepriv->fw_state = WIFI_MP_STATE;
+
+	return	rtStatus;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	MPT_DeInitAdapter()
+ *
+ * Overview:	Extra DeInitialization for Mass Production Test.
+ *
+ * Input:		PADAPTER	pAdapter
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	05/08/2007	MHC		Create Version 0.
+ *	05/18/2007	MHC		Add normal driver MPHalt code.
+ *
+ *---------------------------------------------------------------------------*/
+VOID
+MPT_DeInitAdapter(
+	IN	PADAPTER	pAdapter
+	)
+{
+	PMPT_CONTEXT		pMptCtx = &pAdapter->mppriv.MptCtx;
+
+	pMptCtx->bMptDrvUnload = _TRUE;
+	#ifdef CONFIG_RTL8723A
+	_rtw_free_sema(&(pMptCtx->MPh2c_Sema));
+	_cancel_timer_ex( &pMptCtx->MPh2c_timeout_timer);
+	
+	rtw_write32(pAdapter, 0xcc, (rtw_read32(pAdapter, 0xcc)& 0xFFFFFFFD)| 0x00000002);
+	rtw_write32(pAdapter, 0x6b, (rtw_read32(pAdapter, 0x6b)& 0xFFFFFFFB));
+	rtw_msleep_os(500);
+	rtw_write32(pAdapter, 0x6b, (rtw_read32(pAdapter, 0x6b)& 0xFFFFFFFB)| 0x00000004);
+	rtw_write32(pAdapter, 0xcc, (rtw_read32(pAdapter, 0xcc)& 0xFFFFFFFD));
+	rtw_msleep_os(1000);
+	
+	DBG_871X("_rtw_mp_xmit_priv reinit for normal mode\n");
+	_rtw_mp_xmit_priv(&pAdapter->xmitpriv);
+	#endif
+#if 0 // for Windows
+	PlatformFreeWorkItem( &(pMptCtx->MptWorkItem) );
+
+	while(pMptCtx->bMptWorkItemInProgress)
+	{
+		if(NdisWaitEvent(&(pMptCtx->MptWorkItemEvent), 50))
+		{
+			break;
+		}
+	}
+	NdisFreeSpinLock( &(pMptCtx->MptWorkItemSpinLock) );
+#endif
+	
+}
+
+static u8 mpt_ProStartTest(PADAPTER padapter)
+{
+	PMPT_CONTEXT pMptCtx = &padapter->mppriv.MptCtx;
+
+	pMptCtx->bMassProdTest = _TRUE;
+	pMptCtx->bStartContTx = _FALSE;
+	pMptCtx->bCckContTx = _FALSE;
+	pMptCtx->bOfdmContTx = _FALSE;
+	pMptCtx->bSingleCarrier = _FALSE;
+	pMptCtx->bCarrierSuppression = _FALSE;
+	pMptCtx->bSingleTone = _FALSE;
+
+	return _SUCCESS;
+}
+
+/*
+ * General use
+ */
+s32 SetPowerTracking(PADAPTER padapter, u8 enable)
+{
+
+	Hal_SetPowerTracking( padapter, enable );
+	return 0;
+}
+
+void GetPowerTracking(PADAPTER padapter, u8 *enable)
+{
+	Hal_GetPowerTracking( padapter, enable );
+}
+
+static void disable_dm(PADAPTER padapter)
+{
+#ifndef CONFIG_RTL8723A
+	u8 v8;
+#endif
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_priv	*pdmpriv = &pHalData->dmpriv;
+
+
+	//3 1. disable firmware dynamic mechanism
+	// disable Power Training, Rate Adaptive
+#ifdef CONFIG_RTL8723A
+	SetBcnCtrlReg(padapter, 0, EN_BCN_FUNCTION);
+#else
+	v8 = rtw_read8(padapter, REG_BCN_CTRL);
+	v8 &= ~EN_BCN_FUNCTION;
+	rtw_write8(padapter, REG_BCN_CTRL, v8);
+#endif
+
+	//3 2. disable driver dynamic mechanism
+	// disable Dynamic Initial Gain
+	// disable High Power
+	// disable Power Tracking
+	Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
+
+	// enable APK, LCK and IQK but disable power tracking
+#ifndef CONFIG_RTL8188E
+	pdmpriv->TxPowerTrackControl = _FALSE;
+#endif
+	Switch_DM_Func(padapter, DYNAMIC_RF_CALIBRATION, _TRUE);
+}
+
+
+void MPT_PwrCtlDM(PADAPTER padapter, u32 bstart)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_priv	*pdmpriv = &pHalData->dmpriv;
+	
+	//Switch_DM_Func(padapter, DYNAMIC_RF_CALIBRATION, bstart);
+	if (bstart==1){
+		DBG_871X("in MPT_PwrCtlDM start \n");		
+		Switch_DM_Func(padapter, DYNAMIC_RF_TX_PWR_TRACK, _TRUE);
+		pdmpriv->InitODMFlag |= ODM_RF_TX_PWR_TRACK ;
+		pdmpriv->InitODMFlag |= ODM_RF_CALIBRATION ;
+		pdmpriv->TxPowerTrackControl = _TRUE;
+	}else{
+		DBG_871X("in MPT_PwrCtlDM stop \n");
+		disable_dm(padapter);
+		pdmpriv->TxPowerTrackControl = _FALSE;
+
+	}
+		
+}
+
+
+//This function initializes the DUT to the MP test mode
+s32 mp_start_test(PADAPTER padapter)
+{
+	WLAN_BSSID_EX bssid;
+	struct sta_info *psta;
+	u32 length;
+	u8 val8;
+
+	_irqL irqL;
+	s32 res = _SUCCESS;
+
+	struct mp_priv *pmppriv = &padapter->mppriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+
+	padapter->registrypriv.mp_mode = 1;
+	pmppriv->bSetTxPower=0;		//for  manually set tx power
+	
+	//3 disable dynamic mechanism
+	disable_dm(padapter);
+
+	//3 0. update mp_priv
+
+	if (padapter->registrypriv.rf_config == RF_819X_MAX_TYPE) {
+//		switch (phal->rf_type) {
+		switch (GET_RF_TYPE(padapter)) {
+			case RF_1T1R:
+				pmppriv->antenna_tx = ANTENNA_A;
+				pmppriv->antenna_rx = ANTENNA_A;
+				break;
+			case RF_1T2R:
+			default:
+				pmppriv->antenna_tx = ANTENNA_A;
+				pmppriv->antenna_rx = ANTENNA_AB;
+				break;
+			case RF_2T2R:
+			case RF_2T2R_GREEN:
+				pmppriv->antenna_tx = ANTENNA_AB;
+				pmppriv->antenna_rx = ANTENNA_AB;
+				break;
+			case RF_2T4R:
+				pmppriv->antenna_tx = ANTENNA_AB;
+				pmppriv->antenna_rx = ANTENNA_ABCD;
+				break;
+		}
+	}
+
+	mpt_ProStartTest(padapter);
+
+	//3 1. initialize a new WLAN_BSSID_EX
+//	_rtw_memset(&bssid, 0, sizeof(WLAN_BSSID_EX));
+	_rtw_memcpy(bssid.MacAddress, pmppriv->network_macaddr, ETH_ALEN);
+	bssid.Ssid.SsidLength = strlen("mp_pseudo_adhoc");
+	_rtw_memcpy(bssid.Ssid.Ssid, (u8*)"mp_pseudo_adhoc", bssid.Ssid.SsidLength);
+	bssid.InfrastructureMode = Ndis802_11IBSS;
+	bssid.NetworkTypeInUse = Ndis802_11DS;
+	bssid.IELength = 0;
+
+	length = get_WLAN_BSSID_EX_sz(&bssid);
+	if (length % 4)
+		bssid.Length = ((length >> 2) + 1) << 2; //round up to multiple of 4 bytes.
+	else
+		bssid.Length = length;
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE)
+		goto end_of_mp_start_test;
+
+	//init mp_start_test status
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE) {
+		rtw_disassoc_cmd(padapter, 500, _TRUE);
+		rtw_indicate_disconnect(padapter);
+		rtw_free_assoc_resources(padapter, 1);
+	}
+	pmppriv->prev_fw_state = get_fwstate(pmlmepriv);
+	if (padapter->registrypriv.mp_mode == 1)
+		pmlmepriv->fw_state = WIFI_MP_STATE;
+#if 0
+	if (pmppriv->mode == _LOOPBOOK_MODE_) {
+		set_fwstate(pmlmepriv, WIFI_MP_LPBK_STATE); //append txdesc
+		RT_TRACE(_module_mp_, _drv_notice_, ("+start mp in Lookback mode\n"));
+	} else {
+		RT_TRACE(_module_mp_, _drv_notice_, ("+start mp in normal mode\n"));
+	}
+#endif
+	set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+	//3 2. create a new psta for mp driver
+	//clear psta in the cur_network, if any
+	psta = rtw_get_stainfo(&padapter->stapriv, tgt_network->network.MacAddress);
+	if (psta) rtw_free_stainfo(padapter, psta);
+
+	psta = rtw_alloc_stainfo(&padapter->stapriv, bssid.MacAddress);
+	if (psta == NULL) {
+		RT_TRACE(_module_mp_, _drv_err_, ("mp_start_test: Can't alloc sta_info!\n"));
+		pmlmepriv->fw_state = pmppriv->prev_fw_state;
+		res = _FAIL;
+		goto end_of_mp_start_test;
+	}
+
+	//3 3. join psudo AdHoc
+	tgt_network->join_res = 1;
+	tgt_network->aid = psta->aid = 1;
+	_rtw_memcpy(&tgt_network->network, &bssid, length);
+
+	rtw_indicate_connect(padapter);
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+end_of_mp_start_test:
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+	if (res == _SUCCESS)
+	{
+		// set MSR to WIFI_FW_ADHOC_STATE
+#if  !defined (CONFIG_RTL8712)
+		val8 = rtw_read8(padapter, MSR) & 0xFC; // 0x0102
+		val8 |= WIFI_FW_ADHOC_STATE;
+		rtw_write8(padapter, MSR, val8); // Link in ad hoc network
+#endif
+
+#if  defined (CONFIG_RTL8712)
+		rtw_write8(padapter, MSR, 1); // Link in ad hoc network
+		rtw_write8(padapter, RCR, 0); // RCR : disable all pkt, 0x10250048
+		rtw_write8(padapter, RCR+2, 0x57); // RCR disable Check BSSID, 0x1025004a
+
+		// disable RX filter map , mgt frames will put in RX FIFO 0
+		rtw_write16(padapter, RXFLTMAP0, 0x0); // 0x10250116
+
+		val8 = rtw_read8(padapter, EE_9346CR); // 0x1025000A
+		if (!(val8 & _9356SEL))//boot from EFUSE
+			efuse_change_max_size(padapter);
+#endif
+	}
+
+	return res;
+}
+//------------------------------------------------------------------------------
+//This function change the DUT from the MP test mode into normal mode
+void mp_stop_test(PADAPTER padapter)
+{
+	struct mp_priv *pmppriv = &padapter->mppriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+	struct sta_info *psta;
+
+	_irqL irqL;
+	
+	if(pmppriv->mode==MP_ON)
+	{
+	pmppriv->bSetTxPower=0;
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);	
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _FALSE)
+		goto end_of_mp_stop_test;
+
+	//3 1. disconnect psudo AdHoc
+	rtw_indicate_disconnect(padapter);
+
+	//3 2. clear psta used in mp test mode.
+//	rtw_free_assoc_resources(padapter, 1);
+	psta = rtw_get_stainfo(&padapter->stapriv, tgt_network->network.MacAddress);
+	if (psta) rtw_free_stainfo(padapter, psta);
+
+	//3 3. return to normal state (default:station mode)
+	pmlmepriv->fw_state = pmppriv->prev_fw_state; // WIFI_STATION_STATE;
+
+	//flush the cur_network
+	_rtw_memset(tgt_network, 0, sizeof(struct wlan_network));
+
+	_clr_fwstate_(pmlmepriv, WIFI_MP_STATE);
+
+end_of_mp_stop_test:
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+	}
+}
+/*---------------------------hal\rtl8192c\MPT_Phy.c---------------------------*/
+#if 0
+//#ifdef CONFIG_USB_HCI
+static VOID mpt_AdjustRFRegByRateByChan92CU(PADAPTER pAdapter, u8 RateIdx, u8 Channel, u8 BandWidthID)
+{
+	u8		eRFPath;
+	u32		rfReg0x26;
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(pAdapter);
+
+
+	if (RateIdx < MPT_RATE_6M) {	// CCK rate,for 88cu
+		rfReg0x26 = 0xf400;
+	}
+	else if ((RateIdx >= MPT_RATE_6M) && (RateIdx <= MPT_RATE_54M)) {// OFDM rate,for 88cu
+		if ((4 == Channel) || (8 == Channel) || (12 == Channel))
+			rfReg0x26 = 0xf000;
+		else if ((5 == Channel) || (7 == Channel) || (13 == Channel) || (14 == Channel))
+			rfReg0x26 = 0xf400;
+		else
+			rfReg0x26 = 0x4f200;
+	}
+	else if ((RateIdx >= MPT_RATE_MCS0) && (RateIdx <= MPT_RATE_MCS15)) {// MCS 20M ,for 88cu // MCS40M rate,for 88cu
+
+		if (HT_CHANNEL_WIDTH_20 == BandWidthID) {
+			if ((4 == Channel) || (8 == Channel))
+				rfReg0x26 = 0xf000;
+			else if ((5 == Channel) || (7 == Channel) || (13 == Channel) || (14 == Channel))
+				rfReg0x26 = 0xf400;
+			else
+				rfReg0x26 = 0x4f200;
+		}
+		else{
+			if ((4 == Channel) || (8 == Channel))
+				rfReg0x26 = 0xf000;
+			else if ((5 == Channel) || (7 == Channel))
+				rfReg0x26 = 0xf400;
+			else
+				rfReg0x26 = 0x4f200;
+		}
+	}
+
+//	RT_TRACE(COMP_CMD, DBG_LOUD, ("\n mpt_AdjustRFRegByRateByChan92CU():Chan:%d Rate=%d rfReg0x26:0x%08x\n",Channel, RateIdx,rfReg0x26));
+	for (eRFPath = 0; eRFPath < pHalData->NumTotalRFPath; eRFPath++) {
+		write_rfreg(pAdapter, eRFPath, RF_SYN_G2, rfReg0x26);
+	}
+}
+#endif
+/*-----------------------------------------------------------------------------
+ * Function:	mpt_SwitchRfSetting
+ *
+ * Overview:	Change RF Setting when we siwthc channel/rate/BW for MP.
+ *
+ * Input:       IN	PADAPTER				pAdapter
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 01/08/2009	MHC		Suggestion from SD3 Willis for 92S series.
+ * 01/09/2009	MHC		Add CCK modification for 40MHZ. Suggestion from SD3.
+ *
+ *---------------------------------------------------------------------------*/
+static void mpt_SwitchRfSetting(PADAPTER pAdapter)
+{
+	Hal_mpt_SwitchRfSetting(pAdapter);
+    }
+
+/*---------------------------hal\rtl8192c\MPT_Phy.c---------------------------*/
+/*---------------------------hal\rtl8192c\MPT_HelperFunc.c---------------------------*/
+static void MPT_CCKTxPowerAdjust(PADAPTER Adapter, BOOLEAN bInCH14)
+{
+	Hal_MPT_CCKTxPowerAdjust(Adapter,bInCH14);
+}
+
+static void MPT_CCKTxPowerAdjustbyIndex(PADAPTER pAdapter, BOOLEAN beven)
+{
+	Hal_MPT_CCKTxPowerAdjustbyIndex(pAdapter,beven);
+	}
+
+/*---------------------------hal\rtl8192c\MPT_HelperFunc.c---------------------------*/
+
+/*
+ * SetChannel
+ * Description
+ *	Use H2C command to change channel,
+ *	not only modify rf register, but also other setting need to be done.
+ */
+void SetChannel(PADAPTER pAdapter)
+{
+	Hal_SetChannel(pAdapter);
+
+}
+
+/*
+ * Notice
+ *	Switch bandwitdth may change center frequency(channel)
+ */
+void SetBandwidth(PADAPTER pAdapter)
+{
+	Hal_SetBandwidth(pAdapter);
+
+}
+
+static void SetCCKTxPower(PADAPTER pAdapter, u8 *TxPower)
+{
+	Hal_SetCCKTxPower(pAdapter,TxPower);
+}
+
+static void SetOFDMTxPower(PADAPTER pAdapter, u8 *TxPower)
+{
+	Hal_SetOFDMTxPower(pAdapter,TxPower);
+	}
+
+
+void SetAntenna(PADAPTER pAdapter)
+	{
+	Hal_SetAntenna(pAdapter);
+}
+
+void	SetAntennaPathPower(PADAPTER pAdapter)
+{
+	Hal_SetAntennaPathPower(pAdapter);
+}
+	
+void SetTxPower(PADAPTER pAdapter)
+{
+	Hal_SetTxPower(pAdapter);
+	}
+
+void SetTxAGCOffset(PADAPTER pAdapter, u32 ulTxAGCOffset)
+{
+	u32 TxAGCOffset_B, TxAGCOffset_C, TxAGCOffset_D,tmpAGC;
+
+	TxAGCOffset_B = (ulTxAGCOffset&0x000000ff);
+	TxAGCOffset_C = ((ulTxAGCOffset&0x0000ff00)>>8);
+	TxAGCOffset_D = ((ulTxAGCOffset&0x00ff0000)>>16);
+
+	tmpAGC = (TxAGCOffset_D<<8 | TxAGCOffset_C<<4 | TxAGCOffset_B);
+	write_bbreg(pAdapter, rFPGA0_TxGainStage,
+			(bXBTxAGC|bXCTxAGC|bXDTxAGC), tmpAGC);
+}
+
+void SetDataRate(PADAPTER pAdapter)
+{
+	Hal_SetDataRate(pAdapter);
+}
+
+void MP_PHY_SetRFPathSwitch(PADAPTER pAdapter ,BOOLEAN bMain)
+{
+
+	PHY_SetRFPathSwitch(pAdapter,bMain);
+
+}
+
+#if defined (CONFIG_RTL8712)
+/*------------------------------Define structure----------------------------*/
+typedef struct _R_ANTENNA_SELECT_OFDM {
+	u32 r_tx_antenna:4;
+	u32 r_ant_l:4;
+	u32 r_ant_non_ht:4;
+	u32 r_ant_ht1:4;
+	u32 r_ant_ht2:4;
+	u32 r_ant_ht_s1:4;
+	u32 r_ant_non_ht_s1:4;
+	u32 OFDM_TXSC:2;
+	u32 Reserved:2;
+}R_ANTENNA_SELECT_OFDM;
+
+typedef struct _R_ANTENNA_SELECT_CCK {
+	u8	r_cckrx_enable_2:2;
+	u8	r_cckrx_enable:2;
+	u8	r_ccktx_enable:4;
+}R_ANTENNA_SELECT_CCK;
+#endif
+
+s32 SetThermalMeter(PADAPTER pAdapter, u8 target_ther)
+{
+	return Hal_SetThermalMeter( pAdapter, target_ther);
+}
+
+static void TriggerRFThermalMeter(PADAPTER pAdapter)
+{
+	Hal_TriggerRFThermalMeter(pAdapter);
+}
+
+static u8 ReadRFThermalMeter(PADAPTER pAdapter)
+{
+	return Hal_ReadRFThermalMeter(pAdapter);
+}
+
+void GetThermalMeter(PADAPTER pAdapter, u8 *value)
+{
+	Hal_GetThermalMeter(pAdapter,value);
+}
+
+void SetSingleCarrierTx(PADAPTER pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	Hal_SetSingleCarrierTx(pAdapter,bStart);
+}
+
+void SetSingleToneTx(PADAPTER pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	Hal_SetSingleToneTx(pAdapter,bStart);
+}
+
+void SetCarrierSuppressionTx(PADAPTER pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	Hal_SetCarrierSuppressionTx(pAdapter, bStart);
+}
+
+void SetCCKContinuousTx(PADAPTER pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	Hal_SetCCKContinuousTx(pAdapter,bStart);
+}
+
+void SetOFDMContinuousTx(PADAPTER pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+   	Hal_SetOFDMContinuousTx( pAdapter, bStart);
+}/* mpt_StartOfdmContTx */
+
+void SetContinuousTx(PADAPTER pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	Hal_SetContinuousTx(pAdapter,bStart);
+}
+
+
+void PhySetTxPowerLevel(PADAPTER pAdapter)
+{
+	struct mp_priv *pmp_priv = &pAdapter->mppriv;
+		
+	if (pmp_priv->bSetTxPower==0) // for NO manually set power index
+	{
+#ifdef CONFIG_RTL8188E	
+		PHY_SetTxPowerLevel8188E(pAdapter,pmp_priv->channel);
+#elif defined(CONFIG_RTL8192D)
+		PHY_SetTxPowerLevel8192D(pAdapter,pmp_priv->channel);
+#else
+		PHY_SetTxPowerLevel8192C(pAdapter,pmp_priv->channel);
+#endif
+	}
+}
+
+//------------------------------------------------------------------------------
+static void dump_mpframe(PADAPTER padapter, struct xmit_frame *pmpframe)
+{
+	rtw_hal_mgnt_xmit(padapter, pmpframe);
+}
+
+static struct xmit_frame *alloc_mp_xmitframe(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_frame	*pmpframe;
+	struct xmit_buf	*pxmitbuf;
+
+	if ((pmpframe = rtw_alloc_xmitframe(pxmitpriv)) == NULL)
+	{
+		return NULL;
+	}
+
+	if ((pxmitbuf = rtw_alloc_xmitbuf(pxmitpriv)) == NULL)
+	{
+		rtw_free_xmitframe(pxmitpriv, pmpframe);
+		return NULL;
+	}
+
+	pmpframe->frame_tag = MP_FRAMETAG;
+
+	pmpframe->pxmitbuf = pxmitbuf;
+
+	pmpframe->buf_addr = pxmitbuf->pbuf;
+
+	pxmitbuf->priv_data = pmpframe;
+
+	return pmpframe;
+
+}
+
+static thread_return mp_xmit_packet_thread(thread_context context)
+{
+	struct xmit_frame	*pxmitframe;
+	struct mp_tx		*pmptx;
+	struct mp_priv	*pmp_priv;
+	struct xmit_priv	*pxmitpriv;
+	PADAPTER padapter;
+
+	pmp_priv = (struct mp_priv *)context;
+	pmptx = &pmp_priv->tx;
+	padapter = pmp_priv->papdater;
+	pxmitpriv = &(padapter->xmitpriv);
+
+	thread_enter("RTW_MP_THREAD");
+
+	//DBG_871X("%s:pkTx Start\n", __func__);
+	while (1) {
+		pxmitframe = alloc_mp_xmitframe(pxmitpriv);
+		if (pxmitframe == NULL) {
+			if (pmptx->stop ||
+			    padapter->bSurpriseRemoved ||
+			    padapter->bDriverStopped) {
+				goto exit;
+			}
+			else {
+				rtw_msleep_os(1);
+				continue;
+			}
+		}
+
+		_rtw_memcpy((u8 *)(pxmitframe->buf_addr+TXDESC_OFFSET), pmptx->buf, pmptx->write_size);
+		_rtw_memcpy(&(pxmitframe->attrib), &(pmptx->attrib), sizeof(struct pkt_attrib));
+		
+		dump_mpframe(padapter, pxmitframe);
+
+		pmptx->sended++;
+		pmp_priv->tx_pktcount++;
+
+		if (pmptx->stop ||
+		    padapter->bSurpriseRemoved ||
+		    padapter->bDriverStopped)
+			goto exit;
+		if ((pmptx->count != 0) &&
+		    (pmptx->count == pmptx->sended))
+			goto exit;
+
+		flush_signals_thread();
+	}
+
+exit:
+	//DBG_871X("%s:pkTx Exit\n", __func__);
+	rtw_mfree(pmptx->pallocated_buf, pmptx->buf_size);
+	pmptx->pallocated_buf = NULL;
+	pmptx->stop = 1;
+
+	thread_exit();
+}
+
+void fill_txdesc_for_mp(PADAPTER padapter, struct tx_desc *ptxdesc)
+{		
+	struct mp_priv *pmp_priv = &padapter->mppriv;
+	_rtw_memcpy(ptxdesc, &(pmp_priv->tx.desc), TXDESC_SIZE);
+}
+
+void SetPacketTx(PADAPTER padapter)
+{
+	u8 *ptr, *pkt_start, *pkt_end;
+	u32 pkt_size,offset;
+	struct tx_desc *desc;
+	struct rtw_ieee80211_hdr *hdr;
+	u8 payload;
+	s32 bmcast;
+	struct pkt_attrib *pattrib;
+	struct mp_priv *pmp_priv;
+
+
+	pmp_priv = &padapter->mppriv;
+	if (pmp_priv->tx.stop) return;
+	pmp_priv->tx.sended = 0;
+	pmp_priv->tx.stop = 0;
+	pmp_priv->tx_pktcount = 0;
+
+	//3 1. update_attrib()
+	pattrib = &pmp_priv->tx.attrib;
+	_rtw_memcpy(pattrib->src, padapter->eeprompriv.mac_addr, ETH_ALEN);
+	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+	_rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+	bmcast = IS_MCAST(pattrib->ra);
+	if (bmcast) {
+		pattrib->mac_id = 1;
+		pattrib->psta = rtw_get_bcmc_stainfo(padapter);
+	} else {
+		pattrib->mac_id = 0;
+		pattrib->psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(&padapter->mlmepriv));
+	}
+
+	pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->pktlen;
+
+	//3 2. allocate xmit buffer
+	pkt_size = pattrib->last_txcmdsz;
+
+	if (pmp_priv->tx.pallocated_buf)
+		rtw_mfree(pmp_priv->tx.pallocated_buf, pmp_priv->tx.buf_size);
+	pmp_priv->tx.write_size = pkt_size;
+	pmp_priv->tx.buf_size = pkt_size + XMITBUF_ALIGN_SZ;
+	pmp_priv->tx.pallocated_buf = rtw_zmalloc(pmp_priv->tx.buf_size);
+	if (pmp_priv->tx.pallocated_buf == NULL) {
+		DBG_871X("%s: malloc(%d) fail!!\n", __func__, pmp_priv->tx.buf_size);
+		return;
+	}
+	pmp_priv->tx.buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pmp_priv->tx.pallocated_buf), XMITBUF_ALIGN_SZ);
+	ptr = pmp_priv->tx.buf;
+
+	desc = &(pmp_priv->tx.desc);
+	_rtw_memset(desc, 0, TXDESC_SIZE);
+	pkt_start = ptr;
+	pkt_end = pkt_start + pkt_size;
+
+	//3 3. init TX descriptor
+	// offset 0
+#if defined(CONFIG_RTL8188E) && !defined(CONFIG_RTL8188E_SDIO)
+	desc->txdw0 |= cpu_to_le32(OWN | FSG | LSG);
+	desc->txdw0 |= cpu_to_le32(pkt_size & 0x0000FFFF); // packet size
+	desc->txdw0 |= cpu_to_le32(((TXDESC_SIZE + OFFSET_SZ) << OFFSET_SHT) & 0x00FF0000); //32 bytes for TX Desc
+	if (bmcast) desc->txdw0 |= cpu_to_le32(BMC); // broadcast packet
+
+	desc->txdw1 |= cpu_to_le32((0x01 << 26) & 0xff000000);
+#endif
+	// offset 4
+	#ifndef CONFIG_RTL8188E 
+		desc->txdw1 |= cpu_to_le32(BK); // don't aggregate(AMPDU)
+		desc->txdw1 |= cpu_to_le32((pattrib->mac_id) & 0x1F); //CAM_ID(MAC_ID)
+	#else
+		desc->txdw1 |= cpu_to_le32((pattrib->mac_id) & 0x3F); //CAM_ID(MAC_ID)
+	#endif
+	desc->txdw1 |= cpu_to_le32((pattrib->qsel << QSEL_SHT) & 0x00001F00); // Queue Select, TID
+	
+	#ifdef CONFIG_RTL8188E
+		desc->txdw1 |= cpu_to_le32((pattrib->raid << RATE_ID_SHT) & 0x000F0000); // Rate Adaptive ID
+	#else
+	desc->txdw1 |= cpu_to_le32((pattrib->raid << Rate_ID_SHT) & 0x000F0000); // Rate Adaptive ID
+
+	#endif
+	// offset 8
+	//	desc->txdw2 |= cpu_to_le32(AGG_BK);//AGG BK
+	// offset 12
+
+	desc->txdw3 |= cpu_to_le32((pattrib->seqnum<<16)&0x0fff0000);
+//	desc->txdw3 |= cpu_to_le32((pattrib->seqnum & 0xFFF) << SEQ_SHT);
+	//desc->txdw3 |= cpu_to_le32((pattrib->seqnum << SEQ_SHT) & 0xffff0000);
+
+	// offset 16
+	//desc->txdw4 |= cpu_to_le32(QoS)
+	#ifdef CONFIG_RTL8188E
+		desc->txdw4 |= cpu_to_le32(HW_SSN);
+	#else
+	desc->txdw4 |= cpu_to_le32(HW_SEQ_EN);
+	#endif
+	desc->txdw4 |= cpu_to_le32(USERATE);
+	desc->txdw4 |= cpu_to_le32(DISDATAFB);
+
+	if( pmp_priv->preamble ){
+		if (pmp_priv->rateidx <=  MPT_RATE_54M)
+			desc->txdw4 |= cpu_to_le32(DATA_SHORT); // CCK Short Preamble
+	}
+	if (pmp_priv->bandwidth == HT_CHANNEL_WIDTH_40)
+		desc->txdw4 |= cpu_to_le32(DATA_BW);
+
+	// offset 20
+	desc->txdw5 |= cpu_to_le32(pmp_priv->rateidx & 0x0000001F);
+
+	if( pmp_priv->preamble ){
+		if (pmp_priv->rateidx > MPT_RATE_54M)
+			desc->txdw5 |= cpu_to_le32(SGI); // MCS Short Guard Interval
+	}
+	#ifdef CONFIG_RTL8188E
+		desc->txdw5 |= cpu_to_le32(RTY_LMT_EN); // retry limit enable
+		desc->txdw5 |= cpu_to_le32(0x00180000); // DATA/RTS Rate Fallback Limit	
+	#else
+	desc->txdw5 |= cpu_to_le32(0x0001FF00); // DATA/RTS Rate Fallback Limit
+	#endif
+
+	//3 4. make wlan header, make_wlanhdr()
+	hdr = (struct rtw_ieee80211_hdr *)pkt_start;
+	SetFrameSubType(&hdr->frame_ctl, pattrib->subtype);
+	_rtw_memcpy(hdr->addr1, pattrib->dst, ETH_ALEN); // DA
+	_rtw_memcpy(hdr->addr2, pattrib->src, ETH_ALEN); // SA
+	_rtw_memcpy(hdr->addr3, get_bssid(&padapter->mlmepriv), ETH_ALEN); // RA, BSSID
+
+	//3 5. make payload
+	ptr = pkt_start + pattrib->hdrlen;
+
+	switch (pmp_priv->tx.payload) {
+		case 0:
+			payload = 0x00;
+			break;
+		case 1:
+			payload = 0x5a;
+			break;
+		case 2:
+			payload = 0xa5;
+			break;
+		case 3:
+			payload = 0xff;
+			break;
+		default:
+			payload = 0x00;
+			break;
+	}
+
+	_rtw_memset(ptr, payload, pkt_end - ptr);
+
+	//3 6. start thread
+#ifdef PLATFORM_LINUX
+	pmp_priv->tx.PktTxThread = kthread_run(mp_xmit_packet_thread, pmp_priv, "RTW_MP_THREAD");
+	if (IS_ERR(pmp_priv->tx.PktTxThread))
+		DBG_871X("Create PktTx Thread Fail !!!!!\n");
+#endif
+#ifdef PLATFORM_FREEBSD
+{
+	struct proc *p;
+	struct thread *td;
+	pmp_priv->tx.PktTxThread = kproc_kthread_add(mp_xmit_packet_thread, pmp_priv,
+					&p, &td, RFHIGHPID, 0, "MPXmitThread", "MPXmitThread");
+
+	if (pmp_priv->tx.PktTxThread < 0)
+		DBG_871X("Create PktTx Thread Fail !!!!!\n");
+}
+#endif
+}
+
+void SetPacketRx(PADAPTER pAdapter, u8 bStartRx)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(pAdapter);
+
+	if(bStartRx)
+	{
+	#ifdef CONFIG_RTL8723A
+		rtl8723a_InitAntenna_Selection(pAdapter);	
+	#endif //CONFIG_RTL8723A
+		// Accept CRC error and destination address
+#if 1
+//ndef CONFIG_RTL8723A
+		pHalData->ReceiveConfig = AAP | APM | AM | AB | APP_ICV | ADF | AMF | HTC_LOC_CTRL | APP_MIC | APP_PHYSTS;
+		
+		pHalData->ReceiveConfig |= ACRC32;
+		
+		rtw_write32(pAdapter, REG_RCR, pHalData->ReceiveConfig);
+		
+		// Accept all data frames
+		rtw_write16(pAdapter, REG_RXFLTMAP2, 0xFFFF);
+#else
+		rtw_write32(pAdapter, REG_RCR, 0x70000101);
+#endif
+	}
+	else
+	{
+		rtw_write32(pAdapter, REG_RCR, 0);
+	}
+}
+
+void ResetPhyRxPktCount(PADAPTER pAdapter)
+{
+	u32 i, phyrx_set = 0;
+
+	for (i = 0; i <= 0xF; i++) {
+		phyrx_set = 0;
+		phyrx_set |= _RXERR_RPT_SEL(i);	//select
+		phyrx_set |= RXERR_RPT_RST;	// set counter to zero
+		rtw_write32(pAdapter, REG_RXERR_RPT, phyrx_set);
+	}
+}
+
+static u32 GetPhyRxPktCounts(PADAPTER pAdapter, u32 selbit)
+{
+	//selection
+	u32 phyrx_set = 0, count = 0;
+
+	phyrx_set = _RXERR_RPT_SEL(selbit & 0xF);
+	rtw_write32(pAdapter, REG_RXERR_RPT, phyrx_set);
+
+	//Read packet count
+	count = rtw_read32(pAdapter, REG_RXERR_RPT) & RXERR_COUNTER_MASK;
+
+	return count;
+}
+
+u32 GetPhyRxPktReceived(PADAPTER pAdapter)
+{
+	u32 OFDM_cnt = 0, CCK_cnt = 0, HT_cnt = 0;
+
+	OFDM_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_OFDM_MPDU_OK);
+	CCK_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_CCK_MPDU_OK);
+	HT_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_HT_MPDU_OK);
+
+	return OFDM_cnt + CCK_cnt + HT_cnt;
+}
+
+u32 GetPhyRxPktCRC32Error(PADAPTER pAdapter)
+{
+	u32 OFDM_cnt = 0, CCK_cnt = 0, HT_cnt = 0;
+
+	OFDM_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_OFDM_MPDU_FAIL);
+	CCK_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_CCK_MPDU_FAIL);
+	HT_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_HT_MPDU_FAIL);
+
+	return OFDM_cnt + CCK_cnt + HT_cnt;
+}
+
+//reg 0x808[9:0]: FFT data x
+//reg 0x808[22]:  0  -->  1  to get 1 FFT data y
+//reg 0x8B4[15:0]: FFT data y report
+static u32 rtw_GetPSDData(PADAPTER pAdapter, u32 point)
+{
+	int psd_val;
+
+
+	psd_val = rtw_read32(pAdapter, 0x808);
+	psd_val &= 0xFFBFFC00;
+	psd_val |= point;
+
+	rtw_write32(pAdapter, 0x808, psd_val);
+	rtw_mdelay_os(1);
+	psd_val |= 0x00400000;
+
+	rtw_write32(pAdapter, 0x808, psd_val);
+	rtw_mdelay_os(1);
+	psd_val = rtw_read32(pAdapter, 0x8B4);
+
+	psd_val &= 0x0000FFFF;
+
+	return psd_val;
+}
+
+/*
+ * pts	start_point_min		stop_point_max
+ * 128	64			64 + 128 = 192
+ * 256	128			128 + 256 = 384
+ * 512	256			256 + 512 = 768
+ * 1024	512			512 + 1024 = 1536
+ *
+ */
+u32 mp_query_psd(PADAPTER pAdapter, u8 *data)
+{
+	u32 i, psd_pts=0, psd_start=0, psd_stop=0;
+	u32 psd_data=0;
+
+
+#ifdef PLATFORM_LINUX
+	if (!netif_running(pAdapter->pnetdev)) {
+		RT_TRACE(_module_mp_, _drv_warning_, ("mp_query_psd: Fail! interface not opened!\n"));
+		return 0;
+	}
+#endif
+
+	if (check_fwstate(&pAdapter->mlmepriv, WIFI_MP_STATE) == _FALSE) {
+		RT_TRACE(_module_mp_, _drv_warning_, ("mp_query_psd: Fail! not in MP mode!\n"));
+		return 0;
+	}
+
+	if (strlen(data) == 0) { //default value
+		psd_pts = 128;
+		psd_start = 64;
+		psd_stop = 128;   
+	} else {
+		sscanf(data, "pts=%d,start=%d,stop=%d", &psd_pts, &psd_start, &psd_stop);
+	}
+
+	_rtw_memset(data, '\0', sizeof(data));
+
+	i = psd_start;
+	while (i < psd_stop)
+	{
+		if (i >= psd_pts) {
+			psd_data = rtw_GetPSDData(pAdapter, i-psd_pts);
+		} else {
+			psd_data = rtw_GetPSDData(pAdapter, i);
+		}
+		sprintf(data, "%s%x ", data, psd_data);
+		i++;
+	}
+
+	#ifdef CONFIG_LONG_DELAY_ISSUE
+	rtw_msleep_os(100);
+	#else
+	rtw_mdelay_os(100);
+	#endif
+
+	return strlen(data)+1;
+}
+
+
+
+void _rtw_mp_xmit_priv (struct xmit_priv *pxmitpriv)
+{
+	   int i,res;
+	  _adapter *padapter = pxmitpriv->adapter;
+	struct xmit_frame	*pxmitframe = (struct xmit_frame*) pxmitpriv->pxmit_frame_buf;
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+	
+	u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+	u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
+	if(padapter->registrypriv.mp_mode ==0)
+	{
+		max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+		num_xmit_extbuf = NR_XMIT_EXTBUFF;
+	}
+	else
+	{
+		max_xmit_extbuf_size = 6000;
+		num_xmit_extbuf = 8;
+	}
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+	for(i=0; i<num_xmit_extbuf; i++)
+	{
+		rtw_os_xmit_resource_free(padapter, pxmitbuf,(max_xmit_extbuf_size + XMITBUF_ALIGN_SZ));
+		
+		pxmitbuf++;
+	}
+
+	if(pxmitpriv->pallocated_xmit_extbuf) {
+		rtw_vmfree(pxmitpriv->pallocated_xmit_extbuf, num_xmit_extbuf * sizeof(struct xmit_buf) + 4);
+	}
+
+	if(padapter->registrypriv.mp_mode ==0)
+	{
+		max_xmit_extbuf_size = 6000;
+		num_xmit_extbuf = 8;
+	}
+	else
+	{
+		max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+		num_xmit_extbuf = NR_XMIT_EXTBUFF;
+	}
+	
+	// Init xmit extension buff
+	_rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue);
+
+	pxmitpriv->pallocated_xmit_extbuf = rtw_zvmalloc(num_xmit_extbuf * sizeof(struct xmit_buf) + 4);
+	
+	if (pxmitpriv->pallocated_xmit_extbuf  == NULL){
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("alloc xmit_extbuf fail!\n"));
+		res= _FAIL;
+		goto exit;
+	}
+
+	pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmit_extbuf), 4);
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+
+	for (i = 0; i < num_xmit_extbuf; i++)
+	{
+		_rtw_init_listhead(&pxmitbuf->list);
+
+		pxmitbuf->priv_data = NULL;
+		pxmitbuf->padapter = padapter;
+		pxmitbuf->ext_tag = _TRUE;
+
+/*
+		pxmitbuf->pallocated_buf = rtw_zmalloc(max_xmit_extbuf_size);
+		if (pxmitbuf->pallocated_buf == NULL)
+		{
+			res = _FAIL;
+			goto exit;
+		}
+
+		pxmitbuf->pbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitbuf->pallocated_buf), 4);
+*/		
+
+		if((res=rtw_os_xmit_resource_alloc(padapter, pxmitbuf,max_xmit_extbuf_size + XMITBUF_ALIGN_SZ)) == _FAIL) {
+			res= _FAIL;
+			goto exit;
+		}
+		
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+		pxmitbuf->phead = pxmitbuf->pbuf;
+		pxmitbuf->pend = pxmitbuf->pbuf + max_xmit_extbuf_size;
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+#endif
+
+		rtw_list_insert_tail(&pxmitbuf->list, &(pxmitpriv->free_xmit_extbuf_queue.queue));
+		#ifdef DBG_XMIT_BUF_EXT
+		pxmitbuf->no=i;
+		#endif
+		pxmitbuf++;
+		
+	}
+
+	pxmitpriv->free_xmit_extbuf_cnt = num_xmit_extbuf;
+
+exit:
+	;
+}
+
+
+void Hal_ProSetCrystalCap (PADAPTER pAdapter , u32 CrystalCapVal)
+{
+	HAL_DATA_TYPE		*pHalData	= GET_HAL_DATA(pAdapter);
+
+		CrystalCapVal = CrystalCapVal & 0x3F;
+
+	if(IS_HARDWARE_TYPE_8192D(pAdapter))
+	{
+		PHY_SetBBReg(pAdapter, REG_AFE_XTAL_CTRL, 0xF0, CrystalCapVal & 0x0F);
+		PHY_SetBBReg(pAdapter, REG_AFE_PLL_CTRL, 0xF0000000, (CrystalCapVal & 0xF0) >> 4);
+	}
+	else if(IS_HARDWARE_TYPE_8188E(pAdapter))
+	{
+		// write 0x24[16:11] = 0x24[22:17] = CrystalCap
+		PHY_SetBBReg(pAdapter, REG_AFE_XTAL_CTRL, 0x7FF800, (CrystalCapVal | (CrystalCapVal << 6)));
+	}
+	else
+	{
+		DBG_871X(" not as 88E and 92D Hal_ProSetCrystalCap 0x2c !!!!!\n");
+		PHY_SetBBReg(pAdapter, 0x2c, 0xFFF000, (CrystalCapVal | (CrystalCapVal << 6)));	
+	}
+}
+
+
+#endif
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_mp_ioctl.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_mp_ioctl.c
new file mode 100755
index 00000000..4771bc6a
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_mp_ioctl.c
@@ -0,0 +1,2954 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_MP_IOCTL_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <mlme_osdep.h>
+
+//#include <rtw_mp.h>
+#include <rtw_mp_ioctl.h>
+
+
+//****************  oid_rtl_seg_81_85   section start ****************
+NDIS_STATUS oid_rt_wireless_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS status = NDIS_STATUS_SUCCESS;
+	PADAPTER Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	if (poid_par_priv->information_buf_len < sizeof(u8))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	if (poid_par_priv->type_of_oid == SET_OID) {
+		Adapter->registrypriv.wireless_mode = *(u8*)poid_par_priv->information_buf;
+	} else if (poid_par_priv->type_of_oid == QUERY_OID) {
+		*(u8*)poid_par_priv->information_buf = Adapter->registrypriv.wireless_mode;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+		 RT_TRACE(_module_mp_, _drv_info_, ("-query Wireless Mode=%d\n", Adapter->registrypriv.wireless_mode));
+	} else {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+	}
+
+_func_exit_;
+
+	return status;
+}
+//****************  oid_rtl_seg_81_87_80   section start ****************
+NDIS_STATUS oid_rt_pro_write_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL oldirql;
+#endif
+	struct bb_reg_param *pbbreg;
+	u16 offset;
+	u32 value;
+	NDIS_STATUS status = NDIS_STATUS_SUCCESS;
+	PADAPTER Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_write_bb_reg_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(struct bb_reg_param))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	pbbreg = (struct bb_reg_param *)(poid_par_priv->information_buf);
+
+	offset = (u16)(pbbreg->offset) & 0xFFF; //0ffset :0x800~0xfff
+	if (offset < BB_REG_BASE_ADDR) offset |= BB_REG_BASE_ADDR;
+
+	value = pbbreg->value;
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("oid_rt_pro_write_bb_reg_hdl: offset=0x%03X value=0x%08X\n",
+		  offset, value));
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	write_bbreg(Adapter, offset, 0xFFFFFFFF, value);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_read_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL oldirql;
+#endif
+	struct bb_reg_param *pbbreg;
+	u16 offset;
+	u32 value;
+	NDIS_STATUS status = NDIS_STATUS_SUCCESS;
+	PADAPTER Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_read_bb_reg_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(struct bb_reg_param))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	pbbreg = (struct bb_reg_param *)(poid_par_priv->information_buf);
+
+	offset = (u16)(pbbreg->offset) & 0xFFF; //0ffset :0x800~0xfff
+	if (offset < BB_REG_BASE_ADDR) offset |= BB_REG_BASE_ADDR;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	value = read_bbreg(Adapter, offset, 0xFFFFFFFF);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	pbbreg->value = value;
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("-oid_rt_pro_read_bb_reg_hdl: offset=0x%03X value:0x%08X\n",
+		  offset, value));
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_write_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL oldirql;
+#endif
+	struct rf_reg_param *pbbreg;
+	u8 path;
+	u8 offset;
+	u32 value;
+	NDIS_STATUS status = NDIS_STATUS_SUCCESS;
+	PADAPTER Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_write_rf_reg_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(struct rf_reg_param))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	pbbreg = (struct rf_reg_param *)(poid_par_priv->information_buf);
+
+	if (pbbreg->path >= MAX_RF_PATH_NUMS)
+		return NDIS_STATUS_NOT_ACCEPTED;
+	if (pbbreg->offset > 0xFF)
+		return NDIS_STATUS_NOT_ACCEPTED;
+	if (pbbreg->value > 0xFFFFF)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	path = (u8)pbbreg->path;
+	offset = (u8)pbbreg->offset;
+	value = pbbreg->value;
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("oid_rt_pro_write_rf_reg_hdl: path=%d offset=0x%02X value=0x%05X\n",
+		  path, offset, value));
+
+	_irqlevel_changed_(&oldirql, LOWER);
+ 	write_rfreg(Adapter, path, offset, value);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_read_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL oldirql;
+#endif
+	struct rf_reg_param *pbbreg;
+	u8 path;
+	u8 offset;
+	u32 value;
+	PADAPTER Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	NDIS_STATUS status = NDIS_STATUS_SUCCESS;
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_read_rf_reg_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(struct rf_reg_param))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	pbbreg = (struct rf_reg_param *)(poid_par_priv->information_buf);
+
+	if (pbbreg->path >= MAX_RF_PATH_NUMS)
+		return NDIS_STATUS_NOT_ACCEPTED;
+	if (pbbreg->offset > 0xFF)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	path = (u8)pbbreg->path;
+	offset = (u8)pbbreg->offset;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	value = read_rfreg(Adapter, path, offset);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	pbbreg->value = value;
+
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("-oid_rt_pro_read_rf_reg_hdl: path=%d offset=0x%02X value=0x%05X\n",
+		  path, offset, value));
+
+_func_exit_;
+
+	return status;
+}
+//****************  oid_rtl_seg_81_87_00   section end****************
+//------------------------------------------------------------------------------
+
+//****************  oid_rtl_seg_81_80_00   section start ****************
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_set_data_rate_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u32		ratevalue;//4
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("+oid_rt_pro_set_data_rate_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len != sizeof(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	ratevalue = *((u32*)poid_par_priv->information_buf);//4
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("oid_rt_pro_set_data_rate_hdl: data rate idx=%d\n", ratevalue));
+	if (ratevalue >= MPT_RATE_LAST)
+		return NDIS_STATUS_INVALID_DATA;
+
+	Adapter->mppriv.rateidx = ratevalue;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	SetDataRate(Adapter);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_start_test_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u32		mode;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_start_test_hdl\n"));
+
+	if (Adapter->registrypriv.mp_mode == 0)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	//IQCalibrateBcut(Adapter);
+
+	mode = *((u32*)poid_par_priv->information_buf);
+	Adapter->mppriv.mode = mode;// 1 for loopback
+
+	if (mp_start_test(Adapter) == _FAIL) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		goto exit;
+	}
+
+exit:
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("-oid_rt_pro_start_test_hdl: mp_mode=%d\n", Adapter->mppriv.mode));
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_stop_test_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+Set OID_RT_PRO_STOP_TEST\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	mp_stop_test(Adapter);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("-Set OID_RT_PRO_STOP_TEST\n"));
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_set_channel_direct_call_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u32		Channel;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_set_channel_direct_call_hdl\n"));
+
+	if (poid_par_priv->information_buf_len != sizeof(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	if (poid_par_priv->type_of_oid == QUERY_OID) {
+		*((u32*)poid_par_priv->information_buf) = Adapter->mppriv.channel;
+		return NDIS_STATUS_SUCCESS;
+	}
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	Channel = *((u32*)poid_par_priv->information_buf);
+	RT_TRACE(_module_mp_, _drv_notice_, ("oid_rt_pro_set_channel_direct_call_hdl: Channel=%d\n", Channel));
+	if (Channel > 14)
+		return NDIS_STATUS_NOT_ACCEPTED;
+	Adapter->mppriv.channel = Channel;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	SetChannel(Adapter);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_set_bandwidth_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u16		bandwidth;
+	u16		channel_offset;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_info_,
+		 ("+oid_rt_set_bandwidth_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	bandwidth = *((u32*)poid_par_priv->information_buf);//4
+	channel_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	if (bandwidth != HT_CHANNEL_WIDTH_40)
+		bandwidth = HT_CHANNEL_WIDTH_20;
+	padapter->mppriv.bandwidth = (u8)bandwidth;
+	padapter->mppriv.prime_channel_offset = (u8)channel_offset;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	SetBandwidth(padapter);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("-oid_rt_set_bandwidth_hdl: bandwidth=%d channel_offset=%d\n",
+		  bandwidth, channel_offset));
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_set_antenna_bb_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u32		antenna;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_set_antenna_bb_hdl\n"));
+
+	if (poid_par_priv->information_buf_len != sizeof(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	if (poid_par_priv->type_of_oid == SET_OID)
+	{
+		antenna = *(u32*)poid_par_priv->information_buf;
+
+		Adapter->mppriv.antenna_tx = (u16)((antenna & 0xFFFF0000) >> 16);
+		Adapter->mppriv.antenna_rx = (u16)(antenna & 0x0000FFFF);
+		RT_TRACE(_module_mp_, _drv_notice_,
+			 ("oid_rt_pro_set_antenna_bb_hdl: tx_ant=0x%04x rx_ant=0x%04x\n",
+			  Adapter->mppriv.antenna_tx, Adapter->mppriv.antenna_rx));
+
+		_irqlevel_changed_(&oldirql, LOWER);
+		SetAntenna(Adapter);
+		_irqlevel_changed_(&oldirql, RAISE);
+	} else {
+		antenna = (Adapter->mppriv.antenna_tx << 16)|Adapter->mppriv.antenna_rx;
+		*(u32*)poid_par_priv->information_buf = antenna;
+	}
+
+_func_exit_;
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_pro_set_tx_power_control_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u32		tx_pwr_idx;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_info_, ("+oid_rt_pro_set_tx_power_control_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len != sizeof(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	tx_pwr_idx = *((u32*)poid_par_priv->information_buf);
+	if (tx_pwr_idx > MAX_TX_PWR_INDEX_N_MODE)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	Adapter->mppriv.txpoweridx = (u8)tx_pwr_idx;
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("oid_rt_pro_set_tx_power_control_hdl: idx=0x%2x\n",
+		  Adapter->mppriv.txpoweridx));
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	SetTxPower(Adapter);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+
+//------------------------------------------------------------------------------
+//****************  oid_rtl_seg_81_80_20   section start ****************
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_query_tx_packet_sent_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid !=QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	if (poid_par_priv->information_buf_len == sizeof(ULONG)) {
+		*(ULONG*)poid_par_priv->information_buf =  Adapter->mppriv.tx_pktcount;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_query_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	RT_TRACE(_module_mp_, _drv_alert_, ("===> oid_rt_pro_query_rx_packet_received_hdl.\n"));
+	if (poid_par_priv->information_buf_len == sizeof(ULONG)) {
+		*(ULONG*)poid_par_priv->information_buf =  Adapter->mppriv.rx_pktcount;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+		RT_TRACE(_module_mp_, _drv_alert_, ("recv_ok:%d \n",Adapter->mppriv.rx_pktcount));
+	} else {
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_query_rx_packet_crc32_error_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	RT_TRACE(_module_mp_, _drv_alert_, ("===> oid_rt_pro_query_rx_packet_crc32_error_hdl.\n"));
+	if (poid_par_priv->information_buf_len == sizeof(ULONG)) {
+		*(ULONG*)poid_par_priv->information_buf =  Adapter->mppriv.rx_crcerrpktcount;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+		RT_TRACE(_module_mp_, _drv_alert_, ("recv_err:%d \n",Adapter->mppriv.rx_crcerrpktcount));
+	} else {
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+
+NDIS_STATUS oid_rt_pro_reset_tx_packet_sent_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	RT_TRACE(_module_mp_, _drv_alert_, ("===> oid_rt_pro_reset_tx_packet_sent_hdl.\n"));
+	Adapter->mppriv.tx_pktcount = 0;
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_reset_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+	{
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	if (poid_par_priv->information_buf_len == sizeof(ULONG)) {
+		Adapter->mppriv.rx_pktcount = 0;
+		Adapter->mppriv.rx_crcerrpktcount = 0;
+	} else {
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_reset_phy_rx_packet_count_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	ResetPhyRxPktCount(Adapter);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_phy_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_info_, ("+oid_rt_get_phy_rx_packet_received_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len != sizeof(ULONG))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	*(ULONG*)poid_par_priv->information_buf = GetPhyRxPktReceived(Adapter);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("-oid_rt_get_phy_rx_packet_received_hdl: recv_ok=%d\n", *(ULONG*)poid_par_priv->information_buf));
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_phy_rx_packet_crc32_error_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_info_, ("+oid_rt_get_phy_rx_packet_crc32_error_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+
+	if (poid_par_priv->information_buf_len != sizeof(ULONG))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	*(ULONG*)poid_par_priv->information_buf = GetPhyRxPktCRC32Error(Adapter);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+	RT_TRACE(_module_mp_, _drv_info_, ("-oid_rt_get_phy_rx_packet_crc32_error_hdl: recv_err=%d\n", *(ULONG*)poid_par_priv->information_buf));
+
+_func_exit_;
+
+	return status;
+}
+//****************  oid_rtl_seg_81_80_20   section end ****************
+NDIS_STATUS oid_rt_pro_set_continuous_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u32		bStartTest;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_set_continuous_tx_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	bStartTest = *((u32*)poid_par_priv->information_buf);
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	SetContinuousTx(Adapter,(u8)bStartTest);
+	if (bStartTest) {
+		struct mp_priv *pmp_priv = &Adapter->mppriv;
+		if (pmp_priv->tx.stop == 0) {
+			pmp_priv->tx.stop = 1;
+			DBG_871X("%s: pkt tx is running...\n", __func__);
+			rtw_msleep_os(5);
+		}
+		pmp_priv->tx.stop = 0;
+		pmp_priv->tx.count = 1;
+		SetPacketTx(Adapter);
+	}
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_pro_set_single_carrier_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u32		bStartTest;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_alert_, ("+oid_rt_pro_set_single_carrier_tx_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	bStartTest = *((u32*)poid_par_priv->information_buf);
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	SetSingleCarrierTx(Adapter, (u8)bStartTest);
+	if (bStartTest) {
+		struct mp_priv *pmp_priv = &Adapter->mppriv;
+		if (pmp_priv->tx.stop == 0) {
+			pmp_priv->tx.stop = 1;
+			DBG_871X("%s: pkt tx is running...\n", __func__);
+			rtw_msleep_os(5);
+		}
+		pmp_priv->tx.stop = 0;
+		pmp_priv->tx.count = 1;
+		SetPacketTx(Adapter);
+	}
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_pro_set_carrier_suppression_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u32		bStartTest;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_set_carrier_suppression_tx_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	bStartTest = *((u32*)poid_par_priv->information_buf);
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	SetCarrierSuppressionTx(Adapter, (u8)bStartTest);
+	if (bStartTest) {
+		struct mp_priv *pmp_priv = &Adapter->mppriv;
+		if (pmp_priv->tx.stop == 0) {
+			pmp_priv->tx.stop = 1;
+			DBG_871X("%s: pkt tx is running...\n", __func__);
+			rtw_msleep_os(5);
+		}
+		pmp_priv->tx.stop = 0;
+		pmp_priv->tx.count = 1;
+		SetPacketTx(Adapter);
+	}
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_pro_set_single_tone_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u32 		bStartTest;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_alert_, ("+oid_rt_pro_set_single_tone_tx_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	bStartTest = *((u32*)poid_par_priv->information_buf);
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	SetSingleToneTx(Adapter,(u8)bStartTest);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_pro_set_modulation_hdl(struct oid_par_priv* poid_par_priv)
+{
+	return 0;
+}
+
+NDIS_STATUS oid_rt_pro_trigger_gpio_hdl(struct oid_par_priv *poid_par_priv)
+{
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	rtw_hal_set_hwreg(Adapter, HW_VAR_TRIGGER_GPIO_0, 0);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+//****************  oid_rtl_seg_81_80_00   section end ****************
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro8711_join_bss_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+	PNDIS_802_11_SSID pssid;
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	*poid_par_priv->bytes_needed = (u32)sizeof(NDIS_802_11_SSID);
+	*poid_par_priv->bytes_rw = 0;
+	if (poid_par_priv->information_buf_len < *poid_par_priv->bytes_needed)
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	pssid = (PNDIS_802_11_SSID)poid_par_priv->information_buf;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	if (mp_start_joinbss(Adapter, pssid) == _FAIL)
+		status = NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	*poid_par_priv->bytes_rw = sizeof(NDIS_802_11_SSID);
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_read_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	pRW_Reg 	RegRWStruct;
+	u32		offset, width;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_info_,
+		 ("+oid_rt_pro_read_register_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	RegRWStruct = (pRW_Reg)poid_par_priv->information_buf;
+	offset = RegRWStruct->offset;
+	width = RegRWStruct->width;
+
+	if (offset > 0xFFF)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	switch (width) {
+		case 1:
+			RegRWStruct->value = rtw_read8(Adapter, offset);
+			break;
+		case 2:
+			RegRWStruct->value = rtw_read16(Adapter, offset);
+			break;
+		default:
+			width = 4;
+			RegRWStruct->value = rtw_read32(Adapter, offset);
+			break;
+	}
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("oid_rt_pro_read_register_hdl: offset:0x%04X value:0x%X\n",
+		  offset, RegRWStruct->value));
+
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	*poid_par_priv->bytes_rw = width;
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_write_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	pRW_Reg 	RegRWStruct;
+	u32		offset, width, value;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_info_,
+		 ("+oid_rt_pro_write_register_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	RegRWStruct = (pRW_Reg)poid_par_priv->information_buf;
+	offset = RegRWStruct->offset;
+	width = RegRWStruct->width;
+	value = RegRWStruct->value;
+
+	if (offset > 0xFFF)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	switch (RegRWStruct->width)
+	{
+		case 1:
+			if (value > 0xFF) {
+				status = NDIS_STATUS_NOT_ACCEPTED;
+				break;
+			}
+			rtw_write8(padapter, offset, (u8)value);
+			break;
+		case 2:
+			if (value > 0xFFFF) {
+				status = NDIS_STATUS_NOT_ACCEPTED;
+				break;
+			}
+			rtw_write16(padapter, offset, (u16)value);
+			break;
+		case 4:
+			rtw_write32(padapter, offset, value);
+			break;
+		default:
+			status = NDIS_STATUS_NOT_ACCEPTED;
+			break;
+	}
+
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	RT_TRACE(_module_mp_, _drv_info_,
+		 ("-oid_rt_pro_write_register_hdl: offset=0x%08X width=%d value=0x%X\n",
+		  offset, width, value));
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_burst_read_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	pBurst_RW_Reg	pBstRwReg;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_burst_read_register_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	pBstRwReg = (pBurst_RW_Reg)poid_par_priv->information_buf;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	rtw_read_mem(padapter, pBstRwReg->offset, (u32)pBstRwReg->len, pBstRwReg->Data);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+	RT_TRACE(_module_mp_, _drv_info_, ("-oid_rt_pro_burst_read_register_hdl\n"));
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_burst_write_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	pBurst_RW_Reg	pBstRwReg;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_burst_write_register_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	pBstRwReg = (pBurst_RW_Reg)poid_par_priv->information_buf;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	rtw_write_mem(padapter, pBstRwReg->offset, (u32)pBstRwReg->len, pBstRwReg->Data);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	RT_TRACE(_module_mp_, _drv_info_, ("-oid_rt_pro_burst_write_register_hdl\n"));
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_write_txcmd_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+	PADAPTER	Adapter = (PADAPTER)( poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+
+	TX_CMD_Desc	*TxCmd_Info;
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	RT_TRACE(_module_mp_, _drv_info_, ("+Set OID_RT_PRO_WRITE_TXCMD\n"));
+
+	TxCmd_Info=(TX_CMD_Desc*)poid_par_priv->information_buf;
+
+	RT_TRACE(_module_mp_, _drv_info_, ("WRITE_TXCMD:Addr=%.8X\n", TxCmd_Info->offset));
+  	RT_TRACE(_module_mp_, _drv_info_, ("WRITE_TXCMD:1.)%.8X\n", (ULONG)TxCmd_Info->TxCMD.value[0]));
+	RT_TRACE(_module_mp_, _drv_info_, ("WRITE_TXCMD:2.)%.8X\n", (ULONG)TxCmd_Info->TxCMD.value[1]));
+	RT_TRACE(_module_mp_, _drv_info_, (("WRITE_TXCMD:3.)%.8X\n", (ULONG)TxCmd_Info->TxCMD.value[2]));
+	RT_TRACE(_module_mp_, _drv_info_, ("WRITE_TXCMD:4.)%.8X\n", (ULONG)TxCmd_Info->TxCMD.value[3]));
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	rtw_write32(Adapter, TxCmd_Info->offset + 0, (unsigned int)TxCmd_Info->TxCMD.value[0]);
+	rtw_write32(Adapter, TxCmd_Info->offset + 4, (unsigned int)TxCmd_Info->TxCMD.value[1]);
+
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("-Set OID_RT_PRO_WRITE_TXCMD: status=0x%08X\n", status));
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_read16_eeprom_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	pEEPROM_RWParam pEEPROM;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_info_, ("+Query OID_RT_PRO_READ16_EEPROM\n"));
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	pEEPROM = (pEEPROM_RWParam)poid_par_priv->information_buf;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	pEEPROM->value = eeprom_read16(padapter, (u16)(pEEPROM->offset >> 1));
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+	RT_TRACE(_module_mp_,  _drv_notice_,
+		 ("-Query OID_RT_PRO_READ16_EEPROM: offset=0x%x value=0x%x\n",
+		  pEEPROM->offset, pEEPROM->value));
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_write16_eeprom_hdl (struct oid_par_priv *poid_par_priv)
+{
+#if 0
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	pEEPROM_RWParam pEEPROM;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+Set OID_RT_PRO_WRITE16_EEPROM\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	pEEPROM = (pEEPROM_RWParam)poid_par_priv->information_buf;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	eeprom_write16(padapter, (u16)(pEEPROM->offset >> 1), pEEPROM->value);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro8711_wi_poll_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)( poid_par_priv->adapter_context);
+
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+	struct mp_wiparam *pwi_param;
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(struct mp_wiparam))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	if (Adapter->mppriv.workparam.bcompleted == _FALSE)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	pwi_param = (struct mp_wiparam *)poid_par_priv->information_buf;
+
+	_rtw_memcpy(pwi_param, &Adapter->mppriv.workparam, sizeof(struct mp_wiparam));
+	Adapter->mppriv.act_in_progress = _FALSE;
+//	RT_TRACE(_module_mp_, _drv_info_, ("rf:%x\n", pwiparam->IoValue));
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro8711_pkt_loss_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)( poid_par_priv->adapter_context);
+
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro8711_pkt_loss_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(uint)*2) {
+		RT_TRACE(_module_mp_, _drv_err_, ("-oid_rt_pro8711_pkt_loss_hdl: buf_len=%d\n", (int)poid_par_priv->information_buf_len));
+		return NDIS_STATUS_INVALID_LENGTH;
+	}
+
+	if (*(uint*)poid_par_priv->information_buf == 1)//init==1
+		Adapter->mppriv.rx_pktloss = 0;
+
+	*((uint*)poid_par_priv->information_buf+1) = Adapter->mppriv.rx_pktloss;
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_rd_attrib_mem_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)( poid_par_priv->adapter_context);
+	struct io_queue *pio_queue = (struct io_queue *)Adapter->pio_queue;
+	struct intf_hdl	*pintfhdl = &pio_queue->intf;
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+#ifdef CONFIG_SDIO_HCI
+	void (*_attrib_read)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+#endif
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+Query OID_RT_RD_ATTRIB_MEM\n"));
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+#ifdef CONFIG_SDIO_HCI
+	_irqlevel_changed_(&oldirql, LOWER);
+{
+	u32 *plmem = (u32*)poid_par_priv->information_buf+2;
+	_attrib_read = pintfhdl->io_ops._attrib_read;
+	_attrib_read(pintfhdl, *((u32*)poid_par_priv->information_buf),
+				*((u32*)poid_par_priv->information_buf+1), (u8*)plmem);
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+}
+	_irqlevel_changed_(&oldirql, RAISE);
+#endif
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_wr_attrib_mem_hdl (struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	struct io_queue *pio_queue = (struct io_queue *)Adapter->pio_queue;
+	struct intf_hdl	*pintfhdl = &pio_queue->intf;
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+#ifdef CONFIG_SDIO_HCI
+	void (*_attrib_write)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+#endif
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+#ifdef CONFIG_SDIO_HCI
+	_irqlevel_changed_(&oldirql, LOWER);
+{
+	u32 *plmem = (u32*)poid_par_priv->information_buf + 2;
+	_attrib_write = pintfhdl->io_ops._attrib_write;
+	_attrib_write(pintfhdl, *(u32*)poid_par_priv->information_buf,
+				*((u32*)poid_par_priv->information_buf+1), (u8*)plmem);
+}
+	_irqlevel_changed_(&oldirql, RAISE);
+#endif
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS  oid_rt_pro_set_rf_intfs_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+OID_RT_PRO_SET_RF_INTFS\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	if (rtw_setrfintfs_cmd(Adapter, *(unsigned char*)poid_par_priv->information_buf) == _FAIL)
+		status = NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_poll_rx_status_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	_rtw_memcpy(poid_par_priv->information_buf, (unsigned char*)&Adapter->mppriv.rxstat, sizeof(struct recv_stat));
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_cfg_debug_message_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+	PCFG_DBG_MSG_STRUCT pdbg_msg;
+
+_func_enter_;
+
+//	RT_TRACE(0xffffffffff,_drv_alert_,("===> oid_rt_pro_cfg_debug_message_hdl.\n"));
+
+#if 0//#ifdef CONFIG_DEBUG_RTL871X
+
+	pdbg_msg = (PCFG_DBG_MSG_STRUCT)(poid_par_priv->information_buf);
+
+	if (poid_par_priv->type_of_oid == SET_OID) {
+		RT_TRACE(0xffffffffff, _drv_alert_,
+			 ("===>Set level :0x%08x, H32:0x%08x L32:0x%08x\n",
+			  pdbg_msg->DebugLevel, pdbg_msg->DebugComponent_H32, pdbg_msg->DebugComponent_L32));
+
+		GlobalDebugLevel = pdbg_msg->DebugLevel;
+		GlobalDebugComponents = (pdbg_msg->DebugComponent_H32 << 32) | pdbg_msg->DebugComponent_L32;
+		RT_TRACE(0xffffffffff, _drv_alert_,
+			 ("===> Set level :0x%08x, component:0x%016x\n",
+			  GlobalDebugLevel, (u32)GlobalDebugComponents));
+	} else {
+		pdbg_msg->DebugLevel = GlobalDebugLevel;
+		pdbg_msg->DebugComponent_H32 = (u32)(GlobalDebugComponents >> 32);
+		pdbg_msg->DebugComponent_L32 = (u32)GlobalDebugComponents;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+		RT_TRACE(0xffffffffff, _drv_alert_,
+			 ("===>Query level:0x%08x H32:0x%08x L32:0x%08x\n",
+			  (u32)pdbg_msg->DebugLevel, (u32)pdbg_msg->DebugComponent_H32, (u32)pdbg_msg->DebugComponent_L32));
+	}
+
+#endif
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_set_data_rate_ex_hdl(struct oid_par_priv *poid_par_priv)
+{
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+OID_RT_PRO_SET_DATA_RATE_EX\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	if (rtw_setdatarate_cmd(Adapter, poid_par_priv->information_buf) !=_SUCCESS)
+		status = NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+//-----------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_thermal_meter_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	u8 thermal = 0;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_get_thermal_meter_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	GetThermalMeter(Adapter, &thermal);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	*(u32*)poid_par_priv->information_buf = (u32)thermal;
+	*poid_par_priv->bytes_rw = sizeof(u32);
+
+_func_exit_;
+
+	return status;
+}
+//-----------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_read_tssi_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_read_tssi_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (Adapter->mppriv.act_in_progress == _TRUE)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(u8))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	//init workparam
+	Adapter->mppriv.act_in_progress = _TRUE;
+	Adapter->mppriv.workparam.bcompleted = _FALSE;
+	Adapter->mppriv.workparam.act_type = MPT_READ_TSSI;
+	Adapter->mppriv.workparam.io_offset = 0;
+	Adapter->mppriv.workparam.io_value = 0xFFFFFFFF;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	if (!rtw_gettssi_cmd(Adapter,0, (u8*)&Adapter->mppriv.workparam.io_value))
+		status = NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_set_power_tracking_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+
+_func_enter_;
+
+//	if (poid_par_priv->type_of_oid != SET_OID)
+//		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(u8))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	if (poid_par_priv->type_of_oid == SET_OID) {
+		u8 enable;
+
+		enable = *(u8*)poid_par_priv->information_buf;
+		RT_TRACE(_module_mp_, _drv_notice_,
+			 ("+oid_rt_pro_set_power_tracking_hdl: enable=%d\n", enable));
+
+		SetPowerTracking(Adapter, enable);
+	} else {
+		GetPowerTracking(Adapter, (u8*)poid_par_priv->information_buf);
+	}
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+//-----------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_set_basic_rate_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u32 ratevalue;
+	u8 datarates[NumRates];
+	int i;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_info_, ("+OID_RT_PRO_SET_BASIC_RATE\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+#if 0
+	ratevalue = *((u32*)poid_par_priv->information_buf);
+
+	for (i = 0; i < NumRates; i++) {
+		if (ratevalue == mpdatarate[i])
+			datarates[i] = mpdatarate[i];
+		else
+			datarates[i] = 0xff;
+		RT_TRACE(_module_rtl871x_ioctl_c_, _drv_info_, ("basicrate_inx=%d\n", datarates[i]));
+	}
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	if (rtw_setbasicrate_cmd(padapter, datarates) != _SUCCESS)
+		status = NDIS_STATUS_NOT_ACCEPTED;
+
+	_irqlevel_changed_(&oldirql, RAISE);
+#endif
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("-OID_RT_PRO_SET_BASIC_RATE: status=0x%08X\n", status));
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_qry_pwrstate_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < 8)
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	*poid_par_priv->bytes_rw = 8;
+	_rtw_memcpy(poid_par_priv->information_buf, &(Adapter->pwrctrlpriv.pwr_mode), 8);
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("-oid_rt_pro_qry_pwrstate_hdl: pwr_mode=%d smart_ps=%d\n",
+		  Adapter->pwrctrlpriv.pwr_mode, Adapter->pwrctrlpriv.smart_ps));
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_set_pwrstate_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+	uint pwr_mode, smart_ps;
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+Set OID_RT_PRO_SET_PWRSTATE\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	*poid_par_priv->bytes_rw = 0;
+	*poid_par_priv->bytes_needed = 8;
+
+	if (poid_par_priv->information_buf_len < 8)
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	pwr_mode = *(uint *)(poid_par_priv->information_buf);
+	smart_ps = *(uint *)((int)poid_par_priv->information_buf + 4);
+
+	*poid_par_priv->bytes_rw = 8;
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_h2c_set_rate_table_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+	struct setratable_parm *prate_table;
+	u8		res;
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	*poid_par_priv->bytes_needed  = sizeof(struct setratable_parm);
+	if (poid_par_priv->information_buf_len < sizeof(struct setratable_parm))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	prate_table = (struct setratable_parm*)poid_par_priv->information_buf;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	res = rtw_setrttbl_cmd(Adapter, prate_table);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	if (res == _FAIL)
+		status = NDIS_STATUS_FAILURE;
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_h2c_get_rate_table_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	#if 0
+			struct mp_wi_cntx *pmp_wi_cntx=&(Adapter->mppriv.wi_cntx);
+			u8 res=_SUCCESS;
+			DEBUG_INFO(("===> Set OID_RT_PRO_H2C_GET_RATE_TABLE.\n"));
+
+			if(pmp_wi_cntx->bmp_wi_progress ==_TRUE){
+				DEBUG_ERR(("\n mp workitem is progressing, not allow to set another workitem right now!!!\n"));
+				Status = NDIS_STATUS_NOT_ACCEPTED;
+				break;
+			}
+			else{
+				pmp_wi_cntx->bmp_wi_progress=_TRUE;
+				pmp_wi_cntx->param.bcompleted=_FALSE;
+				pmp_wi_cntx->param.act_type=MPT_GET_RATE_TABLE;
+				pmp_wi_cntx->param.io_offset=0x0;
+				pmp_wi_cntx->param.bytes_cnt=sizeof(struct getratable_rsp);
+				pmp_wi_cntx->param.io_value=0xffffffff;
+
+				res=rtw_getrttbl_cmd(Adapter,(struct getratable_rsp *)pmp_wi_cntx->param.data);
+				*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+				if(res != _SUCCESS)
+				{
+					Status = NDIS_STATUS_NOT_ACCEPTED;
+				}
+			}
+			DEBUG_INFO(("\n <=== Set OID_RT_PRO_H2C_GET_RATE_TABLE.\n"));
+	#endif
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+
+//****************  oid_rtl_seg_87_12_00   section start ****************
+NDIS_STATUS oid_rt_pro_encryption_ctrl_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	struct security_priv *psecuritypriv = &Adapter->securitypriv;
+
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+	ENCRY_CTRL_STATE encry_mode;
+
+
+	*poid_par_priv->bytes_needed = sizeof(u8);
+	if (poid_par_priv->information_buf_len < *poid_par_priv->bytes_needed)
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	if (poid_par_priv->type_of_oid == SET_OID)
+	{
+		encry_mode = *((u8*)poid_par_priv->information_buf);
+		switch (encry_mode)
+		{
+			case HW_CONTROL:
+				#if 0
+				Adapter->registrypriv.software_decrypt=_FALSE;
+				Adapter->registrypriv.software_encrypt=_FALSE;
+				#else
+				psecuritypriv->sw_decrypt = _FALSE;
+				psecuritypriv->sw_encrypt = _FALSE;
+				#endif
+				break;
+			case SW_CONTROL:
+				#if 0
+				Adapter->registrypriv.software_decrypt=_TRUE;
+				Adapter->registrypriv.software_encrypt=_TRUE;
+				#else
+				psecuritypriv->sw_decrypt = _TRUE;
+				psecuritypriv->sw_encrypt = _TRUE;
+				#endif
+				break;
+			case HW_ENCRY_SW_DECRY:
+				#if 0
+				Adapter->registrypriv.software_decrypt=_TRUE;
+				Adapter->registrypriv.software_encrypt=_FALSE;
+				#else
+				psecuritypriv->sw_decrypt = _TRUE;
+				psecuritypriv->sw_encrypt = _FALSE;
+				#endif
+				break;
+			case SW_ENCRY_HW_DECRY:
+				#if 0
+				Adapter->registrypriv.software_decrypt=_FALSE;
+				Adapter->registrypriv.software_encrypt=_TRUE;
+				#else
+				psecuritypriv->sw_decrypt = _FALSE;
+				psecuritypriv->sw_encrypt = _TRUE;
+				#endif
+				break;
+		}
+
+		RT_TRACE(_module_rtl871x_ioctl_c_, _drv_notice_,
+			 ("-oid_rt_pro_encryption_ctrl_hdl: SET encry_mode=0x%x sw_encrypt=0x%x sw_decrypt=0x%x\n",
+			  encry_mode, psecuritypriv->sw_encrypt, psecuritypriv->sw_decrypt));
+	}
+	else {
+		#if 0
+		if (Adapter->registrypriv.software_encrypt == _FALSE) {
+			if (Adapter->registrypriv.software_decrypt == _FALSE)
+				encry_mode = HW_CONTROL;
+			else
+				encry_mode = HW_ENCRY_SW_DECRY;
+		}
+		else {
+			if (Adapter->registrypriv.software_decrypt == _FALSE)
+				encry_mode = SW_ENCRY_HW_DECRY;
+			else
+				encry_mode = SW_CONTROL;
+		}
+		#else
+
+		if ((psecuritypriv->sw_encrypt == _FALSE) && (psecuritypriv->sw_decrypt == _FALSE))
+			encry_mode = HW_CONTROL;
+		else if ((psecuritypriv->sw_encrypt == _FALSE) && (psecuritypriv->sw_decrypt == _TRUE))
+			encry_mode = HW_ENCRY_SW_DECRY;
+		else if ((psecuritypriv->sw_encrypt == _TRUE) && (psecuritypriv->sw_decrypt == _FALSE))
+			encry_mode = SW_ENCRY_HW_DECRY;
+		else if ((psecuritypriv->sw_encrypt == _TRUE) && (psecuritypriv->sw_decrypt == _TRUE))
+			encry_mode = SW_CONTROL;
+
+		#endif
+
+		*(u8*)poid_par_priv->information_buf =  encry_mode;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+		RT_TRACE(_module_mp_, _drv_notice_,
+			 ("-oid_rt_pro_encryption_ctrl_hdl: QUERY encry_mode=0x%x\n",
+			  encry_mode));
+	}
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_add_sta_info_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+	struct sta_info *psta = NULL;
+	UCHAR 		*macaddr;
+
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	*poid_par_priv->bytes_needed = ETH_ALEN;
+	if (poid_par_priv->information_buf_len < *poid_par_priv->bytes_needed)
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	macaddr = (UCHAR *) poid_par_priv->information_buf ;
+
+	RT_TRACE(_module_rtl871x_ioctl_c_,_drv_notice_,
+		 ("OID_RT_PRO_ADD_STA_INFO: addr="MAC_FMT"\n", MAC_ARG(macaddr) ));
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	psta = rtw_get_stainfo(&Adapter->stapriv, macaddr);
+
+	if (psta == NULL) { // the sta have been in sta_info_queue => do nothing
+		psta = rtw_alloc_stainfo(&Adapter->stapriv, macaddr);
+
+		if (psta == NULL) {
+			RT_TRACE(_module_rtl871x_ioctl_c_,_drv_err_,("Can't alloc sta_info when OID_RT_PRO_ADD_STA_INFO\n"));
+			status = NDIS_STATUS_FAILURE;
+		}
+	} else { //(between drv has received this event before and  fw have not yet to set key to CAM_ENTRY)
+		RT_TRACE(_module_rtl871x_ioctl_c_, _drv_err_,
+			("Error: OID_RT_PRO_ADD_STA_INFO: sta has been in sta_hash_queue \n"));
+	}
+
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_dele_sta_info_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+	struct sta_info *psta = NULL;
+	UCHAR		*macaddr;
+
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	*poid_par_priv->bytes_needed = ETH_ALEN;
+	if (poid_par_priv->information_buf_len < *poid_par_priv->bytes_needed)
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	macaddr = (UCHAR *) poid_par_priv->information_buf ;
+	RT_TRACE(_module_rtl871x_ioctl_c_,_drv_notice_,
+		 ("+OID_RT_PRO_ADD_STA_INFO: addr="MAC_FMT"\n", MAC_ARG(macaddr) ));
+
+	psta = rtw_get_stainfo(&Adapter->stapriv, macaddr);
+	if (psta != NULL) {
+		_enter_critical(&(Adapter->stapriv.sta_hash_lock), &irqL);
+		rtw_free_stainfo(Adapter, psta);
+		_exit_critical(&(Adapter->stapriv.sta_hash_lock), &irqL);
+	}
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+#if 0
+#include <sdio_osintf.h>
+static u32 mp_query_drv_var(_adapter *padapter, u8 offset, u32 var)
+{
+#ifdef CONFIG_SDIO_HCI
+
+	if (offset == 1) {
+		u16 tmp_blk_num;
+		tmp_blk_num = rtw_read16(padapter, SDIO_RX0_RDYBLK_NUM);
+		RT_TRACE(_module_mp_, _drv_err_, ("Query Information, mp_query_drv_var  SDIO_RX0_RDYBLK_NUM=0x%x   dvobj.rxblknum=0x%x\n", tmp_blk_num, adapter_to_dvobj(padapter)->rxblknum));
+		if (adapter_to_dvobj(padapter)->rxblknum != tmp_blk_num) {
+			RT_TRACE(_module_mp_,_drv_err_, ("Query Information, mp_query_drv_var  call recv rx\n"));
+		//	sd_recv_rxfifo(padapter);
+		}
+	}
+
+#if 0
+	if(offset <=100){  //For setting data rate and query data rate
+		if(offset==100){ //For query data rate
+			RT_TRACE(_module_mp_, _drv_emerg_, ("\n mp_query_drv_var: offset(%d): query rate=0x%.2x \n",offset,padapter->registrypriv.tx_rate));
+			var=padapter->registrypriv.tx_rate;
+
+		}
+		else if(offset<0x1d){  //For setting data rate
+			padapter->registrypriv.tx_rate=offset;
+			var=padapter->registrypriv.tx_rate;
+			padapter->registrypriv.use_rate=_TRUE;
+			RT_TRACE(_module_mp_, _drv_emerg_, ("\n mp_query_drv_var: offset(%d): set rate=0x%.2x \n",offset,padapter->registrypriv.tx_rate));
+		}
+		else{ //not use the data rate
+			padapter->registrypriv.use_rate=_FALSE;
+			RT_TRACE(_module_mp_, _drv_emerg_, ("\n mp_query_drv_var: offset(%d) out of rate range\n",offset));
+		}
+	}
+	else if (offset<=110){  //for setting debug level
+		RT_TRACE(_module_mp_, _drv_emerg_, (" mp_query_drv_var: offset(%d) for set debug level\n",offset));
+		if(offset==110){ //For query data rate
+			RT_TRACE(_module_mp_, _drv_emerg_, (" mp_query_drv_var: offset(%d): query dbg level=0x%.2x \n",offset,padapter->registrypriv.dbg_level));
+			padapter->registrypriv.dbg_level=GlobalDebugLevel;
+			var=padapter->registrypriv.dbg_level;
+		}
+		else if(offset<110 && offset>100){
+			RT_TRACE(_module_mp_, _drv_emerg_, (" mp_query_drv_var: offset(%d): set dbg level=0x%.2x \n",offset,offset-100));
+			padapter->registrypriv.dbg_level=GlobalDebugLevel=offset-100;
+			var=padapter->registrypriv.dbg_level;
+			RT_TRACE(_module_mp_, _drv_emerg_, (" mp_query_drv_var(_drv_emerg_): offset(%d): set dbg level=0x%.2x \n",offset,GlobalDebugLevel));
+			RT_TRACE(_module_mp_, _drv_alert_, (" mp_query_drv_var(_drv_alert_): offset(%d): set dbg level=0x%.2x \n",offset,GlobalDebugLevel));
+			RT_TRACE(_module_mp_, _drv_crit_, (" mp_query_drv_var(_drv_crit_): offset(%d): set dbg level=0x%.2x \n",offset,GlobalDebugLevel));
+			RT_TRACE(_module_mp_, _drv_err_, (" mp_query_drv_var(_drv_err_): offset(%d): set dbg level=0x%.2x \n",offset,GlobalDebugLevel));
+			RT_TRACE(_module_mp_, _drv_warning_, (" mp_query_drv_var(_drv_warning_): offset(%d): set dbg level=0x%.2x \n",offset,GlobalDebugLevel));
+			RT_TRACE(_module_mp_, _drv_notice_, (" mp_query_drv_var(_drv_notice_): offset(%d): set dbg level=0x%.2x \n",offset,GlobalDebugLevel));
+			RT_TRACE(_module_mp_, _drv_info_, (" mp_query_drv_var(_drv_info_): offset(%d): set dbg level=0x%.2x \n",offset,GlobalDebugLevel));
+			RT_TRACE(_module_mp_, _drv_debug_, (" mp_query_drv_var(_drv_debug_): offset(%d): set dbg level=0x%.2x \n",offset,GlobalDebugLevel));
+
+		}
+	}
+	else if(offset >110 &&offset <116){
+		if(115==offset){
+			RT_TRACE(_module_mp_, _drv_emerg_, (" mp_query_drv_var(_drv_emerg_): offset(%d): query TRX access type: [tx_block_mode=%x,rx_block_mode=%x]\n",\
+															offset, adapter_to_dvobj(padapter)->tx_block_mode, adapter_to_dvobj(padapter)->rx_block_mode));
+		}
+		else {
+			switch(offset){
+				case 111:
+					adapter_to_dvobj(padapter)->tx_block_mode=1;
+					adapter_to_dvobj(padapter)->rx_block_mode=1;
+					RT_TRACE(_module_mp_, _drv_emerg_, \
+						(" mp_query_drv_var(_drv_emerg_): offset(%d): SET TRX access type:(TX block/RX block) [tx_block_mode=%x,rx_block_mode=%x]\n",\
+						offset, adapter_to_dvobj(padapter)->tx_block_mode, adapter_to_dvobj(padapter)->rx_block_mode));
+					break;
+				case 112:
+					adapter_to_dvobj(padapter)->tx_block_mode=1;
+					adapter_to_dvobj(padapter)->rx_block_mode=0;
+					RT_TRACE(_module_mp_, _drv_emerg_, \
+						(" mp_query_drv_var(_drv_emerg_): offset(%d): SET TRX access type:(TX block/RX byte) [tx_block_mode=%x,rx_block_mode=%x]\n",\
+						offset, adapter_to_dvobj(padapter)->tx_block_mode, adapter_to_dvobj(padapter)->rx_block_mode));
+					break;
+				case 113:
+					adapter_to_dvobj(padapter)->tx_block_mode=0;
+					adapter_to_dvobj(padapter)->rx_block_mode=1;
+					RT_TRACE(_module_mp_, _drv_emerg_, \
+						(" mp_query_drv_var(_drv_emerg_): offset(%d): SET TRX access type:(TX byte/RX block) [tx_block_mode=%x,rx_block_mode=%x]\n",\
+						offset, adapter_to_dvobj(padapter)->tx_block_mode, adapter_to_dvobj(padapter)->rx_block_mode));
+					break;
+				case 114:
+					adapter_to_dvobj(padapter)->tx_block_mode=0;
+					adapter_to_dvobj(padapter)->rx_block_mode=0;
+					RT_TRACE(_module_mp_, _drv_emerg_, \
+						(" mp_query_drv_var(_drv_emerg_): offset(%d): SET TRX access type:(TX byte/RX byte) [tx_block_mode=%x,rx_block_mode=%x]\n",\
+						offset, adapter_to_dvobj(padapter)->tx_block_mode, adapter_to_dvobj(padapter)->rx_block_mode));
+					break;
+				default :
+					break;
+
+			}
+
+		}
+
+	}
+	else if(offset>=127){
+		u64	prnt_dbg_comp;
+		u8   chg_idx;
+		u64	tmp_dbg_comp;
+		chg_idx=offset-0x80;
+		tmp_dbg_comp=BIT(chg_idx);
+		prnt_dbg_comp=padapter->registrypriv.dbg_component= GlobalDebugComponents;
+		RT_TRACE(_module_mp_, _drv_emerg_, (" 1: mp_query_drv_var: offset(%d;0x%x):for dbg conpoment prnt_dbg_comp=0x%.16x GlobalDebugComponents=0x%.16x padapter->registrypriv.dbg_component=0x%.16x\n",offset,offset,prnt_dbg_comp,GlobalDebugComponents,padapter->registrypriv.dbg_component));
+		if(offset==127){
+	//		prnt_dbg_comp=padapter->registrypriv.dbg_component= GlobalDebugComponents;
+			var=(u32)(padapter->registrypriv.dbg_component);
+			RT_TRACE(0xffffffff, _drv_emerg_, ("2: mp_query_drv_var: offset(%d;0x%x):for query dbg conpoment=0x%x(l) 0x%x(h)  GlobalDebugComponents=0x%x(l) 0x%x(h) \n",offset,offset,padapter->registrypriv.dbg_component,prnt_dbg_comp));
+			prnt_dbg_comp=GlobalDebugComponents;
+			RT_TRACE(0xffffffff, _drv_emerg_, ("2-1: mp_query_drv_var: offset(%d;0x%x):for query dbg conpoment=0x%x(l) 0x%x(h)  GlobalDebugComponents=0x%x(l) 0x%x(h)\n",offset,offset,padapter->registrypriv.dbg_component,prnt_dbg_comp));
+			prnt_dbg_comp=GlobalDebugComponents=padapter->registrypriv.dbg_component;
+			RT_TRACE(0xffffffff, _drv_emerg_, ("2-2: mp_query_drv_var: offset(%d;0x%x):for query dbg conpoment=0x%x(l) 0x%x(h)  GlobalDebugComponents=0x%x(l) 0x%x(h)\n",offset,offset,padapter->registrypriv.dbg_component,prnt_dbg_comp));
+
+		}
+		else{
+			RT_TRACE(0xffffffff, _drv_emerg_, ("3: mp_query_drv_var: offset(%d;0x%x):for query dbg conpoment=0x%x(l) 0x%x(h) GlobalDebugComponents=0x%x(l) 0x%x(h) chg_idx=%d\n",offset,offset,padapter->registrypriv.dbg_component,prnt_dbg_comp,chg_idx));
+			prnt_dbg_comp=GlobalDebugComponents;
+			RT_TRACE(0xffffffff, _drv_emerg_,("3-1: mp_query_drv_var: offset(%d;0x%x):for query dbg conpoment=0x%x(l) 0x%x(h)  GlobalDebugComponents=0x%x(l) 0x%x(h) chg_idx=%d\n",offset,offset,padapter->registrypriv.dbg_component,prnt_dbg_comp,chg_idx));// ("3-1: mp_query_drv_var: offset(%d;0x%x):before set dbg conpoment=0x%x chg_idx=%d or0x%x BIT(chg_idx[%d]=0x%x)\n",offset,offset,prnt_dbg_comp,chg_idx,chg_idx,(chg_idx),tmp_dbg_comp)
+			prnt_dbg_comp=GlobalDebugComponents=padapter->registrypriv.dbg_component;
+			RT_TRACE(0xffffffff, _drv_emerg_, ("3-2: mp_query_drv_var: offset(%d;0x%x):for query dbg conpoment=0x%x(l) 0x%x(h)  GlobalDebugComponents=0x%x(l) 0x%x(h)\n",offset,offset,padapter->registrypriv.dbg_component,prnt_dbg_comp));
+
+			if(GlobalDebugComponents&tmp_dbg_comp){
+				//this bit is already set, now clear it
+				GlobalDebugComponents=GlobalDebugComponents&(~tmp_dbg_comp);
+			}
+			else{
+				//this bit is not set, now set it.
+				GlobalDebugComponents =GlobalDebugComponents|tmp_dbg_comp;
+			}
+			RT_TRACE(0xffffffff, _drv_emerg_, ("4: mp_query_drv_var: offset(%d;0x%x):before set dbg conpoment tmp_dbg_comp=0x%x GlobalDebugComponents=0x%x(l) 0x%x(h)",offset,offset,tmp_dbg_comp,prnt_dbg_comp));
+			prnt_dbg_comp=GlobalDebugComponents;
+			RT_TRACE(0xffffffff, _drv_emerg_, ("4-1: mp_query_drv_var: offset(%d;0x%x):before set dbg conpoment tmp_dbg_comp=0x%x GlobalDebugComponents=0x%x(l) 0x%x(h)",offset,offset,tmp_dbg_comp,prnt_dbg_comp));
+
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_emerg_, ("0: mp_query_drv_var(_module_rtl871x_xmit_c_:0): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,prnt_dbg_comp));
+			RT_TRACE(_module_xmit_osdep_c_, _drv_emerg_, ("1: mp_query_drv_var(_module_xmit_osdep_c_:1): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_emerg_, ("2: mp_query_drv_var(_module_rtl871x_recv_c_:2): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_recv_osdep_c_, _drv_emerg_, ("3: mp_query_drv_var(_module_recv_osdep_c_:3): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_emerg_, ("4: mp_query_drv_var(_module_rtl871x_mlme_c_:4): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_mlme_osdep_c_, _drv_emerg_, (" 5:mp_query_drv_var(_module_mlme_osdep_c_:5): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_emerg_, ("6: mp_query_drv_var(_module_rtl871x_sta_mgt_c_:6): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_rtl871x_cmd_c_, _drv_emerg_, ("7: mp_query_drv_var(_module_rtl871x_cmd_c_:7): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_cmd_osdep_c_, _drv_emerg_, ("8: mp_query_drv_var(_module_cmd_osdep_c_:8): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_rtl871x_io_c_, _drv_emerg_, ("9: mp_query_drv_var(_module_rtl871x_io_c_:9): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_io_osdep_c_, _drv_emerg_, ("10: mp_query_drv_var(_module_io_osdep_c_:10): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_os_intfs_c_, _drv_emerg_, ("11: mp_query_drv_var(_module_os_intfs_c_:11): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_rtl871x_security_c_, _drv_emerg_, ("12: mp_query_drv_var(_module_rtl871x_security_c_:12): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_rtl871x_eeprom_c_, _drv_emerg_, ("13: mp_query_drv_var(_module_rtl871x_eeprom_c_:13): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_hal_init_c_, _drv_emerg_, ("14: mp_query_drv_var(_module_hal_init_c_:14): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_hci_hal_init_c_, _drv_emerg_, ("15: mp_query_drv_var(_module_hci_hal_init_c_:15): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_rtl871x_ioctl_c_, _drv_emerg_, ("16: mp_query_drv_var(_module_rtl871x_ioctl_c_:16): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_emerg_, ("17: mp_query_drv_var(_module_rtl871x_ioctl_set_c_:17): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_rtl871x_ioctl_query_c_, _drv_emerg_, ("18: mp_query_drv_var(_module_rtl871x_ioctl_query_c_:18): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_emerg_, ("19: mp_query_drv_var(_module_rtl871x_pwrctrl_c_:19): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_hci_intfs_c_, _drv_emerg_, ("20: mp_query_drv_var(_module_hci_intfs_c_:20): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_hci_ops_c_, _drv_emerg_, ("21: mp_query_drv_var(_module_hci_ops_c_:21): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_osdep_service_c_, _drv_emerg_, ("22: mp_query_drv_var(_module_osdep_service_c_:22): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_mp_, _drv_emerg_, ("23: mp_query_drv_var(_module_mp_:23): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			RT_TRACE(_module_hci_ops_os_c_, _drv_emerg_, ("24: mp_query_drv_var(_module_hci_ops_os_c_:24): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+			var=(u32)(GlobalDebugComponents);
+			//GlobalDebugComponents=padapter->registrypriv.dbg_component;
+			RT_TRACE(0xffffffff, _drv_emerg_, (" ==mp_query_drv_var(_module_mp_): offset(%d;0x%x):before set dbg conpoment=0x%x(l) 0x%x(h)\n",offset,offset,GlobalDebugComponents));
+
+		}
+	}
+	else{
+		RT_TRACE(_module_mp_, _drv_emerg_, ("\n mp_query_drv_var: offset(%d) >110\n",offset));
+	}
+#endif
+#endif
+
+	return var;
+}
+#endif
+
+NDIS_STATUS oid_rt_pro_query_dr_variable_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER		Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL			oldirql;
+#endif
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+
+	DR_VARIABLE_STRUCT	*pdrv_var;
+
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	*poid_par_priv->bytes_needed = sizeof(DR_VARIABLE_STRUCT);
+	if (poid_par_priv->information_buf_len < *poid_par_priv->bytes_needed)
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+Query Information, OID_RT_PRO_QUERY_DR_VARIABLE\n"));
+
+	pdrv_var = (struct _DR_VARIABLE_STRUCT_ *)poid_par_priv->information_buf;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	pdrv_var->variable = mp_query_drv_var(Adapter, pdrv_var->offset, pdrv_var->variable);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("-oid_rt_pro_query_dr_variable_hdl: offset=0x%x valule=0x%x\n",
+		  pdrv_var->offset, pdrv_var->variable));
+
+	return status;
+#else
+	return 0;
+#endif
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_rx_packet_type_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+	RT_TRACE(_module_mp_, _drv_err_, ("oid_rt_pro_rx_packet_type_hdl...................\n"));
+
+	if (poid_par_priv->information_buf_len < sizeof (UCHAR)) {
+		status = NDIS_STATUS_INVALID_LENGTH;
+		*poid_par_priv->bytes_needed = sizeof(UCHAR);
+		return status;
+	}
+
+	if (poid_par_priv->type_of_oid == SET_OID) {
+		Adapter->mppriv.rx_with_status = *(UCHAR *) poid_par_priv->information_buf;
+		RT_TRACE(_module_rtl871x_ioctl_c_,_drv_err_, ("Query Information, OID_RT_PRO_RX_PACKET_TYPE:%d \n",\
+												Adapter->mppriv.rx_with_status));
+
+		//*(u32 *)&Adapter->eeprompriv.mac_addr[0]=rtw_read32(Adapter, 0x10250050);
+		//*(u16 *)&Adapter->eeprompriv.mac_addr[4]=rtw_read16(Adapter, 0x10250054);
+		RT_TRACE(_module_rtl871x_ioctl_c_,_drv_err_,("MAC addr=0x%x:0x%x:0x%x:0x%x:0x%x:0x%x  \n",
+			Adapter->eeprompriv.mac_addr[0],Adapter->eeprompriv.mac_addr[1],Adapter->eeprompriv.mac_addr[2],\
+			Adapter->eeprompriv.mac_addr[3],Adapter->eeprompriv.mac_addr[4],Adapter->eeprompriv.mac_addr[5]));
+
+	}
+	else {
+		*(UCHAR *) poid_par_priv->information_buf = Adapter->mppriv.rx_with_status;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+		RT_TRACE(_module_rtl871x_ioctl_c_,_drv_err_, ("Query Information, OID_RT_PRO_RX_PACKET_TYPE:%d \n", \
+												Adapter->mppriv.rx_with_status));
+
+		//*(u32 *)&Adapter->eeprompriv.mac_addr[0]=rtw_read32(Adapter, 0x10250050);
+		//*(u16 *)&Adapter->eeprompriv.mac_addr[4]=rtw_read16(Adapter, 0x10250054);
+		RT_TRACE(_module_rtl871x_ioctl_c_,_drv_err_,("MAC addr=0x%x:0x%x:0x%x:0x%x:0x%x:0x%x  \n",
+			Adapter->eeprompriv.mac_addr[0],Adapter->eeprompriv.mac_addr[1],Adapter->eeprompriv.mac_addr[2],\
+			Adapter->eeprompriv.mac_addr[3],Adapter->eeprompriv.mac_addr[4],Adapter->eeprompriv.mac_addr[5]));
+	}
+#endif
+
+	return NDIS_STATUS_SUCCESS;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_read_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL oldirql;
+#endif
+	PEFUSE_ACCESS_STRUCT pefuse;
+	u8 *data;
+	u16 addr = 0, cnts = 0, max_available_size = 0;
+	NDIS_STATUS status = NDIS_STATUS_SUCCESS;
+	PADAPTER Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(EFUSE_ACCESS_STRUCT))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	pefuse = (PEFUSE_ACCESS_STRUCT)poid_par_priv->information_buf;
+	addr = pefuse->start_addr;
+	cnts = pefuse->cnts;
+	data = pefuse->data;
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		("+oid_rt_pro_read_efuse_hd: buf_len=%d addr=%d cnts=%d\n",
+		 poid_par_priv->information_buf_len, addr, cnts));
+
+	EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (PVOID)&max_available_size, _FALSE);
+
+	if ((addr + cnts) > max_available_size) {
+		RT_TRACE(_module_mp_, _drv_err_, ("!oid_rt_pro_read_efuse_hdl: parameter error!\n"));
+		return NDIS_STATUS_NOT_ACCEPTED;
+	}
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	if (rtw_efuse_access(Adapter, _FALSE, addr, cnts, data) == _FAIL) {
+		RT_TRACE(_module_mp_, _drv_err_, ("!oid_rt_pro_read_efuse_hdl: rtw_efuse_access FAIL!\n"));
+		status = NDIS_STATUS_FAILURE;
+	} else
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_write_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL oldirql;
+#endif
+	PEFUSE_ACCESS_STRUCT pefuse;
+	u8 *data;
+	u16 addr = 0, cnts = 0, max_available_size = 0;
+	NDIS_STATUS status = NDIS_STATUS_SUCCESS;
+	PADAPTER Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	pefuse = (PEFUSE_ACCESS_STRUCT)poid_par_priv->information_buf;
+	addr = pefuse->start_addr;
+	cnts = pefuse->cnts;
+	data = pefuse->data;
+
+	RT_TRACE(_module_mp_, _drv_notice_,
+		 ("+oid_rt_pro_write_efuse_hdl: buf_len=%d addr=0x%04x cnts=%d\n",
+		  poid_par_priv->information_buf_len, addr, cnts));
+
+	EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (PVOID)&max_available_size, _FALSE);
+
+	if ((addr + cnts) > max_available_size) {
+		RT_TRACE(_module_mp_, _drv_err_, ("!oid_rt_pro_write_efuse_hdl: parameter error"));
+		return NDIS_STATUS_NOT_ACCEPTED;
+	}
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	if (rtw_efuse_access(Adapter, _TRUE, addr, cnts, data) == _FAIL)
+		status = NDIS_STATUS_FAILURE;
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_rw_efuse_pgpkt_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	PPGPKT_STRUCT	ppgpkt;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+//	RT_TRACE(_module_mp_, _drv_info_, ("+oid_rt_pro_rw_efuse_pgpkt_hdl\n"));
+
+	*poid_par_priv->bytes_rw = 0;
+
+	if (poid_par_priv->information_buf_len < sizeof(PGPKT_STRUCT))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	ppgpkt = (PPGPKT_STRUCT)poid_par_priv->information_buf;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	if (poid_par_priv->type_of_oid == QUERY_OID)
+	{
+		RT_TRACE(_module_mp_, _drv_notice_,
+			("oid_rt_pro_rw_efuse_pgpkt_hdl: Read offset=0x%x\n",\
+			ppgpkt->offset));
+
+		Efuse_PowerSwitch(Adapter, _FALSE, _TRUE);
+		if (Efuse_PgPacketRead(Adapter, ppgpkt->offset, ppgpkt->data, _FALSE) == _TRUE)
+			*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+		else
+			status = NDIS_STATUS_FAILURE;
+		Efuse_PowerSwitch(Adapter, _FALSE, _FALSE);
+	} else {
+		RT_TRACE(_module_mp_, _drv_notice_,
+			("oid_rt_pro_rw_efuse_pgpkt_hdl: Write offset=0x%x word_en=0x%x\n",\
+			ppgpkt->offset, ppgpkt->word_en));
+
+		Efuse_PowerSwitch(Adapter, _TRUE, _TRUE);
+		if (Efuse_PgPacketWrite(Adapter, ppgpkt->offset, ppgpkt->word_en, ppgpkt->data, _FALSE) == _TRUE)
+			*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+		else
+			status = NDIS_STATUS_FAILURE;
+		Efuse_PowerSwitch(Adapter, _TRUE, _FALSE);
+	}
+
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	RT_TRACE(_module_mp_, _drv_info_,
+		 ("-oid_rt_pro_rw_efuse_pgpkt_hdl: status=0x%08X\n", status));
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_efuse_current_size_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u16 size;
+	u8 ret;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len <sizeof(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	ret = efuse_GetCurrentSize(Adapter, &size);
+	_irqlevel_changed_(&oldirql, RAISE);
+	if (ret == _SUCCESS) {
+		*(u32*)poid_par_priv->information_buf = size;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else
+		status = NDIS_STATUS_FAILURE;
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_efuse_max_size_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	*(u32*)poid_par_priv->information_buf = efuse_GetMaxSize(Adapter);
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+	RT_TRACE(_module_mp_, _drv_info_,
+		 ("-oid_rt_get_efuse_max_size_hdl: size=%d status=0x%08X\n",
+		  *(int*)poid_par_priv->information_buf, status));
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS	status;
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_info_, ("+oid_rt_pro_efuse_hdl\n"));
+
+	if (poid_par_priv->type_of_oid == QUERY_OID)
+		status = oid_rt_pro_read_efuse_hdl(poid_par_priv);
+	else
+		status = oid_rt_pro_write_efuse_hdl(poid_par_priv);
+
+	RT_TRACE(_module_mp_, _drv_info_, ("-oid_rt_pro_efuse_hdl: status=0x%08X\n", status));
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_pro_efuse_map_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u8		*data;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	u16	mapLen=0;
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_pro_efuse_map_hdl\n"));
+
+	EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (PVOID)&mapLen, _FALSE);
+
+	*poid_par_priv->bytes_rw = 0;
+
+	if (poid_par_priv->information_buf_len < mapLen)
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	data = (u8*)poid_par_priv->information_buf;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	if (poid_par_priv->type_of_oid == QUERY_OID)
+	{
+		RT_TRACE(_module_mp_, _drv_info_,
+			("oid_rt_pro_efuse_map_hdl: READ\n"));
+
+		if (rtw_efuse_map_read(Adapter, 0, mapLen, data) == _SUCCESS)
+			*poid_par_priv->bytes_rw = mapLen;
+		else {
+			RT_TRACE(_module_mp_, _drv_err_,
+				("oid_rt_pro_efuse_map_hdl: READ fail\n"));
+			status = NDIS_STATUS_FAILURE;
+		}
+	} else {
+		// SET_OID
+		RT_TRACE(_module_mp_, _drv_info_,
+			("oid_rt_pro_efuse_map_hdl: WRITE\n"));
+
+		if (rtw_efuse_map_write(Adapter, 0, mapLen, data) == _SUCCESS)
+			*poid_par_priv->bytes_rw = mapLen;
+		else {
+			RT_TRACE(_module_mp_, _drv_err_,
+				("oid_rt_pro_efuse_map_hdl: WRITE fail\n"));
+			status = NDIS_STATUS_FAILURE;
+		}
+	}
+
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	RT_TRACE(_module_mp_, _drv_info_,
+		 ("-oid_rt_pro_efuse_map_hdl: status=0x%08X\n", status));
+
+_func_exit_;
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_set_crystal_cap_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+
+	u32		crystal_cap = 0;
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len <sizeof(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	crystal_cap = *((u32*)poid_par_priv->information_buf);//4
+	if (crystal_cap > 0xf)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	Adapter->mppriv.curr_crystalcap = crystal_cap;
+
+	_irqlevel_changed_(&oldirql,LOWER);
+	SetCrystalCap(Adapter);
+	_irqlevel_changed_(&oldirql,RAISE);
+
+_func_exit_;
+
+#endif
+	return status;
+}
+
+NDIS_STATUS oid_rt_set_rx_packet_type_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u8		rx_pkt_type;
+//	u32		rcr_val32;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+//	PADAPTER	padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+oid_rt_set_rx_packet_type_hdl\n"));
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(u8))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	rx_pkt_type = *((u8*)poid_par_priv->information_buf);//4
+
+	RT_TRACE(_module_mp_, _drv_info_, ("rx_pkt_type: %x\n",rx_pkt_type ));
+#if 0
+	_irqlevel_changed_(&oldirql, LOWER);
+#if 0
+	rcr_val8 = rtw_read8(Adapter, 0x10250048);//RCR
+	rcr_val8 &= ~(RCR_AB|RCR_AM|RCR_APM|RCR_AAP);
+
+	if(rx_pkt_type == RX_PKT_BROADCAST){
+		rcr_val8 |= (RCR_AB | RCR_ACRC32 );
+	}
+	else if(rx_pkt_type == RX_PKT_DEST_ADDR){
+		rcr_val8 |= (RCR_AAP| RCR_AM |RCR_ACRC32);
+	}
+	else if(rx_pkt_type == RX_PKT_PHY_MATCH){
+		rcr_val8 |= (RCR_APM|RCR_ACRC32);
+	}
+	else{
+		rcr_val8 &= ~(RCR_AAP|RCR_APM|RCR_AM|RCR_AB|RCR_ACRC32);
+	}
+	rtw_write8(padapter, 0x10250048,rcr_val8);
+#else
+	rcr_val32 = rtw_read32(padapter, RCR);//RCR = 0x10250048
+	rcr_val32 &= ~(RCR_CBSSID|RCR_AB|RCR_AM|RCR_APM|RCR_AAP);
+#if 0
+	if(rx_pkt_type == RX_PKT_BROADCAST){
+		rcr_val32 |= (RCR_AB|RCR_AM|RCR_APM|RCR_AAP|RCR_ACRC32);
+	}
+	else if(rx_pkt_type == RX_PKT_DEST_ADDR){
+		//rcr_val32 |= (RCR_CBSSID|RCR_AAP|RCR_AM|RCR_ACRC32);
+		rcr_val32 |= (RCR_CBSSID|RCR_APM|RCR_ACRC32);
+	}
+	else if(rx_pkt_type == RX_PKT_PHY_MATCH){
+		rcr_val32 |= (RCR_APM|RCR_ACRC32);
+		//rcr_val32 |= (RCR_AAP|RCR_ACRC32);
+	}
+	else{
+		rcr_val32 &= ~(RCR_AAP|RCR_APM|RCR_AM|RCR_AB|RCR_ACRC32);
+	}
+#else
+	switch (rx_pkt_type)
+	{
+		case RX_PKT_BROADCAST :
+			rcr_val32 |= (RCR_AB|RCR_AM|RCR_APM|RCR_AAP|RCR_ACRC32);
+			break;
+		case RX_PKT_DEST_ADDR :
+			rcr_val32 |= (RCR_AB|RCR_AM|RCR_APM|RCR_AAP|RCR_ACRC32);
+			break;
+		case RX_PKT_PHY_MATCH:
+			rcr_val32 |= (RCR_APM|RCR_ACRC32);
+			break;
+		default:
+			rcr_val32 &= ~(RCR_AAP|RCR_APM|RCR_AM|RCR_AB|RCR_ACRC32);
+			break;
+	}
+
+	if (rx_pkt_type == RX_PKT_DEST_ADDR) {
+		padapter->mppriv.check_mp_pkt = 1;
+	} else {
+		padapter->mppriv.check_mp_pkt = 0;
+	}
+#endif
+	rtw_write32(padapter, RCR, rcr_val32);
+
+#endif
+	_irqlevel_changed_(&oldirql, RAISE);
+#endif
+_func_exit_;
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_pro_set_tx_agc_offset_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+
+	u32 		txagc;
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len < sizeof(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	txagc = *(u32*)poid_par_priv->information_buf;
+	RT_TRACE(_module_mp_, _drv_info_,
+		 ("oid_rt_pro_set_tx_agc_offset_hdl: 0x%08x\n", txagc));
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	SetTxAGCOffset(Adapter, txagc);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+
+NDIS_STATUS oid_rt_pro_set_pkt_test_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	PADAPTER		Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+
+	struct mlme_priv	*pmlmepriv = &Adapter->mlmepriv;
+	struct mp_priv		*pmppriv = &Adapter->mppriv;
+	u32 			type;
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	if (poid_par_priv->information_buf_len <sizeof(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	type = *(u32*)poid_par_priv->information_buf;
+
+	if (_LOOPBOOK_MODE_ == type) {
+		pmppriv->mode = type;
+		set_fwstate(pmlmepriv, WIFI_MP_LPBK_STATE); //append txdesc
+		RT_TRACE(_module_mp_, _drv_info_, ("test mode change to loopback mode:0x%08x.\n", get_fwstate(pmlmepriv)));
+	} else if (_2MAC_MODE_ == type){
+		pmppriv->mode = type;
+		_clr_fwstate_(pmlmepriv, WIFI_MP_LPBK_STATE);
+		RT_TRACE(_module_mp_, _drv_info_, ("test mode change to 2mac mode:0x%08x.\n", get_fwstate(pmlmepriv)));
+	} else
+		status = NDIS_STATUS_NOT_ACCEPTED;
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+
+unsigned int mp_ioctl_xmit_packet_hdl(struct oid_par_priv *poid_par_priv)
+{
+	PMP_XMIT_PARM pparm;
+	PADAPTER padapter;
+	struct mp_priv *pmp_priv;
+	struct pkt_attrib *pattrib;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+%s\n", __func__));
+
+	pparm = (PMP_XMIT_PARM)poid_par_priv->information_buf;
+	padapter = (PADAPTER)poid_par_priv->adapter_context;
+	pmp_priv = &padapter->mppriv;
+
+	if (poid_par_priv->type_of_oid == QUERY_OID) {
+		pparm->enable = !pmp_priv->tx.stop;
+		pparm->count = pmp_priv->tx.sended;
+	} else {
+		if (pparm->enable == 0) {
+			pmp_priv->tx.stop = 1;
+		} else if (pmp_priv->tx.stop == 1) {
+			pmp_priv->tx.stop = 0;
+			pmp_priv->tx.count = pparm->count;
+			pmp_priv->tx.payload = pparm->payload_type;
+			pattrib = &pmp_priv->tx.attrib;
+			pattrib->pktlen = pparm->length;
+			_rtw_memcpy(pattrib->dst, pparm->da, ETH_ALEN);
+			SetPacketTx(padapter);
+		} else
+			return NDIS_STATUS_FAILURE;
+	}
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+#if 0
+unsigned int mp_ioctl_xmit_packet_hdl(struct oid_par_priv *poid_par_priv)
+{
+	unsigned char *pframe, *pmp_pkt;
+	struct ethhdr *pethhdr;
+	struct pkt_attrib *pattrib;
+	struct rtw_ieee80211_hdr *pwlanhdr;
+	unsigned short *fctrl;
+	int llc_sz, payload_len;
+	struct mp_xmit_frame *pxframe=  NULL;
+	struct mp_xmit_packet *pmp_xmitpkt = (struct mp_xmit_packet*)param;
+	u8 addr3[] = {0x02, 0xE0, 0x4C, 0x87, 0x66, 0x55};
+
+//	DBG_871X("+mp_ioctl_xmit_packet_hdl\n");
+
+	pxframe = alloc_mp_xmitframe(&padapter->mppriv);
+	if (pxframe == NULL)
+	{
+		DEBUG_ERR(("Can't alloc pmpframe %d:%s\n", __LINE__, __FILE__));
+		return -1;
+	}
+
+	//mp_xmit_pkt
+	payload_len = pmp_xmitpkt->len - 14;
+	pmp_pkt = (unsigned char*)pmp_xmitpkt->mem;
+	pethhdr = (struct ethhdr *)pmp_pkt;
+
+	//DBG_871X("payload_len=%d, pkt_mem=0x%x\n", pmp_xmitpkt->len, (void*)pmp_xmitpkt->mem);
+
+	//DBG_871X("pxframe=0x%x\n", (void*)pxframe);
+	//DBG_871X("pxframe->mem=0x%x\n", (void*)pxframe->mem);
+
+	//update attribute
+	pattrib = &pxframe->attrib;
+	memset((u8 *)(pattrib), 0, sizeof (struct pkt_attrib));
+	pattrib->pktlen = pmp_xmitpkt->len;
+	pattrib->ether_type = ntohs(pethhdr->h_proto);
+	pattrib->hdrlen = 24;
+	pattrib->nr_frags = 1;
+	pattrib->priority = 0;
+#ifndef CONFIG_MP_LINUX
+	if(IS_MCAST(pethhdr->h_dest))
+		pattrib->mac_id = 4;
+	else
+		pattrib->mac_id = 5;
+#else
+	pattrib->mac_id = 5;
+#endif
+
+	//
+	memset(pxframe->mem, 0 , WLANHDR_OFFSET);
+	pframe = (u8 *)(pxframe->mem) + WLANHDR_OFFSET;
+
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+	SetFrameSubType(pframe, WIFI_DATA);
+
+	_rtw_memcpy(pwlanhdr->addr1, pethhdr->h_dest, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, pethhdr->h_source, ETH_ALEN);
+
+	_rtw_memcpy(pwlanhdr->addr3, addr3, ETH_ALEN);
+
+	pwlanhdr->seq_ctl = 0;
+	pframe += pattrib->hdrlen;
+
+	llc_sz= rtw_put_snap(pframe, pattrib->ether_type);
+	pframe += llc_sz;
+
+	_rtw_memcpy(pframe, (void*)(pmp_pkt+14),  payload_len);
+
+	pattrib->last_txcmdsz = pattrib->hdrlen + llc_sz + payload_len;
+
+	DEBUG_INFO(("issuing mp_xmit_frame, tx_len=%d, ether_type=0x%x\n", pattrib->last_txcmdsz, pattrib->ether_type));
+	xmit_mp_frame(padapter, pxframe);
+
+	return _SUCCESS;
+}
+#endif
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_set_power_down_hdl(struct oid_par_priv *poid_par_priv)
+{
+#ifdef PLATFORM_OS_XP
+	_irqL		oldirql;
+#endif
+	u8		bpwrup;
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+#ifdef PLATFORM_LINUX
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+	PADAPTER	padapter = (PADAPTER)(poid_par_priv->adapter_context);
+#endif
+#endif
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	RT_TRACE(_module_mp_, _drv_info_,
+		 ("\n ===> Setoid_rt_set_power_down_hdl.\n"));
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	bpwrup = *(u8 *)poid_par_priv->information_buf;
+	//CALL  the power_down function
+#ifdef PLATFORM_LINUX
+#if defined(CONFIG_RTL8712) //Linux MP insmod unknown symbol
+	dev_power_down(padapter,bpwrup); 
+#endif
+#endif
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	//DEBUG_ERR(("\n <=== Query OID_RT_PRO_READ_REGISTER.
+	//	Add:0x%08x Width:%d Value:0x%08x\n",RegRWStruct->offset,RegRWStruct->width,RegRWStruct->value));
+
+_func_exit_;
+
+	return status;
+}
+//------------------------------------------------------------------------------
+NDIS_STATUS oid_rt_get_power_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+#if 0
+	NDIS_STATUS	status = NDIS_STATUS_SUCCESS;
+	PADAPTER	Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+//#ifdef PLATFORM_OS_XP
+//	_irqL		oldirql;
+//#endif
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	if (poid_par_priv->information_buf_len < sizeof(u32)) {
+		status = NDIS_STATUS_INVALID_LENGTH;
+		return status;
+	}
+
+	RT_TRACE(_module_mp_, _drv_info_,
+		 ("\n ===> oid_rt_get_power_mode_hdl.\n"));
+
+//	_irqlevel_changed_(&oldirql, LOWER);
+	*(int*)poid_par_priv->information_buf = Adapter->registrypriv.low_power ? POWER_LOW : POWER_NORMAL;
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+//	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+#else
+	return 0;
+#endif
+}
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_p2p.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_p2p.c
new file mode 100755
index 00000000..0ed4b5e3
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_p2p.c
@@ -0,0 +1,4797 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_P2P_C_
+
+#include <drv_types.h>
+#include <rtw_p2p.h>
+#include <wifi.h>
+
+#ifdef CONFIG_P2P
+
+int rtw_p2p_is_channel_list_ok( u8 desired_ch, u8* ch_list, u8 ch_cnt )
+{
+	int found = 0, i = 0;
+
+	for( i = 0; i < ch_cnt; i++ )
+	{
+		if ( ch_list[ i ] == desired_ch )
+		{
+			found = 1;
+			break;
+		}
+	}
+	return( found );
+}
+
+int is_any_client_associated(_adapter *padapter)
+{
+	return padapter->stapriv.asoc_list_cnt ? _TRUE : _FALSE;
+}
+
+static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	_irqL irqL;
+	_list	*phead, *plist;
+	u32 len=0;
+	u16 attr_len = 0;
+	u8 tmplen, *pdata_attr, *pstart, *pcur;
+	struct sta_info *psta = NULL;
+	_adapter *padapter = pwdinfo->padapter;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+	pdata_attr = rtw_zmalloc(MAX_P2P_IE_LEN);
+
+	pstart = pdata_attr;
+	pcur = pdata_attr;
+
+	_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+	phead = &pstapriv->asoc_list;
+	plist = get_next(phead);
+
+	//look up sta asoc_queue
+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)	
+	{		
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+		
+		plist = get_next(plist);
+		
+
+		if(psta->is_p2p_device)
+		{
+			tmplen = 0;
+			
+			pcur++;
+		
+			//P2P device address
+			_rtw_memcpy(pcur, psta->dev_addr, ETH_ALEN);
+			pcur += ETH_ALEN;
+
+			//P2P interface address
+			_rtw_memcpy(pcur, psta->hwaddr, ETH_ALEN);
+			pcur += ETH_ALEN;
+
+			*pcur = psta->dev_cap;
+			pcur++;
+
+			//*(u16*)(pcur) = cpu_to_be16(psta->config_methods);
+			RTW_PUT_BE16(pcur, psta->config_methods);
+			pcur += 2;
+
+			_rtw_memcpy(pcur, psta->primary_dev_type, 8);
+			pcur += 8;
+
+			*pcur = psta->num_of_secdev_type;
+			pcur++;
+
+			_rtw_memcpy(pcur, psta->secdev_types_list, psta->num_of_secdev_type*8);
+			pcur += psta->num_of_secdev_type*8;
+			
+			if(psta->dev_name_len>0)
+			{
+				//*(u16*)(pcur) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
+				RTW_PUT_BE16(pcur, WPS_ATTR_DEVICE_NAME);
+				pcur += 2;
+
+				//*(u16*)(pcur) = cpu_to_be16( psta->dev_name_len );
+				RTW_PUT_BE16(pcur, psta->dev_name_len);
+				pcur += 2;
+
+				_rtw_memcpy(pcur, psta->dev_name, psta->dev_name_len);
+				pcur += psta->dev_name_len;
+			}
+
+
+			tmplen = (u8)(pcur-pstart);
+			
+			*pstart = (tmplen-1);
+
+			attr_len += tmplen;
+
+			//pstart += tmplen;
+			pstart = pcur;
+			
+		}
+		
+		
+	}
+	_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+	if(attr_len>0)
+	{
+		len = rtw_set_p2p_attr_content(pbuf, P2P_ATTR_GROUP_INFO, attr_len, pdata_attr);
+	}
+
+	rtw_mfree(pdata_attr, MAX_P2P_IE_LEN);
+
+	return len;
+
+}
+
+static void issue_group_disc_req(struct wifidirect_info *pwdinfo, u8 *da)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	_adapter *padapter = pwdinfo->padapter;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);	
+	unsigned char category = RTW_WLAN_CATEGORY_P2P;//P2P action frame	
+	u32	p2poui = cpu_to_be32(P2POUI);
+	u8	oui_subtype = P2P_GO_DISC_REQUEST;
+	u8	dialogToken=0;
+
+	DBG_871X("[%s]\n", __FUNCTION__);
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	//Build P2P action frame header
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));	
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));	
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));		
+
+	//there is no IE in this P2P action frame
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+}
+
+static void issue_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
+{	
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	_adapter *padapter = pwdinfo->padapter;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);	
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	u32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_DEVDISC_RESP;
+	u8 p2pie[8] = { 0x00 };
+	u32 p2pielen = 0;	
+
+	DBG_871X("[%s]\n", __FUNCTION__);
+	
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, pwdinfo->device_addr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, pwdinfo->device_addr, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	//Build P2P public action frame header
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));	
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));		
+
+
+	//Build P2P IE
+	//	P2P OUI
+	p2pielen = 0;
+	p2pie[ p2pielen++ ] = 0x50;
+	p2pie[ p2pielen++ ] = 0x6F;
+	p2pie[ p2pielen++ ] = 0x9A;
+	p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+	// P2P_ATTR_STATUS
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
+	
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen);	
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+}
+
+static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8* raddr, u8* frame_body, u16 config_method)
+{
+	_adapter *padapter = pwdinfo->padapter;
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	u8			dialogToken = frame_body[7];	//	The Dialog Token of provisioning discovery request frame.
+	u32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_PROVISION_DISC_RESP;
+	u8			wpsie[ 100 ] = { 0x00 };
+	u8			wpsielen = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif //CONFIG_WFD		
+	
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));	
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));		
+
+	wpsielen = 0;
+	//	WPS OUI
+	//*(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
+	RTW_PUT_BE32(wpsie, WPSOUI);
+	wpsielen += 4;
+
+#if 0
+	//	WPS version
+	//	Type:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
+	wpsielen += 2;
+
+	//	Length:
+	*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
+	wpsielen += 2;
+
+	//	Value:
+	wpsie[wpsielen++] = WPS_VERSION_1;	//	Version 1.0
+#endif
+
+	//	Config Method
+	//	Type:
+	//*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
+	RTW_PUT_BE16(wpsie + wpsielen, WPS_ATTR_CONF_METHOD);
+	wpsielen += 2;
+
+	//	Length:
+	//*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
+	RTW_PUT_BE16(wpsie + wpsielen, 0x0002);
+	wpsielen += 2;
+
+	//	Value:
+	//*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( config_method );
+	RTW_PUT_BE16(wpsie + wpsielen, config_method);
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );	
+
+#ifdef CONFIG_WFD
+	wfdielen = build_provdisc_resp_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif //CONFIG_WFD
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+static void issue_p2p_presence_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl;
+	_adapter *padapter = pwdinfo->padapter;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);	
+	unsigned char category = RTW_WLAN_CATEGORY_P2P;//P2P action frame	
+	u32	p2poui = cpu_to_be32(P2POUI);
+	u8	oui_subtype = P2P_PRESENCE_RESPONSE;	
+	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
+	u8 noa_attr_content[32] = { 0x00 };
+	u32 p2pielen = 0;
+
+	DBG_871X("[%s]\n", __FUNCTION__);
+
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		return;
+	}
+
+	//update attribute
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	_rtw_memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	//Build P2P action frame header
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));	
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));	
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));	
+
+
+	//Add P2P IE header
+	//	P2P OUI
+	p2pielen = 0;
+	p2pie[ p2pielen++ ] = 0x50;
+	p2pie[ p2pielen++ ] = 0x6F;
+	p2pie[ p2pielen++ ] = 0x9A;
+	p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+	//Add Status attribute in P2P IE 
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
+
+	//Add NoA attribute in P2P IE
+	noa_attr_content[0] = 0x1;//index
+	noa_attr_content[1] = 0x0;//CTWindow and OppPS Parameters
+	
+	//todo: Notice of Absence Descriptor(s)
+	
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_NOA, 2, noa_attr_content);
+
+
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &(pattrib->pktlen));
+
+	
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+}
+
+u32 build_beacon_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
+	u16 capability=0;
+	u32 len=0, p2pielen = 0;
+	
+
+	//	P2P OUI
+	p2pielen = 0;
+	p2pie[ p2pielen++ ] = 0x50;
+	p2pie[ p2pielen++ ] = 0x6F;
+	p2pie[ p2pielen++ ] = 0x9A;
+	p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+
+	//	According to the P2P Specification, the beacon frame should contain 3 P2P attributes
+	//	1. P2P Capability
+	//	2. P2P Device ID
+	//	3. Notice of Absence ( NOA )	
+
+	//	P2P Capability ATTR
+	//	Type:
+	//	Length:
+	//	Value:
+	//	Device Capability Bitmap, 1 byte
+	//	Be able to participate in additional P2P Groups and
+	//	support the P2P Invitation Procedure	
+	//	Group Capability Bitmap, 1 byte	
+	capability = P2P_DEVCAP_INVITATION_PROC|P2P_DEVCAP_CLIENT_DISCOVERABILITY;
+	capability |=  ((P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS) << 8);
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
+		capability |= (P2P_GRPCAP_GROUP_FORMATION<<8);
+
+	capability = cpu_to_le16(capability);
+
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8*)&capability);
+
+	
+	// P2P Device ID ATTR
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_DEVICE_ID, ETH_ALEN, pwdinfo->device_addr);
+
+	
+	// Notice of Absence ATTR
+	//	Type: 
+	//	Length:
+	//	Value:
+	
+	//go_add_noa_attr(pwdinfo);
+	
+	
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
+	
+	
+	return len;
+	
+}
+
+#ifdef CONFIG_WFD
+u32 build_beacon_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
+
+	//	WFD OUI
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110812
+	//	According to the WFD Specification, the beacon frame should contain 4 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID
+	//	3. Coupled Sink Information
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+
+	if ( P2P_ROLE_GO == pwdinfo->role )
+	{
+		if ( is_any_client_associated( pwdinfo->padapter ) )
+		{
+			//	WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery)
+			RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD );			
+		}
+		else
+		{
+			//	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery)
+			RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD );
+		}
+
+	}
+	else
+	{
+		//	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
+		RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD );
+	}
+	
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+u32 build_probe_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
+
+	//	WFD OUI
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110812
+	//	According to the WFD Specification, the probe request frame should contain 4 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID
+	//	3. Coupled Sink Information
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+
+	if ( 1 == pwdinfo->wfd_tdls_enable )
+	{
+		//	WFD primary sink + available for WFD session + WiFi TDLS mode + WSC ( WFD Service Discovery )	
+		RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | 
+						WFD_DEVINFO_SESSION_AVAIL | 
+						WFD_DEVINFO_WSD |
+						WFD_DEVINFO_PC_TDLS );
+	}
+	else
+	{
+		//	WFD primary sink + available for WFD session + WiFi Direct mode + WSC ( WFD Service Discovery )	
+		RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type |
+						WFD_DEVINFO_SESSION_AVAIL | 
+						WFD_DEVINFO_WSD );
+	}
+
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+u32 build_probe_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 tunneled)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
+
+	//	WFD OUI
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110812
+	//	According to the WFD Specification, the probe response frame should contain 4 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID
+	//	3. Coupled Sink Information
+	//	4. WFD Session Information
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+	//	WFD primary sink + available for WFD session + WiFi Direct mode
+	
+	if (  _TRUE == pwdinfo->session_available )
+	{
+		if ( P2P_ROLE_GO == pwdinfo->role )
+		{
+			if ( is_any_client_associated( pwdinfo->padapter ) )
+			{
+				if ( pwdinfo->wfd_tdls_enable )
+				{
+					//	TDLS mode + WSD ( WFD Service Discovery )
+					RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
+				}
+				else
+				{
+					//	WiFi Direct mode + WSD ( WFD Service Discovery )
+					RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
+				}				
+			}
+			else
+			{
+				if ( pwdinfo->wfd_tdls_enable )
+				{
+					//	available for WFD session + TDLS mode + WSD ( WFD Service Discovery )
+					RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
+				}
+				else
+				{
+					//	available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
+					RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
+				}				
+			}
+		}
+		else
+		{
+			if ( pwdinfo->wfd_tdls_enable )
+			{
+				//	available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
+				RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
+			}
+			else
+			{
+
+				//	available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
+				RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
+			}
+		}
+	}
+	else
+	{
+		if ( pwdinfo->wfd_tdls_enable )
+		{
+			RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD |WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
+		}
+		else
+		{
+			RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
+		}
+
+	}
+
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+	if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+	{
+		//	WFD Session Information ATTR
+		//	Type:
+		wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
+
+		//	Length:
+		//	Note: In the WFD specification, the size of length field is 2.
+		RTW_PUT_BE16(wfdie + wfdielen, 0x0000);
+		wfdielen += 2;
+
+		//	Todo: to add the list of WFD device info descriptor in WFD group.
+
+	}
+#ifdef CONFIG_CONCURRENT_MODE
+#ifdef CONFIG_TDLS
+	if ( ( tunneled == 0 ) && ( padapter->pbuddy_adapter->wdinfo.wfd_tdls_enable == 1 ) )
+	{
+		//	Alternative MAC Address ATTR
+		//	Type:
+		wfdie[ wfdielen++ ] = WFD_ATTR_ALTER_MAC;
+
+		//	Length:
+		//	Note: In the WFD specification, the size of length field is 2.
+		RTW_PUT_BE16(wfdie + wfdielen,  ETH_ALEN );
+		wfdielen += 2;
+
+		//	Value:
+		//	Alternative MAC Address
+		_rtw_memcpy( wfdie + wfdielen, &padapter->pbuddy_adapter->eeprompriv.mac_addr[ 0 ], ETH_ALEN );
+		//	This mac address is used to make the WFD session when TDLS is enable.
+
+		wfdielen += ETH_ALEN;
+	}
+#endif // CONFIG_TDLS
+#endif // CONFIG_CONCURRENT_MODE
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+u32 build_assoc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter 					*padapter = NULL;
+	struct mlme_priv			*pmlmepriv = NULL;
+	struct wifi_display_info		*pwfd_info = NULL;
+
+	//	WFD OUI
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
+	{
+		return 0;
+	}
+
+	padapter = pwdinfo->padapter;
+	pmlmepriv = &padapter->mlmepriv;
+	pwfd_info = padapter->wdinfo.wfd_info;
+	
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110812
+	//	According to the WFD Specification, the probe request frame should contain 4 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID
+	//	3. Coupled Sink Information
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+	//	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD );
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+u32 build_assoc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
+
+	//	WFD OUI
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110812
+	//	According to the WFD Specification, the probe request frame should contain 4 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID
+	//	3. Coupled Sink Information
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+	//	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD );
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+u32 build_nego_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
+
+	//	WFD OUI
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110825
+	//	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID ( Optional )
+	//	3. Local IP Adress ( Optional )
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+	//	WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL);
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+u32 build_nego_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
+
+	//	WFD OUI
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110825
+	//	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID ( Optional )
+	//	3. Local IP Adress ( Optional )
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+	//	WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL);
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+u32 build_nego_confirm_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
+
+	//	WFD OUI
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110825
+	//	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID ( Optional )
+	//	3. Local IP Adress ( Optional )
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+	//	WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL);
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+u32 build_invitation_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
+
+	//	WFD OUI
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110825
+	//	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID ( Optional )
+	//	3. Local IP Adress ( Optional )
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+	//	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD );
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+	if ( P2P_ROLE_GO == pwdinfo->role )
+	{
+		//	WFD Session Information ATTR
+		//	Type:
+		wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
+
+		//	Length:
+		//	Note: In the WFD specification, the size of length field is 2.
+		RTW_PUT_BE16(wfdie + wfdielen, 0x0000);
+		wfdielen += 2;
+
+		//	Todo: to add the list of WFD device info descriptor in WFD group.
+
+	}
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+u32 build_invitation_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
+
+	//	WFD OUI
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110825
+	//	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID ( Optional )
+	//	3. Local IP Adress ( Optional )
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+	//	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD );
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+	if ( P2P_ROLE_GO == pwdinfo->role )
+	{
+		//	WFD Session Information ATTR
+		//	Type:
+		wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
+
+		//	Length:
+		//	Note: In the WFD specification, the size of length field is 2.
+		RTW_PUT_BE16(wfdie + wfdielen, 0x0000);
+		wfdielen += 2;
+
+		//	Todo: to add the list of WFD device info descriptor in WFD group.
+
+	}
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+u32 build_provdisc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
+
+	//	WFD OUI
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110825
+	//	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID ( Optional )
+	//	3. Local IP Adress ( Optional )
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+	//	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD );
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+u32 build_provdisc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len=0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
+
+	//	WFD OUI
+	wfdielen = 0;
+	wfdie[ wfdielen++ ] = 0x50;
+	wfdie[ wfdielen++ ] = 0x6F;
+	wfdie[ wfdielen++ ] = 0x9A;
+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0
+
+	//	Commented by Albert 20110825
+	//	According to the WFD Specification, the provision discovery response frame should contain 3 WFD attributes
+	//	1. WFD Device Information
+	//	2. Associated BSSID ( Optional )
+	//	3. Local IP Adress ( Optional )
+
+
+	//	WFD Device Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value1:
+	//	WFD device information
+	//	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery )
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD );
+	wfdielen += 2;
+
+	//	Value2:
+	//	Session Management Control Port
+	//	Default TCP port for RTSP messages is 554
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );
+	wfdielen += 2;
+
+	//	Value3:
+	//	WFD Device Maximum Throughput
+	//	300Mbps is the maximum throughput
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	//	Associated BSSID ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	//	Value:
+	//	Associated BSSID
+	if ( check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE )
+	{
+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );
+	}
+	else
+	{
+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );
+	}
+
+	wfdielen += ETH_ALEN;
+
+	//	Coupled Sink Information ATTR
+	//	Type:
+	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	//	Length:
+	//	Note: In the WFD specification, the size of length field is 2.
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	//	Value:
+	//	Coupled Sink Status bitmap
+	//	Not coupled/available for Coupling
+	wfdie[ wfdielen++ ] = 0;
+	//  MAC Addr.
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+	wfdie[ wfdielen++ ] = 0;
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+	return len;
+	
+}
+
+
+#endif //CONFIG_WFD
+
+u32 build_probe_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
+	u32 len=0, p2pielen = 0;	
+#ifdef CONFIG_INTEL_WIDI
+	u8 zero_array_check[L2SDTA_SERVICE_VE_LEN] = { 0x00 };
+#endif //CONFIG_INTEL_WIDI
+
+	//	P2P OUI
+	p2pielen = 0;
+	p2pie[ p2pielen++ ] = 0x50;
+	p2pie[ p2pielen++ ] = 0x6F;
+	p2pie[ p2pielen++ ] = 0x9A;
+	p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+	//	Commented by Albert 20100907
+	//	According to the P2P Specification, the probe response frame should contain 5 P2P attributes
+	//	1. P2P Capability
+	//	2. Extended Listen Timing
+	//	3. Notice of Absence ( NOA )	( Only GO needs this )
+	//	4. Device Info
+	//	5. Group Info	( Only GO need this )
+
+	//	P2P Capability ATTR
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
+
+	//	Length:
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
+	RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
+	p2pielen += 2;
+
+	//	Value:
+	//	Device Capability Bitmap, 1 byte
+	p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
+	
+	//	Group Capability Bitmap, 1 byte
+	if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+	{
+		p2pie[ p2pielen ] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS);
+
+		if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
+			p2pie[ p2pielen ] |= P2P_GRPCAP_GROUP_FORMATION;
+
+		p2pielen++;
+	}
+	else if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) )
+	{
+		//	Group Capability Bitmap, 1 byte
+		if ( pwdinfo->persistent_supported )
+			p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+		else
+			p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
+	}
+
+	//	Extended Listen Timing ATTR
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
+
+	//	Length:
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
+	RTW_PUT_LE16(p2pie + p2pielen, 0x0004);
+	p2pielen += 2;
+
+	//	Value:
+	//	Availability Period
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
+	RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
+	p2pielen += 2;
+
+	//	Availability Interval
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
+	RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
+	p2pielen += 2;
+
+
+	// Notice of Absence ATTR
+	//	Type: 
+	//	Length:
+	//	Value:
+	if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+	{
+		//go_add_noa_attr(pwdinfo);
+	}	
+
+	//	Device Info ATTR
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) 
+	//	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
+#ifdef CONFIG_INTEL_WIDI
+	if( _rtw_memcmp( pwdinfo->padapter->mlmepriv.sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN ) == _FALSE )
+	{
+		RTW_PUT_LE16(p2pie + p2pielen, 21 + 8 + pwdinfo->device_name_len);
+	}
+	else
+#endif //CONFIG_INTEL_WIDI
+	RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	//	Value:
+	//	P2P Device Address
+	_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );
+	p2pielen += ETH_ALEN;
+
+	//	Config Method
+	//	This field should be big endian. Noted by P2P specification.
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
+	RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->supported_wps_cm);
+	p2pielen += 2;
+
+	//	Primary Device Type
+	//	Category ID
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	//	OUI
+	//*(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
+	RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
+	p2pielen += 4;
+
+	//	Sub Category ID
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER);
+	p2pielen += 2;
+
+	//	Number of Secondary Device Types
+#ifdef CONFIG_INTEL_WIDI
+	if( _rtw_memcmp( pwdinfo->padapter->mlmepriv.sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN ) == _FALSE )
+	{
+		p2pie[ p2pielen++ ] = 0x01;
+		
+		RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_DISPLAYS);
+		p2pielen += 2;
+
+		RTW_PUT_BE32(p2pie + p2pielen, INTEL_DEV_TYPE_OUI);
+		p2pielen += 4;
+
+		RTW_PUT_BE16(p2pie + p2pielen, P2P_SCID_WIDI_CONSUMER_SINK);
+		p2pielen += 2;
+	}
+	else
+#endif //CONFIG_INTEL_WIDI
+	p2pie[ p2pielen++ ] = 0x00;	//	No Secondary Device Type List
+
+	//	Device Name
+	//	Type:
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	//	Length:
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
+	RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	//	Value:
+	_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
+	p2pielen += pwdinfo->device_name_len;
+
+	// Group Info ATTR
+	//	Type:
+	//	Length:
+	//	Value:
+	if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+	{
+		p2pielen += go_add_group_info_attr(pwdinfo, p2pie + p2pielen);
+	}
+
+	
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
+	
+
+	return len;
+	
+}
+
+u32 build_prov_disc_request_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8* pssid, u8 ussidlen, u8* pdev_raddr )
+{
+	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
+	u32 len=0, p2pielen = 0;	
+
+	//	P2P OUI
+	p2pielen = 0;
+	p2pie[ p2pielen++ ] = 0x50;
+	p2pie[ p2pielen++ ] = 0x6F;
+	p2pie[ p2pielen++ ] = 0x9A;
+	p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+	//	Commented by Albert 20110301
+	//	According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes
+	//	1. P2P Capability
+	//	2. Device Info
+	//	3. Group ID ( When joining an operating P2P Group )
+
+	//	P2P Capability ATTR
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
+
+	//	Length:
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
+	RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
+	p2pielen += 2;
+
+	//	Value:
+	//	Device Capability Bitmap, 1 byte
+	p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
+	
+	//	Group Capability Bitmap, 1 byte
+	if ( pwdinfo->persistent_supported )
+		p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+	else
+		p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
+
+
+	//	Device Info ATTR
+	//	Type:
+	p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
+
+	//	Length:
+	//	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) 
+	//	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
+	RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	//	Value:
+	//	P2P Device Address
+	_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );
+	p2pielen += ETH_ALEN;
+
+	//	Config Method
+	//	This field should be big endian. Noted by P2P specification.
+	if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC )
+	{
+		//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_PBC );
+		RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_PBC);
+	}
+	else
+	{
+		//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
+		RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_DISPLAY);
+	}
+
+	p2pielen += 2;
+
+	//	Primary Device Type
+	//	Category ID
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	//	OUI
+	//*(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
+	RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
+	p2pielen += 4;
+
+	//	Sub Category ID
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER);
+	p2pielen += 2;
+
+	//	Number of Secondary Device Types
+	p2pie[ p2pielen++ ] = 0x00;	//	No Secondary Device Type List
+
+	//	Device Name
+	//	Type:
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	//	Length:
+	//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
+	RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	//	Value:
+	_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
+	p2pielen += pwdinfo->device_name_len;
+
+	if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
+	{
+		//	Added by Albert 2011/05/19
+		//	In this case, the pdev_raddr is the device address of the group owner.
+		
+		//	P2P Group ID ATTR
+		//	Type:
+		p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
+
+		//	Length:
+		//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + ussidlen );
+		RTW_PUT_LE16(p2pie + p2pielen, ETH_ALEN + ussidlen);
+		p2pielen += 2;
+
+		//	Value:
+		_rtw_memcpy( p2pie + p2pielen, pdev_raddr, ETH_ALEN );
+		p2pielen += ETH_ALEN;
+
+		_rtw_memcpy( p2pie + p2pielen, pssid, ussidlen );
+		p2pielen += ussidlen;
+		
+	}
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
+	
+
+	return len;
+	
+}
+
+
+u32 build_assoc_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 status_code)
+{
+	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
+	u32 len=0, p2pielen = 0;	
+
+	//	P2P OUI
+	p2pielen = 0;
+	p2pie[ p2pielen++ ] = 0x50;
+	p2pie[ p2pielen++ ] = 0x6F;
+	p2pie[ p2pielen++ ] = 0x9A;
+	p2pie[ p2pielen++ ] = 0x09;	//	WFA P2P v1.0
+
+	// According to the P2P Specification, the Association response frame should contain 2 P2P attributes
+	//	1. Status
+	//	2. Extended Listen Timing (optional)
+
+
+	//	Status ATTR
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status_code);
+
+
+	// Extended Listen Timing ATTR
+	//	Type:
+	//	Length:
+	//	Value:
+
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
+	
+	return len;
+	
+}
+
+u32 build_deauth_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u32 len=0;
+	
+	return len;
+}
+
+u32 process_probe_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{	
+	u8 *p;
+	u32 ret=_FALSE;
+	u8 *p2pie;
+	u32	p2pielen = 0;
+	int ssid_len=0, rate_cnt = 0;
+
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt,
+			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+	if ( rate_cnt <= 4 )
+	{
+		int i, g_rate =0;
+
+		for( i = 0; i < rate_cnt; i++ )
+		{
+			if ( ( ( *( p + 2 + i ) & 0xff ) != 0x02 ) &&
+				( ( *( p + 2 + i ) & 0xff ) != 0x04 ) &&
+				( ( *( p + 2 + i ) & 0xff ) != 0x0B ) &&
+				( ( *( p + 2 + i ) & 0xff ) != 0x16 ) )
+			{
+				g_rate = 1;
+			}
+		}
+
+		if ( g_rate == 0 )
+		{
+			//	There is no OFDM rate included in SupportedRates IE of this probe request frame
+			//	The driver should response this probe request.
+			return ret;
+		}
+	}
+	else
+	{
+		//	rate_cnt > 4 means the SupportRates IE contains the OFDM rate because the count of CCK rates are 4.
+		//	We should proceed the following check for this probe request.
+	}
+
+	//	Added comments by Albert 20100906
+	//	There are several items we should check here.
+	//	1. This probe request frame must contain the P2P IE. (Done)
+	//	2. This probe request frame must contain the wildcard SSID. (Done)
+	//	3. Wildcard BSSID. (Todo)
+	//	4. Destination Address. ( Done in mgt_dispatcher function )
+	//	5. Requested Device Type in WSC IE. (Todo)
+	//	6. Device ID attribute in P2P IE. (Todo)
+
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len,
+			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);	
+
+	ssid_len &= 0xff;	//	Just last 1 byte is valid for ssid len of the probe request
+	if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+	{
+		if((p2pie=rtw_get_p2p_ie( pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_ , len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_ , NULL, &p2pielen)))
+		{
+			if ( (p != NULL) && _rtw_memcmp( ( void * ) ( p+2 ), ( void * ) pwdinfo->p2p_wildcard_ssid , 7 ))
+			{
+				//todo:
+				//Check Requested Device Type attributes in WSC IE.
+				//Check Device ID attribute in P2P IE
+
+				ret = _TRUE;			
+			}
+			else if ( (p != NULL) && ( ssid_len == 0 ) )
+			{
+				ret = _TRUE;
+			}
+		}
+		else
+		{
+			//non -p2p device
+		}
+		
+	}	
+
+	
+	return ret;
+	
+}
+
+u32 process_assoc_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len, struct sta_info *psta)
+{
+	u8 status_code = P2P_STATUS_SUCCESS;
+	u8 *pbuf, *pattr_content=NULL;
+	u32 attr_contentlen = 0;
+	u16 cap_attr=0;
+	unsigned short	frame_type, ie_offset=0;
+	u8 * ies;
+	u32 ies_len;
+	u8 * p2p_ie;
+	u32	p2p_ielen = 0;
+
+	if(!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+		return P2P_STATUS_FAIL_REQUEST_UNABLE;
+
+	frame_type = GetFrameSubType(pframe);
+	if (frame_type == WIFI_ASSOCREQ)
+	{
+		ie_offset = _ASOCREQ_IE_OFFSET_;
+	}	
+	else // WIFI_REASSOCREQ
+	{
+		ie_offset = _REASOCREQ_IE_OFFSET_;
+	}
+	
+	ies = pframe + WLAN_HDR_A3_LEN + ie_offset;
+	ies_len = len - WLAN_HDR_A3_LEN - ie_offset;
+
+	p2p_ie = rtw_get_p2p_ie(ies , ies_len , NULL, &p2p_ielen);
+
+	if ( !p2p_ie )
+	{
+		DBG_8192C( "[%s] P2P IE not Found!!\n", __FUNCTION__ );
+		status_code =  P2P_STATUS_FAIL_INVALID_PARAM;
+	}
+	else
+	{
+		DBG_8192C( "[%s] P2P IE Found!!\n", __FUNCTION__ );
+	}
+	
+	while ( p2p_ie )
+	{
+		//Check P2P Capability ATTR
+		if( rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*) &attr_contentlen) )
+		{
+			DBG_8192C( "[%s] Got P2P Capability Attr!!\n", __FUNCTION__ );
+			cap_attr = le16_to_cpu(cap_attr);
+			psta->dev_cap = cap_attr&0xff;
+		}
+
+		//Check Extended Listen Timing ATTR
+	
+
+		//Check P2P Device Info ATTR
+		if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint*)&attr_contentlen))
+		{
+			DBG_8192C( "[%s] Got P2P DEVICE INFO Attr!!\n", __FUNCTION__ );		
+			pattr_content = pbuf = rtw_zmalloc(attr_contentlen);
+			if(pattr_content)
+			{
+				u8 num_of_secdev_type;
+				u16 dev_name_len;
+			
+		
+				rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO , pattr_content, (uint*)&attr_contentlen);
+
+				_rtw_memcpy(psta->dev_addr, 	pattr_content, ETH_ALEN);//P2P Device Address
+
+				pattr_content += ETH_ALEN;
+
+				_rtw_memcpy(&psta->config_methods, pattr_content, 2);//Config Methods
+				psta->config_methods = be16_to_cpu(psta->config_methods);
+
+				pattr_content += 2;
+
+				_rtw_memcpy(psta->primary_dev_type, pattr_content, 8);
+
+				pattr_content += 8;
+
+				num_of_secdev_type = *pattr_content;
+				pattr_content += 1;
+
+				if(num_of_secdev_type==0)
+				{
+					psta->num_of_secdev_type = 0;
+				}
+				else
+				{
+					u32 len;
+
+					psta->num_of_secdev_type = num_of_secdev_type;
+
+					len = (sizeof(psta->secdev_types_list)<(num_of_secdev_type*8)) ? (sizeof(psta->secdev_types_list)) : (num_of_secdev_type*8);
+
+					_rtw_memcpy(psta->secdev_types_list, pattr_content, len);
+	
+					pattr_content += (num_of_secdev_type*8);
+				}
+
+
+				//dev_name_len = attr_contentlen - ETH_ALEN - 2 - 8 - 1 - (num_of_secdev_type*8);
+				psta->dev_name_len=0;
+				if(WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(u16*)pattr_content))
+				{				
+					dev_name_len = be16_to_cpu(*(u16*)(pattr_content+2));
+
+					psta->dev_name_len = (sizeof(psta->dev_name)<dev_name_len) ? sizeof(psta->dev_name):dev_name_len;
+
+					_rtw_memcpy(psta->dev_name, pattr_content+4, psta->dev_name_len);
+				}
+
+				rtw_mfree(pbuf, attr_contentlen);
+
+			}
+
+		}
+
+		//Get the next P2P IE
+		p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
+		
+	}
+
+	return status_code;
+
+}
+
+u32 process_p2p_devdisc_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	u8 *frame_body;
+	u8 status, dialogToken;
+	struct sta_info *psta = NULL;
+	_adapter *padapter = pwdinfo->padapter;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *p2p_ie;
+	u32	p2p_ielen = 0;	
+	
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	dialogToken = frame_body[7];
+	status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
+		
+	if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
+	{
+		u8 groupid[ 38 ] = { 0x00 };
+		u8 dev_addr[ETH_ALEN] = { 0x00 };		
+		u32	attr_contentlen = 0;
+
+		if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen))
+		{
+			if(_rtw_memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) && 
+				_rtw_memcmp(pwdinfo->p2p_group_ssid, groupid+ETH_ALEN, pwdinfo->p2p_group_ssid_len))
+			{
+				attr_contentlen=0;
+				if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_ID, dev_addr, &attr_contentlen))
+				{
+					_irqL irqL;
+					_list	*phead, *plist;					
+
+					_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+					phead = &pstapriv->asoc_list;
+					plist = get_next(phead);
+
+					//look up sta asoc_queue
+					while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)	
+					{		
+						psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+		
+						plist = get_next(plist);
+
+						if(psta->is_p2p_device && (psta->dev_cap&P2P_DEVCAP_CLIENT_DISCOVERABILITY) &&
+							_rtw_memcmp(psta->dev_addr, dev_addr, ETH_ALEN))
+						{
+
+							//_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+							//issue GO Discoverability Request
+							issue_group_disc_req(pwdinfo, psta->hwaddr);
+							//_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+							
+							status = P2P_STATUS_SUCCESS;
+							
+							break;
+						}
+						else
+						{
+							status = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+						}
+		
+					}				
+					_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+					
+				}
+				else
+				{
+					status = P2P_STATUS_FAIL_INVALID_PARAM;
+				}
+
+			}
+			else
+			{
+				status = P2P_STATUS_FAIL_INVALID_PARAM;
+			}
+			
+		}	
+		
+	}
+
+	
+	//issue Device Discoverability Response
+	issue_p2p_devdisc_resp(pwdinfo, GetAddr2Ptr(pframe), status, dialogToken);
+	
+	
+	return (status==P2P_STATUS_SUCCESS) ? _TRUE:_FALSE;
+	
+}
+
+u32 process_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	return _TRUE;
+}
+
+u8 process_p2p_provdisc_req(struct wifidirect_info *pwdinfo,  u8 *pframe, uint len )
+{
+	u8 *frame_body;
+	u8 *wpsie;
+	uint	wps_ielen = 0, attr_contentlen = 0;
+	u16	uconfig_method = 0;
+	
+
+	frame_body = (pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	if ( (wpsie=rtw_get_wps_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen)) )
+	{
+		if ( rtw_get_wps_attr_content( wpsie, wps_ielen, WPS_ATTR_CONF_METHOD , ( u8* ) &uconfig_method, &attr_contentlen) )
+		{
+			uconfig_method = be16_to_cpu( uconfig_method );
+			switch( uconfig_method )
+			{
+				case WPS_CM_DISPLYA:
+				{
+					_rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3 );
+					break;
+				}
+				case WPS_CM_LABEL:
+				{
+					_rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "lab", 3 );
+					break;
+				}
+				case WPS_CM_PUSH_BUTTON:
+				{
+					_rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3 );
+					break;
+				}
+				case WPS_CM_KEYPAD:
+				{
+					_rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3 );
+					break;
+				}
+			}
+			issue_p2p_provision_resp( pwdinfo, GetAddr2Ptr(pframe), frame_body, uconfig_method);
+		}
+	}
+	DBG_871X( "[%s] config method = %s\n", __FUNCTION__, pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req );
+	return _TRUE;
+	
+}
+	
+u8 process_p2p_provdisc_resp(struct wifidirect_info *pwdinfo,  u8 *pframe)
+{
+
+	return _TRUE;
+}
+
+u8 rtw_p2p_get_peer_ch_list(struct wifidirect_info *pwdinfo, u8 *ch_content, u8 ch_cnt, u8 *peer_ch_list)
+{
+	u8 i = 0, j = 0;
+	u8 temp = 0;
+	u8 ch_no = 0;
+	ch_content += 3;
+	ch_cnt -= 3;
+
+	while( ch_cnt > 0)
+	{
+		ch_content += 1;
+		ch_cnt -= 1;
+		temp = *ch_content;
+		for( i = 0 ; i < temp ; i++, j++ )
+		{
+			peer_ch_list[j] = *( ch_content + 1 + i );
+		}
+		ch_content += (temp + 1);
+		ch_cnt -= (temp + 1);
+		ch_no += temp ;
+	}
+
+	return ch_no;
+}
+
+u8 rtw_p2p_check_peer_oper_ch(struct mlme_ext_priv *pmlmeext, u8 ch)
+{
+	u8 i = 0;
+
+	for( i = 0; i < pmlmeext->max_chan_nums; i++ )
+	{
+		if ( pmlmeext->channel_set[ i ].ChannelNum == ch )
+		{
+			return _SUCCESS;
+		}
+	}
+
+	return _FAIL;
+}
+
+u8 rtw_p2p_ch_inclusion(struct mlme_ext_priv *pmlmeext, u8 *peer_ch_list, u8 peer_ch_num, u8 *ch_list_inclusioned)
+{
+	int	i = 0, j = 0, temp = 0;
+	u8 ch_no = 0;
+
+	for( i = 0; i < peer_ch_num; i++ )
+	{
+		for( j = temp; j < pmlmeext->max_chan_nums; j++ )
+		{
+			if( *( peer_ch_list + i ) == pmlmeext->channel_set[ j ].ChannelNum )
+			{
+				ch_list_inclusioned[ ch_no++ ] = *( peer_ch_list + i );
+				temp = j;
+				break;
+			}
+		}
+	}
+
+	return ch_no;
+}
+
+u8 process_p2p_group_negotation_req( struct wifidirect_info *pwdinfo, u8 *pframe, uint len )
+{
+	_adapter *padapter = pwdinfo->padapter;
+	u8	result = P2P_STATUS_SUCCESS;
+	u32	p2p_ielen = 0, wps_ielen = 0;
+	u8 * ies;
+	u32 ies_len;
+	u8 *p2p_ie;
+	u8 *wpsie;
+	u16		wps_devicepassword_id = 0x0000;
+	uint	wps_devicepassword_id_len = 0;
+#ifdef CONFIG_WFD
+	u8	wfd_ie[ 128 ] = { 0x00 };
+	u32	wfd_ielen = 0;
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+#endif // CONFIG_TDLS	
+#endif // CONFIG_WFD
+#ifdef CONFIG_CONCURRENT_MODE
+	_adapter				*pbuddy_adapter = pwdinfo->padapter->pbuddy_adapter;
+	struct wifidirect_info	*pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
+	struct mlme_priv		*pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
+	struct mlme_ext_priv	*pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+#endif
+
+	if ( (wpsie=rtw_get_wps_ie( pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen)) )
+	{
+		//	Commented by Kurt 20120113
+		//	If some device wants to do p2p handshake without sending prov_disc_req
+		//	We have to get peer_req_cm from here.
+		if(_rtw_memcmp( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3) )
+		{
+			rtw_get_wps_attr_content( wpsie, wps_ielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len);
+			wps_devicepassword_id = be16_to_cpu( wps_devicepassword_id );
+
+			if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
+			{
+				_rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3 );
+			}
+			else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
+			{
+				_rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3 );	
+			}
+			else
+			{
+				_rtw_memcpy( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3 );	
+			}
+		}
+	}
+	else
+	{
+		DBG_871X( "[%s] WPS IE not Found!!\n", __FUNCTION__ );
+		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+		return( result );
+	}
+
+	if ( pwdinfo->ui_got_wps_info == P2P_NO_WPSINFO )
+	{
+		result = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_INFOR_NOREADY);
+		return( result );
+	}
+
+	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+					
+	p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
+
+	if ( !p2p_ie )
+	{
+		DBG_871X( "[%s] P2P IE not Found!!\n", __FUNCTION__ );
+		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+	}
+	
+	while ( p2p_ie )
+	{
+		u8	attr_content = 0x00;
+		u32	attr_contentlen = 0;
+		u8	ch_content[50] = { 0x00 };
+		uint	ch_cnt = 0;
+		u8	peer_ch_list[50] = { 0x00 };
+		u8	peer_ch_num = 0;
+		u8	ch_list_inclusioned[50] = { 0x00 };
+		u8	ch_num_inclusioned = 0;
+		u16	cap_attr;
+
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING);
+
+		//Check P2P Capability ATTR
+		if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen) )
+		{
+			cap_attr = le16_to_cpu(cap_attr);
+	
+#if defined(CONFIG_WFD) && defined(CONFIG_TDLS)
+			if(!(cap_attr & P2P_GRPCAP_INTRABSS) )
+				ptdlsinfo->ap_prohibited = _TRUE;
+#endif //defined(CONFIG_WFD) && defined(CONFIG_TDLS)
+		}
+
+		if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen) )
+		{
+			DBG_871X( "[%s] GO Intent = %d, tie = %d\n", __FUNCTION__, attr_content >> 1, attr_content & 0x01 );
+			pwdinfo->peer_intent = attr_content;	//	include both intent and tie breaker values.
+
+			if ( pwdinfo->intent == ( pwdinfo->peer_intent >> 1 ) )
+			{
+				//	Try to match the tie breaker value
+				if ( pwdinfo->intent == P2P_MAX_INTENT )
+				{
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+					result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
+				}
+				else
+				{
+					if ( attr_content & 0x01 )
+					{
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+					}
+					else
+					{
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+					}
+				}							
+			}
+			else if ( pwdinfo->intent > ( pwdinfo->peer_intent >> 1 ) )
+			{
+				rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+			}
+			else
+			{
+				rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+			}
+
+			if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+			{
+				//	Store the group id information.
+				_rtw_memcpy( pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN );
+				_rtw_memcpy( pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );		
+			}
+		}
+
+		
+		attr_contentlen = 0;
+		if ( rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen ) )
+		{
+			if ( attr_contentlen != ETH_ALEN )
+			{
+				_rtw_memset( pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN );
+			}
+		}
+
+		if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, ch_content, &ch_cnt) )
+		{
+			peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, ch_content, ch_cnt, peer_ch_list);
+			ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
+
+			if( ch_num_inclusioned == 0)
+			{
+				DBG_871X( "[%s] No common channel in channel list!\n", __FUNCTION__ );
+				result = P2P_STATUS_FAIL_NO_COMMON_CH;
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+				break;
+			}
+
+			if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+			{
+				if ( !rtw_p2p_is_channel_list_ok( pwdinfo->operating_channel, 
+												ch_list_inclusioned, ch_num_inclusioned) )
+				{
+#ifdef CONFIG_CONCURRENT_MODE
+					if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+					{
+						DBG_871X( "[%s] desired channel NOT Found!\n", __FUNCTION__ );
+						result = P2P_STATUS_FAIL_NO_COMMON_CH;
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+						break;
+					}
+					else
+#endif //CONFIG_CONCURRENT_MODE
+					{
+						u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
+						attr_contentlen = 0;
+
+						if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen) )
+						{		
+							peer_operating_ch = operatingch_info[4];
+						}
+
+						if ( rtw_p2p_is_channel_list_ok( peer_operating_ch, 
+														ch_list_inclusioned, ch_num_inclusioned) )
+						{
+							/**
+							 *	Change our operating channel as peer's for compatibility.
+							 */
+							pwdinfo->operating_channel = peer_operating_ch;
+							DBG_871X( "[%s] Change op ch to %02x as peer's\n", __FUNCTION__, pwdinfo->operating_channel);
+						}
+						else
+						{
+							// Take first channel of ch_list_inclusioned as operating channel
+							pwdinfo->operating_channel = ch_list_inclusioned[0];
+							DBG_871X( "[%s] Change op ch to %02x\n", __FUNCTION__, pwdinfo->operating_channel);
+						}
+					}
+
+				}
+			}
+		}
+
+		//Get the next P2P IE
+		p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
+	}
+	
+#ifdef CONFIG_WFD
+	//	Added by Albert 20110823
+	//	Try to get the TCP port information when receiving the negotiation request.
+	if ( rtw_get_wfd_ie( pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen ) )
+	{
+		u8	attr_content[ 10 ] = { 0x00 };
+		u32	attr_contentlen = 0;
+
+		DBG_871X( "[%s] WFD IE Found!!\n", __FUNCTION__ );
+		rtw_get_wfd_attr_content( wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
+		if ( attr_contentlen )
+		{
+			pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16( attr_content + 2 );
+			DBG_871X( "[%s] Peer PORT NUM = %d\n", __FUNCTION__, pwdinfo->wfd_info->peer_rtsp_ctrlport );
+		}
+	}
+#endif // CONFIG_WFD
+
+	return( result );
+}
+
+u8 process_p2p_group_negotation_resp( struct wifidirect_info *pwdinfo, u8 *pframe, uint len )
+{
+	_adapter *padapter = pwdinfo->padapter;
+	u8	result = P2P_STATUS_SUCCESS;
+	u32	p2p_ielen, wps_ielen;
+	u8 * ies;
+	u32 ies_len;
+	u8 * p2p_ie;
+#ifdef CONFIG_WFD
+	u8	wfd_ie[ 128 ] = { 0x00 };
+	u32	wfd_ielen = 0;
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+#endif // CONFIG_TDLS	
+#endif // CONFIG_WFD
+
+	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+	//	Be able to know which one is the P2P GO and which one is P2P client.
+					
+	if ( rtw_get_wps_ie( ies, ies_len, NULL, &wps_ielen) )
+	{
+
+	}
+	else
+	{
+		DBG_871X( "[%s] WPS IE not Found!!\n", __FUNCTION__ );
+		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+	}
+
+	p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
+	if ( !p2p_ie )
+	{
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+	}
+	else
+	{
+
+		u8	attr_content = 0x00;
+		u32	attr_contentlen = 0;
+		u8	operatingch_info[5] = { 0x00 };
+		uint	ch_cnt = 0;
+		u8	ch_content[50] = { 0x00 };
+		u8	groupid[ 38 ];
+		u16	cap_attr;
+		u8	peer_ch_list[50] = { 0x00 };
+		u8	peer_ch_num = 0;
+		u8	ch_list_inclusioned[50] = { 0x00 };
+		u8	ch_num_inclusioned = 0;
+
+		while ( p2p_ie )	//	Found the P2P IE.
+		{
+
+			//Check P2P Capability ATTR
+			if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen) )
+			{
+				cap_attr = le16_to_cpu(cap_attr);
+#ifdef CONFIG_TDLS
+				if(!(cap_attr & P2P_GRPCAP_INTRABSS) )
+					ptdlsinfo->ap_prohibited = _TRUE;
+#endif // CONFIG_TDLS
+			}
+
+			rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+			if ( attr_contentlen == 1 )
+			{			
+				DBG_871X( "[%s] Status = %d\n", __FUNCTION__, attr_content );
+				if ( attr_content == P2P_STATUS_SUCCESS )
+				{
+					//	Do nothing.
+				}
+				else
+				{
+					if ( P2P_STATUS_FAIL_INFO_UNAVAILABLE == attr_content ) {
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INFOR_NOREADY);
+					} else {
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+					}
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+					result = attr_content;
+					break;
+				}
+			}
+
+			//	Try to get the peer's interface address
+			attr_contentlen = 0;
+			if ( rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen ) )
+			{
+				if ( attr_contentlen != ETH_ALEN )
+				{
+					_rtw_memset( pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN );
+				}
+			}
+			
+			//	Try to get the peer's intent and tie breaker value.
+			attr_content = 0x00;
+			attr_contentlen = 0;
+			if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen) )
+			{
+				DBG_871X( "[%s] GO Intent = %d, tie = %d\n", __FUNCTION__, attr_content >> 1, attr_content & 0x01 );
+				pwdinfo->peer_intent = attr_content;	//	include both intent and tie breaker values.
+
+				if ( pwdinfo->intent == ( pwdinfo->peer_intent >> 1 ) )
+				{
+					//	Try to match the tie breaker value
+					if ( pwdinfo->intent == P2P_MAX_INTENT )
+					{
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+						result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+					}
+					else
+					{
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+						rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+						if ( attr_content & 0x01 )
+						{
+							rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+						}
+						else
+						{
+							rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+						}
+					}							
+				}
+				else if ( pwdinfo->intent > ( pwdinfo->peer_intent >> 1 ) )
+				{
+					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+					rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+				}
+				else
+				{
+					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+					rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+				}
+
+				if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+				{
+					//	Store the group id information.
+					_rtw_memcpy( pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN );
+					_rtw_memcpy( pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
+		
+				}
+			}
+			
+			//	Try to get the operation channel information
+					
+			attr_contentlen = 0;
+			if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
+			{
+				DBG_871X( "[%s] Peer's operating channel = %d\n", __FUNCTION__, operatingch_info[4] );
+				pwdinfo->peer_operating_ch = operatingch_info[4];
+			}
+			
+			//	Try to get the channel list information
+			if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, pwdinfo->channel_list_attr, &pwdinfo->channel_list_attr_len ) )
+			{
+				DBG_871X( "[%s] channel list attribute found, len = %d\n", __FUNCTION__,  pwdinfo->channel_list_attr_len );
+
+				peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len, peer_ch_list);
+				ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
+
+				if( ch_num_inclusioned == 0)
+				{
+					DBG_871X( "[%s] No common channel in channel list!\n", __FUNCTION__ );
+					result = P2P_STATUS_FAIL_NO_COMMON_CH;
+					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+					break;
+				}
+
+				if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+				{
+					if ( !rtw_p2p_is_channel_list_ok( pwdinfo->operating_channel, 
+													ch_list_inclusioned, ch_num_inclusioned) )
+					{
+#ifdef CONFIG_CONCURRENT_MODE
+						if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+						{
+							DBG_871X( "[%s] desired channel NOT Found!\n", __FUNCTION__ );
+							result = P2P_STATUS_FAIL_NO_COMMON_CH;
+							rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+							break;
+						}
+						else
+#endif //CONFIG_CONCURRENT_MODE
+						{
+							u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
+							attr_contentlen = 0;
+
+							if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen) )
+							{		
+								peer_operating_ch = operatingch_info[4];
+							}
+
+							if ( rtw_p2p_is_channel_list_ok( peer_operating_ch, 
+															ch_list_inclusioned, ch_num_inclusioned) )
+							{
+								/**
+								 *	Change our operating channel as peer's for compatibility.
+								 */
+								pwdinfo->operating_channel = peer_operating_ch;
+								DBG_871X( "[%s] Change op ch to %02x as peer's\n", __FUNCTION__, pwdinfo->operating_channel);
+							}
+							else
+							{
+								// Take first channel of ch_list_inclusioned as operating channel
+								pwdinfo->operating_channel = ch_list_inclusioned[0];
+								DBG_871X( "[%s] Change op ch to %02x\n", __FUNCTION__, pwdinfo->operating_channel);
+							}
+						}
+
+					}
+				}
+
+			}
+			else
+			{
+				DBG_871X( "[%s] channel list attribute not found!\n", __FUNCTION__);
+			}
+
+			//	Try to get the group id information if peer is GO
+			attr_contentlen = 0;
+			_rtw_memset( groupid, 0x00, 38 );
+			if ( rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen) )
+			{
+				_rtw_memcpy( pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN );
+				_rtw_memcpy( pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN );
+			}
+			
+			//Get the next P2P IE
+			p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
+		}
+		
+	}
+
+#ifdef CONFIG_WFD
+	//	Added by Albert 20111122
+	//	Try to get the TCP port information when receiving the negotiation response.
+	if ( rtw_get_wfd_ie( pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen ) )
+	{
+		u8	attr_content[ 10 ] = { 0x00 };
+		u32	attr_contentlen = 0;
+
+		DBG_8192C( "[%s] WFD IE Found!!\n", __FUNCTION__ );
+		rtw_get_wfd_attr_content( wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
+		if ( attr_contentlen )
+		{
+			pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16( attr_content + 2 );
+			DBG_8192C( "[%s] Peer PORT NUM = %d\n", __FUNCTION__, pwdinfo->wfd_info->peer_rtsp_ctrlport );
+		}
+	}
+#endif // CONFIG_WFD
+
+	return( result );
+
+}
+
+u8 process_p2p_group_negotation_confirm( struct wifidirect_info *pwdinfo, u8 *pframe, uint len )
+{
+	u8 * ies;
+	u32 ies_len;
+	u8 * p2p_ie;
+	u32	p2p_ielen = 0;
+	u8	result = P2P_STATUS_SUCCESS;
+	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+	p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
+	while ( p2p_ie )	//	Found the P2P IE.
+	{
+		u8	attr_content = 0x00, operatingch_info[5] = { 0x00 };
+		u8	groupid[ 38 ] = { 0x00 };
+		u32	attr_contentlen = 0;
+
+		pwdinfo->negotiation_dialog_token = 1;
+		rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+		if ( attr_contentlen == 1 )
+		{
+			DBG_871X( "[%s] Status = %d\n", __FUNCTION__, attr_content );
+			result = attr_content;
+
+			if ( attr_content == P2P_STATUS_SUCCESS )
+			{
+				u8	bcancelled = 0;
+																
+				_cancel_timer( &pwdinfo->restore_p2p_state_timer, &bcancelled );
+
+				//	Commented by Albert 20100911
+				//	Todo: Need to handle the case which both Intents are the same.
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+				rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+				if ( ( pwdinfo->intent ) > ( pwdinfo->peer_intent >> 1 ) )
+				{
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+				}
+				else if ( ( pwdinfo->intent ) < ( pwdinfo->peer_intent >> 1 ) )
+				{
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+				}
+				else
+				{
+					//	Have to compare the Tie Breaker
+					if ( pwdinfo->peer_intent & 0x01 )
+					{
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+					}
+					else
+					{
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+					}
+				}								
+				
+#ifdef CONFIG_CONCURRENT_MODE
+				if ( check_buddy_fwstate(pwdinfo->padapter , _FW_LINKED ) )
+				{
+					//	Switch back to the AP channel soon.
+					_set_timer( &pwdinfo->ap_p2p_switch_timer, 100 );
+				}
+#endif				
+			}
+			else
+			{
+				rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+				break;
+			}
+		}
+
+		//	Try to get the group id information
+		attr_contentlen = 0;
+		_rtw_memset( groupid, 0x00, 38 );
+		if ( rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen) )
+		{
+			DBG_871X( "[%s] Ssid = %s, ssidlen = %zu\n", __FUNCTION__, &groupid[ETH_ALEN], strlen(&groupid[ETH_ALEN]) );
+			_rtw_memcpy( pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN );
+			_rtw_memcpy( pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN );			
+		}
+
+		attr_contentlen = 0;
+		if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen) )
+		{		
+			DBG_871X( "[%s] Peer's operating channel = %d\n", __FUNCTION__, operatingch_info[4] );
+			pwdinfo->peer_operating_ch = operatingch_info[4];
+		}
+
+		//Get the next P2P IE
+		p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
+		
+	}
+
+	return( result );
+}
+
+u8 process_p2p_presence_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	u8 *frame_body;	
+	u8 dialogToken=0;
+	u8 status = P2P_STATUS_SUCCESS;
+	
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	dialogToken = frame_body[6];
+
+	//todo: check NoA attribute
+
+	issue_p2p_presence_resp(pwdinfo, GetAddr2Ptr(pframe), status, dialogToken);
+
+	return _TRUE;
+}
+
+void find_phase_handler( _adapter*	padapter )
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	NDIS_802_11_SSID 	ssid;
+	_irqL				irqL;
+	u8					_status = 0;
+
+_func_enter_;
+
+	_rtw_memset((unsigned char*)&ssid, 0, sizeof(NDIS_802_11_SSID));
+	_rtw_memcpy(ssid.Ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN );
+	ssid.SsidLength = P2P_WILDCARD_SSID_LEN;
+
+	rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
+		
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+	_status = rtw_sitesurvey_cmd(padapter, &ssid, 1, NULL, 0);
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+
+_func_exit_;
+}
+
+void p2p_concurrent_handler(  _adapter* padapter );
+
+void restore_p2p_state_handler( _adapter*	padapter )
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+
+_func_enter_;
+
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
+	{
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+	}
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+	{
+		_adapter				*pbuddy_adapter = padapter->pbuddy_adapter;
+		struct mlme_priv		*pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
+		struct mlme_ext_priv	*pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;	
+	
+		if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP))
+		{
+			set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
+			
+			issue_nulldata(pbuddy_adapter, NULL, 0, 3, 500);
+		}
+	}
+#endif
+
+	rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+	
+	if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE))
+	{
+#ifdef CONFIG_CONCURRENT_MODE
+		p2p_concurrent_handler( padapter );
+#else
+		//	In the P2P client mode, the driver should not switch back to its listen channel
+		//	because this P2P client should stay at the operating channel of P2P GO.
+		set_channel_bwmode( padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+#endif
+	}
+_func_exit_;
+}
+
+void pre_tx_invitereq_handler( _adapter*	padapter )
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	u8	val8 = 1;
+_func_enter_;
+
+	set_channel_bwmode(padapter, pwdinfo->invitereq_info.peer_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+	padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));	
+	issue_probereq_p2p(padapter, NULL);
+	_set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
+	
+_func_exit_;
+}
+
+void pre_tx_provdisc_handler( _adapter*	padapter )
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	u8	val8 = 1;
+_func_enter_;
+
+	set_channel_bwmode(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+	issue_probereq_p2p(padapter, NULL);
+	_set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
+	
+_func_exit_;
+}
+
+void pre_tx_negoreq_handler( _adapter*	padapter )
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	u8	val8 = 1;
+_func_enter_;
+
+	set_channel_bwmode(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));	
+	issue_probereq_p2p(padapter, NULL);
+	_set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
+	
+_func_exit_;
+}
+
+#ifdef CONFIG_CONCURRENT_MODE
+void p2p_concurrent_handler( _adapter*	padapter )
+{
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	//_adapter				*pbuddy_adapter = padapter->pbuddy_adapter;
+	//struct wifidirect_info	*pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
+	//struct mlme_priv		*pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
+	//struct mlme_ext_priv	*pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+	u8					val8;
+_func_enter_;
+
+	if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+	{
+		PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;			
+		struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+
+		pwdinfo->operating_channel = pbuddy_mlmeext->cur_channel;
+#ifdef CONFIG_IOCTL_CFG80211	
+
+		DBG_871X("%s, switch ch back to buddy's cur_channel=%d\n", __func__, pbuddy_mlmeext->cur_channel);
+
+		set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
+		
+		issue_nulldata(pbuddy_adapter, NULL, 0, 3, 500);
+
+#else //CONFIG_IOCTL_CFG80211
+		if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
+		{
+			//	Now, the driver stays on the AP's channel.
+			//	If the pwdinfo->ext_listen_period = 0, that means the P2P listen state is not available on listen channel.
+			if ( pwdinfo->ext_listen_period > 0 )
+			{
+				DBG_8192C( "[%s] P2P_STATE_IDLE, ext_listen_period = %d\n", __FUNCTION__, pwdinfo->ext_listen_period );
+
+				if ( pbuddy_mlmeext->cur_channel != pwdinfo->listen_channel )
+				{
+					//	Will switch to listen channel so that need to send the NULL data with PW bit to AP.
+					issue_nulldata(pbuddy_adapter, NULL, 1, 3, 500);
+					set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+				}
+
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
+				val8 = 1;
+				rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+				//	Todo: To check the value of pwdinfo->ext_listen_period is equal to 0 or not.
+				_set_timer( &pwdinfo->ap_p2p_switch_timer, pwdinfo->ext_listen_period );
+			}
+		}
+		else if ( rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN) || 
+				rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL) ||
+				( rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) && pwdinfo->nego_req_info.benable == _FALSE ) ||
+				rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ) )
+		{
+			//	Now, the driver is in the listen state of P2P mode.
+			DBG_8192C( "[%s] P2P_STATE_IDLE, ext_listen_interval = %d\n", __FUNCTION__, pwdinfo->ext_listen_interval );
+
+			//	Commented by Albert 2012/11/01
+			//	If the AP's channel is the same as the listen channel, we should still be in the listen state
+			//	Other P2P device is still able to find this device out even this device is in the AP's channel.
+			//	So, configure this device to be able to receive the probe request frame and set it to listen state.
+			if ( pbuddy_mlmeext->cur_channel != pwdinfo->listen_channel )
+			{
+				set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
+				val8 = 0;
+				padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_IDLE);
+				issue_nulldata(pbuddy_adapter, NULL, 0, 3, 500);
+			}
+
+			//	Todo: To check the value of pwdinfo->ext_listen_interval is equal to 0 or not.
+			_set_timer( &pwdinfo->ap_p2p_switch_timer, pwdinfo->ext_listen_interval );
+		}
+		else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_OK))
+		{
+			//	The driver had finished the P2P handshake successfully.
+			val8 = 0;
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+			set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
+			issue_nulldata(pbuddy_adapter, NULL, 0, 3, 500);
+		}
+		else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
+		{
+			val8 = 1;
+			set_channel_bwmode(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+			issue_probereq_p2p(padapter, NULL);
+			_set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
+		}
+		else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) && pwdinfo->nego_req_info.benable == _TRUE)
+		{
+			val8 = 1;
+			set_channel_bwmode(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+			issue_probereq_p2p(padapter, NULL);
+			_set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
+		}		
+		else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ ) && pwdinfo->invitereq_info.benable == _TRUE)
+		{
+			/*
+			val8 = 1;
+			set_channel_bwmode(padapter, , HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+			issue_probereq_p2p(padapter, NULL);
+			_set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
+			*/
+		}
+#endif //CONFIG_IOCTL_CFG80211			
+	}	
+	else
+	{
+		set_channel_bwmode( padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);	
+	}
+
+_func_exit_;
+}
+#endif
+
+#ifdef CONFIG_IOCTL_CFG80211
+static void ro_ch_handler(_adapter *padapter)
+{
+	struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+_func_enter_;
+
+	{
+
+#ifdef CONFIG_CONCURRENT_MODE
+		if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+		{
+			PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;			
+			struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+
+			DBG_871X("%s, switch ch back to buddy's cur_channel=%d\n", __func__, pbuddy_mlmeext->cur_channel);
+		
+			set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
+
+			pmlmeext->cur_channel = pbuddy_mlmeext->cur_channel;
+			
+		}else
+#endif //CONFIG_CONCURRENT_MODE
+		if( pcfg80211_wdinfo->restore_channel != pmlmeext->cur_channel )
+		{
+			if ( !check_fwstate(&padapter->mlmepriv, _FW_LINKED ) )
+				pmlmeext->cur_channel = pcfg80211_wdinfo->restore_channel;
+
+			set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+		}	
+		
+		rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+#ifdef CONFIG_DEBUG_CFG80211		
+		DBG_871X("%s, role=%d, p2p_state=%d\n", __func__, rtw_p2p_role(pwdinfo), rtw_p2p_state(pwdinfo));
+#endif
+	}
+
+	pcfg80211_wdinfo->is_ro_ch = _FALSE;
+
+	DBG_871X("cfg80211_remain_on_channel_expired\n");
+
+	rtw_cfg80211_remain_on_channel_expired(padapter, 
+		pcfg80211_wdinfo->remain_on_ch_cookie, 
+		&pcfg80211_wdinfo->remain_on_ch_channel,
+		pcfg80211_wdinfo->remain_on_ch_type, GFP_KERNEL);
+
+_func_exit_;
+}
+
+static void ro_ch_timer_process (void *FunctionContext)
+{
+	_adapter *adapter = (_adapter *)FunctionContext;
+	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(adapter->rtw_wdev);
+
+	//printk("%s \n", __FUNCTION__);
+	
+#ifdef	CONFIG_CONCURRENT_MODE
+	ATOMIC_SET(&pwdev_priv->ro_ch_to, 1);
+#endif
+
+	p2p_protocol_wk_cmd( adapter, P2P_RO_CH_WK);
+}
+
+static void rtw_cfg80211_adjust_p2pie_channel(_adapter *padapter, const u8 *frame_body, u32 len)
+{
+#ifdef CONFIG_CONCURRENT_MODE
+	u8 *ies, *p2p_ie;
+	u32 ies_len, p2p_ielen;
+	PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;			
+	struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+
+	ies = (u8*)(frame_body + _PUBLIC_ACTION_IE_OFFSET_);
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+					
+	p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen );
+
+	while ( p2p_ie )
+	{	
+		u32	attr_contentlen = 0;
+		u8 *pattr = NULL;
+				
+		//Check P2P_ATTR_CH_LIST
+		if((pattr=rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, (uint*)&attr_contentlen))!=NULL)
+		{
+			int i;
+			u32 num_of_ch;
+			u8 *pattr_temp = pattr + 3 ;
+
+			attr_contentlen -= 3;
+			
+			while(attr_contentlen>0)
+			{				
+				num_of_ch = *(pattr_temp+1);
+								
+				for(i=0; i<num_of_ch; i++)
+					*(pattr_temp+2+i) = pbuddy_mlmeext->cur_channel;//forcing to the same channel
+
+				pattr_temp += (2+num_of_ch);
+				attr_contentlen -= (2+num_of_ch);
+			}	
+		}
+
+		//Check P2P_ATTR_OPERATING_CH
+		attr_contentlen = 0;
+		pattr = NULL;
+		if((pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, (uint*)&attr_contentlen))!=NULL)
+		{
+			*(pattr+4) = pbuddy_mlmeext->cur_channel;//forcing to the same channel			
+		}
+
+		//Get the next P2P IE
+		p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
+		
+	}
+
+#endif
+}
+
+#ifdef CONFIG_WFD
+void rtw_append_wfd_ie(_adapter *padapter, u8 *buf, u32* len)
+{
+	unsigned char	*frame_body;
+	u8 category, action, OUI_Subtype, dialogToken=0;
+	u32	wfdielen = 0;
+	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev);
+	
+	frame_body = (unsigned char *)(buf + sizeof(struct rtw_ieee80211_hdr_3addr));	
+	category = frame_body[0];
+
+	if(category == RTW_WLAN_CATEGORY_PUBLIC)
+	{
+		action = frame_body[1];
+		if (action == ACT_PUBLIC_VENDOR
+			&& _rtw_memcmp(frame_body+2, P2P_OUI, 4) == _TRUE
+		)
+		{
+			OUI_Subtype = frame_body[6];
+			dialogToken = frame_body[7];
+			switch( OUI_Subtype )//OUI Subtype
+			{
+				case P2P_GO_NEGO_REQ:
+				{
+					wfdielen = build_nego_req_wfd_ie( &padapter->wdinfo, buf + ( *len ) );
+					(*len) += wfdielen;
+					break;
+				}
+				case P2P_GO_NEGO_RESP:
+				{
+					wfdielen = build_nego_resp_wfd_ie( &padapter->wdinfo, buf + ( *len ) );
+					(*len) += wfdielen;
+					break;
+				}
+				case P2P_GO_NEGO_CONF:
+				{
+					wfdielen = build_nego_confirm_wfd_ie( &padapter->wdinfo, buf + ( *len ) );
+					(*len) += wfdielen;
+					break;
+				}
+				case P2P_INVIT_REQ:
+				{
+					wfdielen = build_invitation_req_wfd_ie( &padapter->wdinfo, buf + ( *len ) );
+					(*len) += wfdielen;
+					break;
+				}
+				case P2P_INVIT_RESP:
+				{
+					wfdielen = build_invitation_resp_wfd_ie( &padapter->wdinfo, buf + ( *len ) );
+					(*len) += wfdielen;
+					break;
+				}
+				case P2P_DEVDISC_REQ:
+					break;
+				case P2P_DEVDISC_RESP:
+
+					break;
+				case P2P_PROVISION_DISC_REQ:
+				{
+					wfdielen = build_provdisc_req_wfd_ie( &padapter->wdinfo, buf + ( *len ) );
+					(*len) += wfdielen;
+					break;
+				}
+				case P2P_PROVISION_DISC_RESP:
+				{
+					wfdielen = build_provdisc_resp_wfd_ie( &padapter->wdinfo, buf + ( *len ) );
+					(*len) += wfdielen;
+					break;
+				}
+				default:
+
+					break;	
+			}
+
+		}
+		
+	}	
+	else if(category == RTW_WLAN_CATEGORY_P2P)
+	{
+		OUI_Subtype = frame_body[5];
+		dialogToken = frame_body[6];
+
+#ifdef CONFIG_DEBUG_CFG80211
+		DBG_871X("ACTION_CATEGORY_P2P: OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n",
+					cpu_to_be32( *( ( u32* ) ( frame_body + 1 ) ) ), OUI_Subtype, dialogToken);
+#endif
+
+		switch(OUI_Subtype)
+		{
+			case P2P_NOTICE_OF_ABSENCE:
+
+				break;			
+			case P2P_PRESENCE_REQUEST:
+
+				break;			
+			case P2P_PRESENCE_RESPONSE:
+
+				break;			
+			case P2P_GO_DISC_REQUEST:
+
+				break;			
+			default:
+
+				break;			
+		}	
+
+	}	
+	else 
+	{
+		DBG_871X("%s, action frame category=%d\n", __func__, category);
+		//is_p2p_frame = (-1);		
+	}
+
+	return;
+}
+#endif
+
+int rtw_p2p_check_frames(_adapter *padapter, const u8 *buf, u32 len, u8 tx)
+{
+	int is_p2p_frame = (-1);
+	unsigned char	*frame_body;
+	u8 category, action, OUI_Subtype, dialogToken=0;
+	u8 *p2p_ie = NULL;
+	uint p2p_ielen = 0;
+	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev);
+	
+	frame_body = (unsigned char *)(buf + sizeof(struct rtw_ieee80211_hdr_3addr));	
+	category = frame_body[0];
+	//just for check
+	if(category == RTW_WLAN_CATEGORY_PUBLIC)
+	{
+		action = frame_body[1];
+		if (action == ACT_PUBLIC_VENDOR
+			&& _rtw_memcmp(frame_body+2, P2P_OUI, 4) == _TRUE
+		)
+		{
+			OUI_Subtype = frame_body[6];
+			dialogToken = frame_body[7];
+			is_p2p_frame = OUI_Subtype;
+			#ifdef CONFIG_DEBUG_CFG80211
+			DBG_871X("ACTION_CATEGORY_PUBLIC: ACT_PUBLIC_VENDOR, OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n",
+				cpu_to_be32( *( ( u32* ) ( frame_body + 2 ) ) ), OUI_Subtype, dialogToken);
+			#endif
+
+			p2p_ie = rtw_get_p2p_ie(
+				(u8 *)buf+sizeof(struct rtw_ieee80211_hdr_3addr)+_PUBLIC_ACTION_IE_OFFSET_,
+				len-sizeof(struct rtw_ieee80211_hdr_3addr)-_PUBLIC_ACTION_IE_OFFSET_,
+				NULL, &p2p_ielen);
+
+			switch( OUI_Subtype )//OUI Subtype
+			{
+				u8 *cont;
+				uint cont_len;
+				case P2P_GO_NEGO_REQ:
+					DBG_871X("RTW_%s:P2P_GO_NEGO_REQ, dialogToken=%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken);
+	
+					if(tx)
+					{
+#ifdef CONFIG_DRV_ISSUE_PROV_REQ // IOT FOR S2
+						if(pwdev_priv->provdisc_req_issued == _FALSE)
+							rtw_cfg80211_issue_p2p_provision_request(padapter, buf, len);
+#endif //CONFIG_DRV_ISSUE_PROV_REQ					
+
+						//pwdev_priv->provdisc_req_issued = _FALSE;
+						
+#ifdef CONFIG_CONCURRENT_MODE
+						if(check_buddy_fwstate(padapter, _FW_LINKED))
+							rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr));
+#endif
+					}
+						
+					break;
+				case P2P_GO_NEGO_RESP:
+					cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
+					DBG_871X("RTW_%s:P2P_GO_NEGO_RESP, dialogToken=%d, status:%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken, cont?*cont:-1);
+
+					if(!tx)
+					{
+						pwdev_priv->provdisc_req_issued = _FALSE;
+					}	
+#ifdef CONFIG_CONCURRENT_MODE			
+					else
+					{
+						if(check_buddy_fwstate(padapter, _FW_LINKED))
+							rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr));
+					}
+#endif
+					break;				
+				case P2P_GO_NEGO_CONF:
+					cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
+					DBG_871X("RTW_%s:P2P_GO_NEGO_CONF, dialogToken=%d, status:%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken, cont?*cont:-1);
+
+#ifdef CONFIG_CONCURRENT_MODE
+					if(tx)
+					{
+						if(check_buddy_fwstate(padapter, _FW_LINKED))
+							rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr));
+					}
+#endif			
+					break;				
+				case P2P_INVIT_REQ:
+				{
+					struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info;
+					int flags = -1;
+					int op_ch = 0;
+
+					if(tx)
+					{
+						#ifdef CONFIG_CONCURRENT_MODE
+						if(check_buddy_fwstate(padapter, _FW_LINKED))
+							rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr));
+						#endif
+					}
+
+					if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, NULL, &cont_len)))
+						flags = *cont;
+					if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
+						op_ch = *(cont+4);
+
+					if (invit_info->token != dialogToken)
+						rtw_wdev_invit_info_init(invit_info);
+
+					invit_info->token = dialogToken;
+					invit_info->flags = (flags==-1) ? 0x0 : flags;
+					invit_info->req_op_ch= op_ch;
+
+					DBG_871X("RTW_%s:P2P_INVIT_REQ, dialogToken=%d, flags:0x%02x, op_ch:%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken, flags, op_ch);
+					break;
+				}
+				case P2P_INVIT_RESP:
+				{
+					struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info;
+					int status = -1;
+					int op_ch = 0;
+
+					if(tx)
+					{
+						#ifdef CONFIG_CONCURRENT_MODE
+						if(check_buddy_fwstate(padapter, _FW_LINKED))
+							rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len-sizeof(struct rtw_ieee80211_hdr_3addr));
+						#endif
+					}
+
+					if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len)))
+						status = *cont;
+					if ((cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
+						op_ch = *(cont+4);
+
+					if (invit_info->token != dialogToken) {
+						rtw_wdev_invit_info_init(invit_info);
+					} else {
+						invit_info->token = 0;
+						invit_info->status = (status==-1) ? 0xff : status;
+						invit_info->rsp_op_ch= op_ch;
+					}
+
+					DBG_871X("RTW_%s:P2P_INVIT_RESP, dialogToken=%d, status:%d, op_ch:%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken, status, op_ch);
+					break;
+				}
+				case P2P_DEVDISC_REQ:
+					DBG_871X("RTW_%s:P2P_DEVDISC_REQ, dialogToken=%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken);
+					break;
+				case P2P_DEVDISC_RESP:
+					cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
+					DBG_871X("RTW_%s:P2P_DEVDISC_RESP, dialogToken=%d, status:%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken, cont?*cont:-1);
+					break;
+				case P2P_PROVISION_DISC_REQ:
+				{
+					size_t frame_body_len = len - sizeof(struct rtw_ieee80211_hdr_3addr);
+					u8 *p2p_ie;
+					uint p2p_ielen = 0;
+					uint contentlen = 0;
+					
+					DBG_871X("RTW_%s:P2P_PROVISION_DISC_REQ, dialogToken=%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken);
+
+					//if(tx)
+					{
+						pwdev_priv->provdisc_req_issued = _FALSE;
+					
+						if( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, frame_body_len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)))
+						{	
+
+							if(rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, NULL, &contentlen))
+							{
+								pwdev_priv->provdisc_req_issued = _FALSE;//case: p2p_client join p2p GO
+							}
+							else
+							{
+								#ifdef CONFIG_DEBUG_CFG80211
+								DBG_871X("provdisc_req_issued is _TRUE\n");
+								#endif //CONFIG_DEBUG_CFG80211
+								pwdev_priv->provdisc_req_issued = _TRUE;//case: p2p_devices connection before Nego req.
+							}
+												
+						}						
+					}				
+				}	
+					break;
+				case P2P_PROVISION_DISC_RESP:
+					DBG_871X("RTW_%s:P2P_PROVISION_DISC_RESP, dialogToken=%d\n", (tx==_TRUE)?"Tx":"Rx", dialogToken);
+					break;
+				default:
+					DBG_871X("RTW_%s:OUI_Subtype=%d, dialogToken=%d\n", (tx==_TRUE)?"Tx":"Rx", OUI_Subtype, dialogToken);
+					break;	
+			}
+
+		}
+		
+	}	
+	else if(category == RTW_WLAN_CATEGORY_P2P)
+	{
+		OUI_Subtype = frame_body[5];
+		dialogToken = frame_body[6];
+
+		#ifdef CONFIG_DEBUG_CFG80211
+		DBG_871X("ACTION_CATEGORY_P2P: OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n",
+			cpu_to_be32( *( ( u32* ) ( frame_body + 1 ) ) ), OUI_Subtype, dialogToken);
+		#endif
+
+		is_p2p_frame = OUI_Subtype;
+
+		switch(OUI_Subtype)
+		{
+			case P2P_NOTICE_OF_ABSENCE:
+				DBG_871X("RTW_%s:P2P_NOTICE_OF_ABSENCE, dialogToken=%d\n", (tx==_TRUE)?"TX":"RX", dialogToken);
+				break;			
+			case P2P_PRESENCE_REQUEST:
+				DBG_871X("RTW_%s:P2P_PRESENCE_REQUEST, dialogToken=%d\n", (tx==_TRUE)?"TX":"RX", dialogToken);
+				break;			
+			case P2P_PRESENCE_RESPONSE:
+				DBG_871X("RTW_%s:P2P_PRESENCE_RESPONSE, dialogToken=%d\n", (tx==_TRUE)?"TX":"RX", dialogToken);
+				break;			
+			case P2P_GO_DISC_REQUEST:
+				DBG_871X("RTW_%s:P2P_GO_DISC_REQUEST, dialogToken=%d\n", (tx==_TRUE)?"TX":"RX", dialogToken);
+				break;			
+			default:
+				DBG_871X("RTW_%s:OUI_Subtype=%d, dialogToken=%d\n", (tx==_TRUE)?"TX":"RX", OUI_Subtype, dialogToken);
+				break;			
+		}	
+
+	}	
+	else 
+	{
+		DBG_871X("RTW_%s:action frame category=%d\n", (tx==_TRUE)?"TX":"RX", category);
+		//is_p2p_frame = (-1);		
+	}
+
+	return is_p2p_frame;
+}
+
+void rtw_init_cfg80211_wifidirect_info( _adapter*	padapter)
+{
+	struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
+
+	_rtw_memset(pcfg80211_wdinfo, 0x00, sizeof(struct cfg80211_wifidirect_info) );
+	
+	_init_timer( &pcfg80211_wdinfo->remain_on_ch_timer, padapter->pnetdev, ro_ch_timer_process, padapter );
+}
+#endif //CONFIG_IOCTL_CFG80211	
+
+void p2p_protocol_wk_hdl(_adapter *padapter, int intCmdType)
+{
+	struct wifidirect_info	*pwdinfo= &(padapter->wdinfo);
+	
+_func_enter_;
+
+	switch(intCmdType)
+	{
+		case P2P_FIND_PHASE_WK:
+		{
+			find_phase_handler( padapter );
+			break;
+		}
+		case P2P_RESTORE_STATE_WK:
+		{
+			restore_p2p_state_handler( padapter );
+			break;
+		}
+		case P2P_PRE_TX_PROVDISC_PROCESS_WK:
+		{
+#ifdef CONFIG_CONCURRENT_MODE
+			if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+			{
+				p2p_concurrent_handler( padapter );
+			}
+			else
+			{
+				pre_tx_provdisc_handler( padapter );
+			}
+#else
+			pre_tx_provdisc_handler( padapter );
+#endif
+			break;
+		}
+		case P2P_PRE_TX_INVITEREQ_PROCESS_WK:
+		{
+#ifdef CONFIG_CONCURRENT_MODE
+			if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+			{
+				p2p_concurrent_handler( padapter );
+			}
+			else
+			{
+				pre_tx_invitereq_handler( padapter );
+			}
+#else			
+			pre_tx_invitereq_handler( padapter );
+#endif
+			break;
+		}
+		case P2P_PRE_TX_NEGOREQ_PROCESS_WK:
+		{
+#ifdef CONFIG_CONCURRENT_MODE
+			if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
+			{
+				p2p_concurrent_handler( padapter );
+			}
+			else
+			{
+				pre_tx_negoreq_handler( padapter );
+			}
+#else			
+			pre_tx_negoreq_handler( padapter );
+#endif
+			break;
+		}
+#ifdef CONFIG_P2P
+#ifdef CONFIG_CONCURRENT_MODE
+		case P2P_AP_P2P_CH_SWITCH_PROCESS_WK:
+		{
+			p2p_concurrent_handler( padapter );
+			break;
+		}		
+#endif
+#endif
+#ifdef CONFIG_IOCTL_CFG80211
+		case P2P_RO_CH_WK:
+		{
+			ro_ch_handler( padapter );
+			break;
+		}		
+#endif //CONFIG_IOCTL_CFG80211		
+
+	}
+	
+_func_exit_;
+}
+
+#ifdef CONFIG_P2P_PS
+void process_p2p_ps_ie(PADAPTER padapter, u8 *IEs, u32 IELength)
+{
+	u8 * ies;
+	u32 ies_len;
+	u8 * p2p_ie;
+	u32	p2p_ielen = 0;
+	u8	noa_attr[MAX_P2P_IE_LEN] = { 0x00 };// NoA length should be n*(13) + 2
+	u32	attr_contentlen = 0;
+	
+	struct wifidirect_info	*pwdinfo = &( padapter->wdinfo );
+	u8	find_p2p = _FALSE, find_p2p_ps = _FALSE;
+	u8	noa_offset, noa_num, noa_index;
+
+_func_enter_;
+
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+	{
+		return;
+	}
+#ifdef CONFIG_CONCURRENT_MODE
+	if(padapter->iface_type != IFACE_PORT0) 
+		return;
+#endif
+	if(IELength <= _BEACON_IE_OFFSET_)
+		return;
+	
+	ies = IEs + _BEACON_IE_OFFSET_;
+	ies_len = IELength - _BEACON_IE_OFFSET_;
+
+	p2p_ie = rtw_get_p2p_ie( ies, ies_len, NULL, &p2p_ielen);
+	
+	while(p2p_ie)
+	{
+		find_p2p = _TRUE;
+		// Get Notice of Absence IE.
+		if(rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_NOA, noa_attr, &attr_contentlen))
+		{
+			find_p2p_ps = _TRUE;
+			noa_index = noa_attr[0];
+
+			if( (pwdinfo->p2p_ps_mode == P2P_PS_NONE) ||
+				(noa_index != pwdinfo->noa_index) )// if index change, driver should reconfigure related setting.
+			{
+				pwdinfo->noa_index = noa_index;
+				pwdinfo->opp_ps = noa_attr[1] >> 7;
+				pwdinfo->ctwindow = noa_attr[1] & 0x7F;
+
+				noa_offset = 2;
+				noa_num = 0;
+				// NoA length should be n*(13) + 2
+				if(attr_contentlen > 2)
+				{
+					while(noa_offset < attr_contentlen)
+					{
+						//_rtw_memcpy(&wifidirect_info->noa_count[noa_num], &noa_attr[noa_offset], 1);
+						pwdinfo->noa_count[noa_num] = noa_attr[noa_offset];
+						noa_offset += 1;
+
+						_rtw_memcpy(&pwdinfo->noa_duration[noa_num], &noa_attr[noa_offset], 4);
+						noa_offset += 4;
+
+						_rtw_memcpy(&pwdinfo->noa_interval[noa_num], &noa_attr[noa_offset], 4);
+						noa_offset += 4;
+
+						_rtw_memcpy(&pwdinfo->noa_start_time[noa_num], &noa_attr[noa_offset], 4);
+						noa_offset += 4;
+
+						noa_num++;
+					}
+				}
+				pwdinfo->noa_num = noa_num;
+
+				if( pwdinfo->opp_ps == 1 )
+				{
+					pwdinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
+					// driver should wait LPS for entering CTWindow
+					if(padapter->pwrctrlpriv.bFwCurrentInPSMode == _TRUE)
+					{
+						p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1);
+					}
+				}
+				else if( pwdinfo->noa_num > 0 )
+				{
+					pwdinfo->p2p_ps_mode = P2P_PS_NOA;
+					p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 1);
+				}
+				else if( pwdinfo->p2p_ps_mode > P2P_PS_NONE)
+				{
+					p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+				}
+			}
+
+			break; // find target, just break.
+		}
+
+		//Get the next P2P IE
+		p2p_ie = rtw_get_p2p_ie(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
+
+	}
+
+	if(find_p2p == _TRUE)
+	{
+		if( (pwdinfo->p2p_ps_mode > P2P_PS_NONE) && (find_p2p_ps == _FALSE) )
+		{
+			p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+		}
+	}
+
+_func_exit_;
+}
+
+void p2p_ps_wk_hdl(_adapter *padapter, u8 p2p_ps_state)
+{
+	struct pwrctrl_priv		*pwrpriv = &padapter->pwrctrlpriv;
+	struct wifidirect_info	*pwdinfo= &(padapter->wdinfo);
+	
+_func_enter_;
+
+	// Pre action for p2p state
+	switch(p2p_ps_state)
+	{
+		case P2P_PS_DISABLE:
+			pwdinfo->p2p_ps_state = p2p_ps_state;
+			
+			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
+
+			pwdinfo->noa_index = 0;
+			pwdinfo->ctwindow = 0;
+			pwdinfo->opp_ps = 0;
+			pwdinfo->noa_num = 0;
+			pwdinfo->p2p_ps_mode = P2P_PS_NONE;
+			if(padapter->pwrctrlpriv.bFwCurrentInPSMode == _TRUE)
+			{
+				if(pwrpriv->smart_ps == 0)
+				{
+					pwrpriv->smart_ps = 2;
+					rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&(padapter->pwrctrlpriv.pwr_mode)));
+				}
+			}
+			break;
+		case P2P_PS_ENABLE:
+			if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
+				pwdinfo->p2p_ps_state = p2p_ps_state;
+				
+				if( pwdinfo->ctwindow > 0 )
+				{
+					if(pwrpriv->smart_ps != 0)
+					{
+						pwrpriv->smart_ps = 0;
+						DBG_871X("%s(): Enter CTW, change SmartPS\n", __FUNCTION__);
+						rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&(padapter->pwrctrlpriv.pwr_mode)));
+					}
+				}
+				rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
+			}
+			break;
+		case P2P_PS_SCAN:
+		case P2P_PS_SCAN_DONE:
+		case P2P_PS_ALLSTASLEEP:
+			if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
+				pwdinfo->p2p_ps_state = p2p_ps_state;
+				rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
+			}
+			break;
+		default:
+			break;
+	}
+
+_func_exit_;
+}
+
+u8 p2p_ps_wk_cmd(_adapter*padapter, u8 p2p_ps_state, u8 enqueue)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct wifidirect_info	*pwdinfo= &(padapter->wdinfo);
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+	
+_func_enter_;
+
+	if ( rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) 
+#ifdef CONFIG_CONCURRENT_MODE
+		|| (padapter->iface_type != IFACE_PORT0) 
+#endif
+		)
+	{
+		return res;
+	}
+
+	if(enqueue)
+	{
+		ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));	
+		if(ph2c==NULL){
+			res= _FAIL;
+			goto exit;
+		}
+		
+		pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); 
+		if(pdrvextra_cmd_parm==NULL){
+			rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+			res= _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm->ec_id = P2P_PS_WK_CID;
+		pdrvextra_cmd_parm->type_size = p2p_ps_state;
+		pdrvextra_cmd_parm->pbuf = NULL;
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	}
+	else
+	{
+		p2p_ps_wk_hdl(padapter, p2p_ps_state);
+	}
+	
+exit:
+	
+_func_exit_;
+
+	return res;
+
+}
+#endif // CONFIG_P2P_PS
+
+static void reset_ch_sitesurvey_timer_process (void *FunctionContext)
+{
+	_adapter *adapter = (_adapter *)FunctionContext;
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	DBG_871X( "[%s] In\n", __FUNCTION__ );
+	//	Reset the operation channel information
+	pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
+	pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
+}
+
+static void reset_ch_sitesurvey_timer_process2 (void *FunctionContext)
+{
+	_adapter *adapter = (_adapter *)FunctionContext;
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	DBG_871X( "[%s] In\n", __FUNCTION__ );
+	//	Reset the operation channel information
+	pwdinfo->p2p_info.operation_ch[0] = 0;
+	pwdinfo->p2p_info.scan_op_ch_only = 0;
+}
+
+static void restore_p2p_state_timer_process (void *FunctionContext)
+{
+	_adapter *adapter = (_adapter *)FunctionContext;
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return; 
+	
+	p2p_protocol_wk_cmd( adapter, P2P_RESTORE_STATE_WK );
+}
+
+static void pre_tx_scan_timer_process (void *FunctionContext)
+{
+	_adapter 							*adapter = (_adapter *) FunctionContext;
+	struct	wifidirect_info				*pwdinfo = &adapter->wdinfo;
+	_irqL							irqL;
+	struct mlme_priv					*pmlmepriv = &adapter->mlmepriv;
+	u8								_status = 0;
+
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+	
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
+	{
+		if ( _TRUE == pwdinfo->tx_prov_disc_info.benable )	//	the provision discovery request frame is trigger to send or not
+		{
+			p2p_protocol_wk_cmd( adapter, P2P_PRE_TX_PROVDISC_PROCESS_WK );			
+			//issue_probereq_p2p(adapter, NULL);
+			//_set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
+		}
+	}
+	else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
+	{
+		if ( _TRUE == pwdinfo->nego_req_info.benable )
+		{
+			p2p_protocol_wk_cmd( adapter, P2P_PRE_TX_NEGOREQ_PROCESS_WK );
+		}
+	}
+	else if ( rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ ) )
+	{
+		if ( _TRUE == pwdinfo->invitereq_info.benable )
+		{
+			p2p_protocol_wk_cmd( adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK );
+		}
+	}
+	else
+	{
+		DBG_8192C( "[%s] p2p_state is %d, ignore!!\n", __FUNCTION__, rtw_p2p_state(pwdinfo) );
+	}
+	
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+}
+
+static void find_phase_timer_process (void *FunctionContext)
+{
+	_adapter *adapter = (_adapter *)FunctionContext;
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	adapter->wdinfo.find_phase_state_exchange_cnt++;
+
+	p2p_protocol_wk_cmd( adapter, P2P_FIND_PHASE_WK );
+}
+
+#ifdef CONFIG_CONCURRENT_MODE
+void ap_p2p_switch_timer_process (void *FunctionContext)
+{
+	_adapter *adapter = (_adapter *)FunctionContext;
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+#ifdef CONFIG_IOCTL_CFG80211	
+	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(adapter->rtw_wdev);
+#endif
+
+	if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	ATOMIC_SET(&pwdev_priv->switch_ch_to, 1);
+#endif
+
+	p2p_protocol_wk_cmd( adapter, P2P_AP_P2P_CH_SWITCH_PROCESS_WK );
+}
+#endif
+
+void reset_global_wifidirect_info( _adapter* padapter )
+{
+	struct wifidirect_info	*pwdinfo;
+
+	pwdinfo = &padapter->wdinfo;
+	pwdinfo->persistent_supported = 0;
+	pwdinfo->session_available = _TRUE;
+	pwdinfo->wfd_tdls_enable = 0;
+	pwdinfo->wfd_tdls_weaksec = 0;
+}
+
+#ifdef CONFIG_WFD
+int rtw_init_wifi_display_info(_adapter* padapter)
+{
+	int	res = _SUCCESS;
+	struct wifi_display_info *pwfd_info = &padapter->wfd_info;
+
+	// Used in P2P and TDLS
+	pwfd_info->rtsp_ctrlport = 554;
+	pwfd_info->peer_rtsp_ctrlport = 0;	//	Reset to 0
+	pwfd_info->wfd_enable = _FALSE;
+	pwfd_info->wfd_device_type = WFD_DEVINFO_PSINK;
+	pwfd_info->scan_result_type = SCAN_RESULT_P2P_ONLY;
+
+	// Used in P2P	
+	pwfd_info->peer_session_avail = _TRUE;
+	pwfd_info->wfd_pc = _FALSE;
+
+	// Used in TDLS
+	_rtw_memset( pwfd_info->ip_address, 0x00, 4 );
+	_rtw_memset( pwfd_info->peer_ip_address, 0x00, 4 );
+	return res;
+
+}
+#endif //CONFIG_WFD
+
+void rtw_init_wifidirect_timers(_adapter* padapter)
+{
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	_init_timer( &pwdinfo->find_phase_timer, padapter->pnetdev, find_phase_timer_process, padapter );
+	_init_timer( &pwdinfo->restore_p2p_state_timer, padapter->pnetdev, restore_p2p_state_timer_process, padapter );
+	_init_timer( &pwdinfo->pre_tx_scan_timer, padapter->pnetdev, pre_tx_scan_timer_process, padapter );
+	_init_timer( &pwdinfo->reset_ch_sitesurvey, padapter->pnetdev, reset_ch_sitesurvey_timer_process, padapter );
+	_init_timer( &pwdinfo->reset_ch_sitesurvey2, padapter->pnetdev, reset_ch_sitesurvey_timer_process2, padapter );
+#ifdef CONFIG_CONCURRENT_MODE
+	_init_timer( &pwdinfo->ap_p2p_switch_timer, padapter->pnetdev, ap_p2p_switch_timer_process, padapter );
+#endif
+}
+
+void rtw_init_wifidirect_addrs(_adapter* padapter, u8 *dev_addr, u8 *iface_addr)
+{
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	/*init device&interface address */
+	if (dev_addr) {
+		_rtw_memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN);
+	}
+	if (iface_addr) {
+		_rtw_memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN);
+	}
+#endif
+}
+
+void init_wifidirect_info( _adapter* padapter, enum P2P_ROLE role)
+{
+	struct wifidirect_info	*pwdinfo;
+#ifdef CONFIG_WFD
+	struct wifi_display_info	*pwfd_info = &padapter->wfd_info;
+#endif
+#ifdef CONFIG_CONCURRENT_MODE
+	_adapter				*pbuddy_adapter = padapter->pbuddy_adapter;
+	struct wifidirect_info	*pbuddy_wdinfo;
+	struct mlme_priv		*pbuddy_mlmepriv;
+	struct mlme_ext_priv	*pbuddy_mlmeext;
+#endif
+
+	pwdinfo = &padapter->wdinfo;
+
+	pwdinfo->padapter = padapter;
+	
+	//	1, 6, 11 are the social channel defined in the WiFi Direct specification.
+	pwdinfo->social_chan[0] = 1;
+	pwdinfo->social_chan[1] = 6;
+	pwdinfo->social_chan[2] = 11;
+	pwdinfo->social_chan[3] = 0;	//	channel 0 for scanning ending in site survey function.
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (pbuddy_adapter) {
+		pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
+		pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
+		pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
+	}
+
+	if ( ( check_buddy_fwstate(padapter, _FW_LINKED ) == _TRUE ) && 
+	      ( ( pbuddy_mlmeext->cur_channel == 1) || ( pbuddy_mlmeext->cur_channel == 6 ) || ( pbuddy_mlmeext->cur_channel == 11 ) )
+	   )
+	{
+		//	Use the AP's channel as the listen channel
+		//	This will avoid the channel switch between AP's channel and listen channel.
+		pwdinfo->listen_channel = pbuddy_mlmeext->cur_channel;
+	}
+	else
+#endif //CONFIG_CONCURRENT_MODE
+	{
+		//	Use the channel 11 as the listen channel
+		pwdinfo->listen_channel = 11;
+	}
+
+	if (role == P2P_ROLE_DEVICE)
+	{
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+		#ifdef CONFIG_CONCURRENT_MODE
+		if ( check_buddy_fwstate(padapter, _FW_LINKED ) == _TRUE )
+		{
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_IDLE);
+		}
+		else
+		#endif
+		{
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
+		}
+		pwdinfo->intent = 1;
+		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_LISTEN);
+	}
+	else if (role == P2P_ROLE_CLIENT)
+	{
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+		pwdinfo->intent = 1;
+		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+	}	
+	else if (role == P2P_ROLE_GO)
+	{
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+		pwdinfo->intent = 15;
+		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+	}
+
+//	Use the OFDM rate in the P2P probe response frame. ( 6(B), 9(B), 12, 18, 24, 36, 48, 54 )	
+	pwdinfo->support_rate[0] = 0x8c;	//	6(B)
+	pwdinfo->support_rate[1] = 0x92;	//	9(B)
+	pwdinfo->support_rate[2] = 0x18;	//	12
+	pwdinfo->support_rate[3] = 0x24;	//	18
+	pwdinfo->support_rate[4] = 0x30;	//	24
+	pwdinfo->support_rate[5] = 0x48;	//	36
+	pwdinfo->support_rate[6] = 0x60;	//	48
+	pwdinfo->support_rate[7] = 0x6c;	//	54
+
+	_rtw_memcpy( ( void* ) pwdinfo->p2p_wildcard_ssid, "DIRECT-", 7 );
+
+	_rtw_memset( pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN );
+	pwdinfo->device_name_len = 0;
+
+	_rtw_memset( &pwdinfo->invitereq_info, 0x00, sizeof( struct tx_invite_req_info ) );
+	pwdinfo->invitereq_info.token = 3;	//	Token used for P2P invitation request frame.
+	
+	_rtw_memset( &pwdinfo->inviteresp_info, 0x00, sizeof( struct tx_invite_resp_info ) );
+	pwdinfo->inviteresp_info.token = 0;
+
+	pwdinfo->profileindex = 0;
+	_rtw_memset( &pwdinfo->profileinfo[ 0 ], 0x00, sizeof( struct profile_info ) * P2P_MAX_PERSISTENT_GROUP_NUM );
+
+	rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
+
+	pwdinfo->listen_dwell = ( u8 ) (( rtw_get_current_time() % 3 ) + 1);
+	//DBG_8192C( "[%s] listen_dwell time is %d00ms\n", __FUNCTION__, pwdinfo->listen_dwell );
+
+	_rtw_memset( &pwdinfo->tx_prov_disc_info, 0x00, sizeof( struct tx_provdisc_req_info ) );
+	pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_NONE;
+
+	_rtw_memset( &pwdinfo->nego_req_info, 0x00, sizeof( struct tx_nego_req_info ) );
+
+	pwdinfo->device_password_id_for_nego = WPS_DPID_PBC;
+	pwdinfo->negotiation_dialog_token = 1;
+
+	_rtw_memset( pwdinfo->nego_ssid, 0x00, WLAN_SSID_MAXLEN );
+	pwdinfo->nego_ssidlen = 0;
+
+	pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO;
+#ifdef CONFIG_WFD
+	pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY  | WPS_CONFIG_METHOD_PBC;
+	pwdinfo->wfd_info = pwfd_info;
+#else
+	pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD;
+#endif //CONFIG_WFD
+	pwdinfo->channel_list_attr_len = 0;
+	_rtw_memset( pwdinfo->channel_list_attr, 0x00, 100 );
+
+	_rtw_memset( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, 0x00, 4 );
+	_rtw_memset( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, '0', 3 );
+	_rtw_memset( &pwdinfo->groupid_info, 0x00, sizeof( struct group_id_info ) );
+#ifdef CONFIG_CONCURRENT_MODE
+#ifdef CONFIG_IOCTL_CFG80211
+	pwdinfo->ext_listen_interval = 1000; //The interval to be available with legacy AP during p2p0-find/scan
+	pwdinfo->ext_listen_period = 3000; //The time period to be available for P2P during nego
+#else //!CONFIG_IOCTL_CFG80211
+	//pwdinfo->ext_listen_interval = 3000;
+	//pwdinfo->ext_listen_period = 400;
+	pwdinfo->ext_listen_interval = 1000;
+	pwdinfo->ext_listen_period = 1000;
+#endif //!CONFIG_IOCTL_CFG80211
+#endif
+	pwdinfo->wfd_tdls_enable = 0;
+	_rtw_memset( pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN );
+	_rtw_memset( pwdinfo->p2p_peer_device_addr, 0x00, ETH_ALEN );
+
+	pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
+	pwdinfo->rx_invitereq_info.operation_ch[1] = 0;	//	Used to indicate the scan end in site survey function
+	pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
+	pwdinfo->p2p_info.operation_ch[0] = 0;
+	pwdinfo->p2p_info.operation_ch[1] = 0;			//	Used to indicate the scan end in site survey function
+	pwdinfo->p2p_info.scan_op_ch_only = 0;
+}
+
+#ifdef CONFIG_DBG_P2P
+char * p2p_role_str[] = {
+	"P2P_ROLE_DISABLE",
+	"P2P_ROLE_DEVICE",
+	"P2P_ROLE_CLIENT",
+	"P2P_ROLE_GO"	
+};
+
+char * p2p_state_str[] = {
+	"P2P_STATE_NONE",
+	"P2P_STATE_IDLE",
+	"P2P_STATE_LISTEN",
+	"P2P_STATE_SCAN",
+	"P2P_STATE_FIND_PHASE_LISTEN",
+	"P2P_STATE_FIND_PHASE_SEARCH",
+	"P2P_STATE_TX_PROVISION_DIS_REQ",
+	"P2P_STATE_RX_PROVISION_DIS_RSP",
+	"P2P_STATE_RX_PROVISION_DIS_REQ",
+	"P2P_STATE_GONEGO_ING",
+	"P2P_STATE_GONEGO_OK",
+	"P2P_STATE_GONEGO_FAIL",
+	"P2P_STATE_RECV_INVITE_REQ_MATCH",
+	"P2P_STATE_PROVISIONING_ING",
+	"P2P_STATE_PROVISIONING_DONE",
+	"P2P_STATE_RECV_INVITE_REQ_DISMATCH",
+	"P2P_STATE_RECV_INVITE_REQ_GO"
+};
+
+void dbg_rtw_p2p_set_state(struct wifidirect_info *wdinfo, enum P2P_STATE state, const char *caller, int line)
+{
+	if(!_rtw_p2p_chk_state(wdinfo, state)) {
+		enum P2P_STATE old_state = _rtw_p2p_state(wdinfo);
+		_rtw_p2p_set_state(wdinfo, state);
+		DBG_871X("[CONFIG_DBG_P2P]%s:%d set_state from %s to %s\n", caller, line
+			, p2p_state_str[old_state], p2p_state_str[_rtw_p2p_state(wdinfo)]
+		);
+	} else {
+		DBG_871X("[CONFIG_DBG_P2P]%s:%d set_state to same state %s\n", caller, line
+			, p2p_state_str[_rtw_p2p_state(wdinfo)]
+		);
+	}
+}
+void dbg_rtw_p2p_set_pre_state(struct wifidirect_info *wdinfo, enum P2P_STATE state, const char *caller, int line)
+{
+	if(_rtw_p2p_pre_state(wdinfo) != state) {
+		enum P2P_STATE old_state = _rtw_p2p_pre_state(wdinfo); 
+		_rtw_p2p_set_pre_state(wdinfo, state);
+		DBG_871X("[CONFIG_DBG_P2P]%s:%d set_pre_state from %s to %s\n", caller, line
+			, p2p_state_str[old_state], p2p_state_str[_rtw_p2p_pre_state(wdinfo)]
+		);
+	} else {
+		DBG_871X("[CONFIG_DBG_P2P]%s:%d set_pre_state to same state %s\n", caller, line
+			, p2p_state_str[_rtw_p2p_pre_state(wdinfo)]
+		);
+	}
+}
+#if 0
+void dbg_rtw_p2p_restore_state(struct wifidirect_info *wdinfo, const char *caller, int line)
+{
+	if(wdinfo->pre_p2p_state != -1) {
+		DBG_871X("[CONFIG_DBG_P2P]%s:%d restore from %s to %s\n", caller, line
+			, p2p_state_str[wdinfo->p2p_state], p2p_state_str[wdinfo->pre_p2p_state]
+		);
+		_rtw_p2p_restore_state(wdinfo);
+	} else {
+		DBG_871X("[CONFIG_DBG_P2P]%s:%d restore no pre state, cur state %s\n", caller, line
+			, p2p_state_str[wdinfo->p2p_state]
+		);		
+	}
+}
+#endif
+void dbg_rtw_p2p_set_role(struct wifidirect_info *wdinfo, enum P2P_ROLE role, const char *caller, int line)
+{
+	if(wdinfo->role != role) {
+		enum P2P_ROLE old_role = wdinfo->role;
+		_rtw_p2p_set_role(wdinfo, role);
+		DBG_871X("[CONFIG_DBG_P2P]%s:%d set_role from %s to %s\n", caller, line
+			, p2p_role_str[old_role], p2p_role_str[wdinfo->role]
+		);
+	} else {
+		DBG_871X("[CONFIG_DBG_P2P]%s:%d set_role to same role %s\n", caller, line
+			, p2p_role_str[wdinfo->role]
+		);
+	}
+}
+#endif //CONFIG_DBG_P2P
+
+
+int rtw_p2p_enable(_adapter *padapter, enum P2P_ROLE role)
+{
+	int ret = _SUCCESS;
+	struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
+	if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT|| role == P2P_ROLE_GO)
+	{
+		u8 channel, ch_offset;
+		u16 bwmode;
+
+#ifdef CONFIG_CONCURRENT_MODE
+		_adapter				*pbuddy_adapter = padapter->pbuddy_adapter;
+		struct wifidirect_info	*pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
+		//	Commented by Albert 2011/12/30
+		//	The driver just supports 1 P2P group operation.
+		//	So, this function will do nothing if the buddy adapter had enabled the P2P function.
+		if(!rtw_p2p_chk_state(pbuddy_wdinfo, P2P_STATE_NONE))
+		{
+			//	The buddy adapter had enabled the P2P function.
+			return ret;
+		}
+#endif //CONFIG_CONCURRENT_MODE
+
+		//leave IPS/Autosuspend
+		if (_FAIL == rtw_pwr_wakeup(padapter)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		//	Added by Albert 2011/03/22
+		//	In the P2P mode, the driver should not support the b mode.
+		//	So, the Tx packet shouldn't use the CCK rate
+		update_tx_basic_rate(padapter, WIRELESS_11AGN);
+
+		//Enable P2P function
+		init_wifidirect_info(padapter, role);
+				
+		rtw_hal_set_odm_var(padapter,HAL_ODM_P2P_STATE,NULL,_TRUE);
+		#ifdef CONFIG_WFD
+		rtw_hal_set_odm_var(padapter,HAL_ODM_WIFI_DISPLAY_STATE,NULL,_TRUE);
+		#endif
+		
+	}
+	else if (role == P2P_ROLE_DISABLE)
+	{
+		if (_FAIL == rtw_pwr_wakeup(padapter)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		//Disable P2P function
+		if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		{
+			_cancel_timer_ex( &pwdinfo->find_phase_timer );
+			_cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
+			_cancel_timer_ex( &pwdinfo->pre_tx_scan_timer);
+			_cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey);
+			_cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey2);
+			reset_ch_sitesurvey_timer_process( padapter );
+			reset_ch_sitesurvey_timer_process2( padapter );
+			#ifdef CONFIG_CONCURRENT_MODE			
+			_cancel_timer_ex( &pwdinfo->ap_p2p_switch_timer);
+			#endif
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
+			rtw_p2p_set_role(pwdinfo, P2P_ROLE_DISABLE);
+			_rtw_memset(&pwdinfo->rx_prov_disc_info, 0x00, sizeof(struct rx_provdisc_req_info));
+		}
+
+		rtw_hal_set_odm_var(padapter,HAL_ODM_P2P_STATE,NULL,_FALSE);
+		#ifdef CONFIG_WFD
+		rtw_hal_set_odm_var(padapter,HAL_ODM_WIFI_DISPLAY_STATE,NULL,_FALSE);
+		#endif
+
+		//Restore to initial setting.
+		update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+	}
+	
+exit:	
+	return ret;
+}
+
+#endif //CONFIG_P2P
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_pwrctrl.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_pwrctrl.c
new file mode 100755
index 00000000..088f8a46
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_pwrctrl.c
@@ -0,0 +1,1852 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_PWRCTRL_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <osdep_intf.h>
+
+#ifdef CONFIG_BT_COEXIST
+#include <rtl8723a_hal.h>
+#endif
+
+#ifdef CONFIG_IPS
+void ips_enter(_adapter * padapter)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;	
+
+	_enter_pwrlock(&pwrpriv->lock);
+
+	pwrpriv->bips_processing = _TRUE;	
+
+	// syn ips_mode with request
+	pwrpriv->ips_mode = pwrpriv->ips_mode_req;
+	
+	pwrpriv->ips_enter_cnts++;	
+	DBG_871X("==>ips_enter cnts:%d\n",pwrpriv->ips_enter_cnts);
+#ifdef CONFIG_BT_COEXIST
+	BTDM_TurnOffBtCoexistBeforeEnterIPS(padapter);
+#endif
+	if(rf_off == pwrpriv->change_rfpwrstate )
+	{	
+		pwrpriv->bpower_saving = _TRUE;
+		DBG_871X_LEVEL(_drv_always_, "nolinked power save enter\n");
+
+		if(pwrpriv->ips_mode == IPS_LEVEL_2)
+			pwrpriv->bkeepfwalive = _TRUE;
+		
+		rtw_ips_pwr_down(padapter);
+		pwrpriv->rf_pwrstate = rf_off;
+	}	
+	pwrpriv->bips_processing = _FALSE;	
+	
+	_exit_pwrlock(&pwrpriv->lock);
+	
+}
+
+int ips_leave(_adapter * padapter)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+	struct security_priv* psecuritypriv=&(padapter->securitypriv);
+	struct mlme_priv		*pmlmepriv = &(padapter->mlmepriv);
+	int result = _SUCCESS;
+	sint keyid;
+
+
+	_enter_pwrlock(&pwrpriv->lock);
+
+	if((pwrpriv->rf_pwrstate == rf_off) &&(!pwrpriv->bips_processing))
+	{
+		pwrpriv->bips_processing = _TRUE;
+		pwrpriv->change_rfpwrstate = rf_on;
+		pwrpriv->ips_leave_cnts++;
+		DBG_871X("==>ips_leave cnts:%d\n",pwrpriv->ips_leave_cnts);
+
+		if ((result = rtw_ips_pwr_up(padapter)) == _SUCCESS) {
+			pwrpriv->rf_pwrstate = rf_on;
+		}
+		DBG_871X_LEVEL(_drv_always_, "nolinked power save leave\n");
+
+		if((_WEP40_ == psecuritypriv->dot11PrivacyAlgrthm) ||(_WEP104_ == psecuritypriv->dot11PrivacyAlgrthm))
+		{
+			DBG_871X("==>%s,channel(%d),processing(%x)\n",__FUNCTION__,padapter->mlmeextpriv.cur_channel,pwrpriv->bips_processing);
+			set_channel_bwmode(padapter, padapter->mlmeextpriv.cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);			
+			for(keyid=0;keyid<4;keyid++){
+				if(pmlmepriv->key_mask & BIT(keyid)){
+					if(keyid == psecuritypriv->dot11PrivacyKeyIndex)
+						result=rtw_set_key(padapter,psecuritypriv, keyid, 1);
+					else
+						result=rtw_set_key(padapter,psecuritypriv, keyid, 0);
+				}
+			}
+		}
+		
+		DBG_871X("==> ips_leave.....LED(0x%08x)...\n",rtw_read32(padapter,0x4c));
+		pwrpriv->bips_processing = _FALSE;
+
+		pwrpriv->bkeepfwalive = _FALSE;
+		pwrpriv->bpower_saving = _FALSE;
+	}
+
+	_exit_pwrlock(&pwrpriv->lock);
+
+	return result;
+}
+
+
+#endif
+
+#ifdef CONFIG_AUTOSUSPEND
+extern void autosuspend_enter(_adapter* padapter);	
+extern int autoresume_enter(_adapter* padapter);
+#endif
+
+#ifdef SUPPORT_HW_RFOFF_DETECTED
+int rtw_hw_suspend(_adapter *padapter );
+int rtw_hw_resume(_adapter *padapter);
+#endif
+
+bool rtw_pwr_unassociated_idle(_adapter *adapter)
+{
+	_adapter *buddy = adapter->pbuddy_adapter;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct xmit_priv *pxmit_priv = &adapter->xmitpriv;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(adapter->wdinfo);
+#ifdef CONFIG_IOCTL_CFG80211
+	struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &adapter->cfg80211_wdinfo;
+#endif
+#endif
+
+	bool ret = _FALSE;
+
+	if (adapter->pwrctrlpriv.ips_deny_time >= rtw_get_current_time()) {
+		//DBG_871X("%s ips_deny_time\n", __func__);
+		goto exit;
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
+		|| check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
+		|| check_fwstate(pmlmepriv, WIFI_AP_STATE)
+		|| check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
+		#if defined(CONFIG_P2P) && defined(CONFIG_IOCTL_CFG80211) && defined(CONFIG_P2P_IPS)
+		|| pcfg80211_wdinfo->is_ro_ch
+		#elif defined(CONFIG_P2P)
+		|| !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)
+		#endif
+	) {
+		goto exit;
+	}
+
+	/* consider buddy, if exist */
+	if (buddy) {
+		struct mlme_priv *b_pmlmepriv = &(buddy->mlmepriv);
+		#ifdef CONFIG_P2P
+		struct wifidirect_info *b_pwdinfo = &(buddy->wdinfo);
+		#ifdef CONFIG_IOCTL_CFG80211
+		struct cfg80211_wifidirect_info *b_pcfg80211_wdinfo = &buddy->cfg80211_wdinfo;
+		#endif
+		#endif
+
+		if (check_fwstate(b_pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
+			|| check_fwstate(b_pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
+			|| check_fwstate(b_pmlmepriv, WIFI_AP_STATE)
+			|| check_fwstate(b_pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
+			#if defined(CONFIG_P2P) && defined(CONFIG_IOCTL_CFG80211) && defined(CONFIG_P2P_IPS)
+			|| b_pcfg80211_wdinfo->is_ro_ch
+			#elif defined(CONFIG_P2P)
+			|| !rtw_p2p_chk_state(b_pwdinfo, P2P_STATE_NONE)
+			#endif
+		) {
+			goto exit;
+		}
+	}
+
+#if (MP_DRIVER == 1)
+	if (adapter->registrypriv.mp_mode == 1)
+		goto exit;
+#endif
+
+#ifdef CONFIG_INTEL_PROXIM
+	if(adapter->proximity.proxim_on==_TRUE){
+		return;
+	}
+#endif	
+
+	if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF ||
+		pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) {
+		DBG_871X_LEVEL(_drv_always_, "There are some pkts to transmit\n");
+		DBG_871X_LEVEL(_drv_info_, "free_xmitbuf_cnt: %d, free_xmit_extbuf_cnt: %d\n", 
+			pxmit_priv->free_xmitbuf_cnt, pxmit_priv->free_xmit_extbuf_cnt);	
+		goto exit;
+	}	
+
+	ret = _TRUE;
+
+exit:
+	return ret;
+}
+
+#if defined (PLATFORM_LINUX)||defined (PLATFORM_FREEBSD)
+void rtw_ps_processor(_adapter*padapter)
+{
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &( padapter->wdinfo );
+#endif //CONFIG_P2P
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+#ifdef SUPPORT_HW_RFOFF_DETECTED
+	rt_rf_power_state rfpwrstate;
+#endif //SUPPORT_HW_RFOFF_DETECTED
+
+	pwrpriv->ps_processing = _TRUE;
+
+#ifdef SUPPORT_HW_RFOFF_DETECTED
+	if(pwrpriv->bips_processing == _TRUE)
+		goto exit;
+	
+	//DBG_871X("==> fw report state(0x%x)\n",rtw_read8(padapter,0x1ca));	
+	if(padapter->pwrctrlpriv.bHWPwrPindetect) 
+	{
+	#ifdef CONFIG_AUTOSUSPEND
+		if(padapter->registrypriv.usbss_enable)
+		{
+			if(pwrpriv->rf_pwrstate == rf_on)
+			{
+				if(padapter->net_closed == _TRUE)
+					pwrpriv->ps_flag = _TRUE;
+
+				rfpwrstate = RfOnOffDetect(padapter);
+				DBG_871X("@@@@- #1  %s==> rfstate:%s \n",__FUNCTION__,(rfpwrstate==rf_on)?"rf_on":"rf_off");
+				if(rfpwrstate!= pwrpriv->rf_pwrstate)
+				{
+					if(rfpwrstate == rf_off)
+					{
+						pwrpriv->change_rfpwrstate = rf_off;
+						
+						pwrpriv->bkeepfwalive = _TRUE;	
+						pwrpriv->brfoffbyhw = _TRUE;						
+						
+						autosuspend_enter(padapter);							
+					}
+				}
+			}			
+		}
+		else
+	#endif //CONFIG_AUTOSUSPEND
+		{
+			rfpwrstate = RfOnOffDetect(padapter);
+			DBG_871X("@@@@- #2  %s==> rfstate:%s \n",__FUNCTION__,(rfpwrstate==rf_on)?"rf_on":"rf_off");
+
+			if(rfpwrstate!= pwrpriv->rf_pwrstate)
+			{
+				if(rfpwrstate == rf_off)
+				{	
+					pwrpriv->change_rfpwrstate = rf_off;														
+					pwrpriv->brfoffbyhw = _TRUE;
+					padapter->bCardDisableWOHSM = _TRUE;
+					rtw_hw_suspend(padapter );	
+				}
+				else
+				{
+					pwrpriv->change_rfpwrstate = rf_on;
+					rtw_hw_resume(padapter );			
+				}
+				DBG_871X("current rf_pwrstate(%s)\n",(pwrpriv->rf_pwrstate == rf_off)?"rf_off":"rf_on");
+			}
+		}
+		pwrpriv->pwr_state_check_cnts ++;	
+	}
+#endif //SUPPORT_HW_RFOFF_DETECTED
+
+	if (pwrpriv->ips_mode_req == IPS_NONE
+		#ifdef CONFIG_CONCURRENT_MODE
+		|| padapter->pbuddy_adapter->pwrctrlpriv.ips_mode_req == IPS_NONE
+		#endif
+	)
+		goto exit;
+
+	if (rtw_pwr_unassociated_idle(padapter) == _FALSE)
+		goto exit;
+
+	if((pwrpriv->rf_pwrstate == rf_on) && ((pwrpriv->pwr_state_check_cnts%4)==0))
+	{
+		DBG_871X("==>%s .fw_state(%x)\n",__FUNCTION__,get_fwstate(pmlmepriv));
+		#if defined (CONFIG_BT_COEXIST)&& defined (CONFIG_AUTOSUSPEND)
+		#else
+		pwrpriv->change_rfpwrstate = rf_off;
+		#endif
+		#ifdef CONFIG_AUTOSUSPEND
+		if(padapter->registrypriv.usbss_enable)
+		{
+			if(pwrpriv->bHWPwrPindetect) 
+				pwrpriv->bkeepfwalive = _TRUE;
+			
+			if(padapter->net_closed == _TRUE)
+				pwrpriv->ps_flag = _TRUE;
+
+			#if defined (CONFIG_BT_COEXIST)&& defined (CONFIG_AUTOSUSPEND)
+			if (_TRUE==pwrpriv->bInternalAutoSuspend) {
+				DBG_871X("<==%s .pwrpriv->bInternalAutoSuspend)(%x)\n",__FUNCTION__,pwrpriv->bInternalAutoSuspend);
+			} else {
+				pwrpriv->change_rfpwrstate = rf_off;
+				padapter->bCardDisableWOHSM = _TRUE;
+				DBG_871X("<==%s .pwrpriv->bInternalAutoSuspend)(%x) call autosuspend_enter\n",__FUNCTION__,pwrpriv->bInternalAutoSuspend);
+				autosuspend_enter(padapter);
+			}		
+			#else
+			padapter->bCardDisableWOHSM = _TRUE;
+			autosuspend_enter(padapter);
+			#endif	//if defined (CONFIG_BT_COEXIST)&& defined (CONFIG_AUTOSUSPEND)
+		}		
+		else if(pwrpriv->bHWPwrPindetect)
+		{
+		}
+		else
+		#endif //CONFIG_AUTOSUSPEND
+		{
+			#if defined (CONFIG_BT_COEXIST)&& defined (CONFIG_AUTOSUSPEND)
+			pwrpriv->change_rfpwrstate = rf_off;
+			#endif	//defined (CONFIG_BT_COEXIST)&& defined (CONFIG_AUTOSUSPEND)
+
+			#ifdef CONFIG_IPS
+			ips_enter(padapter);			
+			#endif
+		}
+	}
+exit:
+	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
+	pwrpriv->ps_processing = _FALSE;
+	return;
+}
+
+void pwr_state_check_handler(void *FunctionContext);
+void pwr_state_check_handler(void *FunctionContext)
+{
+	_adapter *padapter = (_adapter *)FunctionContext;
+	rtw_ps_cmd(padapter);
+}
+#endif
+
+
+
+#ifdef CONFIG_LPS
+/*
+ *
+ * Parameters
+ *	padapter
+ *	pslv			power state level, only could be PS_STATE_S0 ~ PS_STATE_S4
+ *
+ */
+void rtw_set_rpwm(PADAPTER padapter, u8 pslv)
+{
+	u8	rpwm;
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+
+_func_enter_;
+
+	pslv = PS_STATE(pslv);
+
+
+	if (_TRUE == pwrpriv->btcoex_rfon)
+	{
+		if (pslv < PS_STATE_S4)
+			pslv = PS_STATE_S3;
+	}
+
+#ifdef CONFIG_LPS_RPWM_TIMER
+	if (pwrpriv->brpwmtimeout == _TRUE)
+	{
+		DBG_871X("%s: RPWM timeout, force to set RPWM(0x%02X) again!\n", __FUNCTION__, pslv);
+	}
+	else
+#endif // CONFIG_LPS_RPWM_TIMER
+	{
+	if ( (pwrpriv->rpwm == pslv)
+#ifdef CONFIG_LPS_LCLK
+		|| ((pwrpriv->rpwm >= PS_STATE_S2)&&(pslv >= PS_STATE_S2))
+#endif
+		)
+	{
+		RT_TRACE(_module_rtl871x_pwrctrl_c_,_drv_err_,
+			("%s: Already set rpwm[0x%02X], new=0x%02X!\n", __FUNCTION__, pwrpriv->rpwm, pslv));
+		return;
+	}
+	}
+
+	if ((padapter->bSurpriseRemoved == _TRUE) ||
+		(padapter->hw_init_completed == _FALSE))
+	{
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+				 ("%s: SurpriseRemoved(%d) hw_init_completed(%d)\n",
+				  __FUNCTION__, padapter->bSurpriseRemoved, padapter->hw_init_completed));
+
+		pwrpriv->cpwm = PS_STATE_S4;
+
+		return;
+	}
+
+	if (padapter->bDriverStopped == _TRUE)
+	{
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+				 ("%s: change power state(0x%02X) when DriverStopped\n", __FUNCTION__, pslv));
+
+		if (pslv < PS_STATE_S2) {
+			RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+					 ("%s: Reject to enter PS_STATE(0x%02X) lower than S2 when DriverStopped!!\n", __FUNCTION__, pslv));
+			return;
+		}
+	}
+
+	rpwm = pslv | pwrpriv->tog;
+#ifdef CONFIG_LPS_LCLK
+	// only when from PS_STATE S0/S1 to S2 and higher needs ACK
+	if ((pwrpriv->cpwm < PS_STATE_S2) && (pslv >= PS_STATE_S2))
+		rpwm |= PS_ACK;
+#endif
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("rtw_set_rpwm: rpwm=0x%02x cpwm=0x%02x\n", rpwm, pwrpriv->cpwm));
+
+	pwrpriv->rpwm = pslv;
+
+#ifdef CONFIG_LPS_RPWM_TIMER
+	if (rpwm & PS_ACK)
+		_set_timer(&pwrpriv->pwr_rpwm_timer, LPS_RPWM_WAIT_MS);
+#endif // CONFIG_LPS_RPWM_TIMER
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm));
+
+	pwrpriv->tog += 0x80;
+
+#ifdef CONFIG_LPS_LCLK
+	// No LPS 32K, No Ack
+	if (!(rpwm & PS_ACK))
+#endif
+	{
+		pwrpriv->cpwm = pslv;
+	}
+
+_func_exit_;
+}
+
+u8 PS_RDY_CHECK(_adapter * padapter);
+u8 PS_RDY_CHECK(_adapter * padapter)
+{
+	u32 curr_time, delta_time;
+	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+
+
+	curr_time = rtw_get_current_time();	
+	delta_time = curr_time -pwrpriv->DelayLPSLastTimeStamp;
+
+	if(delta_time < LPS_DELAY_TIME)
+	{		
+		return _FALSE;
+	}
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED) == _FALSE) ||
+		(check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == _TRUE) ||
+		(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE) )
+		return _FALSE;
+#ifdef CONFIG_WOWLAN
+	if(_TRUE == pwrpriv->bInSuspend && pwrpriv->wowlan_mode)
+		return _TRUE;
+	else
+		return _FALSE;
+#else
+	if(_TRUE == pwrpriv->bInSuspend )
+		return _FALSE;
+#endif
+	if( (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && (padapter->securitypriv.binstallGrpkey == _FALSE) )
+	{
+		DBG_871X("Group handshake still in progress !!!\n");
+		return _FALSE;
+	}
+#ifdef CONFIG_IOCTL_CFG80211
+	if (!rtw_cfg80211_pwr_mgmt(padapter))
+		return _FALSE;
+#endif	
+
+	return _TRUE;
+}
+
+void rtw_set_ps_mode(PADAPTER padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &( padapter->wdinfo );
+#endif //CONFIG_P2P
+#ifdef CONFIG_TDLS
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	_irqL irqL;
+	int i, j;
+	_list	*plist, *phead;
+	struct sta_info *ptdls_sta;
+#endif //CONFIG_TDLS
+
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("%s: PowerMode=%d Smart_PS=%d\n",
+			  __FUNCTION__, ps_mode, smart_ps));
+
+	if(ps_mode > PM_Card_Disable) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_,_drv_err_,("ps_mode:%d error\n", ps_mode));
+		return;
+	}
+
+	if (pwrpriv->pwr_mode == ps_mode)
+	{
+		if (PS_MODE_ACTIVE == ps_mode) return;
+
+		if ((pwrpriv->smart_ps == smart_ps) &&
+			(pwrpriv->bcn_ant_mode == bcn_ant_mode))
+		{
+			return;
+		}
+	}
+
+#ifdef CONFIG_LPS_LCLK
+	_enter_pwrlock(&pwrpriv->lock);
+#endif
+
+	//if(pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+	if(ps_mode == PS_MODE_ACTIVE)
+	{
+#ifdef CONFIG_P2P_PS
+		if(pwdinfo->opp_ps == 0)
+#endif //CONFIG_P2P_PS
+		{
+			DBG_871X("rtw_set_ps_mode: Leave 802.11 power save\n");
+
+#ifdef CONFIG_TDLS
+			_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+			for(i=0; i< NUM_STA; i++)
+			{
+				phead = &(pstapriv->sta_hash[i]);
+				plist = get_next(phead);
+
+				while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
+				{
+					ptdls_sta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+					if( ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE )
+						issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta, 0);
+					plist = get_next(plist);
+				}
+			}
+
+			_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+#endif //CONFIG_TDLS
+
+			pwrpriv->pwr_mode = ps_mode;
+			rtw_set_rpwm(padapter, PS_STATE_S4);
+			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+			pwrpriv->bFwCurrentInPSMode = _FALSE;
+		}
+	}
+	else
+	{
+		if (PS_RDY_CHECK(padapter)
+#ifdef CONFIG_BT_COEXIST
+			|| (BT_1Ant(padapter) == _TRUE)
+#endif
+			)
+		{
+			DBG_871X("%s: Enter 802.11 power save\n", __FUNCTION__);
+
+#ifdef CONFIG_TDLS
+			_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+			for(i=0; i< NUM_STA; i++)
+			{
+				phead = &(pstapriv->sta_hash[i]);
+				plist = get_next(phead);
+
+				while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
+				{
+					ptdls_sta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+					if( ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE )
+						issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta, 1);
+					plist = get_next(plist);
+				}
+			}
+
+			_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+#endif //CONFIG_TDLS
+
+			pwrpriv->bFwCurrentInPSMode = _TRUE;
+			pwrpriv->pwr_mode = ps_mode;
+			pwrpriv->smart_ps = smart_ps;
+			pwrpriv->bcn_ant_mode = bcn_ant_mode;
+			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+
+#ifdef CONFIG_P2P_PS
+			// Set CTWindow after LPS
+			if(pwdinfo->opp_ps == 1)
+				p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 0);
+#endif //CONFIG_P2P_PS
+
+#ifdef CONFIG_LPS_LCLK
+			if (pwrpriv->alives == 0)
+				rtw_set_rpwm(padapter, PS_STATE_S0);
+#else
+			rtw_set_rpwm(padapter, PS_STATE_S2);
+#endif
+		}
+	}
+
+#ifdef CONFIG_LPS_LCLK
+	_exit_pwrlock(&pwrpriv->lock);
+#endif
+
+_func_exit_;
+}
+
+/*
+ * Return:
+ *	0:	Leave OK
+ *	-1:	Timeout
+ *	-2:	Other error
+ */
+s32 LPS_RF_ON_check(PADAPTER padapter, u32 delay_ms)
+{
+	u32 start_time;
+	u8 bAwake = _FALSE;
+	s32 err = 0;
+
+
+	start_time = rtw_get_current_time();
+	while (1)
+	{
+		rtw_hal_get_hwreg(padapter, HW_VAR_FWLPS_RF_ON, &bAwake);
+		if (_TRUE == bAwake)
+			break;
+
+		if (_TRUE == padapter->bSurpriseRemoved)
+		{
+			err = -2;
+			DBG_871X("%s: device surprise removed!!\n", __FUNCTION__);
+			break;
+		}
+
+		if (rtw_get_passing_time_ms(start_time) > delay_ms)
+		{
+			err = -1;
+			DBG_871X("%s: Wait for FW LPS leave more than %u ms!!!\n", __FUNCTION__, delay_ms);
+			break;
+		}
+		rtw_usleep_os(100);
+	}
+
+	return err;
+}
+
+//
+//	Description:
+//		Enter the leisure power save mode.
+//
+void LPS_Enter(PADAPTER padapter)
+{
+	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	_adapter *buddy = padapter->pbuddy_adapter;
+
+_func_enter_;
+
+//	DBG_871X("+LeisurePSEnter\n");
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (padapter->iface_type != IFACE_PORT0)
+		return; /* Skip power saving for concurrent mode port 1*/
+
+	/* consider buddy, if exist */
+	if (buddy) {
+		struct mlme_priv *b_pmlmepriv = &(buddy->mlmepriv);
+		#ifdef CONFIG_P2P
+		struct wifidirect_info *b_pwdinfo = &(buddy->wdinfo);
+		#ifdef CONFIG_IOCTL_CFG80211		
+		struct cfg80211_wifidirect_info *b_pcfg80211_wdinfo = &buddy->cfg80211_wdinfo;
+		#endif
+		#endif
+		
+		if (check_fwstate(b_pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
+			|| check_fwstate(b_pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
+			|| check_fwstate(b_pmlmepriv, WIFI_AP_STATE)
+			|| check_fwstate(b_pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
+			#if defined(CONFIG_P2P) && defined(CONFIG_IOCTL_CFG80211) && defined(CONFIG_P2P_IPS)
+			|| b_pcfg80211_wdinfo->is_ro_ch
+			#elif defined(CONFIG_P2P)
+			|| !rtw_p2p_chk_state(b_pwdinfo, P2P_STATE_NONE)
+			#endif
+			|| rtw_is_scan_deny(buddy)
+		) {
+			return;
+		}
+	}
+#endif
+
+	if (PS_RDY_CHECK(padapter) == _FALSE)
+		return;
+
+	if (_TRUE == pwrpriv->bLeisurePs)
+	{
+		// Idle for a while if we connect to AP a while ago.
+		if(pwrpriv->LpsIdleCount >= 2) //  4 Sec 
+		{
+			if(pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+			{
+				pwrpriv->bpower_saving = _TRUE;
+				DBG_871X("%s smart_ps:%d\n", __func__, pwrpriv->smart_ps);
+				//For Tenda W311R IOT issue
+				rtw_set_ps_mode(padapter, pwrpriv->power_mgnt, pwrpriv->smart_ps, 0);
+			}
+		}
+		else
+			pwrpriv->LpsIdleCount++;
+	}
+
+//	DBG_871X("-LeisurePSEnter\n");
+
+_func_exit_;
+}
+
+//
+//	Description:
+//		Leave the leisure power save mode.
+//
+void LPS_Leave(PADAPTER padapter)
+{
+#define LPS_LEAVE_TIMEOUT_MS 100
+
+	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
+	u32 start_time;
+	u8 bAwake = _FALSE;
+
+_func_enter_;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (padapter->iface_type != IFACE_PORT0)
+		return; /* Skip power saving for concurrent mode port 1*/
+#endif
+
+//	DBG_871X("+LeisurePSLeave\n");
+
+	if (pwrpriv->bLeisurePs)
+	{
+		if(pwrpriv->pwr_mode != PS_MODE_ACTIVE)
+		{
+			rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0);
+
+			if(pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+				LPS_RF_ON_check(padapter, LPS_LEAVE_TIMEOUT_MS);
+		}
+	}
+
+	pwrpriv->bpower_saving = _FALSE;
+
+//	DBG_871X("-LeisurePSLeave\n");
+
+_func_exit_;
+}
+#endif
+
+//
+// Description: Leave all power save mode: LPS, FwLPS, IPS if needed.
+// Move code to function by tynli. 2010.03.26. 
+//
+void LeaveAllPowerSaveMode(IN PADAPTER Adapter)
+{
+	struct mlme_priv	*pmlmepriv = &(Adapter->mlmepriv);
+	u8	enqueue = 0;
+
+_func_enter_;
+
+	//DBG_871X("%s.....\n",__FUNCTION__);
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+	{ //connect
+#ifdef CONFIG_LPS_LCLK
+		enqueue = 1;
+#endif
+
+#ifdef CONFIG_P2P_PS
+		p2p_ps_wk_cmd(Adapter, P2P_PS_DISABLE, enqueue);
+#endif //CONFIG_P2P_PS
+
+#ifdef CONFIG_LPS
+		rtw_lps_ctrl_wk_cmd(Adapter, LPS_CTRL_LEAVE, enqueue);
+#endif
+
+#ifdef CONFIG_LPS_LCLK
+		LPS_Leave_check(Adapter);
+#endif	
+	}
+	else
+	{
+		if(Adapter->pwrctrlpriv.rf_pwrstate== rf_off)
+		{
+			#ifdef CONFIG_AUTOSUSPEND
+			if(Adapter->registrypriv.usbss_enable)
+			{
+				#if (LINUX_VERSION_CODE>=KERNEL_VERSION(2,6,35))
+				usb_disable_autosuspend(adapter_to_dvobj(Adapter)->pusbdev);
+				#elif (LINUX_VERSION_CODE>=KERNEL_VERSION(2,6,22) && LINUX_VERSION_CODE<=KERNEL_VERSION(2,6,34))
+				adapter_to_dvobj(Adapter)->pusbdev->autosuspend_disabled = Adapter->bDisableAutosuspend;//autosuspend disabled by the user
+				#endif
+			}
+			else
+			#endif
+			{
+#if defined(CONFIG_PLATFORM_SPRD) && defined(CONFIG_RTL8188E)
+				#ifdef CONFIG_IPS
+				if(_FALSE == ips_leave(Adapter))
+				{
+					DBG_871X("======> ips_leave fail.............\n");			
+				}
+				#endif
+#endif //CONFIG_PLATFORM_SPRD && CONFIG_RTL8188E
+			}				
+		}	
+	}
+
+_func_exit_;
+}
+
+#ifdef CONFIG_LPS_LCLK
+void LPS_Leave_check(
+	PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrpriv;
+	u32	start_time;
+	u8	bReady;
+
+_func_enter_;
+
+	pwrpriv = &padapter->pwrctrlpriv;
+
+	bReady = _FALSE;
+	start_time = rtw_get_current_time();
+
+	rtw_yield_os();
+	
+	while(1)
+	{
+		_enter_pwrlock(&pwrpriv->lock);
+
+		if ((padapter->bSurpriseRemoved == _TRUE)
+			|| (padapter->hw_init_completed == _FALSE)
+#ifdef CONFIG_USB_HCI
+			|| (padapter->bDriverStopped== _TRUE)
+#endif
+			|| (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+			)
+		{
+			bReady = _TRUE;
+		}
+
+		_exit_pwrlock(&pwrpriv->lock);
+
+		if(_TRUE == bReady)
+			break;
+
+		if(rtw_get_passing_time_ms(start_time)>100)
+		{
+			DBG_871X("Wait for cpwm event  than 100 ms!!!\n");
+			break;
+		}
+		rtw_msleep_os(1);
+	}
+
+_func_exit_;
+}
+
+/*
+ * Caller:ISR handler...
+ *
+ * This will be called when CPWM interrupt is up.
+ *
+ * using to update cpwn of drv; and drv willl make a decision to up or down pwr level
+ */
+void cpwm_int_hdl(
+	PADAPTER padapter,
+	struct reportpwrstate_parm *preportpwrstate)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+_func_enter_;
+
+	pwrpriv = &padapter->pwrctrlpriv;
+#if 0
+	if (pwrpriv->cpwm_tog == (preportpwrstate->state & PS_TOGGLE)) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+				 ("cpwm_int_hdl: tog(old)=0x%02x cpwm(new)=0x%02x toggle bit didn't change!?\n",
+				  pwrpriv->cpwm_tog, preportpwrstate->state));
+		goto exit;
+	}
+#endif
+
+	_enter_pwrlock(&pwrpriv->lock);
+
+#ifdef CONFIG_LPS_RPWM_TIMER
+	if (pwrpriv->rpwm < PS_STATE_S2)
+	{
+		DBG_871X("%s: Redundant CPWM Int. RPWM=0x%02X CPWM=0x%02x\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+		_exit_pwrlock(&pwrpriv->lock);
+		goto exit;
+	}
+#endif // CONFIG_LPS_RPWM_TIMER
+
+	pwrpriv->cpwm = PS_STATE(preportpwrstate->state);
+	pwrpriv->cpwm_tog = preportpwrstate->state & PS_TOGGLE;
+
+	if (pwrpriv->cpwm >= PS_STATE_S2)
+	{
+		if (pwrpriv->alives & CMD_ALIVE)
+			_rtw_up_sema(&padapter->cmdpriv.cmd_queue_sema);
+
+		if (pwrpriv->alives & XMIT_ALIVE)
+			_rtw_up_sema(&padapter->xmitpriv.xmit_sema);
+	}
+
+	_exit_pwrlock(&pwrpriv->lock);
+
+exit:
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("cpwm_int_hdl: cpwm=0x%02x\n", pwrpriv->cpwm));
+
+_func_exit_;
+}
+
+static void cpwm_event_callback(struct work_struct *work)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(work, struct pwrctrl_priv, cpwm_event);
+	_adapter *adapter = container_of(pwrpriv, _adapter, pwrctrlpriv);
+	struct reportpwrstate_parm report;
+
+	//DBG_871X("%s\n",__FUNCTION__);
+
+	report.state = PS_STATE_S2;
+	cpwm_int_hdl(adapter, &report);
+}
+
+#ifdef CONFIG_LPS_RPWM_TIMER
+static void rpwmtimeout_workitem_callback(struct work_struct *work)
+{
+	PADAPTER padapter;
+	struct pwrctrl_priv *pwrpriv;
+
+
+	pwrpriv = container_of(work, struct pwrctrl_priv, rpwmtimeoutwi);
+	padapter = container_of(pwrpriv, _adapter, pwrctrlpriv);
+//	DBG_871X("+%s: rpwm=0x%02X cpwm=0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+
+	_enter_pwrlock(&pwrpriv->lock);
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2))
+	{
+		DBG_871X("%s: rpwm=0x%02X cpwm=0x%02X CPWM done!\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+		goto exit;
+	}
+	_exit_pwrlock(&pwrpriv->lock);
+
+	if (rtw_read8(padapter, 0x100) != 0xEA)
+	{
+#if 1
+		struct reportpwrstate_parm report;
+
+		report.state = PS_STATE_S2;
+		DBG_871X("\n%s: FW already leave 32K!\n\n", __func__);
+		cpwm_int_hdl(padapter, &report);
+#else
+		DBG_871X("\n%s: FW already leave 32K!\n\n", __func__);
+		cpwm_event_callback(&pwrpriv->cpwm_event);
+#endif
+		return;
+	}
+
+	_enter_pwrlock(&pwrpriv->lock);
+
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2))
+	{
+		DBG_871X("%s: cpwm=%d, nothing to do!\n", __func__, pwrpriv->cpwm);
+		goto exit;
+	}
+	pwrpriv->brpwmtimeout = _TRUE;
+	rtw_set_rpwm(padapter, pwrpriv->rpwm);
+	pwrpriv->brpwmtimeout = _FALSE;
+
+exit:
+	_exit_pwrlock(&pwrpriv->lock);
+}
+
+/*
+ * This function is a timer handler, can't do any IO in it.
+ */
+static void pwr_rpwm_timeout_handler(void *FunctionContext)
+{
+	PADAPTER padapter;
+	struct pwrctrl_priv *pwrpriv;
+
+
+	padapter = (PADAPTER)FunctionContext;
+	pwrpriv = &padapter->pwrctrlpriv;
+//	DBG_871X("+%s: rpwm=0x%02X cpwm=0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2))
+	{
+		DBG_871X("+%s: cpwm=%d, nothing to do!\n", __func__, pwrpriv->cpwm);
+		return;
+	}
+
+	_set_workitem(&pwrpriv->rpwmtimeoutwi);
+}
+#endif // CONFIG_LPS_RPWM_TIMER
+
+__inline static void register_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag)
+{
+	pwrctrl->alives |= tag;
+}
+
+__inline static void unregister_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag)
+{
+	pwrctrl->alives &= ~tag;
+}
+
+/*
+ * Caller: rtw_xmit_thread
+ * 
+ * Check if the fw_pwrstate is okay for xmit.
+ * If not (cpwm is less than S3), then the sub-routine
+ * will raise the cpwm to be greater than or equal to S3. 
+ *
+ * Calling Context: Passive
+ * 
+ * Return Value:
+ *	 _SUCCESS	rtw_xmit_thread can write fifo/txcmd afterwards.
+ *	 _FAIL		rtw_xmit_thread can not do anything.
+ */
+s32 rtw_register_tx_alive(PADAPTER padapter)
+{
+	s32 res;
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+_func_enter_;
+
+	res = _SUCCESS;
+	pwrctrl = &padapter->pwrctrlpriv;
+#ifdef CONFIG_BT_COEXIST
+	if (_TRUE == padapter->pwrctrlpriv.btcoex_rfon)
+		pslv = PS_STATE_S3;
+	else
+#endif
+	{
+		pslv = PS_STATE_S2;
+	}
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, XMIT_ALIVE);
+
+	if (pwrctrl->bFwCurrentInPSMode == _TRUE)
+	{
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+				 ("rtw_register_tx_alive: cpwm=0x%02x alives=0x%08x\n",
+				  pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm < pslv)
+		{
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+_func_exit_;
+
+	return res;	
+}
+
+/*
+ * Caller: rtw_cmd_thread
+ *
+ * Check if the fw_pwrstate is okay for issuing cmd.
+ * If not (cpwm should be is less than S2), then the sub-routine
+ * will raise the cpwm to be greater than or equal to S2.
+ *
+ * Calling Context: Passive
+ *
+ * Return Value:
+ *	_SUCCESS	rtw_cmd_thread can issue cmds to firmware afterwards.
+ *	_FAIL		rtw_cmd_thread can not do anything.
+ */
+s32 rtw_register_cmd_alive(PADAPTER padapter)
+{
+	s32 res;
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+_func_enter_;
+
+	res = _SUCCESS;
+	pwrctrl = &padapter->pwrctrlpriv;
+#ifdef CONFIG_BT_COEXIST
+	if (_TRUE == padapter->pwrctrlpriv.btcoex_rfon)
+		pslv = PS_STATE_S3;
+	else
+#endif
+	{
+		pslv = PS_STATE_S2;
+	}
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, CMD_ALIVE);
+
+	if (pwrctrl->bFwCurrentInPSMode == _TRUE)
+	{
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_info_,
+				 ("rtw_register_cmd_alive: cpwm=0x%02x alives=0x%08x\n",
+				  pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm < pslv)
+		{
+			if (pwrctrl->cpwm < PS_STATE_S2)
+			res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+_func_exit_;
+
+	return res;
+}
+
+/*
+ * Caller: rx_isr
+ *
+ * Calling Context: Dispatch/ISR
+ *
+ * Return Value:
+ *	_SUCCESS
+ *	_FAIL
+ */
+s32 rtw_register_rx_alive(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+
+_func_enter_;
+
+	pwrctrl = &padapter->pwrctrlpriv;
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, RECV_ALIVE);
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("rtw_register_rx_alive: cpwm=0x%02x alives=0x%08x\n",
+			  pwrctrl->cpwm, pwrctrl->alives));
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+_func_exit_;
+
+	return _SUCCESS;
+}
+
+/*
+ * Caller: evt_isr or evt_thread
+ *
+ * Calling Context: Dispatch/ISR or Passive
+ *
+ * Return Value:
+ *	_SUCCESS
+ *	_FAIL
+ */
+s32 rtw_register_evt_alive(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+
+_func_enter_;
+
+	pwrctrl = &padapter->pwrctrlpriv;
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, EVT_ALIVE);
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("rtw_register_evt_alive: cpwm=0x%02x alives=0x%08x\n",
+			  pwrctrl->cpwm, pwrctrl->alives));
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+_func_exit_;
+
+	return _SUCCESS;
+}
+
+/*
+ * Caller: ISR
+ *
+ * If ISR's txdone,
+ * No more pkts for TX,
+ * Then driver shall call this fun. to power down firmware again.
+ */
+void rtw_unregister_tx_alive(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+
+_func_enter_;
+
+	pwrctrl = &padapter->pwrctrlpriv;
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, XMIT_ALIVE);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE) &&
+		(pwrctrl->bFwCurrentInPSMode == _TRUE))
+	{
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+				 ("%s: cpwm=0x%02x alives=0x%08x\n",
+				  __FUNCTION__, pwrctrl->cpwm, pwrctrl->alives));
+
+		if ((pwrctrl->alives == 0) &&
+			(pwrctrl->cpwm > PS_STATE_S0))
+		{
+			rtw_set_rpwm(padapter, PS_STATE_S0);
+		}
+	}
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+_func_exit_;
+}
+
+/*
+ * Caller: ISR
+ *
+ * If all commands have been done,
+ * and no more command to do,
+ * then driver shall call this fun. to power down firmware again.
+ */
+void rtw_unregister_cmd_alive(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+
+_func_enter_;
+
+	pwrctrl = &padapter->pwrctrlpriv;
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, CMD_ALIVE);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE) &&
+		(pwrctrl->bFwCurrentInPSMode == _TRUE))
+	{
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_info_,
+				 ("%s: cpwm=0x%02x alives=0x%08x\n",
+				  __FUNCTION__, pwrctrl->cpwm, pwrctrl->alives));
+
+		if ((pwrctrl->alives == 0) &&
+			(pwrctrl->cpwm > PS_STATE_S0))
+		{
+			rtw_set_rpwm(padapter, PS_STATE_S0);
+		}
+	}
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+_func_exit_;
+}
+
+/*
+ * Caller: ISR
+ */
+void rtw_unregister_rx_alive(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+
+_func_enter_;
+
+	pwrctrl = &padapter->pwrctrlpriv;
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, RECV_ALIVE);
+
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("rtw_unregister_rx_alive: cpwm=0x%02x alives=0x%08x\n",
+			  pwrctrl->cpwm, pwrctrl->alives));
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+_func_exit_;
+}
+
+void rtw_unregister_evt_alive(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+
+_func_enter_;
+
+	pwrctrl = &padapter->pwrctrlpriv;
+
+	unregister_task_alive(pwrctrl, EVT_ALIVE);
+
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("rtw_unregister_evt_alive: cpwm=0x%02x alives=0x%08x\n",
+			  pwrctrl->cpwm, pwrctrl->alives));
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+_func_exit_;
+}
+#endif	/* CONFIG_LPS_LCLK */
+
+#ifdef CONFIG_RESUME_IN_WORKQUEUE
+static void resume_workitem_callback(struct work_struct *work);
+#endif //CONFIG_RESUME_IN_WORKQUEUE
+
+void rtw_init_pwrctrl_priv(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+
+_func_enter_;
+
+#ifdef PLATFORM_WINDOWS
+	pwrctrlpriv->pnp_current_pwr_state=NdisDeviceStateD0;
+#endif
+
+	_init_pwrlock(&pwrctrlpriv->lock);
+	pwrctrlpriv->rf_pwrstate = rf_on;
+	pwrctrlpriv->ips_enter_cnts=0;
+	pwrctrlpriv->ips_leave_cnts=0;
+	pwrctrlpriv->bips_processing = _FALSE;
+
+	pwrctrlpriv->ips_mode = padapter->registrypriv.ips_mode;
+	pwrctrlpriv->ips_mode_req = padapter->registrypriv.ips_mode;
+
+	pwrctrlpriv->pwr_state_check_interval = RTW_PWR_STATE_CHK_INTERVAL;
+	pwrctrlpriv->pwr_state_check_cnts = 0;
+	pwrctrlpriv->bInternalAutoSuspend = _FALSE;
+	pwrctrlpriv->bInSuspend = _FALSE;
+	pwrctrlpriv->bkeepfwalive = _FALSE;
+
+#ifdef CONFIG_AUTOSUSPEND
+#ifdef SUPPORT_HW_RFOFF_DETECTED
+	pwrctrlpriv->pwr_state_check_interval = (pwrctrlpriv->bHWPwrPindetect) ?1000:2000;		
+#endif
+#endif
+
+	pwrctrlpriv->LpsIdleCount = 0;
+	//pwrctrlpriv->FWCtrlPSMode =padapter->registrypriv.power_mgnt;// PS_MODE_MIN;
+	if (padapter->registrypriv.mp_mode == 1)
+		pwrctrlpriv->power_mgnt =PS_MODE_ACTIVE ;
+	else	
+		pwrctrlpriv->power_mgnt =padapter->registrypriv.power_mgnt;// PS_MODE_MIN;
+	pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?_TRUE:_FALSE;
+
+	pwrctrlpriv->bFwCurrentInPSMode = _FALSE;
+
+	pwrctrlpriv->rpwm = 0;
+	pwrctrlpriv->cpwm = PS_STATE_S4;
+
+	pwrctrlpriv->pwr_mode = PS_MODE_ACTIVE;
+	pwrctrlpriv->smart_ps = padapter->registrypriv.smart_ps;
+	pwrctrlpriv->bcn_ant_mode = 0;
+
+	pwrctrlpriv->tog = 0x80;
+
+	pwrctrlpriv->btcoex_rfon = _FALSE;
+
+#ifdef CONFIG_LPS_LCLK
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&pwrctrlpriv->rpwm));
+
+	_init_workitem(&pwrctrlpriv->cpwm_event, cpwm_event_callback, NULL);
+
+#ifdef CONFIG_LPS_RPWM_TIMER
+	pwrctrlpriv->brpwmtimeout = _FALSE;
+	_init_workitem(&pwrctrlpriv->rpwmtimeoutwi, rpwmtimeout_workitem_callback, NULL);
+	_init_timer(&pwrctrlpriv->pwr_rpwm_timer, padapter->pnetdev, pwr_rpwm_timeout_handler, padapter);
+#endif // CONFIG_LPS_RPWM_TIMER
+#endif // CONFIG_LPS_LCLK
+
+#ifdef PLATFORM_LINUX
+	_init_timer(&(pwrctrlpriv->pwr_state_check_timer), padapter->pnetdev, pwr_state_check_handler, (u8 *)padapter);
+#endif
+
+	#ifdef CONFIG_RESUME_IN_WORKQUEUE
+	_init_workitem(&pwrctrlpriv->resume_work, resume_workitem_callback, NULL);
+	pwrctrlpriv->rtw_workqueue = create_singlethread_workqueue("rtw_workqueue");
+	#endif //CONFIG_RESUME_IN_WORKQUEUE
+
+	#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_ANDROID_POWER)
+	pwrctrlpriv->early_suspend.suspend = NULL;
+	rtw_register_early_suspend(pwrctrlpriv);
+	#endif //CONFIG_HAS_EARLYSUSPEND || CONFIG_ANDROID_POWER
+
+
+_func_exit_;
+
+}
+
+
+void rtw_free_pwrctrl_priv(PADAPTER adapter)
+{
+	struct pwrctrl_priv *pwrctrlpriv = &adapter->pwrctrlpriv;
+
+_func_enter_;
+
+	//_rtw_memset((unsigned char *)pwrctrlpriv, 0, sizeof(struct pwrctrl_priv));
+
+
+	#ifdef CONFIG_RESUME_IN_WORKQUEUE
+	if (pwrctrlpriv->rtw_workqueue) { 
+		flush_workqueue(pwrctrlpriv->rtw_workqueue);
+		destroy_workqueue(pwrctrlpriv->rtw_workqueue);
+	}
+	#endif
+
+
+	#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_ANDROID_POWER)
+	rtw_unregister_early_suspend(pwrctrlpriv);
+	#endif //CONFIG_HAS_EARLYSUSPEND || CONFIG_ANDROID_POWER
+
+	_free_pwrlock(&pwrctrlpriv->lock);
+
+_func_exit_;
+}
+
+#ifdef CONFIG_RESUME_IN_WORKQUEUE
+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+extern int rtw_resume_process(_adapter *padapter);
+#endif
+static void resume_workitem_callback(struct work_struct *work)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(work, struct pwrctrl_priv, resume_work);
+	_adapter *adapter = container_of(pwrpriv, _adapter, pwrctrlpriv);
+
+	DBG_871X("%s\n",__FUNCTION__);
+
+	#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+	rtw_resume_process(adapter);
+	#endif
+	
+}
+
+void rtw_resume_in_workqueue(struct pwrctrl_priv *pwrpriv)
+{
+	// accquire system's suspend lock preventing from falliing asleep while resume in workqueue
+	rtw_lock_suspend();
+	
+	#if 1
+	queue_work(pwrpriv->rtw_workqueue, &pwrpriv->resume_work);	
+	#else
+	_set_workitem(&pwrpriv->resume_work);
+	#endif
+}
+#endif //CONFIG_RESUME_IN_WORKQUEUE
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_ANDROID_POWER)
+inline bool rtw_is_earlysuspend_registered(struct pwrctrl_priv *pwrpriv)
+{
+	return (pwrpriv->early_suspend.suspend) ? _TRUE : _FALSE;
+}
+
+inline bool rtw_is_do_late_resume(struct pwrctrl_priv *pwrpriv)
+{
+	return (pwrpriv->do_late_resume) ? _TRUE : _FALSE;
+}
+
+inline void rtw_set_do_late_resume(struct pwrctrl_priv *pwrpriv, bool enable)
+{
+	pwrpriv->do_late_resume = enable;
+}
+#endif
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+extern int rtw_resume_process(_adapter *padapter);
+#endif
+static void rtw_early_suspend(struct early_suspend *h)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend);
+	DBG_871X("%s\n",__FUNCTION__);
+
+	rtw_set_do_late_resume(pwrpriv, _FALSE);
+}
+
+static void rtw_late_resume(struct early_suspend *h)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend);
+	_adapter *adapter = container_of(pwrpriv, _adapter, pwrctrlpriv);
+
+	DBG_871X("%s\n",__FUNCTION__);
+	if(pwrpriv->do_late_resume) {
+		#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+		rtw_set_do_late_resume(pwrpriv, _FALSE);
+		rtw_resume_process(adapter);
+		#endif
+	}
+}
+
+void rtw_register_early_suspend(struct pwrctrl_priv *pwrpriv)
+{
+	_adapter *adapter = container_of(pwrpriv, _adapter, pwrctrlpriv);
+
+#if defined(CONFIG_CONCURRENT_MODE)
+	if (adapter->adapter_type != PRIMARY_ADAPTER)
+		return;
+#endif
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+	//jeff: set the early suspend level before blank screen, so we wll do late resume after scree is lit
+	pwrpriv->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN - 20;
+	pwrpriv->early_suspend.suspend = rtw_early_suspend;
+	pwrpriv->early_suspend.resume = rtw_late_resume;
+	register_early_suspend(&pwrpriv->early_suspend);	
+
+	
+}
+
+void rtw_unregister_early_suspend(struct pwrctrl_priv *pwrpriv)
+{
+	_adapter *adapter = container_of(pwrpriv, _adapter, pwrctrlpriv);
+
+#if defined(CONFIG_CONCURRENT_MODE)
+	if (adapter->adapter_type != PRIMARY_ADAPTER)
+		return;
+#endif
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+	rtw_set_do_late_resume(pwrpriv, _FALSE);
+
+	if (pwrpriv->early_suspend.suspend) 
+		unregister_early_suspend(&pwrpriv->early_suspend);
+
+	pwrpriv->early_suspend.suspend = NULL;
+	pwrpriv->early_suspend.resume = NULL;
+}
+#endif //CONFIG_HAS_EARLYSUSPEND
+
+#ifdef CONFIG_ANDROID_POWER
+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+extern int rtw_resume_process(PADAPTER padapter);
+#endif
+static void rtw_early_suspend(android_early_suspend_t *h)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend);
+	DBG_871X("%s\n",__FUNCTION__);
+
+	rtw_set_do_late_resume(pwrpriv, _FALSE);
+}
+
+static void rtw_late_resume(android_early_suspend_t *h)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend);
+	_adapter *adapter = container_of(pwrpriv, _adapter, pwrctrlpriv);
+
+	DBG_871X("%s\n",__FUNCTION__);
+	if(pwrpriv->do_late_resume) {
+		#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+		rtw_set_do_late_resume(pwrpriv, _FALSE);
+		rtw_resume_process(adapter);
+		#endif
+	}
+}
+
+void rtw_register_early_suspend(struct pwrctrl_priv *pwrpriv)
+{
+	_adapter *adapter = container_of(pwrpriv, _adapter, pwrctrlpriv);
+
+#if defined(CONFIG_CONCURRENT_MODE)
+	if (adapter->adapter_type != PRIMARY_ADAPTER)
+		return;
+#endif
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+	//jeff: set the early suspend level before blank screen, so we wll do late resume after scree is lit
+	pwrpriv->early_suspend.level = ANDROID_EARLY_SUSPEND_LEVEL_BLANK_SCREEN - 20;
+	pwrpriv->early_suspend.suspend = rtw_early_suspend;
+	pwrpriv->early_suspend.resume = rtw_late_resume;
+	android_register_early_suspend(&pwrpriv->early_suspend);	
+}
+
+void rtw_unregister_early_suspend(struct pwrctrl_priv *pwrpriv)
+{
+	_adapter *adapter = container_of(pwrpriv, _adapter, pwrctrlpriv);
+
+#if defined(CONFIG_CONCURRENT_MODE)
+	if (adapter->adapter_type != PRIMARY_ADAPTER)
+		return;
+#endif
+
+	DBG_871X("%s\n", __FUNCTION__);
+
+	rtw_set_do_late_resume(pwrpriv, _FALSE);
+
+	if (pwrpriv->early_suspend.suspend) 
+		android_unregister_early_suspend(&pwrpriv->early_suspend);
+
+	pwrpriv->early_suspend.suspend = NULL;
+	pwrpriv->early_suspend.resume = NULL;
+}
+#endif //CONFIG_ANDROID_POWER
+
+u8 rtw_interface_ps_func(_adapter *padapter,HAL_INTF_PS_FUNC efunc_id,u8* val)
+{
+	u8 bResult = _TRUE;
+	rtw_hal_intf_ps_func(padapter,efunc_id,val);
+	
+	return bResult;
+}
+
+
+inline void rtw_set_ips_deny(_adapter *padapter, u32 ms)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+	pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime(ms);
+}
+
+/*
+* rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend
+* @adapter: pointer to _adapter structure
+* @ips_deffer_ms: the ms wiil prevent from falling into IPS after wakeup
+* Return _SUCCESS or _FAIL
+*/
+
+int _rtw_pwr_wakeup(_adapter *padapter, u32 ips_deffer_ms, const char *caller)
+{
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;	
+	int ret = _SUCCESS;
+	u32 start = rtw_get_current_time();
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (padapter->pbuddy_adapter)
+		LeaveAllPowerSaveMode(padapter->pbuddy_adapter);
+
+	if ((padapter->isprimary == _FALSE) && padapter->pbuddy_adapter){
+		padapter = padapter->pbuddy_adapter;
+		pwrpriv = &padapter->pwrctrlpriv;
+		pmlmepriv = &padapter->mlmepriv;
+	}
+#endif
+
+	if (pwrpriv->ips_deny_time < rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms))
+		pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms);
+
+
+	if (pwrpriv->ps_processing) {
+		DBG_871X("%s wait ps_processing...\n", __func__);
+		while (pwrpriv->ps_processing && rtw_get_passing_time_ms(start) <= 3000)
+			rtw_msleep_os(10);
+		if (pwrpriv->ps_processing)
+			DBG_871X("%s wait ps_processing timeout\n", __func__);
+		else
+			DBG_871X("%s wait ps_processing done\n", __func__);
+	}
+
+#ifdef DBG_CONFIG_ERROR_DETECT
+	if (rtw_hal_sreset_inprogress(padapter)) {
+		DBG_871X("%s wait sreset_inprogress...\n", __func__);
+		while (rtw_hal_sreset_inprogress(padapter) && rtw_get_passing_time_ms(start) <= 4000)
+			rtw_msleep_os(10);
+		if (rtw_hal_sreset_inprogress(padapter))
+			DBG_871X("%s wait sreset_inprogress timeout\n", __func__);
+		else
+			DBG_871X("%s wait sreset_inprogress done\n", __func__);
+	}
+#endif
+
+	if (pwrpriv->bInternalAutoSuspend == _FALSE && pwrpriv->bInSuspend) {
+		DBG_871X("%s wait bInSuspend...\n", __func__);
+		while (pwrpriv->bInSuspend 
+			&& ((rtw_get_passing_time_ms(start) <= 3000 && !rtw_is_do_late_resume(pwrpriv))
+				|| (rtw_get_passing_time_ms(start) <= 500 && rtw_is_do_late_resume(pwrpriv)))
+		) {
+			rtw_msleep_os(10);
+		}
+		if (pwrpriv->bInSuspend)
+			DBG_871X("%s wait bInSuspend timeout\n", __func__);
+		else
+			DBG_871X("%s wait bInSuspend done\n", __func__);
+	}
+
+	//System suspend is not allowed to wakeup
+	if((pwrpriv->bInternalAutoSuspend == _FALSE) && (_TRUE == pwrpriv->bInSuspend )){
+		ret = _FAIL;
+		goto exit;
+	}
+
+	//block???
+	if((pwrpriv->bInternalAutoSuspend == _TRUE)  && (padapter->net_closed == _TRUE)) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	//I think this should be check in IPS, LPS, autosuspend functions...
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+	{
+#if defined (CONFIG_BT_COEXIST)&& defined (CONFIG_AUTOSUSPEND)
+		if(_TRUE==pwrpriv->bInternalAutoSuspend){
+			if(0==pwrpriv->autopm_cnt){
+			#if (LINUX_VERSION_CODE>=KERNEL_VERSION(2,6,33))				
+				if (usb_autopm_get_interface(adapter_to_dvobj(padapter)->pusbintf) < 0) 
+				{
+					DBG_871X( "can't get autopm: \n");
+				}			
+			#elif (LINUX_VERSION_CODE>=KERNEL_VERSION(2,6,20))				
+				usb_autopm_disable(adapter_to_dvobj(padapter)->pusbintf);
+			#else
+				usb_autoresume_device(adapter_to_dvobj(padapter)->pusbdev, 1);
+			#endif
+			pwrpriv->autopm_cnt++;
+			}
+#endif	//#if defined (CONFIG_BT_COEXIST)&& defined (CONFIG_AUTOSUSPEND)
+		ret = _SUCCESS;
+		goto exit;
+#if defined (CONFIG_BT_COEXIST)&& defined (CONFIG_AUTOSUSPEND)
+		}
+#endif	//#if defined (CONFIG_BT_COEXIST)&& defined (CONFIG_AUTOSUSPEND)
+	}	
+
+	if(rf_off == pwrpriv->rf_pwrstate )
+	{		
+#ifdef CONFIG_USB_HCI
+#ifdef CONFIG_AUTOSUSPEND
+		 if(pwrpriv->brfoffbyhw==_TRUE)
+		{
+			DBG_8192C("hw still in rf_off state ...........\n");
+			ret = _FAIL;
+			goto exit;
+		}
+		else if(padapter->registrypriv.usbss_enable)
+		{
+			DBG_8192C("%s call autoresume_enter....\n",__FUNCTION__);
+			if(_FAIL ==  autoresume_enter(padapter))
+			{
+				DBG_8192C("======> autoresume fail.............\n");
+				ret = _FAIL;
+				goto exit;
+			}	
+		}
+		else
+#endif
+#endif
+		{
+#ifdef CONFIG_IPS
+			DBG_8192C("%s call ips_leave....\n",__FUNCTION__);
+			if(_FAIL ==  ips_leave(padapter))
+			{
+				DBG_8192C("======> ips_leave fail.............\n");
+				ret = _FAIL;
+				goto exit;
+			}
+#endif
+		}
+	}
+
+	//TODO: the following checking need to be merged...
+	if(padapter->bDriverStopped
+		|| !padapter->bup
+		|| !padapter->hw_init_completed
+	){
+		DBG_8192C("%s: bDriverStopped=%d, bup=%d, hw_init_completed=%u\n"
+			, caller
+		   	, padapter->bDriverStopped
+		   	, padapter->bup
+		   	, padapter->hw_init_completed);
+		ret= _FALSE;
+		goto exit;
+	}
+
+exit:
+	if (pwrpriv->ips_deny_time < rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms))
+		pwrpriv->ips_deny_time = rtw_get_current_time() + rtw_ms_to_systime(ips_deffer_ms);
+	return ret;
+
+}
+
+int rtw_pm_set_lps(_adapter *padapter, u8 mode)
+{
+	int	ret = 0;	
+	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+	
+	if ( mode < PS_MODE_NUM )
+	{
+		if(pwrctrlpriv->power_mgnt !=mode)
+		{
+			if(PS_MODE_ACTIVE == mode)
+			{
+				LeaveAllPowerSaveMode(padapter);
+			}
+			else
+			{
+				pwrctrlpriv->LpsIdleCount = 2;
+			}
+			pwrctrlpriv->power_mgnt = mode;
+			pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?_TRUE:_FALSE;
+		}
+	}
+	else
+	{
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+int rtw_pm_set_ips(_adapter *padapter, u8 mode)
+{
+	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
+
+	if( mode == IPS_NORMAL || mode == IPS_LEVEL_2 ) {
+		rtw_ips_mode_req(pwrctrlpriv, mode);
+		DBG_871X("%s %s\n", __FUNCTION__, mode == IPS_NORMAL?"IPS_NORMAL":"IPS_LEVEL_2");
+		return 0;
+	} 
+	else if(mode ==IPS_NONE){
+		rtw_ips_mode_req(pwrctrlpriv, mode);
+		DBG_871X("%s %s\n", __FUNCTION__, "IPS_NONE");
+		if((padapter->bSurpriseRemoved ==0)&&(_FAIL == rtw_pwr_wakeup(padapter)) )
+			return -EFAULT;
+	}
+	else {
+		return -EINVAL;
+	}
+	return 0;
+}
+
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_recv.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_recv.c
new file mode 100755
index 00000000..e7f8708c
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_recv.c
@@ -0,0 +1,4233 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_RECV_C_
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <recv_osdep.h>
+#include <mlme_osdep.h>
+#include <ip.h>
+#include <if_ether.h>
+#include <ethernet.h>
+
+#ifdef CONFIG_USB_HCI
+#include <usb_ops.h>
+#endif
+
+#if defined (PLATFORM_LINUX) && defined (PLATFORM_WINDOWS)
+
+#error "Shall be Linux or Windows, but not both!\n"
+
+#endif
+
+#include <wifi.h>
+#include <circ_buf.h>
+
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS);
+#endif //CONFIG_NEW_SIGNAL_STAT_PROCESS
+
+
+void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv)
+{
+
+
+_func_enter_;
+
+	_rtw_memset((u8 *)psta_recvpriv, 0, sizeof (struct sta_recv_priv));
+
+	_rtw_spinlock_init(&psta_recvpriv->lock);
+
+	//for(i=0; i<MAX_RX_NUMBLKS; i++)
+	//	_rtw_init_queue(&psta_recvpriv->blk_strms[i]);
+
+	_rtw_init_queue(&psta_recvpriv->defrag_q);
+
+_func_exit_;
+
+}
+
+sint _rtw_init_recv_priv(struct recv_priv *precvpriv, _adapter *padapter)
+{
+	sint i;
+
+	union recv_frame *precvframe;
+
+	sint	res=_SUCCESS;
+
+_func_enter_;
+
+	// We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc().
+	//_rtw_memset((unsigned char *)precvpriv, 0, sizeof (struct  recv_priv));
+
+	_rtw_spinlock_init(&precvpriv->lock);
+
+	_rtw_init_queue(&precvpriv->free_recv_queue);
+	_rtw_init_queue(&precvpriv->recv_pending_queue);
+	_rtw_init_queue(&precvpriv->uc_swdec_pending_queue);
+
+	precvpriv->adapter = padapter;
+
+	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
+
+	rtw_os_recv_resource_init(precvpriv, padapter);
+
+	precvpriv->pallocated_frame_buf = rtw_zvmalloc(NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ);
+	
+	if(precvpriv->pallocated_frame_buf==NULL){
+		res= _FAIL;
+		goto exit;
+	}
+	//_rtw_memset(precvpriv->pallocated_frame_buf, 0, NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ);
+
+	precvpriv->precv_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(precvpriv->pallocated_frame_buf), RXFRAME_ALIGN_SZ);
+	//precvpriv->precv_frame_buf = precvpriv->pallocated_frame_buf + RXFRAME_ALIGN_SZ -
+	//						((SIZE_PTR) (precvpriv->pallocated_frame_buf) &(RXFRAME_ALIGN_SZ-1));
+
+	precvframe = (union recv_frame*) precvpriv->precv_frame_buf;
+
+
+	for(i=0; i < NR_RECVFRAME ; i++)
+	{
+		_rtw_init_listhead(&(precvframe->u.list));
+
+		rtw_list_insert_tail(&(precvframe->u.list), &(precvpriv->free_recv_queue.queue));
+
+		res = rtw_os_recv_resource_alloc(padapter, precvframe);
+
+		precvframe->u.hdr.len = 0;
+
+		precvframe->u.hdr.adapter =padapter;
+		precvframe++;
+
+	}
+
+#ifdef CONFIG_USB_HCI
+
+	precvpriv->rx_pending_cnt=1;
+
+	_rtw_init_sema(&precvpriv->allrxreturnevt, 0);
+
+#endif
+
+	res = rtw_hal_init_recv_priv(padapter);
+
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+	#ifdef PLATFORM_LINUX
+	_init_timer(&precvpriv->signal_stat_timer, padapter->pnetdev, RTW_TIMER_HDL_NAME(signal_stat), padapter);
+	#elif defined(PLATFORM_OS_CE) || defined(PLATFORM_WINDOWS)
+	_init_timer(&precvpriv->signal_stat_timer, padapter->hndis_adapter, RTW_TIMER_HDL_NAME(signal_stat), padapter);
+	#endif
+
+	precvpriv->signal_stat_sampling_interval = 1000; //ms
+	//precvpriv->signal_stat_converging_constant = 5000; //ms
+
+	rtw_set_signal_stat_timer(precvpriv);
+#endif //CONFIG_NEW_SIGNAL_STAT_PROCESS
+
+exit:
+
+_func_exit_;
+
+	return res;
+
+}
+
+void rtw_mfree_recv_priv_lock(struct recv_priv *precvpriv);
+void rtw_mfree_recv_priv_lock(struct recv_priv *precvpriv)
+{
+	_rtw_spinlock_free(&precvpriv->lock);
+#ifdef CONFIG_RECV_THREAD_MODE	
+	_rtw_free_sema(&precvpriv->recv_sema);
+	_rtw_free_sema(&precvpriv->terminate_recvthread_sema);
+#endif
+
+	_rtw_spinlock_free(&precvpriv->free_recv_queue.lock);
+	_rtw_spinlock_free(&precvpriv->recv_pending_queue.lock);
+
+	_rtw_spinlock_free(&precvpriv->free_recv_buf_queue.lock);
+
+#ifdef CONFIG_USE_USB_BUFFER_ALLOC_RX
+	_rtw_spinlock_free(&precvpriv->recv_buf_pending_queue.lock);
+#endif	// CONFIG_USE_USB_BUFFER_ALLOC_RX
+}
+
+void _rtw_free_recv_priv (struct recv_priv *precvpriv)
+{
+	_adapter	*padapter = precvpriv->adapter;
+
+_func_enter_;
+
+	rtw_free_uc_swdec_pending_queue(padapter);
+
+	rtw_mfree_recv_priv_lock(precvpriv);
+
+	rtw_os_recv_resource_free(precvpriv);
+
+	if(precvpriv->pallocated_frame_buf) {
+		rtw_vmfree(precvpriv->pallocated_frame_buf, NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ);
+	}
+
+	rtw_hal_free_recv_priv(padapter);
+
+_func_exit_;
+
+}
+
+union recv_frame *_rtw_alloc_recvframe (_queue *pfree_recv_queue)
+{
+
+	union recv_frame  *precvframe;
+	_list	*plist, *phead;
+	_adapter *padapter;
+	struct recv_priv *precvpriv;
+_func_enter_;
+
+	if(_rtw_queue_empty(pfree_recv_queue) == _TRUE)
+	{
+		precvframe = NULL;
+	}
+	else
+	{
+		phead = get_list_head(pfree_recv_queue);
+
+		plist = get_next(phead);
+
+		precvframe = LIST_CONTAINOR(plist, union recv_frame, u);
+
+		rtw_list_delete(&precvframe->u.hdr.list);
+		padapter=precvframe->u.hdr.adapter;
+		if(padapter !=NULL){
+			precvpriv=&padapter->recvpriv;
+			if(pfree_recv_queue == &precvpriv->free_recv_queue)
+				precvpriv->free_recvframe_cnt--;
+		}
+	}
+
+_func_exit_;
+
+	return precvframe;
+
+}
+
+union recv_frame *rtw_alloc_recvframe (_queue *pfree_recv_queue)
+{
+	_irqL irqL;
+	union recv_frame  *precvframe;
+	
+	_enter_critical_bh(&pfree_recv_queue->lock, &irqL);
+
+	precvframe = _rtw_alloc_recvframe(pfree_recv_queue);
+
+	_exit_critical_bh(&pfree_recv_queue->lock, &irqL);
+
+	return precvframe;
+}
+
+void rtw_init_recvframe(union recv_frame *precvframe, struct recv_priv *precvpriv)
+{
+	/* Perry: This can be removed */
+	_rtw_init_listhead(&precvframe->u.hdr.list);
+
+	precvframe->u.hdr.len=0;
+}
+
+int rtw_free_recvframe(union recv_frame *precvframe, _queue *pfree_recv_queue)
+{
+	_irqL irqL;
+	_adapter *padapter=precvframe->u.hdr.adapter;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+_func_enter_;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if(padapter->adapter_type > PRIMARY_ADAPTER)
+	{
+		padapter = padapter->pbuddy_adapter;//get primary_padapter
+		precvpriv = &padapter->recvpriv;
+		pfree_recv_queue = &precvpriv->free_recv_queue;
+		precvframe->u.hdr.adapter = padapter;		
+	}	
+#endif
+
+
+#ifdef PLATFORM_WINDOWS
+	rtw_os_read_port(padapter, precvframe->u.hdr.precvbuf);
+#endif
+
+#if defined(PLATFORM_LINUX) || defined(PLATFORM_FREEBSD)
+
+	if(precvframe->u.hdr.pkt)
+	{
+#ifdef CONFIG_BSD_RX_USE_MBUF
+		m_freem(precvframe->u.hdr.pkt);
+#else	// CONFIG_BSD_RX_USE_MBUF
+		dev_kfree_skb_any(precvframe->u.hdr.pkt);//free skb by driver
+#endif	// CONFIG_BSD_RX_USE_MBUF
+		precvframe->u.hdr.pkt = NULL;
+	}
+
+#endif //defined(PLATFORM_LINUX) || defined(PLATFORM_FREEBSD)
+
+	_enter_critical_bh(&pfree_recv_queue->lock, &irqL);
+
+	rtw_list_delete(&(precvframe->u.hdr.list));
+
+	precvframe->u.hdr.len = 0;
+
+	rtw_list_insert_tail(&(precvframe->u.hdr.list), get_list_head(pfree_recv_queue));
+
+	if(padapter !=NULL){
+		if(pfree_recv_queue == &precvpriv->free_recv_queue)
+				precvpriv->free_recvframe_cnt++;
+	}
+
+      _exit_critical_bh(&pfree_recv_queue->lock, &irqL);
+
+_func_exit_;
+
+	return _SUCCESS;
+
+}
+
+
+
+
+sint _rtw_enqueue_recvframe(union recv_frame *precvframe, _queue *queue)
+{
+
+	_adapter *padapter=precvframe->u.hdr.adapter;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+_func_enter_;
+
+	//_rtw_init_listhead(&(precvframe->u.hdr.list));
+	rtw_list_delete(&(precvframe->u.hdr.list));
+
+
+	rtw_list_insert_tail(&(precvframe->u.hdr.list), get_list_head(queue));
+
+	if (padapter != NULL) {
+		if (queue == &precvpriv->free_recv_queue)
+			precvpriv->free_recvframe_cnt++;
+	}
+
+_func_exit_;
+
+	return _SUCCESS;
+}
+
+sint rtw_enqueue_recvframe(union recv_frame *precvframe, _queue *queue)
+{
+	sint ret;
+	_irqL irqL;
+	
+	//_spinlock(&pfree_recv_queue->lock);
+	_enter_critical_bh(&queue->lock, &irqL);
+	ret = _rtw_enqueue_recvframe(precvframe, queue);
+	//_rtw_spinunlock(&pfree_recv_queue->lock);
+	_exit_critical_bh(&queue->lock, &irqL);
+
+	return ret;
+}
+
+/*
+sint	rtw_enqueue_recvframe(union recv_frame *precvframe, _queue *queue)
+{
+	return rtw_free_recvframe(precvframe, queue);
+}
+*/
+
+
+
+
+/*
+caller : defrag ; recvframe_chk_defrag in recv_thread  (passive)
+pframequeue: defrag_queue : will be accessed in recv_thread  (passive)
+
+using spinlock to protect
+
+*/
+
+void rtw_free_recvframe_queue(_queue *pframequeue,  _queue *pfree_recv_queue)
+{
+	union	recv_frame 	*precvframe;
+	_list	*plist, *phead;
+
+_func_enter_;
+	_rtw_spinlock(&pframequeue->lock);
+
+	phead = get_list_head(pframequeue);
+	plist = get_next(phead);
+
+	while(rtw_end_of_queue_search(phead, plist) == _FALSE)
+	{
+		precvframe = LIST_CONTAINOR(plist, union recv_frame, u);
+
+		plist = get_next(plist);
+
+		//rtw_list_delete(&precvframe->u.hdr.list); // will do this in rtw_free_recvframe()
+
+		rtw_free_recvframe(precvframe, pfree_recv_queue);
+	}
+
+	_rtw_spinunlock(&pframequeue->lock);
+
+_func_exit_;
+
+}
+
+u32 rtw_free_uc_swdec_pending_queue(_adapter *adapter)
+{
+	u32 cnt = 0;
+	union recv_frame *pending_frame;
+	while((pending_frame=rtw_alloc_recvframe(&adapter->recvpriv.uc_swdec_pending_queue))) {
+		rtw_free_recvframe(pending_frame, &adapter->recvpriv.free_recv_queue);
+		DBG_871X("%s: dequeue uc_swdec_pending_queue\n", __func__);
+		cnt++;
+	}
+
+	return cnt;
+}
+
+
+sint rtw_enqueue_recvbuf_to_head(struct recv_buf *precvbuf, _queue *queue)
+{
+	_irqL irqL;
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	rtw_list_delete(&precvbuf->list);
+	rtw_list_insert_head(&precvbuf->list, get_list_head(queue));
+
+	_exit_critical_bh(&queue->lock, &irqL);
+
+	return _SUCCESS;
+}
+
+sint rtw_enqueue_recvbuf(struct recv_buf *precvbuf, _queue *queue)
+{
+	_irqL irqL;	
+#ifdef CONFIG_SDIO_HCI
+	_enter_critical_bh(&queue->lock, &irqL);
+#else
+	_enter_critical_ex(&queue->lock, &irqL);
+#endif/*#ifdef  CONFIG_SDIO_HCI*/
+
+	rtw_list_delete(&precvbuf->list);
+
+	rtw_list_insert_tail(&precvbuf->list, get_list_head(queue));
+#ifdef CONFIG_SDIO_HCI	
+	_exit_critical_bh(&queue->lock, &irqL);
+#else
+	_exit_critical_ex(&queue->lock, &irqL);
+#endif/*#ifdef  CONFIG_SDIO_HCI*/
+	return _SUCCESS;
+	
+}
+
+struct recv_buf *rtw_dequeue_recvbuf (_queue *queue)
+{
+	_irqL irqL;
+	struct recv_buf *precvbuf;
+	_list	*plist, *phead;	
+
+#ifdef CONFIG_SDIO_HCI
+	_enter_critical_bh(&queue->lock, &irqL);
+#else
+	_enter_critical_ex(&queue->lock, &irqL);
+#endif/*#ifdef  CONFIG_SDIO_HCI*/
+	
+	if(_rtw_queue_empty(queue) == _TRUE)
+	{
+		precvbuf = NULL;
+	}
+	else
+	{
+		phead = get_list_head(queue);
+
+		plist = get_next(phead);
+
+		precvbuf = LIST_CONTAINOR(plist, struct recv_buf, list);
+
+		rtw_list_delete(&precvbuf->list);
+		
+	}
+
+#ifdef CONFIG_SDIO_HCI
+	_exit_critical_bh(&queue->lock, &irqL);
+#else
+	_exit_critical_ex(&queue->lock, &irqL);
+#endif/*#ifdef  CONFIG_SDIO_HCI*/
+
+	return precvbuf;
+
+}
+
+sint recvframe_chkmic(_adapter *adapter,  union recv_frame *precvframe);
+sint recvframe_chkmic(_adapter *adapter,  union recv_frame *precvframe){
+
+	sint	i,res=_SUCCESS;
+	u32	datalen;
+	u8	miccode[8];
+	u8	bmic_err=_FALSE,brpt_micerror = _TRUE;
+	u8	*pframe, *payload,*pframemic;
+	u8	*mickey;
+	//u8	*iv,rxdata_key_idx=0;
+	struct	sta_info		*stainfo;
+	struct	rx_pkt_attrib	*prxattrib=&precvframe->u.hdr.attrib;
+	struct 	security_priv	*psecuritypriv=&adapter->securitypriv;
+
+	struct mlme_ext_priv	*pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+_func_enter_;
+
+	stainfo=rtw_get_stainfo(&adapter->stapriv ,&prxattrib->ta[0]);
+
+	if(prxattrib->encrypt ==_TKIP_)
+	{
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("\n recvframe_chkmic:prxattrib->encrypt ==_TKIP_\n"));
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("\n recvframe_chkmic:da=0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+			prxattrib->ra[0],prxattrib->ra[1],prxattrib->ra[2],prxattrib->ra[3],prxattrib->ra[4],prxattrib->ra[5]));
+
+		//calculate mic code
+		if(stainfo!= NULL)
+		{
+			if(IS_MCAST(prxattrib->ra))
+			{
+				//mickey=&psecuritypriv->dot118021XGrprxmickey.skey[0];
+				//iv = precvframe->u.hdr.rx_data+prxattrib->hdrlen;
+				//rxdata_key_idx =( ((iv[3])>>6)&0x3) ;
+				mickey=&psecuritypriv->dot118021XGrprxmickey[prxattrib->key_index].skey[0];
+				
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("\n recvframe_chkmic: bcmc key \n"));
+				//DBG_871X("\n recvframe_chkmic: bcmc key psecuritypriv->dot118021XGrpKeyid(%d),pmlmeinfo->key_index(%d) ,recv key_id(%d)\n",
+				//								psecuritypriv->dot118021XGrpKeyid,pmlmeinfo->key_index,rxdata_key_idx);
+				
+				if(psecuritypriv->binstallGrpkey==_FALSE)
+				{
+					res=_FAIL;
+					RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n"));
+					DBG_871X("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n");
+					goto exit;
+				}
+			}
+			else{
+				mickey=&stainfo->dot11tkiprxmickey.skey[0];
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("\n recvframe_chkmic: unicast key \n"));
+			}
+
+			datalen=precvframe->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len-prxattrib->icv_len-8;//icv_len included the mic code
+			pframe=precvframe->u.hdr.rx_data;
+			payload=pframe+prxattrib->hdrlen+prxattrib->iv_len;
+
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("\n prxattrib->iv_len=%d prxattrib->icv_len=%d\n",prxattrib->iv_len,prxattrib->icv_len));
+
+			//rtw_seccalctkipmic(&stainfo->dot11tkiprxmickey.skey[0],pframe,payload, datalen ,&miccode[0],(unsigned char)prxattrib->priority); //care the length of the data
+
+			rtw_seccalctkipmic(mickey,pframe,payload, datalen ,&miccode[0],(unsigned char)prxattrib->priority); //care the length of the data
+
+			pframemic=payload+datalen;
+
+			bmic_err=_FALSE;
+
+			for(i=0;i<8;i++){
+				if(miccode[i] != *(pframemic+i)){
+					RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("recvframe_chkmic:miccode[%d](%02x) != *(pframemic+%d)(%02x) ",i,miccode[i],i,*(pframemic+i)));
+					bmic_err=_TRUE;
+				}
+			}
+
+
+			if(bmic_err==_TRUE){
+
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("\n *(pframemic-8)-*(pframemic-1)=0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+					*(pframemic-8),*(pframemic-7),*(pframemic-6),*(pframemic-5),*(pframemic-4),*(pframemic-3),*(pframemic-2),*(pframemic-1)));
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("\n *(pframemic-16)-*(pframemic-9)=0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+					*(pframemic-16),*(pframemic-15),*(pframemic-14),*(pframemic-13),*(pframemic-12),*(pframemic-11),*(pframemic-10),*(pframemic-9)));
+
+				{
+					uint i;
+					RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("\n ======demp packet (len=%d)======\n",precvframe->u.hdr.len));
+					for(i=0;i<precvframe->u.hdr.len;i=i+8){
+						RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x",
+							*(precvframe->u.hdr.rx_data+i),*(precvframe->u.hdr.rx_data+i+1),
+							*(precvframe->u.hdr.rx_data+i+2),*(precvframe->u.hdr.rx_data+i+3),
+							*(precvframe->u.hdr.rx_data+i+4),*(precvframe->u.hdr.rx_data+i+5),
+							*(precvframe->u.hdr.rx_data+i+6),*(precvframe->u.hdr.rx_data+i+7)));
+					}
+					RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("\n ======demp packet end [len=%d]======\n",precvframe->u.hdr.len));
+					RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("\n hrdlen=%d, \n",prxattrib->hdrlen));
+				}
+
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("ra=0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x psecuritypriv->binstallGrpkey=%d ",
+					prxattrib->ra[0],prxattrib->ra[1],prxattrib->ra[2],
+					prxattrib->ra[3],prxattrib->ra[4],prxattrib->ra[5],psecuritypriv->binstallGrpkey));
+
+				// double check key_index for some timing issue ,
+				// cannot compare with psecuritypriv->dot118021XGrpKeyid also cause timing issue
+				if((IS_MCAST(prxattrib->ra)==_TRUE)  && (prxattrib->key_index != pmlmeinfo->key_index ))
+					brpt_micerror = _FALSE;
+				
+				if((prxattrib->bdecrypted ==_TRUE)&& (brpt_micerror == _TRUE))
+				{
+					rtw_handle_tkip_mic_err(adapter,(u8)IS_MCAST(prxattrib->ra));
+					RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,(" mic error :prxattrib->bdecrypted=%d ",prxattrib->bdecrypted));
+					DBG_871X(" mic error :prxattrib->bdecrypted=%d\n",prxattrib->bdecrypted);
+				}
+				else
+				{
+					RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,(" mic error :prxattrib->bdecrypted=%d ",prxattrib->bdecrypted));
+					DBG_871X(" mic error :prxattrib->bdecrypted=%d\n",prxattrib->bdecrypted);
+				}
+
+				res=_FAIL;
+
+			}
+			else{
+				//mic checked ok
+				if((psecuritypriv->bcheck_grpkey ==_FALSE)&&(IS_MCAST(prxattrib->ra)==_TRUE)){
+					psecuritypriv->bcheck_grpkey =_TRUE;
+					RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("psecuritypriv->bcheck_grpkey =_TRUE"));
+				}
+			}
+
+		}
+		else
+		{
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("recvframe_chkmic: rtw_get_stainfo==NULL!!!\n"));
+		}
+
+		recvframe_pull_tail(precvframe, 8);
+
+	}
+
+exit:
+
+_func_exit_;
+
+	return res;
+
+}
+
+//decrypt and set the ivlen,icvlen of the recv_frame
+union recv_frame * decryptor(_adapter *padapter,union recv_frame *precv_frame);
+union recv_frame * decryptor(_adapter *padapter,union recv_frame *precv_frame)
+{
+
+	struct rx_pkt_attrib *prxattrib = &precv_frame->u.hdr.attrib;
+	struct security_priv *psecuritypriv=&padapter->securitypriv;
+	union recv_frame *return_packet=precv_frame;
+	u32	 res=_SUCCESS;
+_func_enter_;
+
+	RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("prxstat->decrypted=%x prxattrib->encrypt = 0x%03x\n",prxattrib->bdecrypted,prxattrib->encrypt));
+
+	if(prxattrib->encrypt>0)
+	{
+		u8 *iv = precv_frame->u.hdr.rx_data+prxattrib->hdrlen;
+		prxattrib->key_index = ( ((iv[3])>>6)&0x3) ;
+
+		if(prxattrib->key_index > WEP_KEYS)
+		{
+			DBG_871X("prxattrib->key_index(%d) > WEP_KEYS \n", prxattrib->key_index);
+
+			switch(prxattrib->encrypt){
+				case _WEP40_:
+				case _WEP104_:
+					prxattrib->key_index = psecuritypriv->dot11PrivacyKeyIndex;
+					break;
+				case _TKIP_:			
+				case _AES_:						
+				default:
+					prxattrib->key_index = psecuritypriv->dot118021XGrpKeyid;
+					break;
+			}	
+		}			
+	}
+
+	if((prxattrib->encrypt>0) && ((prxattrib->bdecrypted==0) ||(psecuritypriv->sw_decrypt==_TRUE)))
+	{
+
+#ifdef CONFIG_CONCURRENT_MODE
+		if(!IS_MCAST(prxattrib->ra))//bc/mc packets use sw decryption for concurrent mode
+#endif			
+		psecuritypriv->hw_decrypted=_FALSE;
+
+		#ifdef DBG_RX_DECRYPTOR
+		DBG_871X("prxstat->bdecrypted:%d,  prxattrib->encrypt:%d,  Setting psecuritypriv->hw_decrypted = %d\n"
+			, prxattrib->bdecrypted ,prxattrib->encrypt, psecuritypriv->hw_decrypted);
+		#endif
+
+		switch(prxattrib->encrypt){
+		case _WEP40_:
+		case _WEP104_:
+			rtw_wep_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		case _TKIP_:
+			res = rtw_tkip_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		case _AES_:
+			res = rtw_aes_decrypt(padapter, (u8 * )precv_frame);
+			break;
+#ifdef CONFIG_WAPI_SUPPORT
+		case _SMS4_:
+			rtw_sms4_decrypt(padapter, (u8 * )precv_frame);
+			break;
+#endif
+		default:
+				break;
+		}
+	}
+	else if(prxattrib->bdecrypted==1
+		&& prxattrib->encrypt >0
+		&& (psecuritypriv->busetkipkey==1 || prxattrib->encrypt !=_TKIP_ )
+		)
+	{
+#if 0
+		if((prxstat->icv==1)&&(prxattrib->encrypt!=_AES_))
+		{
+			psecuritypriv->hw_decrypted=_FALSE;
+
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("psecuritypriv->hw_decrypted=_FALSE"));
+
+			rtw_free_recvframe(precv_frame, &padapter->recvpriv.free_recv_queue);
+
+			return_packet=NULL;
+
+		}
+		else
+#endif
+		{
+			psecuritypriv->hw_decrypted=_TRUE;
+			#ifdef DBG_RX_DECRYPTOR
+			DBG_871X("prxstat->bdecrypted:%d,  prxattrib->encrypt:%d,  Setting psecuritypriv->hw_decrypted = %d\n"
+			, prxattrib->bdecrypted ,prxattrib->encrypt, psecuritypriv->hw_decrypted);
+			#endif
+
+		}
+	}
+	else {
+		#ifdef DBG_RX_DECRYPTOR
+		DBG_871X("prxstat->bdecrypted:%d,  prxattrib->encrypt:%d,  psecuritypriv->hw_decrypted:%d\n"
+		, prxattrib->bdecrypted ,prxattrib->encrypt, psecuritypriv->hw_decrypted);
+		#endif
+	}
+	
+	if(res == _FAIL)
+	{
+		rtw_free_recvframe(return_packet,&padapter->recvpriv.free_recv_queue);			
+		return_packet = NULL;
+		
+	}
+	//recvframe_chkmic(adapter, precv_frame);   //move to recvframme_defrag function
+
+_func_exit_;
+
+	return return_packet;
+
+}
+//###set the security information in the recv_frame
+union recv_frame * portctrl(_adapter *adapter,union recv_frame * precv_frame);
+union recv_frame * portctrl(_adapter *adapter,union recv_frame * precv_frame)
+{
+	u8   *psta_addr, *ptr;
+	uint  auth_alg;
+	struct recv_frame_hdr *pfhdr;
+	struct sta_info *psta;
+	struct sta_priv *pstapriv ;
+	union recv_frame *prtnframe;
+	u16	ether_type=0;
+	u16  eapol_type = 0x888e;//for Funia BD's WPA issue  
+	struct rx_pkt_attrib *pattrib;
+
+_func_enter_;
+
+	pstapriv = &adapter->stapriv;
+	psta = rtw_get_stainfo(pstapriv, psta_addr);
+
+	auth_alg = adapter->securitypriv.dot11AuthAlgrthm;
+
+	ptr = get_recvframe_data(precv_frame);
+	pfhdr = &precv_frame->u.hdr;
+	pattrib = &pfhdr->attrib;
+	psta_addr = pattrib->ta;
+
+	prtnframe = NULL;
+
+	RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("########portctrl:adapter->securitypriv.dot11AuthAlgrthm=%d\n",adapter->securitypriv.dot11AuthAlgrthm));
+
+	if(auth_alg==2)
+	{
+		if ((psta!=NULL) && (psta->ieee8021x_blocked))
+		{
+			//blocked
+			//only accept EAPOL frame
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("########portctrl:psta->ieee8021x_blocked==1\n"));
+
+			prtnframe=precv_frame;
+
+			//get ether_type
+			ptr=ptr+pfhdr->attrib.hdrlen+pfhdr->attrib.iv_len+LLC_HEADER_SIZE;
+			_rtw_memcpy(&ether_type,ptr, 2);
+			ether_type= ntohs((unsigned short )ether_type);
+
+		        if (ether_type == eapol_type) {
+				prtnframe=precv_frame;
+			}
+			else {
+				//free this frame
+				rtw_free_recvframe(precv_frame, &adapter->recvpriv.free_recv_queue);
+				prtnframe=NULL;
+			}
+		}
+		else
+		{
+			//allowed
+			//check decryption status, and decrypt the frame if needed
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("########portctrl:psta->ieee8021x_blocked==0\n"));
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("portctrl:precv_frame->hdr.attrib.privacy=%x\n",precv_frame->u.hdr.attrib.privacy));
+
+			if (pattrib->bdecrypted == 0)
+			{
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("portctrl:prxstat->decrypted=%x\n", pattrib->bdecrypted));
+			}
+
+			prtnframe=precv_frame;
+			//check is the EAPOL frame or not (Rekey)
+			if(ether_type == eapol_type){
+
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_notice_,("########portctrl:ether_type == 0x888e\n"));
+				//check Rekey
+
+				prtnframe=precv_frame;
+			}
+			else{
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("########portctrl:ether_type=0x%04x\n", ether_type));
+			}
+		}
+	}
+	else
+	{
+		prtnframe=precv_frame;
+	}
+
+_func_exit_;
+
+		return prtnframe;
+
+}
+
+sint recv_decache(union recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache);
+sint recv_decache(union recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache)
+{
+	sint tid = precv_frame->u.hdr.attrib.priority;
+
+	u16 seq_ctrl = ( (precv_frame->u.hdr.attrib.seq_num&0xffff) << 4) |
+		(precv_frame->u.hdr.attrib.frag_num & 0xf);
+
+_func_enter_;
+
+	if(tid>15)
+	{
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_decache, (tid>15)! seq_ctrl=0x%x, tid=0x%x\n", seq_ctrl, tid));
+
+		return _FAIL;
+	}
+
+	if(1)//if(bretry)
+	{
+		if(seq_ctrl == prxcache->tid_rxseq[tid])
+		{
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_decache, seq_ctrl=0x%x, tid=0x%x, tid_rxseq=0x%x\n", seq_ctrl, tid, prxcache->tid_rxseq[tid]));
+
+			return _FAIL;
+		}
+	}
+
+	prxcache->tid_rxseq[tid] = seq_ctrl;
+
+_func_exit_;
+
+	return _SUCCESS;
+
+}
+
+void process_pwrbit_data(_adapter *padapter, union recv_frame *precv_frame);
+void process_pwrbit_data(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_AP_MODE
+	unsigned char pwrbit;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta=NULL;
+
+	psta = rtw_get_stainfo(pstapriv, pattrib->src);
+
+	pwrbit = GetPwrMgt(ptr);
+
+	if(psta)
+	{
+		if(pwrbit)
+		{
+			if(!(psta->state & WIFI_SLEEP_STATE))
+			{
+				//psta->state |= WIFI_SLEEP_STATE;
+				//pstapriv->sta_dz_bitmap |= BIT(psta->aid);
+
+				stop_sta_xmit(padapter, psta);
+
+				//DBG_871X("to sleep, sta_dz_bitmap=%x\n", pstapriv->sta_dz_bitmap);
+			}
+		}
+		else
+		{
+			if(psta->state & WIFI_SLEEP_STATE)
+			{
+				//psta->state ^= WIFI_SLEEP_STATE;
+				//pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+
+				wakeup_sta_to_xmit(padapter, psta);
+
+				//DBG_871X("to wakeup, sta_dz_bitmap=%x\n", pstapriv->sta_dz_bitmap);
+			}
+		}
+
+	}
+
+#endif
+}
+
+void process_wmmps_data(_adapter *padapter, union recv_frame *precv_frame);
+void process_wmmps_data(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_AP_MODE		
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta=NULL;
+
+	psta = rtw_get_stainfo(pstapriv, pattrib->src);
+	
+	if(!psta) return;
+
+#ifdef CONFIG_TDLS
+	if( !(psta->tdls_sta_state & TDLS_LINKED_STATE ) )
+	{
+#endif //CONFIG_TDLS
+
+	if(!psta->qos_option)
+		return;
+
+	if(!(psta->qos_info&0xf))
+		return;
+		
+#ifdef CONFIG_TDLS
+	}
+#endif //CONFIG_TDLS		
+
+	if(psta->state&WIFI_SLEEP_STATE)
+	{
+		u8 wmmps_ac=0;	
+		
+		switch(pattrib->priority)
+		{
+			case 1:
+			case 2:
+				wmmps_ac = psta->uapsd_bk&BIT(1);
+				break;
+			case 4:
+			case 5:
+				wmmps_ac = psta->uapsd_vi&BIT(1);
+				break;
+			case 6:
+			case 7:
+				wmmps_ac = psta->uapsd_vo&BIT(1);
+				break;
+			case 0:
+			case 3:
+			default:
+				wmmps_ac = psta->uapsd_be&BIT(1);
+				break;	
+		}
+
+		if(wmmps_ac)
+		{
+			if(psta->sleepq_ac_len>0)
+			{
+				//process received triggered frame
+				xmit_delivery_enabled_frames(padapter, psta);
+			}
+			else
+			{
+				//issue one qos null frame with More data bit = 0 and the EOSP bit set (=1)
+				issue_qos_nulldata(padapter, psta->hwaddr, (u16)pattrib->priority, 0, 0);
+			}
+		}
+				
+	}
+
+	
+#endif	
+
+}
+
+#ifdef CONFIG_TDLS
+sint OnTDLS(_adapter *adapter, union recv_frame *precv_frame)
+{
+	struct rx_pkt_attrib	*pattrib = & precv_frame->u.hdr.attrib;
+	sint ret = _SUCCESS;
+	u8 *paction = get_recvframe_data(precv_frame);
+	u8 category_field = 1;
+#ifdef CONFIG_WFD
+	u8 WFA_OUI[3] = { 0x50, 0x6f, 0x9a };
+#endif //CONFIG_WFD
+	struct tdls_info *ptdlsinfo = &(adapter->tdlsinfo);
+
+	//point to action field
+	paction+=pattrib->hdrlen 
+			+ pattrib->iv_len 
+			+ SNAP_SIZE 
+			+ ETH_TYPE_LEN 
+			+ PAYLOAD_TYPE_LEN 
+			+ category_field;
+
+	if(ptdlsinfo->enable == 0)
+	{
+		DBG_871X("recv tdls frame, "
+				"but tdls haven't enabled\n");
+		ret = _FAIL;
+		return ret;
+	}
+	
+	switch(*paction){
+		case TDLS_SETUP_REQUEST:
+			DBG_871X("recv tdls setup request frame\n");
+			ret=On_TDLS_Setup_Req(adapter, precv_frame);
+			break;
+		case TDLS_SETUP_RESPONSE:
+			DBG_871X("recv tdls setup response frame\n");			
+			ret=On_TDLS_Setup_Rsp(adapter, precv_frame);
+			break;
+		case TDLS_SETUP_CONFIRM:
+			DBG_871X("recv tdls setup confirm frame\n");
+			ret=On_TDLS_Setup_Cfm(adapter, precv_frame);
+			break;
+		case TDLS_TEARDOWN:
+			DBG_871X("recv tdls teardown, free sta_info\n");
+			ret=On_TDLS_Teardown(adapter, precv_frame);
+			break;
+		case TDLS_DISCOVERY_REQUEST:
+			DBG_871X("recv tdls discovery request frame\n");
+			ret=On_TDLS_Dis_Req(adapter, precv_frame);
+			break;
+		case TDLS_PEER_TRAFFIC_RESPONSE:
+			DBG_871X("recv tdls peer traffic response frame\n");
+			ret=On_TDLS_Peer_Traffic_Rsp(adapter, precv_frame);
+			break;
+		case TDLS_CHANNEL_SWITCH_REQUEST:
+			DBG_871X("recv tdls channel switch request frame\n");
+			ret=On_TDLS_Ch_Switch_Req(adapter, precv_frame);
+			break;
+		case TDLS_CHANNEL_SWITCH_RESPONSE:
+			DBG_871X("recv tdls channel switch response frame\n");
+			ret=On_TDLS_Ch_Switch_Rsp(adapter, precv_frame);
+			break;
+#ifdef CONFIG_WFD			
+		case 0x50:	//First byte of WFA OUI
+			if( _rtw_memcmp(WFA_OUI, (paction), 3) )
+			{
+				if( *(paction + 3) == 0x04)	//Probe request frame
+				{
+					//WFDTDLS: for sigma test, do not setup direct link automatically
+					ptdlsinfo->dev_discovered = 1;
+					DBG_871X("recv tunneled probe request frame\n");
+					issue_tunneled_probe_rsp(adapter, precv_frame);
+				}
+				if( *(paction + 3) == 0x05)	//Probe response frame
+				{
+					//WFDTDLS: for sigma test, do not setup direct link automatically
+					ptdlsinfo->dev_discovered = 1;
+					DBG_871X("recv tunneled probe response frame\n");
+				}
+			}
+			break;
+#endif //CONFIG_WFD
+		default:
+			DBG_871X("receive TDLS frame but not supported\n");
+			ret=_FAIL;
+			break;
+	}
+
+exit:
+	return ret;
+	
+}
+#endif
+
+void count_rx_stats(_adapter *padapter, union recv_frame *prframe, struct sta_info*sta);
+void count_rx_stats(_adapter *padapter, union recv_frame *prframe, struct sta_info*sta)
+{
+	int	sz;
+	struct sta_info		*psta = NULL;
+	struct stainfo_stats	*pstats = NULL;
+	struct rx_pkt_attrib	*pattrib = & prframe->u.hdr.attrib;
+	struct recv_priv		*precvpriv = &padapter->recvpriv;
+
+	sz = get_recvframe_len(prframe);
+	precvpriv->rx_bytes += sz;
+
+	padapter->mlmepriv.LinkDetectInfo.NumRxOkInPeriod++;
+
+	if( (!MacAddr_isBcst(pattrib->dst)) && (!IS_MCAST(pattrib->dst))){
+		padapter->mlmepriv.LinkDetectInfo.NumRxUnicastOkInPeriod++;
+	}
+
+	if(sta)
+		psta = sta;
+	else
+		psta = prframe->u.hdr.psta;
+
+	if(psta)
+	{
+		pstats = &psta->sta_stats;
+
+		pstats->rx_data_pkts++;
+		pstats->rx_bytes += sz;
+	}
+
+}
+
+sint sta2sta_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info**psta
+);
+sint sta2sta_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info**psta
+)
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	sint ret = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = & precv_frame->u.hdr.attrib;
+	struct	sta_priv 		*pstapriv = &adapter->stapriv;
+	struct	mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = myid(&adapter->eeprompriv);
+	u8 * sta_addr = NULL;
+	sint bmcast = IS_MCAST(pattrib->dst);
+
+#ifdef CONFIG_TDLS	
+	struct tdls_info *ptdlsinfo = &adapter->tdlsinfo;
+	struct sta_info *ptdls_sta=NULL;
+	u8 *psnap_type=ptr+pattrib->hdrlen + pattrib->iv_len+SNAP_SIZE;
+	//frame body located after [+2]: ether-type, [+1]: payload type
+	u8 *pframe_body = psnap_type+2+1;
+#endif
+
+_func_enter_;
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE))
+	{
+
+		// filter packets that SA is myself or multicast or broadcast
+		if (_rtw_memcmp(myhwaddr, pattrib->src, ETH_ALEN)){
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,(" SA==myself \n"));
+			ret= _FAIL;
+			goto exit;
+		}
+
+		if( (!_rtw_memcmp(myhwaddr, pattrib->dst, ETH_ALEN))	&& (!bmcast) ){
+			ret= _FAIL;
+			goto exit;
+		}
+
+		if( _rtw_memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		   _rtw_memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		   (!_rtw_memcmp(pattrib->bssid, mybssid, ETH_ALEN)) ) {
+			ret= _FAIL;
+			goto exit;
+		}
+
+		sta_addr = pattrib->src;
+
+	}
+	else if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE)
+	{
+#ifdef CONFIG_TDLS
+
+		//direct link data transfer
+		if(ptdlsinfo->setup_state == TDLS_LINKED_STATE){
+			ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->src);
+			if(ptdls_sta==NULL)
+			{
+				ret=_FAIL;
+				goto exit;
+			}
+			else if(ptdls_sta->tdls_sta_state&TDLS_LINKED_STATE)
+			{
+
+				//drop QoS-SubType Data, including QoS NULL, excluding QoS-Data
+				if( (GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE )== WIFI_QOS_DATA_TYPE)
+				{
+					if(GetFrameSubType(ptr)&(BIT(4)|BIT(5)|BIT(6)))
+					{
+						DBG_871X("drop QoS-Sybtype Data\n");
+					ret= _FAIL;
+					goto exit;
+					}
+				}
+				// filter packets that SA is myself or multicast or broadcast
+				if (_rtw_memcmp(myhwaddr, pattrib->src, ETH_ALEN)){
+					ret= _FAIL;
+					goto exit;
+				}
+				// da should be for me
+				if((!_rtw_memcmp(myhwaddr, pattrib->dst, ETH_ALEN))&& (!bmcast))
+				{
+					ret= _FAIL;
+					goto exit;
+				}
+				// check BSSID
+				if( _rtw_memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+				     _rtw_memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+				     (!_rtw_memcmp(pattrib->bssid, mybssid, ETH_ALEN)) )
+				{
+					ret= _FAIL;
+					goto exit;
+				}
+
+				//process UAPSD tdls sta
+				process_pwrbit_data(adapter, precv_frame);
+
+				// if NULL-frame, check pwrbit
+				if ((GetFrameSubType(ptr)) == WIFI_DATA_NULL)
+				{
+					//NULL-frame with pwrbit=1, buffer_STA should buffer frames for sleep_STA
+					if(GetPwrMgt(ptr))
+					{
+						DBG_871X("TDLS: recv peer null frame with pwr bit 1\n");
+						ptdls_sta->tdls_sta_state|=TDLS_PEER_SLEEP_STATE;
+					// it would be triggered when we are off channel and receiving NULL DATA
+					// we can confirm that peer STA is at off channel
+					}
+					else if(ptdls_sta->tdls_sta_state&TDLS_CH_SWITCH_ON_STATE)
+					{
+						if((ptdls_sta->tdls_sta_state & TDLS_PEER_AT_OFF_STATE) != TDLS_PEER_AT_OFF_STATE)
+						{
+							issue_nulldata_to_TDLS_peer_STA(adapter, ptdls_sta, 0);
+							ptdls_sta->tdls_sta_state |= TDLS_PEER_AT_OFF_STATE;
+							On_TDLS_Peer_Traffic_Rsp(adapter, precv_frame);
+						}
+					}
+
+					ret= _FAIL;
+					goto exit;
+				}
+				//receive some of all TDLS management frames, process it at ON_TDLS
+				if((_rtw_memcmp(psnap_type, SNAP_ETH_TYPE_TDLS, 2))){
+					ret= OnTDLS(adapter, precv_frame);
+					goto exit;
+				}
+				
+			}
+
+			sta_addr = pattrib->src;
+			
+		}		
+		else
+#endif //CONFIG_TDLS
+		{
+			// For Station mode, sa and bssid should always be BSSID, and DA is my mac-address
+			if(!_rtw_memcmp(pattrib->bssid, pattrib->src, ETH_ALEN) )
+			{
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("bssid != TA under STATION_MODE; drop pkt\n"));
+				ret= _FAIL;
+				goto exit;
+		}
+
+		sta_addr = pattrib->bssid;
+		}
+
+	}
+	else if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+	{
+		if (bmcast)
+		{
+			// For AP mode, if DA == MCAST, then BSSID should be also MCAST
+			if (!IS_MCAST(pattrib->bssid)){
+					ret= _FAIL;
+					goto exit;
+			}
+		}
+		else // not mc-frame
+		{
+			// For AP mode, if DA is non-MCAST, then it must be BSSID, and bssid == BSSID
+			if(!_rtw_memcmp(pattrib->bssid, pattrib->dst, ETH_ALEN)) {
+				ret= _FAIL;
+				goto exit;
+			}
+
+			sta_addr = pattrib->src;
+		}
+
+	}
+	else if(check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE)
+	{
+		_rtw_memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+		_rtw_memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN);
+		_rtw_memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+		_rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+		sta_addr = mybssid;
+	}
+	else
+	{
+		ret  = _FAIL;
+	}
+
+
+
+	if(bmcast)
+		*psta = rtw_get_bcmc_stainfo(adapter);
+	else
+		*psta = rtw_get_stainfo(pstapriv, sta_addr); // get ap_info
+
+#ifdef CONFIG_TDLS
+	if(ptdls_sta != NULL)
+		*psta = ptdls_sta;
+#endif //CONFIG_TDLS
+
+	if (*psta == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("can't get psta under sta2sta_data_frame ; drop pkt\n"));
+#ifdef CONFIG_MP_INCLUDED
+		if (adapter->registrypriv.mp_mode == 1)
+		{
+			if(check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE)
+			adapter->mppriv.rx_pktloss++;
+		}
+#endif
+		ret= _FAIL;
+		goto exit;
+	}
+
+exit:
+_func_exit_;
+	return ret;
+
+}
+
+sint ap2sta_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info**psta );
+sint ap2sta_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info**psta )
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = & precv_frame->u.hdr.attrib;
+	sint ret = _SUCCESS;
+	struct	sta_priv 		*pstapriv = &adapter->stapriv;
+	struct	mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = myid(&adapter->eeprompriv);
+	sint bmcast = IS_MCAST(pattrib->dst);
+
+_func_enter_;
+
+	if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == _TRUE)
+		&& (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE 
+			|| check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == _TRUE	)
+		)
+	{
+
+		// filter packets that SA is myself or multicast or broadcast
+		if (_rtw_memcmp(myhwaddr, pattrib->src, ETH_ALEN)){
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,(" SA==myself \n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s SA="MAC_FMT", myhwaddr="MAC_FMT"\n",
+				__FUNCTION__, MAC_ARG(pattrib->src), MAC_ARG(myhwaddr));
+			#endif			
+			ret= _FAIL;
+			goto exit;
+		}
+
+		// da should be for me
+		if((!_rtw_memcmp(myhwaddr, pattrib->dst, ETH_ALEN))&& (!bmcast))
+		{
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,
+				(" ap2sta_data_frame:  compare DA fail; DA="MAC_FMT"\n", MAC_ARG(pattrib->dst)));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s DA="MAC_FMT"\n", __func__, MAC_ARG(pattrib->dst));
+			#endif
+			ret= _FAIL;
+			goto exit;
+		}
+
+
+		// check BSSID
+		if( _rtw_memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		     _rtw_memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		     (!_rtw_memcmp(pattrib->bssid, mybssid, ETH_ALEN)) )
+		{
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,
+				(" ap2sta_data_frame:  compare BSSID fail ; BSSID="MAC_FMT"\n", MAC_ARG(pattrib->bssid)));
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("mybssid="MAC_FMT"\n", MAC_ARG(mybssid)));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s BSSID="MAC_FMT", mybssid="MAC_FMT"\n",
+				__FUNCTION__, MAC_ARG(pattrib->bssid), MAC_ARG(mybssid));
+			DBG_871X( "this adapter = %d, buddy adapter = %d\n", adapter->adapter_type, adapter->pbuddy_adapter->adapter_type );
+			#endif
+
+			if(!bmcast)
+			{
+				DBG_871X("issue_deauth to the nonassociated ap=" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->bssid));
+				issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+			}
+
+			ret= _FAIL;
+			goto exit;
+		}
+
+		if(bmcast)
+			*psta = rtw_get_bcmc_stainfo(adapter);
+		else
+			*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); // get ap_info
+
+		if (*psta == NULL) {
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("ap2sta: can't get psta under STATION_MODE ; drop pkt\n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s can't get psta under STATION_MODE ; drop pkt\n", __FUNCTION__);
+			#endif
+			ret= _FAIL;
+			goto exit;
+		}
+
+		//if ((GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) {
+		//}
+
+		if (GetFrameSubType(ptr) & BIT(6)) {
+			/* No data, will not indicate to upper layer, temporily count it here */
+			count_rx_stats(adapter, precv_frame, *psta);
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+
+	}
+	else if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE) &&
+		     (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE) )
+	{
+		_rtw_memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+		_rtw_memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN);
+		_rtw_memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+		_rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+		//
+		_rtw_memcpy(pattrib->bssid,  mybssid, ETH_ALEN);
+
+
+		*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); // get sta_info
+		if (*psta == NULL) {
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("can't get psta under MP_MODE ; drop pkt\n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s can't get psta under WIFI_MP_STATE ; drop pkt\n", __FUNCTION__);
+			#endif
+			ret= _FAIL;
+			goto exit;
+		}
+
+
+	}
+	else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+	{
+		/* Special case */
+		ret = RTW_RX_HANDLED;
+		goto exit;
+	}
+	else
+	{
+		if(_rtw_memcmp(myhwaddr, pattrib->dst, ETH_ALEN)&& (!bmcast))
+		{
+			*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); // get sta_info
+			if (*psta == NULL)
+			{
+				DBG_871X("issue_deauth to the ap=" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->bssid));
+	
+				issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+			}
+		}	
+	
+		ret = _FAIL;
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s fw_state:0x%x\n", __FUNCTION__, get_fwstate(pmlmepriv));
+		#endif
+	}
+
+exit:
+
+_func_exit_;
+
+	return ret;
+
+}
+
+sint sta2ap_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info**psta );
+sint sta2ap_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info**psta )
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = & precv_frame->u.hdr.attrib;
+	struct	sta_priv 		*pstapriv = &adapter->stapriv;
+	struct	mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	unsigned char *mybssid  = get_bssid(pmlmepriv);	
+	sint ret=_SUCCESS;
+
+_func_enter_;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+	{
+		//For AP mode, RA=BSSID, TX=STA(SRC_ADDR), A3=DST_ADDR
+		if(!_rtw_memcmp(pattrib->bssid, mybssid, ETH_ALEN))
+		{
+			ret= _FAIL;
+			goto exit;
+		}
+
+		*psta = rtw_get_stainfo(pstapriv, pattrib->src);
+		if (*psta == NULL)
+		{
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("can't get psta under AP_MODE; drop pkt\n"));
+			DBG_871X("issue_deauth to sta=" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->src));
+
+			issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+
+		process_pwrbit_data(adapter, precv_frame);
+
+		if ((GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) {
+			process_wmmps_data(adapter, precv_frame);
+		}
+
+		if (GetFrameSubType(ptr) & BIT(6)) {
+			/* No data, will not indicate to upper layer, temporily count it here */
+			count_rx_stats(adapter, precv_frame, *psta);
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+	}
+	else {
+		u8 *myhwaddr = myid(&adapter->eeprompriv);
+		if (!_rtw_memcmp(pattrib->ra, myhwaddr, ETH_ALEN)) {
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+		DBG_871X("issue_deauth to sta=" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->src));
+		issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+		ret = RTW_RX_HANDLED;
+		goto exit;
+	}
+
+exit:
+
+_func_exit_;
+
+	return ret;
+
+}
+
+sint validate_recv_ctrl_frame(_adapter *padapter, union recv_frame *precv_frame);
+sint validate_recv_ctrl_frame(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_AP_MODE
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	//uint len = precv_frame->u.hdr.len;
+		
+	//DBG_871X("+validate_recv_ctrl_frame\n");
+
+	if (GetFrameType(pframe) != WIFI_CTRL_TYPE)
+	{		
+		return _FAIL;
+	}
+
+	//receive the frames that ra(a1) is my address
+	if (!_rtw_memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN))
+	{
+		return _FAIL;
+	}
+
+	//only handle ps-poll
+	if(GetFrameSubType(pframe) == WIFI_PSPOLL)
+	{
+		u16 aid;
+		u8 wmmps_ac=0;	
+		struct sta_info *psta=NULL;
+	
+		aid = GetAid(pframe);
+		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+		
+		if((psta==NULL) || (psta->aid!=aid))
+		{
+			return _FAIL;
+		}
+
+		//for rx pkt statistics
+		psta->sta_stats.rx_ctrl_pkts++;
+
+		switch(pattrib->priority)
+		{
+			case 1:
+			case 2:
+				wmmps_ac = psta->uapsd_bk&BIT(0);
+				break;
+			case 4:
+			case 5:
+				wmmps_ac = psta->uapsd_vi&BIT(0);
+				break;
+			case 6:
+			case 7:
+				wmmps_ac = psta->uapsd_vo&BIT(0);
+				break;
+			case 0:
+			case 3:
+			default:
+				wmmps_ac = psta->uapsd_be&BIT(0);
+				break;	
+		}
+
+		if(wmmps_ac)
+			return _FAIL;
+
+		if(psta->state & WIFI_STA_ALIVE_CHK_STATE)
+		{					
+			DBG_871X("%s alive check-rx ps-poll\n", __func__);
+			psta->expire_to = pstapriv->expire_to;
+			psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+		}	
+
+		if((psta->state&WIFI_SLEEP_STATE) && (pstapriv->sta_dz_bitmap&BIT(psta->aid)))
+		{
+			_irqL irqL;	 
+			_list	*xmitframe_plist, *xmitframe_phead;
+			struct xmit_frame *pxmitframe=NULL;
+			struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+		
+			//_enter_critical_bh(&psta->sleep_q.lock, &irqL);
+			_enter_critical_bh(&pxmitpriv->lock, &irqL);
+
+			xmitframe_phead = get_list_head(&psta->sleep_q);
+			xmitframe_plist = get_next(xmitframe_phead);
+
+			if ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
+			{			
+				pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+				xmitframe_plist = get_next(xmitframe_plist);
+
+				rtw_list_delete(&pxmitframe->list);
+
+				psta->sleepq_len--;
+
+				if(psta->sleepq_len>0)
+					pxmitframe->attrib.mdata = 1;
+                                else
+					pxmitframe->attrib.mdata = 0;
+
+				pxmitframe->attrib.triggered = 1;
+
+	                        //DBG_871X("handling ps-poll, q_len=%d, tim=%x\n", psta->sleepq_len, pstapriv->tim_bitmap);
+
+#if 0
+                                _exit_critical_bh(&psta->sleep_q.lock, &irqL);	
+				if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE)
+				{		
+					rtw_os_xmit_complete(padapter, pxmitframe);
+				}
+                                _enter_critical_bh(&psta->sleep_q.lock, &irqL);	
+#endif
+				rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+				if(psta->sleepq_len==0)
+				{
+					pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+					//DBG_871X("after handling ps-poll, tim=%x\n", pstapriv->tim_bitmap);
+
+					//upate BCN for TIM IE
+					//update_BCNTIM(padapter);		
+					update_beacon(padapter, _TIM_IE_, NULL, _FALSE);
+				}
+				
+				//_exit_critical_bh(&psta->sleep_q.lock, &irqL);
+				_exit_critical_bh(&pxmitpriv->lock, &irqL);
+				
+			}
+			else
+			{
+				//_exit_critical_bh(&psta->sleep_q.lock, &irqL);
+				_exit_critical_bh(&pxmitpriv->lock, &irqL);
+			
+				//DBG_871X("no buffered packets to xmit\n");
+				if(pstapriv->tim_bitmap&BIT(psta->aid))
+				{
+					if(psta->sleepq_len==0)
+					{
+						DBG_871X("no buffered packets to xmit\n");
+
+						//issue nulldata with More data bit = 0 to indicate we have no buffered packets
+						issue_nulldata(padapter, psta->hwaddr, 0, 0, 0);
+					}
+					else
+					{
+						DBG_871X("error!psta->sleepq_len=%d\n", psta->sleepq_len);
+						psta->sleepq_len=0;						
+					}
+				
+					pstapriv->tim_bitmap &= ~BIT(psta->aid);					
+
+					//upate BCN for TIM IE
+					//update_BCNTIM(padapter);
+					update_beacon(padapter, _TIM_IE_, NULL, _FALSE);
+				}
+				
+			}				
+			
+		}
+		
+	}
+	
+#endif
+
+	return _FAIL;
+
+}
+
+union recv_frame* recvframe_chk_defrag(PADAPTER padapter, union recv_frame *precv_frame);
+sint validate_recv_mgnt_frame(PADAPTER padapter, union recv_frame *precv_frame);
+sint validate_recv_mgnt_frame(PADAPTER padapter, union recv_frame *precv_frame)
+{
+	//struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("+validate_recv_mgnt_frame\n"));
+
+#if 0
+	if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+	{
+#ifdef CONFIG_NATIVEAP_MLME
+		mgt_dispatcher(padapter, precv_frame);
+#else
+		rtw_hostapd_mlme_rx(padapter, precv_frame);
+#endif
+	}
+	else
+	{
+		mgt_dispatcher(padapter, precv_frame);
+	}
+#endif
+
+	precv_frame = recvframe_chk_defrag(padapter, precv_frame);
+	if (precv_frame == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,("%s: fragment packet\n",__FUNCTION__));
+		return _SUCCESS;
+	}
+
+	{
+		//for rx pkt statistics
+		struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(precv_frame->u.hdr.rx_data));
+		if (psta) {
+			psta->sta_stats.rx_mgnt_pkts++;
+			if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_BEACON)
+				psta->sta_stats.rx_beacon_pkts++;
+			else if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_PROBEREQ)
+				psta->sta_stats.rx_probereq_pkts++;
+			else if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_PROBERSP) {
+				if (_rtw_memcmp(padapter->eeprompriv.mac_addr, GetAddr1Ptr(precv_frame->u.hdr.rx_data), ETH_ALEN) == _TRUE)
+					psta->sta_stats.rx_probersp_pkts++;
+				else if (is_broadcast_mac_addr(GetAddr1Ptr(precv_frame->u.hdr.rx_data))
+					|| is_multicast_mac_addr(GetAddr1Ptr(precv_frame->u.hdr.rx_data)))
+					psta->sta_stats.rx_probersp_bm_pkts++;
+				else 
+					psta->sta_stats.rx_probersp_uo_pkts++;
+			}
+		}
+	}
+
+#ifdef CONFIG_INTEL_PROXIM
+	if(padapter->proximity.proxim_on==_TRUE)
+	{
+		struct rx_pkt_attrib * pattrib=&precv_frame->u.hdr.attrib;
+		 struct recv_stat* prxstat=( struct recv_stat * )  precv_frame->u.hdr.rx_head ;
+		 u8 * pda,*psa,*pbssid,*ptr;
+		 ptr=precv_frame->u.hdr.rx_data; 
+		pda = get_da(ptr);
+		psa = get_sa(ptr);
+		pbssid = get_hdr_bssid(ptr);
+
+
+		_rtw_memcpy(pattrib->dst, pda, ETH_ALEN);
+		_rtw_memcpy(pattrib->src, psa, ETH_ALEN);
+
+		_rtw_memcpy(pattrib->bssid, pbssid, ETH_ALEN);
+
+	switch(pattrib->to_fr_ds)
+	{
+		case 0:
+			_rtw_memcpy(pattrib->ra, pda, ETH_ALEN);
+			_rtw_memcpy(pattrib->ta, psa, ETH_ALEN);
+			break;
+
+		case 1:
+			_rtw_memcpy(pattrib->ra, pda, ETH_ALEN);
+			_rtw_memcpy(pattrib->ta, pbssid, ETH_ALEN);
+			break;
+
+		case 2:
+			_rtw_memcpy(pattrib->ra, pbssid, ETH_ALEN);
+			_rtw_memcpy(pattrib->ta, psa, ETH_ALEN);
+			break;
+
+		case 3:
+			_rtw_memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN);
+			_rtw_memcpy(pattrib->ta, GetAddr2Ptr(ptr), ETH_ALEN);
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,(" case 3\n"));
+			break;
+
+		default:
+			break;
+
+		}	
+			pattrib->priority=0;
+			pattrib->hdrlen = pattrib->to_fr_ds==3 ? 30 : 24;
+
+		 padapter->proximity.proxim_rx(padapter,precv_frame);
+	}
+#endif
+	mgt_dispatcher(padapter, precv_frame);
+
+	return _SUCCESS;
+
+}
+
+sint validate_recv_data_frame(_adapter *adapter, union recv_frame *precv_frame);
+sint validate_recv_data_frame(_adapter *adapter, union recv_frame *precv_frame)
+{
+	u8 bretry;
+	u8 *psa, *pda, *pbssid;
+	struct sta_info *psta = NULL;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib	*pattrib = & precv_frame->u.hdr.attrib;
+	struct sta_priv 	*pstapriv = &adapter->stapriv;
+	struct security_priv	*psecuritypriv = &adapter->securitypriv;	
+	sint ret = _SUCCESS;
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &adapter->tdlsinfo;
+#endif //CONFIG_TDLS
+
+_func_enter_;
+
+	bretry = GetRetry(ptr);
+	pda = get_da(ptr);
+	psa = get_sa(ptr);
+	pbssid = get_hdr_bssid(ptr);
+
+	if(pbssid == NULL){
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s pbssid == NULL\n", __func__);
+		#endif
+		ret= _FAIL;
+		goto exit;
+	}
+
+	_rtw_memcpy(pattrib->dst, pda, ETH_ALEN);
+	_rtw_memcpy(pattrib->src, psa, ETH_ALEN);
+
+	_rtw_memcpy(pattrib->bssid, pbssid, ETH_ALEN);
+
+	switch(pattrib->to_fr_ds)
+	{
+		case 0:
+			_rtw_memcpy(pattrib->ra, pda, ETH_ALEN);
+			_rtw_memcpy(pattrib->ta, psa, ETH_ALEN);
+			ret = sta2sta_data_frame(adapter, precv_frame, &psta);
+			break;
+
+		case 1:
+			_rtw_memcpy(pattrib->ra, pda, ETH_ALEN);
+			_rtw_memcpy(pattrib->ta, pbssid, ETH_ALEN);
+			ret = ap2sta_data_frame(adapter, precv_frame, &psta);
+			break;
+
+		case 2:
+			_rtw_memcpy(pattrib->ra, pbssid, ETH_ALEN);
+			_rtw_memcpy(pattrib->ta, psa, ETH_ALEN);
+			ret = sta2ap_data_frame(adapter, precv_frame, &psta);
+			break;
+
+		case 3:
+			_rtw_memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN);
+			_rtw_memcpy(pattrib->ta, GetAddr2Ptr(ptr), ETH_ALEN);
+			ret =_FAIL;
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,(" case 3\n"));
+			break;
+
+		default:
+			ret =_FAIL;
+			break;
+
+	}
+
+	if(ret ==_FAIL){
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s case:%d, res:%d\n", __FUNCTION__, pattrib->to_fr_ds, ret);
+		#endif
+		goto exit;
+	} else if (ret == RTW_RX_HANDLED) {
+		goto exit;
+	}
+
+
+	if(psta==NULL){
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,(" after to_fr_ds_chk; psta==NULL \n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s psta == NULL\n", __func__);
+		#endif
+		ret= _FAIL;
+		goto exit;
+	}
+	
+	//psta->rssi = prxcmd->rssi;
+	//psta->signal_quality= prxcmd->sq;
+	precv_frame->u.hdr.psta = psta;
+		
+
+	pattrib->amsdu=0;
+	pattrib->ack_policy = 0;
+	//parsing QC field
+	if(pattrib->qos == 1)
+	{
+		pattrib->priority = GetPriority((ptr + 24));
+		pattrib->ack_policy = GetAckpolicy((ptr + 24));
+		pattrib->amsdu = GetAMsdu((ptr + 24));
+		pattrib->hdrlen = pattrib->to_fr_ds==3 ? 32 : 26;
+
+		if(pattrib->priority!=0 && pattrib->priority!=3)
+		{
+			adapter->recvpriv.bIsAnyNonBEPkts = _TRUE;
+		}
+	}
+	else
+	{
+		pattrib->priority=0;
+		pattrib->hdrlen = pattrib->to_fr_ds==3 ? 30 : 24;
+	}
+
+
+	if(pattrib->order)//HT-CTRL 11n
+	{
+		pattrib->hdrlen += 4;
+	}
+
+	precv_frame->u.hdr.preorder_ctrl = &psta->recvreorder_ctrl[pattrib->priority];
+
+	// decache, drop duplicate recv packets
+	if(recv_decache(precv_frame, bretry, &psta->sta_recvpriv.rxcache) == _FAIL)
+	{
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("decache : drop pkt\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s recv_decache return _FAIL\n", __func__);
+		#endif
+		ret= _FAIL;
+		goto exit;
+	}
+
+#if 0
+	if(psta->tdls_sta_state & TDLS_LINKED_STATE )
+	{
+		if(psta->dot118021XPrivacy==_AES_)
+			pattrib->encrypt=psta->dot118021XPrivacy;
+	}
+#endif //CONFIG_TDLS
+
+	if(pattrib->privacy){
+
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("validate_recv_data_frame:pattrib->privacy=%x\n", pattrib->privacy));
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("\n ^^^^^^^^^^^IS_MCAST(pattrib->ra(0x%02x))=%d^^^^^^^^^^^^^^^6\n", pattrib->ra[0],IS_MCAST(pattrib->ra)));
+
+#ifdef CONFIG_TDLS
+		if((psta->tdls_sta_state & TDLS_LINKED_STATE) && (psta->dot118021XPrivacy==_AES_))
+		{
+			pattrib->encrypt=psta->dot118021XPrivacy;
+		}
+		else
+#endif //CONFIG_TDLS
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, IS_MCAST(pattrib->ra));
+
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("\n pattrib->encrypt=%d\n",pattrib->encrypt));
+
+		SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt);
+	}
+	else
+	{
+		pattrib->encrypt = 0;
+		pattrib->iv_len = pattrib->icv_len = 0;
+	}
+
+exit:
+
+_func_exit_;
+
+	return ret;
+}
+
+sint validate_recv_frame(_adapter *adapter, union recv_frame *precv_frame);
+sint validate_recv_frame(_adapter *adapter, union recv_frame *precv_frame)
+{
+	//shall check frame subtype, to / from ds, da, bssid
+
+	//then call check if rx seq/frag. duplicated.
+
+	u8 type;
+	u8 subtype;
+	sint retval = _SUCCESS;
+
+	struct rx_pkt_attrib *pattrib = & precv_frame->u.hdr.attrib;
+
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	u8  ver =(unsigned char) (*ptr)&0x3 ;
+#ifdef CONFIG_FIND_BEST_CHANNEL
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+#endif
+
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &adapter->tdlsinfo;
+#endif //CONFIG_TDLS
+#ifdef CONFIG_WAPI_SUPPORT
+	PRT_WAPI_T	pWapiInfo = &adapter->wapiInfo;
+	struct recv_frame_hdr *phdr = &precv_frame->u.hdr;
+	u8 wai_pkt = 0;
+	u16 sc;
+	u8	external_len = 0;
+#endif
+
+_func_enter_;
+
+
+#ifdef CONFIG_FIND_BEST_CHANNEL
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		int ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, rtw_get_oper_ch(adapter));
+		if (ch_set_idx >= 0)
+			pmlmeext->channel_set[ch_set_idx].rx_count++;
+	}
+#endif
+
+#ifdef CONFIG_TDLS
+	if(ptdlsinfo->ch_sensing==1 && ptdlsinfo->cur_channel !=0){
+		ptdlsinfo->collect_pkt_num[ptdlsinfo->cur_channel-1]++;
+	}
+#endif //CONFIG_TDLS
+
+#ifdef RTK_DMP_PLATFORM
+	if ( 0 )
+	{
+		DBG_871X("++\n");
+		{
+			int i;
+			for(i=0; i<64;i=i+8)
+				DBG_871X("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:", *(ptr+i),
+				*(ptr+i+1), *(ptr+i+2) ,*(ptr+i+3) ,*(ptr+i+4),*(ptr+i+5), *(ptr+i+6), *(ptr+i+7));
+
+		}
+		DBG_871X("--\n");
+	}
+#endif //RTK_DMP_PLATFORM
+
+	//add version chk
+	if(ver!=0){
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("validate_recv_data_frame fail! (ver!=0)\n"));
+		retval= _FAIL;
+		goto exit;
+	}
+
+	type =  GetFrameType(ptr);
+	subtype = GetFrameSubType(ptr); //bit(7)~bit(2)
+
+	pattrib->to_fr_ds = get_tofr_ds(ptr);
+
+	pattrib->frag_num = GetFragNum(ptr);
+	pattrib->seq_num = GetSequence(ptr);
+
+	pattrib->pw_save = GetPwrMgt(ptr);
+	pattrib->mfrag = GetMFrag(ptr);
+	pattrib->mdata = GetMData(ptr);
+	pattrib->privacy = GetPrivacy(ptr);
+	pattrib->order = GetOrder(ptr);
+#ifdef CONFIG_WAPI_SUPPORT
+	sc = (pattrib->seq_num<<4) | pattrib->frag_num;
+#endif
+
+#if 1 //Dump rx packets
+{
+	u8 bDumpRxPkt;
+	rtw_hal_get_def_var(adapter, HAL_DEF_DBG_DUMP_RXPKT, &(bDumpRxPkt));
+	if(bDumpRxPkt ==1){//dump all rx packets
+		int i;
+		DBG_871X("############################# \n");
+		
+		for(i=0; i<64;i=i+8)
+			DBG_871X("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n", *(ptr+i),
+			*(ptr+i+1), *(ptr+i+2) ,*(ptr+i+3) ,*(ptr+i+4),*(ptr+i+5), *(ptr+i+6), *(ptr+i+7));
+		DBG_871X("############################# \n");
+	}
+	else if(bDumpRxPkt ==2){
+		if(type== WIFI_MGT_TYPE){
+			int i;
+			DBG_871X("############################# \n");
+
+			for(i=0; i<64;i=i+8)
+				DBG_871X("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n", *(ptr+i),
+				*(ptr+i+1), *(ptr+i+2) ,*(ptr+i+3) ,*(ptr+i+4),*(ptr+i+5), *(ptr+i+6), *(ptr+i+7));
+			DBG_871X("############################# \n");
+		}
+	}
+	else if(bDumpRxPkt ==3){
+		if(type== WIFI_DATA_TYPE){
+			int i;
+			DBG_871X("############################# \n");
+			
+			for(i=0; i<64;i=i+8)
+				DBG_871X("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n", *(ptr+i),
+				*(ptr+i+1), *(ptr+i+2) ,*(ptr+i+3) ,*(ptr+i+4),*(ptr+i+5), *(ptr+i+6), *(ptr+i+7));
+			DBG_871X("############################# \n");
+		}
+	}
+}
+#endif
+	switch (type)
+	{
+		case WIFI_MGT_TYPE: //mgnt
+			retval = validate_recv_mgnt_frame(adapter, precv_frame);
+			if (retval == _FAIL)
+			{
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("validate_recv_mgnt_frame fail\n"));
+			}
+			retval = _FAIL; // only data frame return _SUCCESS
+			break;
+		case WIFI_CTRL_TYPE: //ctrl
+			retval = validate_recv_ctrl_frame(adapter, precv_frame);
+			if (retval == _FAIL)
+			{
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("validate_recv_ctrl_frame fail\n"));
+			}
+			retval = _FAIL; // only data frame return _SUCCESS
+			break;
+		case WIFI_DATA_TYPE: //data
+#ifdef CONFIG_WAPI_SUPPORT
+			if(pattrib->qos)
+				external_len = 2;
+			else
+				external_len= 0;
+			
+			wai_pkt = rtw_wapi_is_wai_packet(adapter,ptr);
+
+			phdr->bIsWaiPacket = wai_pkt;
+
+			if(wai_pkt !=0){
+				if(sc != adapter->wapiInfo.wapiSeqnumAndFragNum)
+				{
+					adapter->wapiInfo.wapiSeqnumAndFragNum = sc;
+				}
+				else
+				{
+					retval = _FAIL;
+					break;
+				}
+			}
+			else{
+
+					if(rtw_wapi_drop_for_key_absent(adapter,GetAddr2Ptr(ptr))){
+						retval=_FAIL;
+						WAPI_TRACE(WAPI_RX,"drop for key absent for rx \n");
+						break;
+					}
+			}
+
+#endif
+
+			rtw_led_control(adapter, LED_CTL_RX);
+			pattrib->qos = (subtype & BIT(7))? 1:0;
+			retval = validate_recv_data_frame(adapter, precv_frame);
+			if (retval == _FAIL)
+			{
+				struct recv_priv *precvpriv = &adapter->recvpriv;
+				//RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("validate_recv_data_frame fail\n"));
+				precvpriv->rx_drop++;
+			}
+			break;
+		default:
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("validate_recv_data_frame fail! type=0x%x\n", type));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME validate_recv_data_frame fail! type=0x%x\n", type);
+			#endif
+			retval = _FAIL;
+			break;
+	}
+
+exit:
+
+_func_exit_;
+
+	return retval;
+}
+
+
+//remove the wlanhdr and add the eth_hdr
+#if 1
+
+sint wlanhdr_to_ethhdr ( union recv_frame *precvframe);
+sint wlanhdr_to_ethhdr ( union recv_frame *precvframe)
+{
+	sint	rmv_len;
+	u16	eth_type, len;
+	u8	bsnaphdr;
+	u8	*psnap_type;
+	struct ieee80211_snap_hdr	*psnap;
+	
+	sint ret=_SUCCESS;
+	_adapter			*adapter =precvframe->u.hdr.adapter;
+	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+
+	u8	*ptr = get_recvframe_data(precvframe) ; // point to frame_ctrl field
+	struct rx_pkt_attrib *pattrib = & precvframe->u.hdr.attrib;
+
+_func_enter_;
+
+	if(pattrib->encrypt){
+		recvframe_pull_tail(precvframe, pattrib->icv_len);	
+	}
+
+	psnap=(struct ieee80211_snap_hdr	*)(ptr+pattrib->hdrlen + pattrib->iv_len);
+	psnap_type=ptr+pattrib->hdrlen + pattrib->iv_len+SNAP_SIZE;
+	/* convert hdr + possible LLC headers into Ethernet header */
+	//eth_type = (psnap_type[0] << 8) | psnap_type[1];
+	if((_rtw_memcmp(psnap, rtw_rfc1042_header, SNAP_SIZE) &&
+		(_rtw_memcmp(psnap_type, SNAP_ETH_TYPE_IPX, 2) == _FALSE) && 
+		(_rtw_memcmp(psnap_type, SNAP_ETH_TYPE_APPLETALK_AARP, 2)==_FALSE) )||
+		//eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) ||
+		 _rtw_memcmp(psnap, rtw_bridge_tunnel_header, SNAP_SIZE)){
+		/* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
+		bsnaphdr = _TRUE;
+	}
+	else {
+		/* Leave Ethernet header part of hdr and full payload */
+		bsnaphdr = _FALSE;
+	}
+
+	rmv_len = pattrib->hdrlen + pattrib->iv_len +(bsnaphdr?SNAP_SIZE:0);
+	len = precvframe->u.hdr.len - rmv_len;
+
+	RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("\n===pattrib->hdrlen: %x,  pattrib->iv_len:%x ===\n\n", pattrib->hdrlen,  pattrib->iv_len));
+
+	_rtw_memcpy(&eth_type, ptr+rmv_len, 2);
+	eth_type= ntohs((unsigned short )eth_type); //pattrib->ether_type
+	pattrib->eth_type = eth_type;
+
+	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE))	   	
+	{
+		ptr += rmv_len ;	
+		*ptr = 0x87;
+		*(ptr+1) = 0x12;
+
+		eth_type = 0x8712;
+		// append rx status for mp test packets
+		ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr)+2)-24);
+		_rtw_memcpy(ptr, get_rxmem(precvframe), 24);
+		ptr+=24;
+	}
+	else {
+		ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr)+ (bsnaphdr?2:0)));
+	}
+
+	_rtw_memcpy(ptr, pattrib->dst, ETH_ALEN);
+	_rtw_memcpy(ptr+ETH_ALEN, pattrib->src, ETH_ALEN);
+
+	if(!bsnaphdr) {
+		len = htons(len);
+		_rtw_memcpy(ptr+12, &len, 2);
+	}
+
+_func_exit_;	
+	return ret;
+
+}
+
+#else
+
+sint wlanhdr_to_ethhdr ( union recv_frame *precvframe)
+{
+	sint rmv_len;
+	u16 eth_type;
+	u8	bsnaphdr;
+	u8	*psnap_type;
+	struct ieee80211_snap_hdr	*psnap;
+
+	sint ret=_SUCCESS;
+	_adapter	*adapter =precvframe->u.hdr.adapter;
+	struct	mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+
+	u8* ptr = get_recvframe_data(precvframe) ; // point to frame_ctrl field
+	struct rx_pkt_attrib *pattrib = & precvframe->u.hdr.attrib;
+	struct _vlan *pvlan = NULL;
+
+_func_enter_;
+
+	psnap=(struct ieee80211_snap_hdr	*)(ptr+pattrib->hdrlen + pattrib->iv_len);
+	psnap_type=ptr+pattrib->hdrlen + pattrib->iv_len+SNAP_SIZE;
+	if (psnap->dsap==0xaa && psnap->ssap==0xaa && psnap->ctrl==0x03)
+	{
+		if (_rtw_memcmp(psnap->oui, oui_rfc1042, WLAN_IEEE_OUI_LEN))
+			bsnaphdr=_TRUE;//wlan_pkt_format = WLAN_PKT_FORMAT_SNAP_RFC1042;	
+		else if (_rtw_memcmp(psnap->oui, SNAP_HDR_APPLETALK_DDP, WLAN_IEEE_OUI_LEN) &&
+			_rtw_memcmp(psnap_type, SNAP_ETH_TYPE_APPLETALK_DDP, 2) )
+			bsnaphdr=_TRUE;	//wlan_pkt_format = WLAN_PKT_FORMAT_APPLETALK;
+		else if (_rtw_memcmp( psnap->oui, oui_8021h, WLAN_IEEE_OUI_LEN))
+			bsnaphdr=_TRUE;	//wlan_pkt_format = WLAN_PKT_FORMAT_SNAP_TUNNEL;
+		else {
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("drop pkt due to invalid frame format!\n"));
+			ret= _FAIL;
+			goto exit;
+		}
+
+	} else
+		bsnaphdr=_FALSE;//wlan_pkt_format = WLAN_PKT_FORMAT_OTHERS;
+
+	rmv_len = pattrib->hdrlen + pattrib->iv_len +(bsnaphdr?SNAP_SIZE:0);
+	RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("===pattrib->hdrlen: %x,  pattrib->iv_len:%x ===\n", pattrib->hdrlen,  pattrib->iv_len));
+
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE)
+	{
+		ptr += rmv_len ;
+		*ptr = 0x87;
+		*(ptr+1) = 0x12;
+
+		//back to original pointer
+		ptr -= rmv_len;
+	}
+
+	ptr += rmv_len ;
+
+	_rtw_memcpy(&eth_type, ptr, 2);
+	eth_type= ntohs((unsigned short )eth_type); //pattrib->ether_type
+	ptr +=2;
+
+	if(pattrib->encrypt){
+		recvframe_pull_tail(precvframe, pattrib->icv_len);
+	}
+
+	if(eth_type == 0x8100) //vlan
+	{
+		pvlan = (struct _vlan *) ptr;
+
+		//eth_type = get_vlan_encap_proto(pvlan);
+		//eth_type = pvlan->h_vlan_encapsulated_proto;//?
+		rmv_len += 4;
+		ptr+=4;
+	}
+
+	if(eth_type==0x0800)//ip
+	{
+		//struct iphdr*  piphdr = (struct iphdr*) ptr;
+		//__u8 tos = (unsigned char)(pattrib->priority & 0xff);
+
+		//piphdr->tos = tos;
+
+		//if (piphdr->protocol == 0x06)
+		//{
+		//	RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("@@@===recv tcp len:%d @@@===\n", precvframe->u.hdr.len));
+		//}
+	}
+	else if(eth_type==0x8712)// append rx status for mp test packets
+	{
+		//ptr -= 16;
+		//_rtw_memcpy(ptr, get_rxmem(precvframe), 16);
+	}
+	else
+	{
+#ifdef PLATFORM_OS_XP
+		NDIS_PACKET_8021Q_INFO VlanPriInfo;
+		UINT32 UserPriority = precvframe->u.hdr.attrib.priority;
+		UINT32 VlanID = (pvlan!=NULL ? get_vlan_id(pvlan) : 0 );
+
+		VlanPriInfo.Value =          // Get current value.
+				NDIS_PER_PACKET_INFO_FROM_PACKET(precvframe->u.hdr.pkt, Ieee8021QInfo);
+
+		VlanPriInfo.TagHeader.UserPriority = UserPriority;
+		VlanPriInfo.TagHeader.VlanId =  VlanID ;
+
+		VlanPriInfo.TagHeader.CanonicalFormatId = 0; // Should be zero.
+		VlanPriInfo.TagHeader.Reserved = 0; // Should be zero.
+		NDIS_PER_PACKET_INFO_FROM_PACKET(precvframe->u.hdr.pkt, Ieee8021QInfo) = VlanPriInfo.Value;
+#endif
+	}
+
+	if(eth_type==0x8712)// append rx status for mp test packets
+	{
+		ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr)+2)-24);
+		_rtw_memcpy(ptr, get_rxmem(precvframe), 24);
+		ptr+=24;
+	}
+	else
+		ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr)+2));
+
+	_rtw_memcpy(ptr, pattrib->dst, ETH_ALEN);
+	_rtw_memcpy(ptr+ETH_ALEN, pattrib->src, ETH_ALEN);
+
+	eth_type = htons((unsigned short)eth_type) ;
+	_rtw_memcpy(ptr+12, &eth_type, 2);
+
+exit:
+
+_func_exit_;
+
+	return ret;
+}
+#endif
+
+
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+#ifdef PLATFORM_LINUX
+static void recvframe_expand_pkt(
+	PADAPTER padapter,
+	union recv_frame *prframe)
+{
+	struct recv_frame_hdr *pfhdr;
+	_pkt *ppkt;
+	u8 shift_sz;
+	u32 alloc_sz;
+
+
+	pfhdr = &prframe->u.hdr;
+
+	//	6 is for IP header 8 bytes alignment in QoS packet case.
+	if (pfhdr->attrib.qos)
+		shift_sz = 6;
+	else
+		shift_sz = 0;
+
+	// for first fragment packet, need to allocate
+	// (1536 + RXDESC_SIZE + drvinfo_sz) to reassemble packet
+	//	8 is for skb->data 8 bytes alignment.
+//	alloc_sz = _RND(1536 + RXDESC_SIZE + pfhdr->attrib.drvinfosize + shift_sz + 8, 128);
+	alloc_sz = 1664; // round (1536 + 24 + 32 + shift_sz + 8) to 128 bytes alignment
+
+	//3 1. alloc new skb
+	// prepare extra space for 4 bytes alignment
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,18)) // http://www.mail-archive.com/netdev@vger.kernel.org/msg17214.html
+	ppkt = dev_alloc_skb(alloc_sz);
+	if (ppkt) ppkt->dev = padapter->pnetdev;
+#else
+	ppkt = netdev_alloc_skb(padapter->pnetdev, alloc_sz);
+#endif
+	if (!ppkt) return; // no way to expand
+
+	//3 2. Prepare new skb to replace & release old skb
+	// force ppkt->data at 8-byte alignment address
+	skb_reserve(ppkt, 8 - ((SIZE_PTR)ppkt->data & 7));
+	// force ip_hdr at 8-byte alignment address according to shift_sz
+	skb_reserve(ppkt, shift_sz);
+
+	// copy data to new pkt
+	_rtw_memcpy(skb_put(ppkt, pfhdr->len), pfhdr->rx_data, pfhdr->len);
+
+	dev_kfree_skb_any(pfhdr->pkt);
+
+	// attach new pkt to recvframe
+	pfhdr->pkt = ppkt;
+	pfhdr->rx_head = ppkt->head;
+	pfhdr->rx_data = ppkt->data;
+	pfhdr->rx_tail = skb_tail_pointer(ppkt);
+	pfhdr->rx_end = skb_end_pointer(ppkt);
+}
+#else
+#warning "recvframe_expand_pkt not implement, defrag may crash system"
+#endif
+#endif
+
+//perform defrag
+union recv_frame * recvframe_defrag(_adapter *adapter,_queue *defrag_q);
+union recv_frame * recvframe_defrag(_adapter *adapter,_queue *defrag_q)
+{
+	_list	 *plist, *phead;
+	u8	*data,wlanhdr_offset;
+	u8	curfragnum;
+	struct recv_frame_hdr *pfhdr,*pnfhdr;
+	union recv_frame* prframe, *pnextrframe;
+	_queue	*pfree_recv_queue;
+
+_func_enter_;
+
+	curfragnum=0;
+	pfree_recv_queue=&adapter->recvpriv.free_recv_queue;
+
+	phead = get_list_head(defrag_q);
+	plist = get_next(phead);
+	prframe = LIST_CONTAINOR(plist, union recv_frame, u);
+	pfhdr=&prframe->u.hdr;
+	rtw_list_delete(&(prframe->u.list));
+
+	if(curfragnum!=pfhdr->attrib.frag_num)
+	{
+		//the first fragment number must be 0
+		//free the whole queue
+		rtw_free_recvframe(prframe, pfree_recv_queue);
+		rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+
+		return NULL;
+	}
+
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+#ifndef CONFIG_SDIO_RX_COPY
+	recvframe_expand_pkt(adapter, prframe);
+#endif
+#endif
+
+	curfragnum++;
+
+	plist= get_list_head(defrag_q);
+
+	plist = get_next(plist);
+
+	data=get_recvframe_data(prframe);
+
+	while(rtw_end_of_queue_search(phead, plist) == _FALSE)
+	{
+		pnextrframe = LIST_CONTAINOR(plist, union recv_frame , u);
+		pnfhdr=&pnextrframe->u.hdr;
+
+
+		//check the fragment sequence  (2nd ~n fragment frame)
+
+		if(curfragnum!=pnfhdr->attrib.frag_num)
+		{
+			//the fragment number must be increasing  (after decache)
+			//release the defrag_q & prframe
+			rtw_free_recvframe(prframe, pfree_recv_queue);
+			rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+			return NULL;
+		}
+
+		curfragnum++;
+
+		//copy the 2nd~n fragment frame's payload to the first fragment
+		//get the 2nd~last fragment frame's payload
+
+		wlanhdr_offset = pnfhdr->attrib.hdrlen + pnfhdr->attrib.iv_len;
+
+		recvframe_pull(pnextrframe, wlanhdr_offset);
+
+		//append  to first fragment frame's tail (if privacy frame, pull the ICV)
+		recvframe_pull_tail(prframe, pfhdr->attrib.icv_len);
+
+		//memcpy
+		_rtw_memcpy(pfhdr->rx_tail, pnfhdr->rx_data, pnfhdr->len);
+
+		recvframe_put(prframe, pnfhdr->len);
+
+		pfhdr->attrib.icv_len=pnfhdr->attrib.icv_len;
+		plist = get_next(plist);
+
+	};
+
+	//free the defrag_q queue and return the prframe
+	rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+
+	RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("Performance defrag!!!!!\n"));
+
+_func_exit_;
+
+	return prframe;
+}
+
+//check if need to defrag, if needed queue the frame to defrag_q
+union recv_frame* recvframe_chk_defrag(PADAPTER padapter, union recv_frame *precv_frame)
+{
+	u8	ismfrag;
+	u8	fragnum;
+	u8	*psta_addr;
+	struct recv_frame_hdr *pfhdr;
+	struct sta_info *psta;
+	struct sta_priv *pstapriv;
+	_list *phead;
+	union recv_frame *prtnframe = NULL;
+	_queue *pfree_recv_queue, *pdefrag_q;
+
+_func_enter_;
+
+	pstapriv = &padapter->stapriv;
+
+	pfhdr = &precv_frame->u.hdr;
+
+	pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+	//need to define struct of wlan header frame ctrl
+	ismfrag = pfhdr->attrib.mfrag;
+	fragnum = pfhdr->attrib.frag_num;
+
+	psta_addr = pfhdr->attrib.ta;
+	psta = rtw_get_stainfo(pstapriv, psta_addr);
+	if (psta == NULL)
+	{
+		u8 type = GetFrameType(pfhdr->rx_data);
+		if (type != WIFI_DATA_TYPE) {
+			psta = rtw_get_bcmc_stainfo(padapter);
+			pdefrag_q = &psta->sta_recvpriv.defrag_q;
+		} else
+			pdefrag_q = NULL;
+	}
+	else
+		pdefrag_q = &psta->sta_recvpriv.defrag_q;
+
+	if ((ismfrag==0) && (fragnum==0))
+	{
+		prtnframe = precv_frame;//isn't a fragment frame
+	}
+
+	if (ismfrag==1)
+	{
+		//0~(n-1) fragment frame
+		//enqueue to defraf_g
+		if(pdefrag_q != NULL)
+		{
+			if(fragnum==0)
+			{
+				//the first fragment
+				if(_rtw_queue_empty(pdefrag_q) == _FALSE)
+				{
+					//free current defrag_q
+					rtw_free_recvframe_queue(pdefrag_q, pfree_recv_queue);
+				}
+			}
+
+
+			//Then enqueue the 0~(n-1) fragment into the defrag_q
+
+			//_rtw_spinlock(&pdefrag_q->lock);
+			phead = get_list_head(pdefrag_q);
+			rtw_list_insert_tail(&pfhdr->list, phead);
+			//_rtw_spinunlock(&pdefrag_q->lock);
+
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("Enqueuq: ismfrag = %d, fragnum= %d\n", ismfrag,fragnum));
+
+			prtnframe=NULL;
+
+		}
+		else
+		{
+			//can't find this ta's defrag_queue, so free this recv_frame
+			rtw_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe=NULL;
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("Free because pdefrag_q ==NULL: ismfrag = %d, fragnum= %d\n", ismfrag, fragnum));
+		}
+
+	}
+
+	if((ismfrag==0)&&(fragnum!=0))
+	{
+		//the last fragment frame
+		//enqueue the last fragment
+		if(pdefrag_q != NULL)
+		{
+			//_rtw_spinlock(&pdefrag_q->lock);
+			phead = get_list_head(pdefrag_q);
+			rtw_list_insert_tail(&pfhdr->list,phead);
+			//_rtw_spinunlock(&pdefrag_q->lock);
+
+			//call recvframe_defrag to defrag
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("defrag: ismfrag = %d, fragnum= %d\n", ismfrag, fragnum));
+			precv_frame = recvframe_defrag(padapter, pdefrag_q);
+			prtnframe=precv_frame;
+
+		}
+		else
+		{
+			//can't find this ta's defrag_queue, so free this recv_frame
+			rtw_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe=NULL;
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("Free because pdefrag_q ==NULL: ismfrag = %d, fragnum= %d\n", ismfrag,fragnum));
+		}
+
+	}
+
+
+	if((prtnframe!=NULL)&&(prtnframe->u.hdr.attrib.privacy))
+	{
+		//after defrag we must check tkip mic code
+		if(recvframe_chkmic(padapter,  prtnframe)==_FAIL)
+		{
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("recvframe_chkmic(padapter,  prtnframe)==_FAIL\n"));
+			rtw_free_recvframe(prtnframe,pfree_recv_queue);
+			prtnframe=NULL;
+		}
+	}
+
+_func_exit_;
+
+	return prtnframe;
+
+}
+
+#define ENDIAN_FREE 1
+
+int amsdu_to_msdu(_adapter *padapter, union recv_frame *prframe);
+int amsdu_to_msdu(_adapter *padapter, union recv_frame *prframe)
+{
+#if defined (PLATFORM_LINUX) || defined (PLATFORM_FREEBSD)  //for amsdu TP improvement,Creator: Thomas 
+	int	a_len, padding_len;
+	u16	eth_type, nSubframe_Length;	
+	u8	nr_subframes, i;
+	unsigned char *pdata;
+	struct rx_pkt_attrib *pattrib;
+#ifndef PLATFORM_FREEBSD
+	unsigned char *data_ptr;
+	_pkt *sub_skb,*subframes[MAX_SUBFRAME_COUNT];
+#endif //PLATFORM_FREEBSD
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	_queue *pfree_recv_queue = &(precvpriv->free_recv_queue);
+	int	ret = _SUCCESS;
+#ifdef PLATFORM_FREEBSD
+	struct mbuf *sub_m=NULL, *subframes[MAX_SUBFRAME_COUNT];	
+	u8 *ptr,offset;
+#endif //PLATFORM_FREEBSD
+	nr_subframes = 0;
+
+	pattrib = &prframe->u.hdr.attrib;
+
+	recvframe_pull(prframe, prframe->u.hdr.attrib.hdrlen);
+	
+	if(prframe->u.hdr.attrib.iv_len >0)
+	{
+		recvframe_pull(prframe, prframe->u.hdr.attrib.iv_len);
+	}
+
+	a_len = prframe->u.hdr.len;
+
+	pdata = prframe->u.hdr.rx_data;
+
+	while(a_len > ETH_HLEN) {
+		
+		/* Offset 12 denote 2 mac address */
+#ifdef ENDIAN_FREE
+		//nSubframe_Length = ntohs(*((u16*)(pdata + 12)));
+		nSubframe_Length = RTW_GET_BE16(pdata + 12);
+#else // ENDIAN_FREE
+		nSubframe_Length = *((u16*)(pdata + 12));
+		//==m==>change the length order
+		nSubframe_Length = (nSubframe_Length>>8) + (nSubframe_Length<<8);
+		//ntohs(nSubframe_Length);
+#endif // ENDIAN_FREE
+
+		if( a_len < (ETHERNET_HEADER_SIZE + nSubframe_Length) ) {
+			DBG_871X("nRemain_Length is %d and nSubframe_Length is : %d\n",a_len,nSubframe_Length);
+			goto exit;
+		}
+
+#ifndef PLATFORM_FREEBSD
+		/* move the data point to data content */
+		pdata += ETH_HLEN;
+		a_len -= ETH_HLEN;
+
+		/* Allocate new skb for releasing to upper layer */
+#ifdef CONFIG_SKB_COPY
+		sub_skb = dev_alloc_skb(nSubframe_Length + 12);
+		if(sub_skb)
+		{
+			skb_reserve(sub_skb, 12);
+			data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
+			_rtw_memcpy(data_ptr, pdata, nSubframe_Length);
+		}
+		else
+#endif // CONFIG_SKB_COPY
+		{
+			sub_skb = skb_clone(prframe->u.hdr.pkt, GFP_ATOMIC);
+			if(sub_skb)
+			{
+				sub_skb->data = pdata;
+				sub_skb->len = nSubframe_Length;
+				skb_set_tail_pointer(sub_skb, nSubframe_Length);
+			}
+			else
+			{
+				DBG_871X("skb_clone() Fail!!! , nr_subframes = %d\n",nr_subframes);
+				break;
+			}
+		}
+
+#else   // PLATFORM_FREEBSD
+
+		//PLATFORM_FREEBSD
+		//Allocate a mbuff,  
+		//sub_m =m_devget(pdata, nSubframe_Length+12, 12, padapter->pifp,NULL);
+		sub_m =m_devget(pdata, nSubframe_Length+ETH_HLEN, ETHER_ALIGN, padapter->pifp,NULL);
+
+		pdata += ETH_HLEN;
+		a_len -= ETH_HLEN;
+#endif	// PLATFORM_FREEBSD	
+
+#ifndef PLATFORM_FREEBSD
+		//sub_skb->dev = padapter->pnetdev;
+		subframes[nr_subframes++] = sub_skb;
+#else  //PLATFORM_FREEBSD
+		//PLATFORM_FREEBSD
+		subframes[nr_subframes++] = sub_m;
+#endif  //PLATFORM_FREEBSD
+
+		if(nr_subframes >= MAX_SUBFRAME_COUNT) {
+			DBG_871X("ParseSubframe(): Too many Subframes! Packets dropped!\n");
+			break;
+		}
+
+		pdata += nSubframe_Length;
+		a_len -= nSubframe_Length;
+		if(a_len != 0) {
+			padding_len = 4 - ((nSubframe_Length + ETH_HLEN) & (4-1));
+			if(padding_len == 4) {
+				padding_len = 0;
+			}
+
+			if(a_len < padding_len) {
+				goto exit;
+			}
+			pdata += padding_len;
+			a_len -= padding_len;
+		}
+	}
+
+	for(i=0; i<nr_subframes; i++){
+#ifndef PLATFORM_FREEBSD
+		sub_skb = subframes[i];
+		/* convert hdr + possible LLC headers into Ethernet header */
+#ifdef ENDIAN_FREE
+		//eth_type = ntohs(*(u16*)&sub_skb->data[6]);
+		eth_type = RTW_GET_BE16(&sub_skb->data[6]);
+#else // ENDIAN_FREE		
+		eth_type = (sub_skb->data[6] << 8) | sub_skb->data[7];
+#endif // ENDIAN_FREE
+		if (sub_skb->len >= 8 &&
+			((_rtw_memcmp(sub_skb->data, rtw_rfc1042_header, SNAP_SIZE) &&
+			  eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) ||
+			 _rtw_memcmp(sub_skb->data, rtw_bridge_tunnel_header, SNAP_SIZE) )) {
+			/* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
+			skb_pull(sub_skb, SNAP_SIZE);
+			_rtw_memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src, ETH_ALEN);
+			_rtw_memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst, ETH_ALEN);
+		} else {
+			u16 len;
+			/* Leave Ethernet header part of hdr and full payload */
+			len = htons(sub_skb->len);
+			_rtw_memcpy(skb_push(sub_skb, 2), &len, 2);
+			_rtw_memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src, ETH_ALEN);
+			_rtw_memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst, ETH_ALEN);
+		}
+
+		/* Indicat the packets to upper layer */
+		if (sub_skb) {
+			//memset(sub_skb->cb, 0, sizeof(sub_skb->cb));
+
+#ifdef CONFIG_BR_EXT
+			// Insert NAT2.5 RX here!
+			struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+			void *br_port = NULL;
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+			br_port = padapter->pnetdev->br_port;
+#else   // (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+			rcu_read_lock();
+			br_port = rcu_dereference(padapter->pnetdev->rx_handler_data);
+			rcu_read_unlock();
+#endif  // (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+
+
+			if( br_port && (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) == _TRUE) )
+			{
+				int nat25_handle_frame(_adapter *priv, struct sk_buff *skb);
+				if (nat25_handle_frame(padapter, sub_skb) == -1) {
+					//priv->ext_stats.rx_data_drops++;
+					//DEBUG_ERR("RX DROP: nat25_handle_frame fail!\n");
+					//return FAIL;
+					
+#if 1
+					// bypass this frame to upper layer!!
+#else
+					dev_kfree_skb_any(sub_skb);
+					continue;
+#endif
+				}							
+			}
+#endif	// CONFIG_BR_EXT
+
+			sub_skb->protocol = eth_type_trans(sub_skb, padapter->pnetdev);
+			sub_skb->dev = padapter->pnetdev;
+
+#ifdef CONFIG_TCP_CSUM_OFFLOAD_RX
+			if ( (pattrib->tcpchk_valid == 1) && (pattrib->tcp_chkrpt == 1) ) {
+				sub_skb->ip_summed = CHECKSUM_UNNECESSARY;
+			} else {
+				sub_skb->ip_summed = CHECKSUM_NONE;
+			}
+#else /* !CONFIG_TCP_CSUM_OFFLOAD_RX */
+			sub_skb->ip_summed = CHECKSUM_NONE;
+#endif //CONFIG_TCP_CSUM_OFFLOAD_RX
+
+			netif_rx(sub_skb);
+		}
+#else //PLATFORM_FREEBSD
+
+		//PLATFORM_FREEBSD
+		sub_m = subframes[i];
+		ptr=mtod(sub_m, u8 *);
+		offset=ETH_HLEN;
+		/* convert hdr + possible LLC headers into Ethernet header */
+#ifdef ENDIAN_FREE
+		eth_type = ntohs(*(u16*)&ptr[offset+6]);
+#else // ENDIAN_FREE
+		eth_type = (  ptr[offset+6] << 8) | ptr[offset+7];
+#endif // ENDIAN_FREE
+		if (sub_m->m_pkthdr.len >= ETH_HLEN+8 &&
+			((_rtw_memcmp(ptr+ETH_HLEN, rtw_rfc1042_header, SNAP_SIZE) &&
+			  eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) ||
+			 _rtw_memcmp(ptr+ETH_HLEN, rtw_bridge_tunnel_header, SNAP_SIZE) )) {
+			/* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
+			offset+=SNAP_SIZE;
+			_rtw_memcpy(&ptr[offset-ETH_ALEN], pattrib->src, ETH_ALEN);
+			offset-=ETH_ALEN;
+			_rtw_memcpy(&ptr[offset-ETH_ALEN], pattrib->dst, ETH_ALEN);
+			offset-=ETH_ALEN;
+		} else {
+			u16 len;
+			/* Leave Ethernet header part of hdr and full payload */
+			len = htons(sub_m->m_pkthdr.len-offset);
+			_rtw_memcpy(&ptr[offset- 2], &len, 2);
+			offset-=2;
+			_rtw_memcpy(&ptr[offset-ETH_ALEN], pattrib->src, ETH_ALEN);
+			offset-=ETH_ALEN;
+			_rtw_memcpy(&ptr[offset-ETH_ALEN], pattrib->dst, ETH_ALEN);
+			offset-=ETH_ALEN;
+		}
+
+		m_adj(sub_m,offset);
+
+		/* Indicat the packets to upper layer */
+		if (sub_m) {
+
+#if 0
+#ifdef CONFIG_TCP_CSUM_OFFLOAD_RX
+			if ( (pattrib->tcpchk_valid == 1) && (pattrib->tcp_chkrpt == 1) ) {
+				sub_skb->ip_summed = CHECKSUM_UNNECESSARY;
+			} else {
+				sub_skb->ip_summed = CHECKSUM_NONE;
+			}
+#else /* !CONFIG_TCP_CSUM_OFFLOAD_RX */
+			sub_skb->ip_summed = CHECKSUM_NONE;
+#endif //CONFIG_TCP_CSUM_OFFLOAD_RX
+#endif //0
+
+			if ( ((u32)(mtod(sub_m, caddr_t) + 14) % 4) != 0)
+				printf("%s()-%d: mtod(sub_m) = %p\n", __FUNCTION__, __LINE__, mtod(sub_m, caddr_t));
+#ifdef CONFIG_RX_INDICATE_QUEUE
+			IF_ENQUEUE(&precvpriv->rx_indicate_queue, sub_m);
+			if (_IF_QLEN(&precvpriv->rx_indicate_queue) <= 1) {
+				taskqueue_enqueue(taskqueue_thread, &precvpriv->rx_indicate_tasklet);
+			}
+#else	// CONFIG_RX_INDICATE_QUEUE
+			(*padapter->pifp->if_input)(padapter->pifp, sub_m);	
+#endif	// CONFIG_RX_INDICATE_QUEUE
+		}		
+
+#endif //PLATFORM_FREEBSD
+	}
+
+exit:
+
+	prframe->u.hdr.len=0;
+	rtw_free_recvframe(prframe, pfree_recv_queue);//free this recv_frame
+	
+	return ret;
+#else  // || defined (PLATFORM_LINUX) || defined (PLATFORM_FREEBSD) 
+#ifdef PLATFORM_WINDOWS
+	_irqL irql;
+#endif //PLATFORM_WINDOWS
+	unsigned char *ptr, *pdata, *pbuf, *psnap_type;
+	union recv_frame *pnrframe, *pnrframe_new;
+	int a_len, mv_len, padding_len;
+	u16 eth_type, type_len;
+	u8 bsnaphdr;
+	struct ieee80211_snap_hdr	*psnap;
+	struct _vlan *pvlan;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	_queue *pfree_recv_queue = &(precvpriv->free_recv_queue);
+	int ret = _SUCCESS;
+#ifdef PLATFORM_WINDOWS
+	struct recv_buf *precvbuf = prframe->u.hdr.precvbuf;
+#endif //PLATFORM_WINDOWS
+	a_len = prframe->u.hdr.len - prframe->u.hdr.attrib.hdrlen;
+
+	recvframe_pull(prframe, prframe->u.hdr.attrib.hdrlen);
+
+	if(prframe->u.hdr.attrib.iv_len >0)
+	{
+		recvframe_pull(prframe, prframe->u.hdr.attrib.iv_len);
+	}
+
+	pdata = prframe->u.hdr.rx_data;
+
+	prframe->u.hdr.len=0;
+
+	pnrframe = prframe;
+
+
+	do{
+
+		mv_len=0;
+		pnrframe->u.hdr.rx_data = pnrframe->u.hdr.rx_tail = pdata;
+		ptr = pdata;
+
+
+		_rtw_memcpy(pnrframe->u.hdr.attrib.dst, ptr, ETH_ALEN);
+		ptr+=ETH_ALEN;
+		_rtw_memcpy(pnrframe->u.hdr.attrib.src, ptr, ETH_ALEN);
+		ptr+=ETH_ALEN;
+
+		_rtw_memcpy(&type_len, ptr, 2);
+		type_len= ntohs((unsigned short )type_len);
+		ptr +=2;
+		mv_len += ETH_HLEN;
+
+		recvframe_put(pnrframe, type_len+ETH_HLEN);//update tail;
+
+		if(pnrframe->u.hdr.rx_data >= pnrframe->u.hdr.rx_tail || type_len<8)
+		{
+			//panic("pnrframe->u.hdr.rx_data >= pnrframe->u.hdr.rx_tail || type_len<8\n");
+
+			rtw_free_recvframe(pnrframe, pfree_recv_queue);
+
+			goto exit;
+		}
+
+		psnap=(struct ieee80211_snap_hdr *)(ptr);
+		psnap_type=ptr+SNAP_SIZE;
+		if (psnap->dsap==0xaa && psnap->ssap==0xaa && psnap->ctrl==0x03)
+		{
+			if ( _rtw_memcmp(psnap->oui, oui_rfc1042, WLAN_IEEE_OUI_LEN))
+			{
+				bsnaphdr=_TRUE;//wlan_pkt_format = WLAN_PKT_FORMAT_SNAP_RFC1042;
+			}
+			else if (_rtw_memcmp(psnap->oui, SNAP_HDR_APPLETALK_DDP, WLAN_IEEE_OUI_LEN) &&
+					_rtw_memcmp(psnap_type, SNAP_ETH_TYPE_APPLETALK_DDP, 2) )
+			{
+				bsnaphdr=_TRUE;	//wlan_pkt_format = WLAN_PKT_FORMAT_APPLETALK;
+			}
+			else if (_rtw_memcmp( psnap->oui, oui_8021h, WLAN_IEEE_OUI_LEN))
+			{
+				bsnaphdr=_TRUE;	//wlan_pkt_format = WLAN_PKT_FORMAT_SNAP_TUNNEL;
+			}
+			else
+			{
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("drop pkt due to invalid frame format!\n"));
+
+				//KeBugCheckEx(0x87123333, 0xe0, 0x4c, 0x87, 0xdd);
+
+				//panic("0x87123333, 0xe0, 0x4c, 0x87, 0xdd\n");
+
+				rtw_free_recvframe(pnrframe, pfree_recv_queue);
+
+				goto exit;
+			}
+
+		}
+		else
+		{
+			bsnaphdr=_FALSE;//wlan_pkt_format = WLAN_PKT_FORMAT_OTHERS;
+		}
+
+		ptr += (bsnaphdr?SNAP_SIZE:0);
+		_rtw_memcpy(&eth_type, ptr, 2);
+		eth_type= ntohs((unsigned short )eth_type); //pattrib->ether_type
+
+		mv_len+= 2+(bsnaphdr?SNAP_SIZE:0);
+		ptr += 2;//now move to iphdr;
+
+		pvlan = NULL;
+		if(eth_type == 0x8100) //vlan
+		{
+			pvlan = (struct _vlan *)ptr;
+			ptr+=4;
+			mv_len+=4;
+		}
+
+		if(eth_type==0x0800)//ip
+		{
+			struct iphdr*  piphdr = (struct iphdr*)ptr;
+
+
+			if (piphdr->protocol == 0x06)
+			{
+				RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("@@@===recv tcp len:%d @@@===\n", pnrframe->u.hdr.len));
+			}
+		}
+#ifdef PLATFORM_OS_XP
+		else
+		{
+			NDIS_PACKET_8021Q_INFO VlanPriInfo;
+			UINT32 UserPriority = pnrframe->u.hdr.attrib.priority;
+			UINT32 VlanID = (pvlan!=NULL ? get_vlan_id(pvlan) : 0 );
+
+			VlanPriInfo.Value =          // Get current value.
+					NDIS_PER_PACKET_INFO_FROM_PACKET(pnrframe->u.hdr.pkt, Ieee8021QInfo);
+
+			VlanPriInfo.TagHeader.UserPriority = UserPriority;
+			VlanPriInfo.TagHeader.VlanId =  VlanID;
+
+			VlanPriInfo.TagHeader.CanonicalFormatId = 0; // Should be zero.
+			VlanPriInfo.TagHeader.Reserved = 0; // Should be zero.
+			NDIS_PER_PACKET_INFO_FROM_PACKET(pnrframe->u.hdr.pkt, Ieee8021QInfo) = VlanPriInfo.Value;
+
+		}
+#endif //PLATFORM_OS_XP
+
+		pbuf = recvframe_pull(pnrframe, (mv_len-sizeof(struct ethhdr)));
+
+		_rtw_memcpy(pbuf, pnrframe->u.hdr.attrib.dst, ETH_ALEN);
+		_rtw_memcpy(pbuf+ETH_ALEN, pnrframe->u.hdr.attrib.src, ETH_ALEN);
+
+		eth_type = htons((unsigned short)eth_type) ;
+		_rtw_memcpy(pbuf+12, &eth_type, 2);
+
+		padding_len = (4) - ((type_len + ETH_HLEN)&(4-1));
+
+		a_len -= (type_len + ETH_HLEN + padding_len) ;
+
+
+#if 0
+
+	if(a_len > ETH_HLEN)
+	{
+		pnrframe_new = rtw_alloc_recvframe(pfree_recv_queue);
+		if(pnrframe_new)
+		{
+			_pkt *pskb_copy;
+			unsigned int copy_len  = pnrframe->u.hdr.len;
+
+			_rtw_init_listhead(&pnrframe_new->u.hdr.list);
+
+	#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,18)) // http://www.mail-archive.com/netdev@vger.kernel.org/msg17214.html
+			pskb_copy = dev_alloc_skb(copy_len+64);
+	#else
+			pskb_copy = netdev_alloc_skb(padapter->pnetdev, copy_len + 64);
+	#endif
+			if(pskb_copy==NULL)
+			{
+				DBG_871X("amsdu_to_msdu:can not all(ocate memory for skb copy\n");
+			}
+
+			pnrframe_new->u.hdr.pkt = pskb_copy;
+
+			_rtw_memcpy(pskb_copy->data, pnrframe->u.hdr.rx_data, copy_len);
+
+			pnrframe_new->u.hdr.rx_data = pnrframe->u.hdr.rx_data;
+			pnrframe_new->u.hdr.rx_tail = pnrframe->u.hdr.rx_data + copy_len;
+
+
+			if ((padapter->bDriverStopped ==_FALSE)&&( padapter->bSurpriseRemoved==_FALSE))
+			{
+				rtw_recv_indicatepkt(padapter, pnrframe_new);//indicate this recv_frame
+			}
+			else
+			{
+				rtw_free_recvframe(pnrframe_new, pfree_recv_queue);//free this recv_frame
+			}
+
+		}
+		else
+		{
+			DBG_871X("amsdu_to_msdu:can not allocate memory for pnrframe_new\n");
+		}
+
+	}
+	else
+	{
+		if ((padapter->bDriverStopped ==_FALSE)&&( padapter->bSurpriseRemoved==_FALSE))
+		{
+			rtw_recv_indicatepkt(padapter, pnrframe);//indicate this recv_frame
+		}
+		else
+		{
+			rtw_free_recvframe(pnrframe, pfree_recv_queue);//free this recv_frame
+		}
+
+		pnrframe = NULL;
+
+	}
+
+#else // 0
+
+		//padding_len = (4) - ((type_len + ETH_HLEN)&(4-1));
+
+		//a_len -= (type_len + ETH_HLEN + padding_len) ;
+
+		pnrframe_new = NULL;
+
+
+		if(a_len > ETH_HLEN)
+		{
+			pnrframe_new = rtw_alloc_recvframe(pfree_recv_queue);
+
+			if(pnrframe_new)
+			{
+
+
+				//pnrframe_new->u.hdr.precvbuf = precvbuf;//precvbuf is assigned before call rtw_init_recvframe()
+				//rtw_init_recvframe(pnrframe_new, precvpriv);
+				{
+#ifdef PLATFORM_LINUX
+						_pkt *pskb = pnrframe->u.hdr.pkt;
+#endif //PLATFORM_LINUX						
+						_rtw_init_listhead(&pnrframe_new->u.hdr.list);
+
+						pnrframe_new->u.hdr.len=0;
+
+#ifdef PLATFORM_LINUX
+						if(pskb)
+						{
+							pnrframe_new->u.hdr.pkt = skb_clone(pskb, GFP_ATOMIC);
+						}
+#endif //PLATFORM_LINUX
+
+				}
+
+				pdata += (type_len + ETH_HLEN + padding_len);
+				pnrframe_new->u.hdr.rx_head = pnrframe_new->u.hdr.rx_data = pnrframe_new->u.hdr.rx_tail = pdata;
+				pnrframe_new->u.hdr.rx_end = pdata + a_len + padding_len;//
+
+#ifdef PLATFORM_WINDOWS
+				pnrframe_new->u.hdr.precvbuf=precvbuf;
+				_enter_critical_bh(&precvbuf->recvbuf_lock, &irql);
+				precvbuf->ref_cnt++;
+				_exit_critical_bh(&precvbuf->recvbuf_lock, &irql);
+#endif //PLATFORM_WINDOWS
+
+			}
+			else
+			{
+				//panic("pnrframe_new=%x\n", pnrframe_new);
+			}
+		}
+
+
+		if ((padapter->bDriverStopped ==_FALSE)&&( padapter->bSurpriseRemoved==_FALSE) )
+		{
+			rtw_recv_indicatepkt(padapter, pnrframe);//indicate this recv_frame
+		}
+		else
+		{
+			rtw_free_recvframe(pnrframe, pfree_recv_queue);//free this recv_frame
+		}
+
+
+		pnrframe = NULL;
+		if(pnrframe_new)
+		{
+			pnrframe = pnrframe_new;
+		}
+
+
+#endif // end defined (PLATFORM_LINUX) || defined (PLATFORM_FREEBSD) 
+
+	}while(pnrframe);
+
+exit:
+
+	return ret;
+#endif
+}
+
+int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num);
+int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num)
+{
+	u8	wsize = preorder_ctrl->wsize_b;
+	u16	wend = (preorder_ctrl->indicate_seq + wsize -1) & 0xFFF;//% 4096;
+
+	// Rx Reorder initialize condition.
+	if (preorder_ctrl->indicate_seq == 0xFFFF)
+	{
+		preorder_ctrl->indicate_seq = seq_num;
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d init IndicateSeq: %d, NewSeq: %d\n", __FUNCTION__, __LINE__,
+			preorder_ctrl->indicate_seq, seq_num);
+		#endif
+
+		//DbgPrint("check_indicate_seq, 1st->indicate_seq=%d\n", precvpriv->indicate_seq);
+	}
+
+	//DbgPrint("enter->check_indicate_seq(): IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num);
+
+	// Drop out the packet which SeqNum is smaller than WinStart
+	if( SN_LESS(seq_num, preorder_ctrl->indicate_seq) )
+	{
+		//RT_TRACE(COMP_RX_REORDER, DBG_LOUD, ("CheckRxTsIndicateSeq(): Packet Drop! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, NewSeqNum));
+		//DbgPrint("CheckRxTsIndicateSeq(): Packet Drop! IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num);
+
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("%s IndicateSeq: %d > NewSeq: %d\n", __FUNCTION__, 
+			preorder_ctrl->indicate_seq, seq_num);
+		#endif
+
+
+		return _FALSE;
+	}
+
+	//
+	// Sliding window manipulation. Conditions includes:
+	// 1. Incoming SeqNum is equal to WinStart =>Window shift 1
+	// 2. Incoming SeqNum is larger than the WinEnd => Window shift N
+	//
+	if( SN_EQUAL(seq_num, preorder_ctrl->indicate_seq) )
+	{
+		preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d SN_EQUAL IndicateSeq: %d, NewSeq: %d\n", __FUNCTION__, __LINE__,
+			preorder_ctrl->indicate_seq, seq_num);
+		#endif
+	}
+	else if(SN_LESS(wend, seq_num))
+	{
+		//RT_TRACE(COMP_RX_REORDER, DBG_LOUD, ("CheckRxTsIndicateSeq(): Window Shift! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, NewSeqNum));
+		//DbgPrint("CheckRxTsIndicateSeq(): Window Shift! IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num);
+
+		// boundary situation, when seq_num cross 0xFFF
+		if(seq_num >= (wsize - 1))
+			preorder_ctrl->indicate_seq = seq_num + 1 -wsize;
+		else
+			preorder_ctrl->indicate_seq = 0xFFF - (wsize - (seq_num + 1)) + 1;
+
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d SN_LESS(wend, seq_num) IndicateSeq: %d, NewSeq: %d\n", __FUNCTION__, __LINE__,
+			preorder_ctrl->indicate_seq, seq_num);
+		#endif
+	}
+
+	//DbgPrint("exit->check_indicate_seq(): IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num);
+
+	return _TRUE;
+}
+
+int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, union recv_frame *prframe);
+int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, union recv_frame *prframe)
+{
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	_queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+	_list	*phead, *plist;
+	union recv_frame *pnextrframe;
+	struct rx_pkt_attrib *pnextattrib;
+
+	//DbgPrint("+enqueue_reorder_recvframe()\n");
+
+	//_enter_critical_ex(&ppending_recvframe_queue->lock, &irql);
+	//_rtw_spinlock_ex(&ppending_recvframe_queue->lock);
+
+
+	phead = get_list_head(ppending_recvframe_queue);
+	plist = get_next(phead);
+
+	while(rtw_end_of_queue_search(phead, plist) == _FALSE)
+	{
+		pnextrframe = LIST_CONTAINOR(plist, union recv_frame, u);
+		pnextattrib = &pnextrframe->u.hdr.attrib;
+
+		if(SN_LESS(pnextattrib->seq_num, pattrib->seq_num))
+		{
+			plist = get_next(plist);
+		}
+		else if( SN_EQUAL(pnextattrib->seq_num, pattrib->seq_num))
+		{
+			//Duplicate entry is found!! Do not insert current entry.
+			//RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("InsertRxReorderList(): Duplicate packet is dropped!! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum));
+
+			//_exit_critical_ex(&ppending_recvframe_queue->lock, &irql);
+
+			return _FALSE;
+		}
+		else
+		{
+			break;
+		}
+
+		//DbgPrint("enqueue_reorder_recvframe():while\n");
+
+	}
+
+
+	//_enter_critical_ex(&ppending_recvframe_queue->lock, &irql);
+	//_rtw_spinlock_ex(&ppending_recvframe_queue->lock);
+
+	rtw_list_delete(&(prframe->u.hdr.list));
+
+	rtw_list_insert_tail(&(prframe->u.hdr.list), plist);
+
+	//_rtw_spinunlock_ex(&ppending_recvframe_queue->lock);
+	//_exit_critical_ex(&ppending_recvframe_queue->lock, &irql);
+
+
+	//RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("InsertRxReorderList(): Pkt insert into buffer!! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum));
+	return _TRUE;
+
+}
+
+int recv_indicatepkts_in_order(_adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced);
+int recv_indicatepkts_in_order(_adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced)
+{
+	//_irqL irql;
+	//u8 bcancelled;
+	_list	*phead, *plist;
+	union recv_frame *prframe;
+	struct rx_pkt_attrib *pattrib;
+	//u8 index = 0;
+	int bPktInBuf = _FALSE;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	_queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+	//DbgPrint("+recv_indicatepkts_in_order\n");
+
+	//_enter_critical_ex(&ppending_recvframe_queue->lock, &irql);
+	//_rtw_spinlock_ex(&ppending_recvframe_queue->lock);
+
+	phead = 	get_list_head(ppending_recvframe_queue);
+	plist = get_next(phead);
+
+#if 0
+	// Check if there is any other indication thread running.
+	if(pTS->RxIndicateState == RXTS_INDICATE_PROCESSING)
+		return;
+#endif
+
+	// Handling some condition for forced indicate case.
+	if(bforced==_TRUE)
+	{
+		if(rtw_is_list_empty(phead))
+		{
+			// _exit_critical_ex(&ppending_recvframe_queue->lock, &irql);
+			//_rtw_spinunlock_ex(&ppending_recvframe_queue->lock);
+			return _TRUE;
+		}
+	
+		 prframe = LIST_CONTAINOR(plist, union recv_frame, u);
+	        pattrib = &prframe->u.hdr.attrib;	
+		preorder_ctrl->indicate_seq = pattrib->seq_num;		
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __FUNCTION__, __LINE__,
+			preorder_ctrl->indicate_seq, pattrib->seq_num);
+		#endif
+	}
+
+	// Prepare indication list and indication.
+	// Check if there is any packet need indicate.
+	while(!rtw_is_list_empty(phead))
+	{
+	
+		prframe = LIST_CONTAINOR(plist, union recv_frame, u);
+		pattrib = &prframe->u.hdr.attrib;
+
+		if(!SN_LESS(preorder_ctrl->indicate_seq, pattrib->seq_num))
+		{
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
+				 ("recv_indicatepkts_in_order: indicate=%d seq=%d amsdu=%d\n",
+				  preorder_ctrl->indicate_seq, pattrib->seq_num, pattrib->amsdu));
+
+#if 0
+			// This protect buffer from overflow.
+			if(index >= REORDER_WIN_SIZE)
+			{
+				RT_ASSERT(FALSE, ("IndicateRxReorderList(): Buffer overflow!! \n"));
+				bPktInBuf = TRUE;
+				break;
+			}
+#endif
+
+			plist = get_next(plist);
+			rtw_list_delete(&(prframe->u.hdr.list));
+
+			if(SN_EQUAL(preorder_ctrl->indicate_seq, pattrib->seq_num))
+			{
+				preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+				#ifdef DBG_RX_SEQ
+				DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __FUNCTION__, __LINE__,
+					preorder_ctrl->indicate_seq, pattrib->seq_num);
+				#endif
+			}
+
+#if 0
+			index++;
+			if(index==1)
+			{
+				//Cancel previous pending timer.
+				//PlatformCancelTimer(Adapter, &pTS->RxPktPendingTimer);
+				if(bforced!=_TRUE)
+				{
+					//DBG_871X("_cancel_timer(&preorder_ctrl->reordering_ctrl_timer, &bcancelled);\n");
+					_cancel_timer(&preorder_ctrl->reordering_ctrl_timer, &bcancelled);
+				}
+			}
+#endif
+
+			//Set this as a lock to make sure that only one thread is indicating packet.
+			//pTS->RxIndicateState = RXTS_INDICATE_PROCESSING;
+
+			// Indicate packets
+			//RT_ASSERT((index<=REORDER_WIN_SIZE), ("RxReorderIndicatePacket(): Rx Reorder buffer full!! \n"));
+
+
+			//indicate this recv_frame
+			//DbgPrint("recv_indicatepkts_in_order, indicate_seq=%d, seq_num=%d\n", precvpriv->indicate_seq, pattrib->seq_num);
+			if(!pattrib->amsdu)
+			{
+				//DBG_871X("recv_indicatepkts_in_order, amsdu!=1, indicate_seq=%d, seq_num=%d\n", preorder_ctrl->indicate_seq, pattrib->seq_num);
+
+				if ((padapter->bDriverStopped == _FALSE) &&
+				    (padapter->bSurpriseRemoved == _FALSE))
+				{
+					
+					rtw_recv_indicatepkt(padapter, prframe);//indicate this recv_frame
+										
+				}
+			}
+			else if(pattrib->amsdu==1)
+			{
+				if(amsdu_to_msdu(padapter, prframe)!=_SUCCESS)
+				{
+					rtw_free_recvframe(prframe, &precvpriv->free_recv_queue);
+				}
+			}
+			else
+			{
+				//error condition;
+			}
+
+
+			//Update local variables.
+			bPktInBuf = _FALSE;
+
+		}
+		else
+		{
+			bPktInBuf = _TRUE;
+			break;
+		}
+
+		//DbgPrint("recv_indicatepkts_in_order():while\n");
+
+	}
+
+	//_rtw_spinunlock_ex(&ppending_recvframe_queue->lock);
+	//_exit_critical_ex(&ppending_recvframe_queue->lock, &irql);
+
+/*
+	//Release the indication lock and set to new indication step.
+	if(bPktInBuf)
+	{
+		// Set new pending timer.
+		//pTS->RxIndicateState = RXTS_INDICATE_REORDER;
+		//PlatformSetTimer(Adapter, &pTS->RxPktPendingTimer, pHTInfo->RxReorderPendingTime);
+		//DBG_871X("_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME)\n");
+		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+	}
+	else
+	{
+		//pTS->RxIndicateState = RXTS_INDICATE_IDLE;
+	}
+*/
+	//_exit_critical_ex(&ppending_recvframe_queue->lock, &irql);
+
+	//return _TRUE;
+	return bPktInBuf;
+
+}
+
+int recv_indicatepkt_reorder(_adapter *padapter, union recv_frame *prframe);
+int recv_indicatepkt_reorder(_adapter *padapter, union recv_frame *prframe)
+{
+	_irqL irql;
+	int retval = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct recv_reorder_ctrl *preorder_ctrl = prframe->u.hdr.preorder_ctrl;
+	_queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+	if(!pattrib->amsdu)
+	{
+		//s1.
+		wlanhdr_to_ethhdr(prframe);
+
+		if ((pattrib->qos!=1) /*|| pattrib->priority!=0 || IS_MCAST(pattrib->ra)*/
+			|| (pattrib->eth_type==0x0806) || (pattrib->ack_policy!=0))
+		{
+			if ((padapter->bDriverStopped == _FALSE) &&
+			    (padapter->bSurpriseRemoved == _FALSE))
+			{
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@  recv_indicatepkt_reorder -recv_func recv_indicatepkt\n" ));
+
+				rtw_recv_indicatepkt(padapter, prframe);
+				return _SUCCESS;
+
+			}
+			
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s pattrib->qos !=1\n", __FUNCTION__);
+			#endif
+			
+			return _FAIL;
+		
+		}
+
+		if (preorder_ctrl->enable == _FALSE)
+		{
+			//indicate this recv_frame			
+			preorder_ctrl->indicate_seq = pattrib->seq_num;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __FUNCTION__, __LINE__,
+				preorder_ctrl->indicate_seq, pattrib->seq_num);
+			#endif
+			
+			rtw_recv_indicatepkt(padapter, prframe);		
+			
+			preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1)%4096;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __FUNCTION__, __LINE__,
+				preorder_ctrl->indicate_seq, pattrib->seq_num);
+			#endif
+			
+			return _SUCCESS;	
+		}			
+
+#ifndef CONFIG_RECV_REORDERING_CTRL
+		//indicate this recv_frame
+		rtw_recv_indicatepkt(padapter, prframe);
+		return _SUCCESS;
+#endif
+
+	}
+	else if(pattrib->amsdu==1) //temp filter -> means didn't support A-MSDUs in a A-MPDU
+	{
+		if (preorder_ctrl->enable == _FALSE)
+		{
+			preorder_ctrl->indicate_seq = pattrib->seq_num;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __FUNCTION__, __LINE__,
+				preorder_ctrl->indicate_seq, pattrib->seq_num);
+			#endif
+
+			retval = amsdu_to_msdu(padapter, prframe);
+
+			preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1)%4096;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __FUNCTION__, __LINE__,
+				preorder_ctrl->indicate_seq, pattrib->seq_num);
+			#endif
+
+			if(retval != _SUCCESS){
+				#ifdef DBG_RX_DROP_FRAME
+				DBG_871X("DBG_RX_DROP_FRAME %s amsdu_to_msdu fail\n", __FUNCTION__);
+				#endif
+			}
+
+			return retval;
+		}
+	}
+	else
+	{
+
+	}
+
+	_enter_critical_bh(&ppending_recvframe_queue->lock, &irql);
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
+		 ("recv_indicatepkt_reorder: indicate=%d seq=%d\n",
+		  preorder_ctrl->indicate_seq, pattrib->seq_num));
+
+	//s2. check if winstart_b(indicate_seq) needs to been updated
+	if(!check_indicate_seq(preorder_ctrl, pattrib->seq_num))
+	{
+		//pHTInfo->RxReorderDropCounter++;
+		//ReturnRFDList(Adapter, pRfd);
+		//RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("RxReorderIndicatePacket() ==> Packet Drop!!\n"));
+		//_exit_critical_ex(&ppending_recvframe_queue->lock, &irql);
+		//return _FAIL;
+
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s check_indicate_seq fail\n", __FUNCTION__);
+		#endif
+#if 0		
+		rtw_recv_indicatepkt(padapter, prframe);
+
+		_exit_critical_bh(&ppending_recvframe_queue->lock, &irql);
+		
+		goto _success_exit;
+#else
+		goto _err_exit;
+#endif
+	}
+
+
+	//s3. Insert all packet into Reorder Queue to maintain its ordering.
+	if(!enqueue_reorder_recvframe(preorder_ctrl, prframe))
+	{
+		//DbgPrint("recv_indicatepkt_reorder, enqueue_reorder_recvframe fail!\n");
+		//_exit_critical_ex(&ppending_recvframe_queue->lock, &irql);
+		//return _FAIL;
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s enqueue_reorder_recvframe fail\n", __FUNCTION__);
+		#endif
+		goto _err_exit;
+	}
+
+
+	//s4.
+	// Indication process.
+	// After Packet dropping and Sliding Window shifting as above, we can now just indicate the packets
+	// with the SeqNum smaller than latest WinStart and buffer other packets.
+	//
+	// For Rx Reorder condition:
+	// 1. All packets with SeqNum smaller than WinStart => Indicate
+	// 2. All packets with SeqNum larger than or equal to WinStart => Buffer it.
+	//
+
+	//recv_indicatepkts_in_order(padapter, preorder_ctrl, _TRUE);
+	if(recv_indicatepkts_in_order(padapter, preorder_ctrl, _FALSE)==_TRUE)
+	{
+		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+		_exit_critical_bh(&ppending_recvframe_queue->lock, &irql);
+	}
+	else
+	{
+		_exit_critical_bh(&ppending_recvframe_queue->lock, &irql);
+		_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+	}
+
+
+_success_exit:
+
+	return _SUCCESS;
+
+_err_exit:
+
+        _exit_critical_bh(&ppending_recvframe_queue->lock, &irql);
+
+	return _FAIL;
+}
+
+
+void rtw_reordering_ctrl_timeout_handler(void *pcontext)
+{
+	_irqL irql;
+	struct recv_reorder_ctrl *preorder_ctrl = (struct recv_reorder_ctrl *)pcontext;
+	_adapter *padapter = preorder_ctrl->padapter;
+	_queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+
+	if(padapter->bDriverStopped ||padapter->bSurpriseRemoved)
+	{
+		return;
+	}
+
+	//DBG_871X("+rtw_reordering_ctrl_timeout_handler()=>\n");
+
+	_enter_critical_bh(&ppending_recvframe_queue->lock, &irql);
+
+	if(recv_indicatepkts_in_order(padapter, preorder_ctrl, _TRUE)==_TRUE)
+	{
+		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);		
+	}
+
+	_exit_critical_bh(&ppending_recvframe_queue->lock, &irql);
+
+}
+
+int process_recv_indicatepkts(_adapter *padapter, union recv_frame *prframe);
+int process_recv_indicatepkts(_adapter *padapter, union recv_frame *prframe)
+{
+	int retval = _SUCCESS;
+	//struct recv_priv *precvpriv = &padapter->recvpriv;
+	//struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+#ifdef CONFIG_TDLS
+	struct sta_info *psta = prframe->u.hdr.psta;
+#endif //CONFIG_TDLS
+
+#ifdef CONFIG_80211N_HT
+
+	struct ht_priv	*phtpriv = &pmlmepriv->htpriv;
+
+#ifdef CONFIG_TDLS
+	if( (phtpriv->ht_option==_TRUE) ||
+		((psta->tdls_sta_state & TDLS_LINKED_STATE) && 
+		 (psta->htpriv.ht_option==_TRUE) &&
+		 (psta->htpriv.ampdu_enable==_TRUE))) //B/G/N Mode
+#else
+	if(phtpriv->ht_option==_TRUE)  //B/G/N Mode
+#endif //CONFIG_TDLS
+	{
+		//prframe->u.hdr.preorder_ctrl = &precvpriv->recvreorder_ctrl[pattrib->priority];
+
+		if(recv_indicatepkt_reorder(padapter, prframe)!=_SUCCESS)// including perform A-MPDU Rx Ordering Buffer Control
+		{
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s recv_indicatepkt_reorder error!\n", __FUNCTION__);
+			#endif
+		
+			if ((padapter->bDriverStopped == _FALSE) &&
+			    (padapter->bSurpriseRemoved == _FALSE))
+			{
+				retval = _FAIL;
+				return retval;
+			}
+		}
+	}
+	else //B/G mode
+#endif
+	{
+		retval=wlanhdr_to_ethhdr (prframe);
+		if(retval != _SUCCESS)
+		{
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("wlanhdr_to_ethhdr: drop pkt \n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s wlanhdr_to_ethhdr error!\n", __FUNCTION__);
+			#endif
+			return retval;
+		}
+
+		if ((padapter->bDriverStopped ==_FALSE)&&( padapter->bSurpriseRemoved==_FALSE))
+		{
+			//indicate this recv_frame
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@ process_recv_indicatepkts- recv_func recv_indicatepkt\n" ));
+			rtw_recv_indicatepkt(padapter, prframe);
+
+
+		}
+		else
+		{
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@ process_recv_indicatepkts- recv_func free_indicatepkt\n" ));
+
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_func:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved));
+			retval = _FAIL;
+			return retval;
+		}
+
+	}
+
+	return retval;
+
+}
+
+int recv_func_prehandle(_adapter *padapter, union recv_frame *rframe)
+{
+	int ret = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &rframe->u.hdr.attrib;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+	
+#ifdef CONFIG_MP_INCLUDED
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+#endif //CONFIG_MP_INCLUDED
+
+#ifdef CONFIG_MP_INCLUDED
+	if (padapter->registrypriv.mp_mode == 1)
+	{
+		if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE))//&&(padapter->mppriv.check_mp_pkt == 0))
+		{
+		if (pattrib->crc_err == 1)
+			padapter->mppriv.rx_crcerrpktcount++;
+		else
+			padapter->mppriv.rx_pktcount++;
+
+		if (check_fwstate(pmlmepriv, WIFI_MP_LPBK_STATE) == _FALSE) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_alert_, ("MP - Not in loopback mode , drop pkt \n"));
+			ret = _FAIL;
+			rtw_free_recvframe(rframe, pfree_recv_queue);//free this recv_frame
+			goto exit;
+		}
+		}
+	}
+#endif
+
+	//check the frame crtl field and decache
+	ret = validate_recv_frame(padapter, rframe);
+	if (ret != _SUCCESS)
+	{
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("recv_func: validate_recv_frame fail! drop pkt\n"));
+		rtw_free_recvframe(rframe, pfree_recv_queue);//free this recv_frame
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+int recv_func_posthandle(_adapter *padapter, union recv_frame *prframe)
+{
+	int ret = _SUCCESS;
+	union recv_frame *orig_prframe = prframe;
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+	
+
+#ifdef CONFIG_TDLS
+	u8 *psnap_type, *pcategory;
+	struct sta_info *ptdls_sta = NULL;
+#endif //CONFIG_TDLS
+
+
+	// DATA FRAME
+	rtw_led_control(padapter, LED_CTL_RX);
+
+	prframe = decryptor(padapter, prframe);
+	if (prframe == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("decryptor: drop pkt\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s decryptor: drop pkt\n", __FUNCTION__);
+		#endif
+		ret = _FAIL;
+		goto _recv_data_drop;
+	}
+
+#if 0
+	if ( padapter->adapter_type == PRIMARY_ADAPTER )
+	{
+		DBG_871X("+++\n");
+		{
+			int i;
+			u8	*ptr = get_recvframe_data(prframe);
+			for(i=0; i<140;i=i+8)
+				DBG_871X("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:", *(ptr+i),
+				*(ptr+i+1), *(ptr+i+2) ,*(ptr+i+3) ,*(ptr+i+4),*(ptr+i+5), *(ptr+i+6), *(ptr+i+7));
+
+		}
+		DBG_871X("---\n");
+	}
+#endif
+
+#ifdef CONFIG_TDLS
+	//check TDLS frame
+	psnap_type = get_recvframe_data(orig_prframe);
+	psnap_type+=pattrib->hdrlen + pattrib->iv_len+SNAP_SIZE;
+	pcategory = psnap_type + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN;
+
+	if((_rtw_memcmp(psnap_type, SNAP_ETH_TYPE_TDLS, ETH_TYPE_LEN)) &&
+		((*pcategory==RTW_WLAN_CATEGORY_TDLS) || (*pcategory==RTW_WLAN_CATEGORY_P2P))){
+		ret = OnTDLS(padapter, prframe);	//all of functions will return _FAIL
+		goto _exit_recv_func;
+	}
+#endif //CONFIG_TDLS
+
+	prframe = recvframe_chk_defrag(padapter, prframe);
+	if(prframe==NULL)	{
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("recvframe_chk_defrag: drop pkt\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s recvframe_chk_defrag: drop pkt\n", __FUNCTION__);
+		#endif
+		goto _recv_data_drop;		
+	}
+
+	prframe=portctrl(padapter, prframe);
+	if (prframe == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("portctrl: drop pkt \n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s portctrl: drop pkt\n", __FUNCTION__);
+		#endif
+		ret = _FAIL;
+		goto _recv_data_drop;
+	}
+
+#ifdef CONFIG_TDLS
+	if(padapter->tdlsinfo.setup_state == TDLS_LINKED_STATE)
+		ptdls_sta = rtw_get_stainfo(&padapter->stapriv, pattrib->src);
+	count_rx_stats(padapter, prframe, ptdls_sta);
+#else
+	count_rx_stats(padapter, prframe, NULL);
+#endif //CONFIG_TDLS
+
+#ifdef CONFIG_WAPI_SUPPORT
+	rtw_wapi_update_info(padapter, prframe);
+#endif
+
+#ifdef CONFIG_80211N_HT
+	ret = process_recv_indicatepkts(padapter, prframe);
+	if (ret != _SUCCESS)
+	{
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("recv_func: process_recv_indicatepkts fail! \n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s process_recv_indicatepkts fail!\n", __FUNCTION__);
+		#endif
+		rtw_free_recvframe(orig_prframe, pfree_recv_queue);//free this recv_frame
+		goto _recv_data_drop;
+	}
+#else // CONFIG_80211N_HT
+	if (!pattrib->amsdu)
+	{
+		ret = wlanhdr_to_ethhdr (prframe);
+		if (ret != _SUCCESS)
+		{
+			RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("wlanhdr_to_ethhdr: drop pkt \n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s wlanhdr_to_ethhdr: drop pkt\n", __FUNCTION__);
+			#endif
+			rtw_free_recvframe(orig_prframe, pfree_recv_queue);//free this recv_frame
+			goto _recv_data_drop;
+		}
+
+		if ((padapter->bDriverStopped == _FALSE) && (padapter->bSurpriseRemoved == _FALSE))
+		{
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_alert_, ("@@@@ recv_func: recv_func rtw_recv_indicatepkt\n" ));
+			//indicate this recv_frame
+			ret = rtw_recv_indicatepkt(padapter, prframe);
+			if (ret != _SUCCESS)
+			{	
+				#ifdef DBG_RX_DROP_FRAME
+				DBG_871X("DBG_RX_DROP_FRAME %s rtw_recv_indicatepkt fail!\n", __FUNCTION__);
+				#endif
+				goto _recv_data_drop;
+			}
+		}
+		else
+		{
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_alert_, ("@@@@  recv_func: rtw_free_recvframe\n" ));
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_debug_, ("recv_func:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s ecv_func:bDriverStopped(%d) OR bSurpriseRemoved(%d)\n", __FUNCTION__,
+				padapter->bDriverStopped, padapter->bSurpriseRemoved);
+			#endif
+			ret = _FAIL;
+			rtw_free_recvframe(orig_prframe, pfree_recv_queue); //free this recv_frame
+		}
+
+	}
+	else if(pattrib->amsdu==1)
+	{
+
+		ret = amsdu_to_msdu(padapter, prframe);
+		if(ret != _SUCCESS)
+		{
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s amsdu_to_msdu fail\n", __FUNCTION__);
+			#endif
+			rtw_free_recvframe(orig_prframe, pfree_recv_queue);
+			goto _recv_data_drop;
+		}
+	}
+	else
+	{
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s what is this condition??\n", __FUNCTION__);
+		#endif
+		goto _recv_data_drop;
+	}
+#endif // CONFIG_80211N_HT
+
+_exit_recv_func:
+	return ret;
+
+_recv_data_drop:
+	precvpriv->rx_drop++;
+	return ret;
+}
+
+
+int recv_func(_adapter *padapter, union recv_frame *rframe);
+int recv_func(_adapter *padapter, union recv_frame *rframe)
+{
+	int ret;
+	struct rx_pkt_attrib *prxattrib = &rframe->u.hdr.attrib;
+	struct recv_priv *recvpriv = &padapter->recvpriv;
+	struct security_priv *psecuritypriv=&padapter->securitypriv;
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+
+	/* check if need to handle uc_swdec_pending_queue*/
+	if (check_fwstate(mlmepriv, WIFI_STATION_STATE) && psecuritypriv->busetkipkey)
+	{
+		union recv_frame *pending_frame;
+		_irqL irqL;
+
+		while((pending_frame=rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue))) {
+			if (recv_func_posthandle(padapter, pending_frame) == _SUCCESS)
+				DBG_871X("%s: dequeue uc_swdec_pending_queue\n", __func__);
+		}
+	}
+
+	ret = recv_func_prehandle(padapter, rframe);
+
+	if(ret == _SUCCESS) {
+		
+		/* check if need to enqueue into uc_swdec_pending_queue*/
+		if (check_fwstate(mlmepriv, WIFI_STATION_STATE) &&
+			!IS_MCAST(prxattrib->ra) && prxattrib->encrypt>0 &&
+			(prxattrib->bdecrypted == 0 ||psecuritypriv->sw_decrypt == _TRUE) &&
+			!is_wep_enc(psecuritypriv->dot11PrivacyAlgrthm) &&
+			!psecuritypriv->busetkipkey) {
+			rtw_enqueue_recvframe(rframe, &padapter->recvpriv.uc_swdec_pending_queue);
+			DBG_871X("%s: no key, enqueue uc_swdec_pending_queue\n", __func__);
+			goto exit;
+		}
+		
+		ret = recv_func_posthandle(padapter, rframe);
+	}
+
+exit:
+	return ret;
+}
+
+
+s32 rtw_recv_entry(union recv_frame *precvframe)
+{
+	_adapter *padapter;
+	struct recv_priv *precvpriv;
+	s32 ret=_SUCCESS;
+
+_func_enter_;
+
+//	RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("+rtw_recv_entry\n"));
+
+	padapter = precvframe->u.hdr.adapter;
+
+	precvpriv = &padapter->recvpriv;
+
+
+	if ((ret = recv_func(padapter, precvframe)) == _FAIL)
+	{
+		RT_TRACE(_module_rtl871x_recv_c_,_drv_info_,("rtw_recv_entry: recv_func return fail!!!\n"));
+		goto _recv_entry_drop;
+	}
+
+
+	precvpriv->rx_pkts++;
+
+_func_exit_;
+
+	return ret;
+
+_recv_entry_drop:
+
+#ifdef CONFIG_MP_INCLUDED
+	if (padapter->registrypriv.mp_mode == 1)
+		padapter->mppriv.rx_pktloss = precvpriv->rx_drop;
+#endif
+
+	//RT_TRACE(_module_rtl871x_recv_c_,_drv_err_,("_recv_entry_drop\n"));
+
+_func_exit_;
+
+	return ret;
+}
+
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS){
+	_adapter *adapter = (_adapter *)FunctionContext;
+	struct recv_priv *recvpriv = &adapter->recvpriv;
+	
+	u32 tmp_s, tmp_q;
+	u8 avg_signal_strength = 0;
+	u8 avg_signal_qual = 0;
+	u32 num_signal_strength = 0;
+	u32 num_signal_qual = 0;
+	u8 _alpha = 3; // this value is based on converging_constant = 5000 and sampling_interval = 1000
+
+	if(adapter->recvpriv.is_signal_dbg) {
+		//update the user specific value, signal_strength_dbg, to signal_strength, rssi
+		adapter->recvpriv.signal_strength= adapter->recvpriv.signal_strength_dbg;
+		adapter->recvpriv.rssi=(s8)translate_percentage_to_dbm((u8)adapter->recvpriv.signal_strength_dbg);
+	} else {
+
+		if(recvpriv->signal_strength_data.update_req == 0) {// update_req is clear, means we got rx
+			avg_signal_strength = recvpriv->signal_strength_data.avg_val;
+			num_signal_strength = recvpriv->signal_strength_data.total_num;
+			// after avg_vals are accquired, we can re-stat the signal values
+			recvpriv->signal_strength_data.update_req = 1;
+		}
+		
+		if(recvpriv->signal_qual_data.update_req == 0) {// update_req is clear, means we got rx
+			avg_signal_qual = recvpriv->signal_qual_data.avg_val;
+			num_signal_qual = recvpriv->signal_qual_data.total_num;
+			// after avg_vals are accquired, we can re-stat the signal values
+			recvpriv->signal_qual_data.update_req = 1;
+		}
+
+		//update value of signal_strength, rssi, signal_qual
+		if(check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY) == _FALSE) {
+			tmp_s = (avg_signal_strength+(_alpha-1)*recvpriv->signal_strength);
+			if(tmp_s %_alpha)
+				tmp_s = tmp_s/_alpha + 1;
+			else
+				tmp_s = tmp_s/_alpha;
+			if(tmp_s>100)
+				tmp_s = 100;
+
+			tmp_q = (avg_signal_qual+(_alpha-1)*recvpriv->signal_qual);
+			if(tmp_q %_alpha)
+				tmp_q = tmp_q/_alpha + 1;
+			else
+				tmp_q = tmp_q/_alpha;
+			if(tmp_q>100)
+				tmp_q = 100;
+
+			recvpriv->signal_strength = tmp_s;
+			recvpriv->rssi = (s8)translate_percentage_to_dbm(tmp_s);
+			recvpriv->signal_qual = tmp_q;
+
+			#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1
+			DBG_871X("%s signal_strength:%3u, rssi:%3d, signal_qual:%3u"
+				", num_signal_strength:%u, num_signal_qual:%u"
+				"\n"
+				, __FUNCTION__
+				, recvpriv->signal_strength
+				, recvpriv->rssi
+				, recvpriv->signal_qual
+				, num_signal_strength, num_signal_qual
+			);
+			#endif
+		}
+	}
+	rtw_set_signal_stat_timer(recvpriv);
+	
+}
+#endif //CONFIG_NEW_SIGNAL_STAT_PROCESS
+
+
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_rf.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_rf.c
new file mode 100755
index 00000000..7ae86351
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_rf.c
@@ -0,0 +1,95 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_RF_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <recv_osdep.h>
+#include <xmit_osdep.h>
+
+
+struct ch_freq {
+	u32 channel;
+	u32 frequency;
+};
+
+struct ch_freq ch_freq_map[] = {
+	{1, 2412},{2, 2417},{3, 2422},{4, 2427},{5, 2432},
+	{6, 2437},{7, 2442},{8, 2447},{9, 2452},{10, 2457},
+	{11, 2462},{12, 2467},{13, 2472},{14, 2484},
+	/*  UNII */
+	{36, 5180},{40, 5200},{44, 5220},{48, 5240},{52, 5260},
+	{56, 5280},{60, 5300},{64, 5320},{149, 5745},{153, 5765},
+	{157, 5785},{161, 5805},{165, 5825},{167, 5835},{169, 5845},
+	{171, 5855},{173, 5865},
+	/* HiperLAN2 */
+	{100, 5500},{104, 5520},{108, 5540},{112, 5560},{116, 5580},
+	{120, 5600},{124, 5620},{128, 5640},{132, 5660},{136, 5680},
+	{140, 5700},
+	/* Japan MMAC */
+	{34, 5170},{38, 5190},{42, 5210},{46, 5230},
+	/*  Japan */
+	{184, 4920},{188, 4940},{192, 4960},{196, 4980},
+	{208, 5040},/* Japan, means J08 */
+	{212, 5060},/* Japan, means J12 */
+	{216, 5080},/* Japan, means J16 */
+};
+
+int ch_freq_map_num = (sizeof(ch_freq_map) / sizeof(struct ch_freq));
+
+u32 rtw_ch2freq(u32 channel)
+{
+	u8	i;
+	u32	freq = 0;
+
+	for (i = 0; i < ch_freq_map_num; i++)
+	{
+		if (channel == ch_freq_map[i].channel)
+		{
+			freq = ch_freq_map[i].frequency;
+				break;
+		}
+	}
+	if (i == ch_freq_map_num)
+		freq = 2412;
+
+	return freq;
+}
+
+u32 rtw_freq2ch(u32 freq)
+{
+	u8	i;
+	u32	ch = 0;
+
+	for (i = 0; i < ch_freq_map_num; i++)
+	{
+		if (freq == ch_freq_map[i].frequency)
+		{
+			ch = ch_freq_map[i].channel;
+				break;
+		}
+	}
+	if (i == ch_freq_map_num)
+		ch = 1;
+
+	return ch;
+}
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_security.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_security.c
new file mode 100755
index 00000000..4a3b20a9
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_security.c
@@ -0,0 +1,2893 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define  _RTW_SECURITY_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <wifi.h>
+#include <osdep_intf.h>
+
+
+//=====WEP related===== 
+
+#define CRC32_POLY 0x04c11db7
+
+struct arc4context
+{
+	u32 x;
+	u32 y;
+	u8 state[256];
+};
+
+
+static void arcfour_init(struct arc4context 	*parc4ctx, u8 * key,u32	key_len)
+{
+	u32	t, u;
+	u32	keyindex;
+	u32	stateindex;
+	u8 * state;
+	u32	counter;
+_func_enter_;
+	state = parc4ctx->state;
+	parc4ctx->x = 0;
+	parc4ctx->y = 0;
+	for (counter = 0; counter < 256; counter++)
+		state[counter] = (u8)counter;
+	keyindex = 0;
+	stateindex = 0;
+	for (counter = 0; counter < 256; counter++)
+	{
+		t = state[counter];
+		stateindex = (stateindex + key[keyindex] + t) & 0xff;
+		u = state[stateindex];
+		state[stateindex] = (u8)t;
+		state[counter] = (u8)u;
+		if (++keyindex >= key_len)
+			keyindex = 0;
+	}
+_func_exit_;	
+}
+static u32 arcfour_byte(	struct arc4context	*parc4ctx)
+{
+	u32 x;
+	u32 y;
+	u32 sx, sy;
+	u8 * state;
+_func_enter_;
+	state = parc4ctx->state;
+	x = (parc4ctx->x + 1) & 0xff;
+	sx = state[x];
+	y = (sx + parc4ctx->y) & 0xff;
+	sy = state[y];
+	parc4ctx->x = x;
+	parc4ctx->y = y;
+	state[y] = (u8)sx;
+	state[x] = (u8)sy;
+_func_exit_;	
+	return state[(sx + sy) & 0xff];
+}
+              
+           
+static void arcfour_encrypt(	struct arc4context	*parc4ctx, 
+	u8 * dest,
+	u8 * src, 
+	u32 len)
+{
+	u32	i;
+_func_enter_;	
+	for (i = 0; i < len; i++)
+		dest[i] = src[i] ^ (unsigned char)arcfour_byte(parc4ctx);
+_func_exit_;		
+}
+
+static sint bcrc32initialized = 0;
+static u32 crc32_table[256];
+
+
+static u8 crc32_reverseBit( u8 data)
+{
+	return( (u8)((data<<7)&0x80) | ((data<<5)&0x40) | ((data<<3)&0x20) | ((data<<1)&0x10) | ((data>>1)&0x08) | ((data>>3)&0x04) | ((data>>5)&0x02) | ((data>>7)&0x01) );
+}
+
+static void crc32_init(void)
+{
+_func_enter_;	
+	if (bcrc32initialized == 1) 
+		goto exit;
+	else{
+		sint i, j;
+		u32 c;
+		u8 *p=(u8 *)&c, *p1;
+		u8 k;
+
+		c = 0x12340000;
+
+		for (i = 0; i < 256; ++i) 
+		{
+			k = crc32_reverseBit((u8)i);
+			for (c = ((u32)k) << 24, j = 8; j > 0; --j){
+				c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY : (c << 1);
+			}
+			p1 = (u8 *)&crc32_table[i];
+
+			p1[0] = crc32_reverseBit(p[3]);
+			p1[1] = crc32_reverseBit(p[2]);
+			p1[2] = crc32_reverseBit(p[1]);
+			p1[3] = crc32_reverseBit(p[0]);
+		}
+		bcrc32initialized= 1;
+	}
+exit:	
+_func_exit_;			
+}
+
+static u32 getcrc32(u8 *buf, sint len)
+{
+	u8 *p;
+	u32  crc;
+_func_enter_;	
+	if (bcrc32initialized == 0) crc32_init();
+
+	crc = 0xffffffff;       /* preload shift register, per CRC-32 spec */
+
+	for (p = buf; len > 0; ++p, --len) 
+	{
+		crc = crc32_table[ (crc ^ *p) & 0xff] ^ (crc >> 8);
+	}
+_func_exit_;				
+	return ~crc;    /* transmit complement, per CRC-32 spec */
+}
+
+
+/*
+	Need to consider the fragment  situation
+*/
+void rtw_wep_encrypt(_adapter *padapter, u8 *pxmitframe)
+{																	// exclude ICV
+	
+	unsigned char	crc[4];
+	struct arc4context	 mycontext;
+
+	sint	curfragnum,length;
+	u32	keylength;
+
+	u8	*pframe, *payload,*iv;    //,*wepkey
+	u8	wepkey[16];
+	u8   hw_hdr_offset=0;
+	struct	pkt_attrib	 *pattrib = &((struct xmit_frame*)pxmitframe)->attrib;
+	struct 	security_priv	*psecuritypriv=&padapter->securitypriv;
+	struct	xmit_priv		*pxmitpriv=&padapter->xmitpriv;
+	
+_func_enter_;	
+
+	
+	if(((struct xmit_frame*)pxmitframe)->buf_addr==NULL)
+		return;
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+	hw_hdr_offset = TXDESC_SIZE +
+		 (((struct xmit_frame*)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);	
+#else
+	#ifdef CONFIG_TX_EARLY_MODE
+	hw_hdr_offset = TXDESC_OFFSET+EARLY_MODE_INFO_SIZE;
+	#else
+	hw_hdr_offset = TXDESC_OFFSET;
+	#endif
+#endif
+
+	pframe = ((struct xmit_frame*)pxmitframe)->buf_addr + hw_hdr_offset;
+	
+	//start to encrypt each fragment
+	if((pattrib->encrypt==_WEP40_)||(pattrib->encrypt==_WEP104_))
+	{
+		keylength=psecuritypriv->dot11DefKeylen[psecuritypriv->dot11PrivacyKeyIndex];
+
+		for(curfragnum=0;curfragnum<pattrib->nr_frags;curfragnum++)
+		{
+			iv=pframe+pattrib->hdrlen;
+			_rtw_memcpy(&wepkey[0], iv, 3);
+			_rtw_memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0],keylength);
+			payload=pframe+pattrib->iv_len+pattrib->hdrlen;
+
+			if((curfragnum+1)==pattrib->nr_frags)
+			{	//the last fragment
+			
+				length=pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len- pattrib->icv_len;
+			
+				*((unsigned long *)crc)=cpu_to_le32(getcrc32(payload,length));
+
+				arcfour_init(&mycontext, wepkey,3+keylength);
+				arcfour_encrypt(&mycontext, payload, payload, length);
+				arcfour_encrypt(&mycontext, payload+length, crc, 4);
+
+			}
+			else
+			{
+			length=pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
+				*((unsigned long *)crc)=cpu_to_le32(getcrc32(payload,length));
+				arcfour_init(&mycontext, wepkey,3+keylength);
+				arcfour_encrypt(&mycontext, payload, payload, length);
+				arcfour_encrypt(&mycontext, payload+length, crc, 4);
+	
+			pframe+=pxmitpriv->frag_len;
+			pframe=(u8 *)RND4((SIZE_PTR)(pframe));
+
+			}
+			
+		}		
+						
+	}
+	
+_func_exit_;						
+
+}
+
+void rtw_wep_decrypt(_adapter  *padapter, u8 *precvframe)
+{								
+	// exclude ICV
+	u8	crc[4];
+	struct arc4context	 mycontext;
+	sint 	length;
+	u32	keylength;
+	u8	*pframe, *payload,*iv,wepkey[16];
+	u8	 keyindex;
+	struct	rx_pkt_attrib	 *prxattrib = &(((union recv_frame*)precvframe)->u.hdr.attrib);
+	struct 	security_priv	*psecuritypriv=&padapter->securitypriv;
+
+_func_enter_;	
+
+	pframe=(unsigned char *)((union recv_frame*)precvframe)->u.hdr.rx_data;
+	
+	//start to decrypt recvframe
+	if((prxattrib->encrypt==_WEP40_)||(prxattrib->encrypt==_WEP104_))
+	{
+		iv=pframe+prxattrib->hdrlen;
+		//keyindex=(iv[3]&0x3);
+		keyindex = prxattrib->key_index;
+		keylength=psecuritypriv->dot11DefKeylen[keyindex];
+		_rtw_memcpy(&wepkey[0], iv, 3);
+		//_rtw_memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0],keylength);
+		_rtw_memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0],keylength);
+		length= ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
+
+		payload=pframe+prxattrib->iv_len+prxattrib->hdrlen;
+		
+		//decrypt payload include icv
+		arcfour_init(&mycontext, wepkey,3+keylength);
+		arcfour_encrypt(&mycontext, payload, payload,  length);
+				
+		//calculate icv and compare the icv
+		*((unsigned long *)crc)=le32_to_cpu(getcrc32(payload,length-4));
+		
+		if(crc[3]!=payload[length-1] || crc[2]!=payload[length-2] || crc[1]!=payload[length-3] || crc[0]!=payload[length-4])
+		{
+			RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_wep_decrypt:icv error crc[3](%x)!=payload[length-1](%x) || crc[2](%x)!=payload[length-2](%x) || crc[1](%x)!=payload[length-3](%x) || crc[0](%x)!=payload[length-4](%x)\n",
+						crc[3],payload[length-1],crc[2],payload[length-2],crc[1],payload[length-3],crc[0],payload[length-4]));
+		}	
+						
+	}
+	
+_func_exit_;		
+
+	return;
+	
+}
+
+//3 		=====TKIP related=====
+
+static u32 secmicgetuint32( u8 * p )
+// Convert from Byte[] to Us4Byte32 in a portable way
+{
+	s32 i;
+	u32 res = 0;
+_func_enter_;		
+	for( i=0; i<4; i++ )
+	{
+		res |= ((u32)(*p++)) << (8*i);
+	}
+_func_exit_;		
+	return res;
+}
+
+static void secmicputuint32( u8 * p, u32 val )
+// Convert from Us4Byte32 to Byte[] in a portable way
+{
+	long i;
+_func_enter_;			
+	for( i=0; i<4; i++ )
+	{
+		*p++ = (u8) (val & 0xff);
+		val >>= 8;
+	}
+_func_exit_;		
+}
+
+static void secmicclear(struct mic_data *pmicdata)
+{
+// Reset the state to the empty message.
+_func_enter_;	
+	pmicdata->L = pmicdata->K0;
+	pmicdata->R = pmicdata->K1;
+	pmicdata->nBytesInM = 0;
+	pmicdata->M = 0;
+_func_exit_;	
+}
+
+void rtw_secmicsetkey(struct mic_data *pmicdata, u8 * key )
+{
+	// Set the key
+_func_enter_;		
+	pmicdata->K0 = secmicgetuint32( key );
+	pmicdata->K1 = secmicgetuint32( key + 4 );
+	// and reset the message
+	secmicclear(pmicdata);
+_func_exit_;		
+}
+
+void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b )
+{
+_func_enter_;	
+	// Append the byte to our word-sized buffer
+	pmicdata->M |= ((unsigned long)b) << (8*pmicdata->nBytesInM);
+	pmicdata->nBytesInM++;
+	// Process the word if it is full.
+	if( pmicdata->nBytesInM >= 4 )
+	{
+		pmicdata->L ^= pmicdata->M;
+		pmicdata->R ^= ROL32( pmicdata->L, 17 );
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ((pmicdata->L & 0xff00ff00) >> 8) | ((pmicdata->L & 0x00ff00ff) << 8);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ROL32( pmicdata->L, 3 );
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ROR32( pmicdata->L, 2 );
+		pmicdata->L += pmicdata->R;
+		// Clear the buffer
+		pmicdata->M = 0;
+		pmicdata->nBytesInM = 0;
+	}
+_func_exit_;			
+}
+
+void rtw_secmicappend(struct mic_data *pmicdata, u8 * src, u32 nbytes )
+{
+_func_enter_;	
+	// This is simple
+	while( nbytes > 0 )
+	{
+		rtw_secmicappendbyte(pmicdata, *src++ );
+		nbytes--;
+	}
+_func_exit_;			
+}
+
+void rtw_secgetmic(struct mic_data *pmicdata, u8 * dst )
+{
+_func_enter_;	
+	// Append the minimum padding
+	rtw_secmicappendbyte(pmicdata, 0x5a );
+	rtw_secmicappendbyte(pmicdata, 0 );
+	rtw_secmicappendbyte(pmicdata, 0 );
+	rtw_secmicappendbyte(pmicdata, 0 );
+	rtw_secmicappendbyte(pmicdata, 0 );
+	// and then zeroes until the length is a multiple of 4
+	while( pmicdata->nBytesInM != 0 )
+	{
+		rtw_secmicappendbyte(pmicdata, 0 );
+	}
+	// The appendByte function has already computed the result.
+	secmicputuint32( dst, pmicdata->L );
+	secmicputuint32( dst+4, pmicdata->R );
+	// Reset to the empty message.
+	secmicclear(pmicdata);
+_func_exit_;		
+}
+
+
+void rtw_seccalctkipmic(u8 * key,u8 *header,u8 *data,u32 data_len,u8 *mic_code, u8 pri)
+{
+
+	struct mic_data	micdata;
+	u8 priority[4]={0x0,0x0,0x0,0x0};
+_func_enter_;		
+	rtw_secmicsetkey(&micdata, key);
+	priority[0]=pri;
+	
+	/* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */
+	if(header[1]&1){   //ToDS==1
+			rtw_secmicappend(&micdata, &header[16], 6);  //DA
+		if(header[1]&2)  //From Ds==1
+			rtw_secmicappend(&micdata, &header[24], 6);
+		else
+			rtw_secmicappend(&micdata, &header[10], 6);		
+	}	
+	else{	//ToDS==0
+		rtw_secmicappend(&micdata, &header[4], 6);   //DA
+		if(header[1]&2)  //From Ds==1
+			rtw_secmicappend(&micdata, &header[16], 6);
+		else
+			rtw_secmicappend(&micdata, &header[10], 6);
+
+	}
+	rtw_secmicappend(&micdata, &priority[0], 4);
+
+	
+	rtw_secmicappend(&micdata, data, data_len);
+
+	rtw_secgetmic(&micdata,mic_code);
+_func_exit_;	
+}
+
+
+
+
+/* macros for extraction/creation of unsigned char/unsigned short values  */
+#define RotR1(v16)   ((((v16) >> 1) & 0x7FFF) ^ (((v16) & 1) << 15))
+#define   Lo8(v16)   ((u8)( (v16)       & 0x00FF))
+#define   Hi8(v16)   ((u8)(((v16) >> 8) & 0x00FF))
+#define  Lo16(v32)   ((u16)( (v32)       & 0xFFFF))
+#define  Hi16(v32)   ((u16)(((v32) >>16) & 0xFFFF))
+#define  Mk16(hi,lo) ((lo) ^ (((u16)(hi)) << 8))
+
+/* select the Nth 16-bit word of the temporal key unsigned char array TK[]   */
+#define  TK16(N)     Mk16(tk[2*(N)+1],tk[2*(N)])
+
+/* S-box lookup: 16 bits --> 16 bits */
+#define _S_(v16)     (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)])
+
+/* fixed algorithm "parameters" */
+#define PHASE1_LOOP_CNT   8    /* this needs to be "big enough"     */
+#define TA_SIZE           6    /*  48-bit transmitter address       */
+#define TK_SIZE          16    /* 128-bit temporal key              */
+#define P1K_SIZE         10    /*  80-bit Phase1 key                */
+#define RC4_KEY_SIZE     16    /* 128-bit RC4KEY (104 bits unknown) */
+
+
+/* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */
+static const unsigned short Sbox1[2][256]=       /* Sbox for hash (can be in ROM)     */
+{ {
+   0xC6A5,0xF884,0xEE99,0xF68D,0xFF0D,0xD6BD,0xDEB1,0x9154,
+   0x6050,0x0203,0xCEA9,0x567D,0xE719,0xB562,0x4DE6,0xEC9A,
+   0x8F45,0x1F9D,0x8940,0xFA87,0xEF15,0xB2EB,0x8EC9,0xFB0B,
+   0x41EC,0xB367,0x5FFD,0x45EA,0x23BF,0x53F7,0xE496,0x9B5B,
+   0x75C2,0xE11C,0x3DAE,0x4C6A,0x6C5A,0x7E41,0xF502,0x834F,
+   0x685C,0x51F4,0xD134,0xF908,0xE293,0xAB73,0x6253,0x2A3F,
+   0x080C,0x9552,0x4665,0x9D5E,0x3028,0x37A1,0x0A0F,0x2FB5,
+   0x0E09,0x2436,0x1B9B,0xDF3D,0xCD26,0x4E69,0x7FCD,0xEA9F,
+   0x121B,0x1D9E,0x5874,0x342E,0x362D,0xDCB2,0xB4EE,0x5BFB,
+   0xA4F6,0x764D,0xB761,0x7DCE,0x527B,0xDD3E,0x5E71,0x1397,
+   0xA6F5,0xB968,0x0000,0xC12C,0x4060,0xE31F,0x79C8,0xB6ED,
+   0xD4BE,0x8D46,0x67D9,0x724B,0x94DE,0x98D4,0xB0E8,0x854A,
+   0xBB6B,0xC52A,0x4FE5,0xED16,0x86C5,0x9AD7,0x6655,0x1194,
+   0x8ACF,0xE910,0x0406,0xFE81,0xA0F0,0x7844,0x25BA,0x4BE3,
+   0xA2F3,0x5DFE,0x80C0,0x058A,0x3FAD,0x21BC,0x7048,0xF104,
+   0x63DF,0x77C1,0xAF75,0x4263,0x2030,0xE51A,0xFD0E,0xBF6D,
+   0x814C,0x1814,0x2635,0xC32F,0xBEE1,0x35A2,0x88CC,0x2E39,
+   0x9357,0x55F2,0xFC82,0x7A47,0xC8AC,0xBAE7,0x322B,0xE695,
+   0xC0A0,0x1998,0x9ED1,0xA37F,0x4466,0x547E,0x3BAB,0x0B83,
+   0x8CCA,0xC729,0x6BD3,0x283C,0xA779,0xBCE2,0x161D,0xAD76,
+   0xDB3B,0x6456,0x744E,0x141E,0x92DB,0x0C0A,0x486C,0xB8E4,
+   0x9F5D,0xBD6E,0x43EF,0xC4A6,0x39A8,0x31A4,0xD337,0xF28B,
+   0xD532,0x8B43,0x6E59,0xDAB7,0x018C,0xB164,0x9CD2,0x49E0,
+   0xD8B4,0xACFA,0xF307,0xCF25,0xCAAF,0xF48E,0x47E9,0x1018,
+   0x6FD5,0xF088,0x4A6F,0x5C72,0x3824,0x57F1,0x73C7,0x9751,
+   0xCB23,0xA17C,0xE89C,0x3E21,0x96DD,0x61DC,0x0D86,0x0F85,
+   0xE090,0x7C42,0x71C4,0xCCAA,0x90D8,0x0605,0xF701,0x1C12,
+   0xC2A3,0x6A5F,0xAEF9,0x69D0,0x1791,0x9958,0x3A27,0x27B9,
+   0xD938,0xEB13,0x2BB3,0x2233,0xD2BB,0xA970,0x0789,0x33A7,
+   0x2DB6,0x3C22,0x1592,0xC920,0x8749,0xAAFF,0x5078,0xA57A,
+   0x038F,0x59F8,0x0980,0x1A17,0x65DA,0xD731,0x84C6,0xD0B8,
+   0x82C3,0x29B0,0x5A77,0x1E11,0x7BCB,0xA8FC,0x6DD6,0x2C3A,
+  },
+ 
+
+  {  /* second half of table is unsigned char-reversed version of first! */
+   0xA5C6,0x84F8,0x99EE,0x8DF6,0x0DFF,0xBDD6,0xB1DE,0x5491,
+   0x5060,0x0302,0xA9CE,0x7D56,0x19E7,0x62B5,0xE64D,0x9AEC,
+   0x458F,0x9D1F,0x4089,0x87FA,0x15EF,0xEBB2,0xC98E,0x0BFB,
+   0xEC41,0x67B3,0xFD5F,0xEA45,0xBF23,0xF753,0x96E4,0x5B9B,
+   0xC275,0x1CE1,0xAE3D,0x6A4C,0x5A6C,0x417E,0x02F5,0x4F83,
+   0x5C68,0xF451,0x34D1,0x08F9,0x93E2,0x73AB,0x5362,0x3F2A,
+   0x0C08,0x5295,0x6546,0x5E9D,0x2830,0xA137,0x0F0A,0xB52F,
+   0x090E,0x3624,0x9B1B,0x3DDF,0x26CD,0x694E,0xCD7F,0x9FEA,
+   0x1B12,0x9E1D,0x7458,0x2E34,0x2D36,0xB2DC,0xEEB4,0xFB5B,
+   0xF6A4,0x4D76,0x61B7,0xCE7D,0x7B52,0x3EDD,0x715E,0x9713,
+   0xF5A6,0x68B9,0x0000,0x2CC1,0x6040,0x1FE3,0xC879,0xEDB6,
+   0xBED4,0x468D,0xD967,0x4B72,0xDE94,0xD498,0xE8B0,0x4A85,
+   0x6BBB,0x2AC5,0xE54F,0x16ED,0xC586,0xD79A,0x5566,0x9411,
+   0xCF8A,0x10E9,0x0604,0x81FE,0xF0A0,0x4478,0xBA25,0xE34B,
+   0xF3A2,0xFE5D,0xC080,0x8A05,0xAD3F,0xBC21,0x4870,0x04F1,
+   0xDF63,0xC177,0x75AF,0x6342,0x3020,0x1AE5,0x0EFD,0x6DBF,
+   0x4C81,0x1418,0x3526,0x2FC3,0xE1BE,0xA235,0xCC88,0x392E,
+   0x5793,0xF255,0x82FC,0x477A,0xACC8,0xE7BA,0x2B32,0x95E6,
+   0xA0C0,0x9819,0xD19E,0x7FA3,0x6644,0x7E54,0xAB3B,0x830B,
+   0xCA8C,0x29C7,0xD36B,0x3C28,0x79A7,0xE2BC,0x1D16,0x76AD,
+   0x3BDB,0x5664,0x4E74,0x1E14,0xDB92,0x0A0C,0x6C48,0xE4B8,
+   0x5D9F,0x6EBD,0xEF43,0xA6C4,0xA839,0xA431,0x37D3,0x8BF2,
+   0x32D5,0x438B,0x596E,0xB7DA,0x8C01,0x64B1,0xD29C,0xE049,
+   0xB4D8,0xFAAC,0x07F3,0x25CF,0xAFCA,0x8EF4,0xE947,0x1810,
+   0xD56F,0x88F0,0x6F4A,0x725C,0x2438,0xF157,0xC773,0x5197,
+   0x23CB,0x7CA1,0x9CE8,0x213E,0xDD96,0xDC61,0x860D,0x850F,
+   0x90E0,0x427C,0xC471,0xAACC,0xD890,0x0506,0x01F7,0x121C,
+   0xA3C2,0x5F6A,0xF9AE,0xD069,0x9117,0x5899,0x273A,0xB927,
+   0x38D9,0x13EB,0xB32B,0x3322,0xBBD2,0x70A9,0x8907,0xA733,
+   0xB62D,0x223C,0x9215,0x20C9,0x4987,0xFFAA,0x7850,0x7AA5,
+   0x8F03,0xF859,0x8009,0x171A,0xDA65,0x31D7,0xC684,0xB8D0,
+   0xC382,0xB029,0x775A,0x111E,0xCB7B,0xFCA8,0xD66D,0x3A2C,
+  }
+};
+ 
+ /*
+**********************************************************************
+* Routine: Phase 1 -- generate P1K, given TA, TK, IV32
+*
+* Inputs:
+*     tk[]      = temporal key                         [128 bits]
+*     ta[]      = transmitter's MAC address            [ 48 bits]
+*     iv32      = upper 32 bits of IV                  [ 32 bits]
+* Output:
+*     p1k[]     = Phase 1 key                          [ 80 bits]
+*
+* Note:
+*     This function only needs to be called every 2**16 packets,
+*     although in theory it could be called every packet.
+*
+**********************************************************************
+*/
+static void phase1(u16 *p1k,const u8 *tk,const u8 *ta,u32 iv32)
+{
+	sint  i;
+_func_enter_;
+	/* Initialize the 80 bits of P1K[] from IV32 and TA[0..5]     */
+	p1k[0]      = Lo16(iv32);
+	p1k[1]      = Hi16(iv32);
+	p1k[2]      = Mk16(ta[1],ta[0]); /* use TA[] as little-endian */
+	p1k[3]      = Mk16(ta[3],ta[2]);
+	p1k[4]      = Mk16(ta[5],ta[4]);
+
+	/* Now compute an unbalanced Feistel cipher with 80-bit block */
+	/* size on the 80-bit block P1K[], using the 128-bit key TK[] */
+	for (i=0; i < PHASE1_LOOP_CNT ;i++)
+	{                 /* Each add operation here is mod 2**16 */
+      		p1k[0] += _S_(p1k[4] ^ TK16((i&1)+0));
+		p1k[1] += _S_(p1k[0] ^ TK16((i&1)+2));
+		p1k[2] += _S_(p1k[1] ^ TK16((i&1)+4));
+		p1k[3] += _S_(p1k[2] ^ TK16((i&1)+6));
+		p1k[4] += _S_(p1k[3] ^ TK16((i&1)+0));
+		p1k[4] +=  (unsigned short)i;                    /* avoid "slide attacks" */
+        }
+_func_exit_;
+}
+ 
+
+/*
+**********************************************************************
+* Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16
+*
+* Inputs:
+*     tk[]      = Temporal key                         [128 bits]
+*     p1k[]     = Phase 1 output key                   [ 80 bits]
+*     iv16      = low 16 bits of IV counter            [ 16 bits]
+* Output:
+*     rc4key[]  = the key used to encrypt the packet   [128 bits]
+*
+* Note:
+*     The value {TA,IV32,IV16} for Phase1/Phase2 must be unique
+*     across all packets using the same key TK value. Then, for a
+*     given value of TK[], this TKIP48 construction guarantees that
+*     the final RC4KEY value is unique across all packets.
+*
+* Suggested implementation optimization: if PPK[] is "overlaid"
+*     appropriately on RC4KEY[], there is no need for the final
+*     for loop below that copies the PPK[] result into RC4KEY[].
+*
+**********************************************************************
+*/
+static void phase2(u8 *rc4key,const u8 *tk,const u16 *p1k,u16 iv16)
+{
+	sint  i;
+	u16 PPK[6];                          /* temporary key for mixing    */
+_func_enter_;
+	/* Note: all adds in the PPK[] equations below are mod 2**16         */
+	for (i=0;i<5;i++) PPK[i]=p1k[i];      /* first, copy P1K to PPK      */
+		PPK[5]  =  p1k[4] +iv16;             /* next,  add in IV16          */
+
+	/* Bijective non-linear mixing of the 96 bits of PPK[0..5]           */
+	PPK[0] +=    _S_(PPK[5] ^ TK16(0));   /* Mix key in each "round"     */
+	PPK[1] +=    _S_(PPK[0] ^ TK16(1));
+	PPK[2] +=    _S_(PPK[1] ^ TK16(2));
+	PPK[3] +=    _S_(PPK[2] ^ TK16(3));
+	PPK[4] +=    _S_(PPK[3] ^ TK16(4));
+	PPK[5] +=    _S_(PPK[4] ^ TK16(5));   /* Total # S-box lookups == 6  */
+
+	/* Final sweep: bijective, "linear". Rotates kill LSB correlations   */
+	PPK[0] +=  RotR1(PPK[5] ^ TK16(6));
+	PPK[1] +=  RotR1(PPK[0] ^ TK16(7));   /* Use all of TK[] in Phase2   */
+	PPK[2] +=  RotR1(PPK[1]);
+	PPK[3] +=  RotR1(PPK[2]);
+	PPK[4] +=  RotR1(PPK[3]);
+	PPK[5] +=  RotR1(PPK[4]);
+	/* Note: At this point, for a given key TK[0..15], the 96-bit output */
+	/*       value PPK[0..5] is guaranteed to be unique, as a function   */
+	/*       of the 96-bit "input" value   {TA,IV32,IV16}. That is, P1K  */
+	/*       is now a keyed permutation of {TA,IV32,IV16}.               */
+
+	/* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key   */
+	rc4key[0] = Hi8(iv16);                /* RC4KEY[0..2] is the WEP IV  */
+	rc4key[1] =(Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys  */
+	rc4key[2] = Lo8(iv16);
+	rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1);
+	 
+
+	/* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian)       */
+	for (i=0;i<6;i++)
+	{
+		rc4key[4+2*i] = Lo8(PPK[i]);
+		rc4key[5+2*i] = Hi8(PPK[i]);
+	}
+_func_exit_;	
+}
+
+
+//The hlen isn't include the IV
+u32	rtw_tkip_encrypt(_adapter *padapter, u8 *pxmitframe)
+{																	// exclude ICV
+	u16	pnl;
+	u32	pnh;
+	u8	rc4key[16];
+	u8   ttkey[16];
+	u8	crc[4];
+	u8   hw_hdr_offset = 0;
+	struct arc4context mycontext;
+	sint 			curfragnum,length;
+	u32	prwskeylen;
+
+	u8	*pframe, *payload,*iv,*prwskey;
+	union pn48 dot11txpn;
+	struct	sta_info		*stainfo;
+	struct	pkt_attrib	 *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+	struct 	security_priv	*psecuritypriv=&padapter->securitypriv;
+	struct	xmit_priv		*pxmitpriv=&padapter->xmitpriv;
+	u32	res=_SUCCESS;
+_func_enter_;
+
+	if(((struct xmit_frame*)pxmitframe)->buf_addr==NULL)
+		return _FAIL;
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+	hw_hdr_offset = TXDESC_SIZE +
+		 (((struct xmit_frame*)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);	
+#else
+	#ifdef CONFIG_TX_EARLY_MODE
+	hw_hdr_offset = TXDESC_OFFSET+EARLY_MODE_INFO_SIZE;
+	#else
+	hw_hdr_offset = TXDESC_OFFSET;
+	#endif
+#endif
+
+	pframe = ((struct xmit_frame*)pxmitframe)->buf_addr + hw_hdr_offset;
+	//4 start to encrypt each fragment
+	if(pattrib->encrypt==_TKIP_){
+
+		if(pattrib->psta)
+		{
+			stainfo = pattrib->psta;
+		}
+		else
+		{
+			DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+			stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] );
+		}	
+		
+		if (stainfo!=NULL){
+
+			if(!(stainfo->state &_FW_LINKED))
+			{
+				DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
+				return _FAIL;
+			}
+			
+			RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_tkip_encrypt: stainfo!=NULL!!!\n"));
+
+			if(IS_MCAST(pattrib->ra))
+			{
+				prwskey=psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+			}
+			else
+			{
+				prwskey=&stainfo->dot118021x_UncstKey.skey[0];
+			}
+
+			prwskeylen=16;
+
+			for(curfragnum=0;curfragnum<pattrib->nr_frags;curfragnum++){
+				iv=pframe+pattrib->hdrlen;
+				payload=pframe+pattrib->iv_len+pattrib->hdrlen;
+				
+				GET_TKIP_PN(iv, dot11txpn);
+
+				pnl=(u16)(dot11txpn.val);
+				pnh=(u32)(dot11txpn.val>>16);
+
+				phase1((u16 *)&ttkey[0],prwskey,&pattrib->ta[0],pnh);
+
+				phase2(&rc4key[0],prwskey,(u16 *)&ttkey[0],pnl);	
+
+				if((curfragnum+1)==pattrib->nr_frags){	//4 the last fragment
+					length=pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len- pattrib->icv_len;
+					RT_TRACE(_module_rtl871x_security_c_,_drv_info_,("pattrib->iv_len =%x, pattrib->icv_len =%x\n", pattrib->iv_len,pattrib->icv_len));
+					*((u32 *)crc)=cpu_to_le32(getcrc32(payload,length));/* modified by Amy*/
+
+					arcfour_init(&mycontext, rc4key,16);
+					arcfour_encrypt(&mycontext, payload, payload, length);
+					arcfour_encrypt(&mycontext, payload+length, crc, 4);
+
+				}
+				else{
+					length=pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
+					*((u32 *)crc)=cpu_to_le32(getcrc32(payload,length));/* modified by Amy*/
+					arcfour_init(&mycontext,rc4key,16);
+					arcfour_encrypt(&mycontext, payload, payload, length);
+					arcfour_encrypt(&mycontext, payload+length, crc, 4);
+	
+				pframe+=pxmitpriv->frag_len;
+				pframe=(u8 *)RND4((SIZE_PTR)(pframe));
+
+				}
+			}
+
+
+		}
+		else{
+			RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_tkip_encrypt: stainfo==NULL!!!\n"));
+                        DBG_871X("%s, psta==NUL\n", __func__);
+			res=_FAIL;
+		}
+						
+	}
+_func_exit_;	
+	return res;
+				
+}
+
+
+//The hlen isn't include the IV
+u32 rtw_tkip_decrypt(_adapter *padapter, u8 *precvframe)
+{																	// exclude ICV
+	u16 pnl;
+	u32 pnh;
+	u8   rc4key[16];
+	u8   ttkey[16];
+	u8	crc[4];
+	struct arc4context mycontext;
+	sint 			length;
+	u32	prwskeylen;
+
+	u8	*pframe, *payload,*iv,*prwskey;
+	union pn48 dot11txpn;
+	struct	sta_info		*stainfo;
+	struct	rx_pkt_attrib	 *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
+	struct 	security_priv	*psecuritypriv=&padapter->securitypriv;
+//	struct	recv_priv		*precvpriv=&padapter->recvpriv;
+	u32		res=_SUCCESS;
+
+_func_enter_;
+
+	pframe=(unsigned char *)((union recv_frame*)precvframe)->u.hdr.rx_data;
+	
+	//4 start to decrypt recvframe
+	if(prxattrib->encrypt==_TKIP_){
+
+		stainfo=rtw_get_stainfo(&padapter->stapriv ,&prxattrib->ta[0] );
+		if (stainfo!=NULL){
+
+			if(IS_MCAST(prxattrib->ra))
+			{
+				if(psecuritypriv->binstallGrpkey==_FALSE)
+				{
+					res=_FAIL;				
+					DBG_8192C("%s:rx bc/mc packets,but didn't install group key!!!!!!!!!!\n",__FUNCTION__);
+					goto exit;
+				}
+				//DBG_871X("rx bc/mc packets, to perform sw rtw_tkip_decrypt\n");
+				//prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+				prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+				prwskeylen=16;
+			}
+			else
+			{
+			        RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_tkip_decrypt: stainfo!=NULL!!!\n"));
+				prwskey=&stainfo->dot118021x_UncstKey.skey[0];
+				prwskeylen=16;
+			}
+			
+			iv=pframe+prxattrib->hdrlen;
+			payload=pframe+prxattrib->iv_len+prxattrib->hdrlen;
+			length= ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
+				
+			GET_TKIP_PN(iv, dot11txpn);
+
+			pnl=(u16)(dot11txpn.val);
+			pnh=(u32)(dot11txpn.val>>16);
+
+			phase1((u16 *)&ttkey[0],prwskey,&prxattrib->ta[0],pnh);
+			phase2(&rc4key[0],prwskey,(unsigned short *)&ttkey[0],pnl);	
+
+			//4 decrypt payload include icv
+					
+			arcfour_init(&mycontext, rc4key,16);
+			arcfour_encrypt(&mycontext, payload, payload, length);
+
+			*((u32 *)crc)=le32_to_cpu(getcrc32(payload,length-4));
+
+			if(crc[3]!=payload[length-1] || crc[2]!=payload[length-2] || crc[1]!=payload[length-3] || crc[0]!=payload[length-4])
+			{
+			    RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_wep_decrypt:icv error crc[3](%x)!=payload[length-1](%x) || crc[2](%x)!=payload[length-2](%x) || crc[1](%x)!=payload[length-3](%x) || crc[0](%x)!=payload[length-4](%x)\n",
+						crc[3],payload[length-1],crc[2],payload[length-2],crc[1],payload[length-3],crc[0],payload[length-4]));
+				res=_FAIL;
+			}
+						
+		
+		}
+		else{
+			RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_tkip_decrypt: stainfo==NULL!!!\n"));
+			res=_FAIL;
+		}
+						
+	}
+_func_exit_;	
+exit:
+	return res;
+				
+}
+
+
+//3			=====AES related=====
+
+
+
+#define MAX_MSG_SIZE	2048
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+  static  u8 sbox_table[256] =
+    {
+        0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
+        0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
+        0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
+        0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
+        0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
+        0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
+        0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
+        0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
+        0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
+        0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
+        0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
+        0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
+        0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
+        0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
+        0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
+        0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
+        0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
+        0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
+        0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
+        0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
+        0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
+        0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
+        0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
+        0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
+        0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
+        0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
+        0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
+        0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
+        0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
+        0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
+        0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
+        0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
+    };
+
+/*****************************/
+/**** Function Prototypes ****/
+/*****************************/
+
+static void bitwise_xor(u8 *ina, u8 *inb, u8 *out);
+static void construct_mic_iv(
+                        u8 *mic_header1,
+                        sint qc_exists,
+                        sint a4_exists,
+                        u8 *mpdu,
+                        uint payload_length,
+                        u8 * pn_vector);
+static void construct_mic_header1(
+                        u8 *mic_header1,
+                        sint header_length,
+                        u8 *mpdu);
+static void construct_mic_header2(
+                    u8 *mic_header2,
+                    u8 *mpdu,
+                    sint a4_exists,
+                    sint qc_exists);
+static void construct_ctr_preload(
+                        u8 *ctr_preload,
+                        sint a4_exists,
+                        sint qc_exists,
+                        u8 *mpdu,
+                        u8 *pn_vector,
+                        sint c);
+static void xor_128(u8 *a, u8 *b, u8 *out);
+static void xor_32(u8 *a, u8 *b, u8 *out);
+static u8 sbox(u8 a);
+static void next_key(u8 *key, sint round);
+static void byte_sub(u8 *in, u8 *out);
+static void shift_row(u8 *in, u8 *out);
+static void mix_column(u8 *in, u8 *out);
+#ifndef PLATFORM_FREEBSD 
+static void add_round_key( u8 *shiftrow_in,
+                    u8 *mcol_in,
+                    u8 *block_in,
+                    sint round,
+                    u8 *out);
+#endif //PLATFORM_FREEBSD
+static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext);
+
+
+/****************************************/
+/* aes128k128d()                        */
+/* Performs a 128 bit AES encrypt with  */
+/* 128 bit data.                        */
+/****************************************/
+static void xor_128(u8 *a, u8 *b, u8 *out)
+{
+    sint i;
+_func_enter_;	
+    for (i=0;i<16; i++)
+    {
+        out[i] = a[i] ^ b[i];
+    }
+_func_exit_;		
+}
+
+
+static void xor_32(u8 *a, u8 *b, u8 *out)
+{
+    sint i;
+_func_enter_;		
+    for (i=0;i<4; i++)
+    {
+        out[i] = a[i] ^ b[i];
+    }
+_func_exit_;		
+}
+
+
+static u8 sbox(u8 a)
+{
+    return sbox_table[(sint)a];
+}
+
+
+static void next_key(u8 *key, sint round)
+{
+    u8 rcon;
+    u8 sbox_key[4];
+    u8 rcon_table[12] =
+    {
+        0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+        0x1b, 0x36, 0x36, 0x36
+    };
+_func_enter_;		
+    sbox_key[0] = sbox(key[13]);
+    sbox_key[1] = sbox(key[14]);
+    sbox_key[2] = sbox(key[15]);
+    sbox_key[3] = sbox(key[12]);
+
+    rcon = rcon_table[round];
+
+    xor_32(&key[0], sbox_key, &key[0]);
+    key[0] = key[0] ^ rcon;
+
+    xor_32(&key[4], &key[0], &key[4]);
+    xor_32(&key[8], &key[4], &key[8]);
+    xor_32(&key[12], &key[8], &key[12]);
+_func_exit_;		
+}
+
+
+static void byte_sub(u8 *in, u8 *out)
+{
+    sint i;
+_func_enter_;			
+    for (i=0; i< 16; i++)
+    {
+        out[i] = sbox(in[i]);
+    }
+_func_exit_;	
+}
+
+
+static void shift_row(u8 *in, u8 *out)
+{
+_func_enter_;	
+    out[0] =  in[0];
+    out[1] =  in[5];
+    out[2] =  in[10];
+    out[3] =  in[15];
+    out[4] =  in[4];
+    out[5] =  in[9];
+    out[6] =  in[14];
+    out[7] =  in[3];
+    out[8] =  in[8];
+    out[9] =  in[13];
+    out[10] = in[2];
+    out[11] = in[7];
+    out[12] = in[12];
+    out[13] = in[1];
+    out[14] = in[6];
+    out[15] = in[11];
+_func_exit_;		
+}
+
+
+static void mix_column(u8 *in, u8 *out)
+{
+    sint i;
+    u8 add1b[4];
+    u8 add1bf7[4];
+    u8 rotl[4];
+    u8 swap_halfs[4];
+    u8 andf7[4];
+    u8 rotr[4];
+    u8 temp[4];
+    u8 tempb[4];
+_func_enter_;	
+    for (i=0 ; i<4; i++)
+    {
+        if ((in[i] & 0x80)== 0x80)
+            add1b[i] = 0x1b;
+        else
+            add1b[i] = 0x00;
+    }
+
+    swap_halfs[0] = in[2];    /* Swap halfs */
+    swap_halfs[1] = in[3];
+    swap_halfs[2] = in[0];
+    swap_halfs[3] = in[1];
+
+    rotl[0] = in[3];        /* Rotate left 8 bits */
+    rotl[1] = in[0];
+    rotl[2] = in[1];
+    rotl[3] = in[2];
+
+    andf7[0] = in[0] & 0x7f;
+    andf7[1] = in[1] & 0x7f;
+    andf7[2] = in[2] & 0x7f;
+    andf7[3] = in[3] & 0x7f;
+
+    for (i = 3; i>0; i--)    /* logical shift left 1 bit */
+    {
+        andf7[i] = andf7[i] << 1;
+        if ((andf7[i-1] & 0x80) == 0x80)
+        {
+            andf7[i] = (andf7[i] | 0x01);
+        }
+    }
+    andf7[0] = andf7[0] << 1;
+    andf7[0] = andf7[0] & 0xfe;
+
+    xor_32(add1b, andf7, add1bf7);
+
+    xor_32(in, add1bf7, rotr);
+
+    temp[0] = rotr[0];         /* Rotate right 8 bits */
+    rotr[0] = rotr[1];
+    rotr[1] = rotr[2];
+    rotr[2] = rotr[3];
+    rotr[3] = temp[0];
+
+    xor_32(add1bf7, rotr, temp);
+    xor_32(swap_halfs, rotl,tempb);
+    xor_32(temp, tempb, out);
+_func_exit_;		
+}
+
+
+static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext)
+{
+    sint round;
+    sint i;
+    u8 intermediatea[16];
+    u8 intermediateb[16];
+    u8 round_key[16];
+_func_enter_;	
+    for(i=0; i<16; i++) round_key[i] = key[i];
+
+    for (round = 0; round < 11; round++)
+    {
+        if (round == 0)
+        {
+            xor_128(round_key, data, ciphertext);
+            next_key(round_key, round);
+        }
+        else if (round == 10)
+        {
+            byte_sub(ciphertext, intermediatea);
+            shift_row(intermediatea, intermediateb);
+            xor_128(intermediateb, round_key, ciphertext);
+        }
+        else    /* 1 - 9 */
+        {
+            byte_sub(ciphertext, intermediatea);
+            shift_row(intermediatea, intermediateb);
+            mix_column(&intermediateb[0], &intermediatea[0]);
+            mix_column(&intermediateb[4], &intermediatea[4]);
+            mix_column(&intermediateb[8], &intermediatea[8]);
+            mix_column(&intermediateb[12], &intermediatea[12]);
+            xor_128(intermediatea, round_key, ciphertext);
+            next_key(round_key, round);
+        }
+    }
+_func_exit_;	
+}
+
+
+/************************************************/
+/* construct_mic_iv()                           */
+/* Builds the MIC IV from header fields and PN  */
+/************************************************/
+static void construct_mic_iv(
+                        u8 *mic_iv,
+                        sint qc_exists,
+                        sint a4_exists,
+                        u8 *mpdu,
+                        uint payload_length,
+                        u8 *pn_vector
+                        )
+{
+    sint i;
+_func_enter_;	
+    mic_iv[0] = 0x59;
+    if (qc_exists && a4_exists) mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
+    if (qc_exists && !a4_exists) mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
+    if (!qc_exists) mic_iv[1] = 0x00;
+    for (i = 2; i < 8; i++)
+        mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
+    #ifdef CONSISTENT_PN_ORDER
+        for (i = 8; i < 14; i++)
+            mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
+    #else
+        for (i = 8; i < 14; i++)
+            mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
+    #endif
+    mic_iv[14] = (unsigned char) (payload_length / 256);
+    mic_iv[15] = (unsigned char) (payload_length % 256);
+_func_exit_;		
+}
+
+
+/************************************************/
+/* construct_mic_header1()                      */
+/* Builds the first MIC header block from       */
+/* header fields.                               */
+/************************************************/
+static void construct_mic_header1(
+                        u8 *mic_header1,
+                        sint header_length,
+                        u8 *mpdu
+                        )
+{
+_func_enter_;	
+    mic_header1[0] = (u8)((header_length - 2) / 256);
+    mic_header1[1] = (u8)((header_length - 2) % 256);
+    mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
+    mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
+    mic_header1[4] = mpdu[4];       /* A1 */
+    mic_header1[5] = mpdu[5];
+    mic_header1[6] = mpdu[6];
+    mic_header1[7] = mpdu[7];
+    mic_header1[8] = mpdu[8];
+    mic_header1[9] = mpdu[9];
+    mic_header1[10] = mpdu[10];     /* A2 */
+    mic_header1[11] = mpdu[11];
+    mic_header1[12] = mpdu[12];
+    mic_header1[13] = mpdu[13];
+    mic_header1[14] = mpdu[14];
+    mic_header1[15] = mpdu[15];
+_func_exit_;	
+}
+
+
+/************************************************/
+/* construct_mic_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/************************************************/
+static void construct_mic_header2(
+                u8 *mic_header2,
+                u8 *mpdu,
+                sint a4_exists,
+                sint qc_exists
+                )
+{
+    sint i;
+_func_enter_;	
+    for (i = 0; i<16; i++) mic_header2[i]=0x00;
+
+    mic_header2[0] = mpdu[16];    /* A3 */
+    mic_header2[1] = mpdu[17];
+    mic_header2[2] = mpdu[18];
+    mic_header2[3] = mpdu[19];
+    mic_header2[4] = mpdu[20];
+    mic_header2[5] = mpdu[21];
+
+    //mic_header2[6] = mpdu[22] & 0xf0;   /* SC */
+    mic_header2[6] = 0x00;
+    mic_header2[7] = 0x00; /* mpdu[23]; */
+
+
+    if (!qc_exists && a4_exists)
+    {
+        for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+    }
+
+    if (qc_exists && !a4_exists)
+    {
+        mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+        mic_header2[9] = mpdu[25] & 0x00;
+    }
+
+    if (qc_exists && a4_exists)
+    {
+        for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+        mic_header2[14] = mpdu[30] & 0x0f;
+        mic_header2[15] = mpdu[31] & 0x00;
+    }
+
+_func_exit_;	
+}
+
+
+/************************************************/
+/* construct_mic_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/************************************************/
+static void construct_ctr_preload(
+                        u8 *ctr_preload,
+                        sint a4_exists,
+                        sint qc_exists,
+                        u8 *mpdu,
+                        u8 *pn_vector,
+                        sint c
+                        )
+{
+    sint i = 0;
+_func_enter_;		
+    for (i=0; i<16; i++) ctr_preload[i] = 0x00;
+    i = 0;
+
+    ctr_preload[0] = 0x01;                                  /* flag */
+    if (qc_exists && a4_exists) 
+		ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control */
+    if (qc_exists && !a4_exists) 
+		ctr_preload[1] = mpdu[24] & 0x0f;
+
+    for (i = 2; i < 8; i++)
+        ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+    #ifdef CONSISTENT_PN_ORDER
+      for (i = 8; i < 14; i++)
+            ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
+    #else
+      for (i = 8; i < 14; i++)
+            ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
+    #endif
+    ctr_preload[14] =  (unsigned char) (c / 256); /* Ctr */
+    ctr_preload[15] =  (unsigned char) (c % 256);
+_func_exit_;		
+}
+
+
+/************************************/
+/* bitwise_xor()                    */
+/* A 128 bit, bitwise exclusive or  */
+/************************************/
+static void bitwise_xor(u8 *ina, u8 *inb, u8 *out)
+{
+    sint i;
+_func_enter_;		
+    for (i=0; i<16; i++)
+    {
+        out[i] = ina[i] ^ inb[i];
+    }
+_func_exit_;		
+}
+
+
+static sint aes_cipher(u8 *key, uint	hdrlen,
+			u8 *pframe, uint plen)
+{
+//	/*static*/ unsigned char	message[MAX_MSG_SIZE];
+	uint	qc_exists, a4_exists, i, j, payload_remainder,
+		num_blocks, payload_index;
+
+	u8 pn_vector[6];
+	u8 mic_iv[16];
+	u8 mic_header1[16];
+	u8 mic_header2[16];
+	u8 ctr_preload[16];
+
+	/* Intermediate Buffers */
+	u8 chain_buffer[16];
+	u8 aes_out[16];
+	u8 padded_buffer[16];
+	u8 mic[8];
+//	uint	offset = 0;
+	uint	frtype  = GetFrameType(pframe);
+	uint	frsubtype  = GetFrameSubType(pframe);
+	
+_func_enter_;		
+	frsubtype=frsubtype>>4;
+
+
+	_rtw_memset((void *)mic_iv, 0, 16);
+	_rtw_memset((void *)mic_header1, 0, 16);
+	_rtw_memset((void *)mic_header2, 0, 16);
+	_rtw_memset((void *)ctr_preload, 0, 16);
+	_rtw_memset((void *)chain_buffer, 0, 16);
+	_rtw_memset((void *)aes_out, 0, 16);
+	_rtw_memset((void *)padded_buffer, 0, 16);
+
+	if ((hdrlen == WLAN_HDR_A3_LEN )||(hdrlen ==  WLAN_HDR_A3_QOS_LEN))
+		a4_exists = 0;
+	else
+		a4_exists = 1;
+
+	if (
+		(frtype == WIFI_DATA_CFACK) ||
+		(frtype == WIFI_DATA_CFPOLL)||
+		(frtype == WIFI_DATA_CFACKPOLL))
+		{
+			qc_exists = 1;
+					if(hdrlen !=  WLAN_HDR_A3_QOS_LEN){
+				 
+					hdrlen += 2;
+			}
+		}
+	else if (
+		(frsubtype == 0x08) ||
+		(frsubtype == 0x09)||
+		(frsubtype == 0x0a)||
+		(frsubtype == 0x0b))
+		{
+			if(hdrlen !=  WLAN_HDR_A3_QOS_LEN){
+				 
+					hdrlen += 2;
+			}
+			qc_exists = 1;
+		}
+	else
+		qc_exists = 0;
+
+	pn_vector[0]=pframe[hdrlen];
+	pn_vector[1]=pframe[hdrlen+1];
+	pn_vector[2]=pframe[hdrlen+4];
+	pn_vector[3]=pframe[hdrlen+5];
+	pn_vector[4]=pframe[hdrlen+6];
+	pn_vector[5]=pframe[hdrlen+7];
+	
+	construct_mic_iv(
+                        mic_iv,
+                        qc_exists,
+                        a4_exists,
+                        pframe,	 //message,
+                        plen,
+                        pn_vector
+                        );
+
+    construct_mic_header1(
+                            mic_header1,
+                            hdrlen,
+                            pframe	//message
+                            );
+    construct_mic_header2(
+                            mic_header2,
+                            pframe,	//message,
+                            a4_exists,
+                            qc_exists
+                            );
+
+
+	payload_remainder = plen % 16;
+    num_blocks = plen / 16;
+
+    /* Find start of payload */
+    payload_index = (hdrlen + 8);
+
+    /* Calculate MIC */
+    aes128k128d(key, mic_iv, aes_out);
+    bitwise_xor(aes_out, mic_header1, chain_buffer);
+    aes128k128d(key, chain_buffer, aes_out);
+    bitwise_xor(aes_out, mic_header2, chain_buffer);
+    aes128k128d(key, chain_buffer, aes_out);
+
+	for (i = 0; i < num_blocks; i++)
+    {
+        bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);//bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+
+        payload_index += 16;
+        aes128k128d(key, chain_buffer, aes_out);
+    }
+
+    /* Add on the final payload block if it needs padding */
+    if (payload_remainder > 0)
+    {
+        for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
+        for (j = 0; j < payload_remainder; j++)
+        {
+            padded_buffer[j] = pframe[payload_index++];//padded_buffer[j] = message[payload_index++];
+        }
+        bitwise_xor(aes_out, padded_buffer, chain_buffer);
+        aes128k128d(key, chain_buffer, aes_out);
+
+    }
+
+    for (j = 0 ; j < 8; j++) mic[j] = aes_out[j];
+
+    /* Insert MIC into payload */
+    for (j = 0; j < 8; j++)
+    	pframe[payload_index+j] = mic[j];	//message[payload_index+j] = mic[j];
+
+	payload_index = hdrlen + 8;
+	for (i=0; i< num_blocks; i++)
+    {
+        construct_ctr_preload(
+                                ctr_preload,
+                                a4_exists,
+                                qc_exists,
+                                pframe,	//message,
+                                pn_vector,
+                                i+1);
+        aes128k128d(key, ctr_preload, aes_out);
+        bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);//bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+        for (j=0; j<16;j++) pframe[payload_index++] = chain_buffer[j];//for (j=0; j<16;j++) message[payload_index++] = chain_buffer[j];
+    }
+
+    if (payload_remainder > 0)          /* If there is a short final block, then pad it,*/
+    {                                   /* encrypt it and copy the unpadded part back   */
+        construct_ctr_preload(
+                                ctr_preload,
+                                a4_exists,
+                                qc_exists,
+                                pframe,	//message,
+                                pn_vector,
+                                num_blocks+1);
+
+        for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
+        for (j = 0; j < payload_remainder; j++)
+        {
+            padded_buffer[j] = pframe[payload_index+j];//padded_buffer[j] = message[payload_index+j];
+        }
+        aes128k128d(key, ctr_preload, aes_out);
+        bitwise_xor(aes_out, padded_buffer, chain_buffer);
+        for (j=0; j<payload_remainder;j++) pframe[payload_index++] = chain_buffer[j];//for (j=0; j<payload_remainder;j++) message[payload_index++] = chain_buffer[j];
+    }
+
+    /* Encrypt the MIC */
+    construct_ctr_preload(
+                        ctr_preload,
+                        a4_exists,
+                        qc_exists,
+                        pframe,	//message,
+                        pn_vector,
+                        0);
+
+    for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
+    for (j = 0; j < 8; j++)
+    {
+        padded_buffer[j] = pframe[j+hdrlen+8+plen];//padded_buffer[j] = message[j+hdrlen+8+plen];
+    }
+
+    aes128k128d(key, ctr_preload, aes_out);
+    bitwise_xor(aes_out, padded_buffer, chain_buffer);
+    for (j=0; j<8;j++) pframe[payload_index++] = chain_buffer[j];//for (j=0; j<8;j++) message[payload_index++] = chain_buffer[j];
+_func_exit_;		
+	return _SUCCESS;
+}
+
+
+
+
+
+u32	rtw_aes_encrypt(_adapter *padapter, u8 *pxmitframe)
+{	// exclude ICV
+
+
+	/*static*/ 
+//	unsigned char	message[MAX_MSG_SIZE];
+
+    	/* Intermediate Buffers */
+	sint 	curfragnum,length;
+	u32	prwskeylen;
+	u8	*pframe,*prwskey;	//, *payload,*iv
+	u8   hw_hdr_offset = 0;
+	struct	sta_info		*stainfo;
+	struct	pkt_attrib	 *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+	struct 	security_priv	*psecuritypriv=&padapter->securitypriv;
+	struct	xmit_priv		*pxmitpriv=&padapter->xmitpriv;
+
+//	uint	offset = 0;
+	u32 res=_SUCCESS;
+_func_enter_;		
+
+	if(((struct xmit_frame*)pxmitframe)->buf_addr==NULL)
+		return _FAIL;
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+	hw_hdr_offset = TXDESC_SIZE +
+		 (((struct xmit_frame*)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
+#else
+	#ifdef CONFIG_TX_EARLY_MODE
+	hw_hdr_offset = TXDESC_OFFSET+EARLY_MODE_INFO_SIZE;
+	#else
+	hw_hdr_offset = TXDESC_OFFSET;
+	#endif	
+#endif
+
+	pframe = ((struct xmit_frame*)pxmitframe)->buf_addr + hw_hdr_offset;
+
+	//4 start to encrypt each fragment
+	if((pattrib->encrypt==_AES_)){
+
+		if(pattrib->psta)
+		{
+			stainfo = pattrib->psta;
+		}
+		else
+		{
+			DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+			stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] );
+		}	
+		
+		if (stainfo!=NULL){
+
+			if(!(stainfo->state &_FW_LINKED))
+			{
+				DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
+				return _FAIL;
+			}
+			
+			RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_aes_encrypt: stainfo!=NULL!!!\n"));
+
+			if(IS_MCAST(pattrib->ra))
+			{
+				prwskey=psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+			}
+			else
+			{
+				prwskey=&stainfo->dot118021x_UncstKey.skey[0];
+			}
+
+#ifdef CONFIG_TDLS	//swencryption
+			{
+				struct	sta_info		*ptdls_sta;
+				ptdls_sta=rtw_get_stainfo(&padapter->stapriv ,&pattrib->dst[0] );
+				if((ptdls_sta != NULL) && (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) )
+				{
+					DBG_871X("[%s] for tdls link\n", __FUNCTION__);
+					prwskey=&ptdls_sta->tpk.tk[0];
+				}
+			}
+#endif //CONFIG_TDLS
+
+			prwskeylen=16;
+	
+			for(curfragnum=0;curfragnum<pattrib->nr_frags;curfragnum++){
+			
+				if((curfragnum+1)==pattrib->nr_frags){	//4 the last fragment
+					length=pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len- pattrib->icv_len;
+				
+					aes_cipher(prwskey,pattrib->hdrlen,pframe, length);
+				}
+				else{
+					length=pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
+				
+					aes_cipher(prwskey,pattrib->hdrlen,pframe, length);
+				pframe+=pxmitpriv->frag_len;
+				pframe=(u8*)RND4((SIZE_PTR)(pframe));
+
+				}
+			}
+
+
+		}
+		else{
+			RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_aes_encrypt: stainfo==NULL!!!\n"));
+                        DBG_871X("%s, psta==NUL\n", __func__);
+			res=_FAIL;
+		}
+						
+	}
+
+
+
+_func_exit_;
+		return res;
+}
+
+static sint aes_decipher(u8 *key, uint	hdrlen,
+			u8 *pframe, uint plen)
+{
+	static u8	message[MAX_MSG_SIZE];
+	uint	qc_exists, a4_exists, i, j, payload_remainder,
+			num_blocks, payload_index;
+	sint res = _SUCCESS;
+	u8 pn_vector[6];
+	u8 mic_iv[16];
+	u8 mic_header1[16];
+	u8 mic_header2[16];
+	u8 ctr_preload[16];
+
+    /* Intermediate Buffers */
+	u8 chain_buffer[16];
+	u8 aes_out[16];
+	u8 padded_buffer[16];
+	u8 mic[8];
+
+
+//	uint	offset = 0;
+	uint	frtype  = GetFrameType(pframe);
+	uint	frsubtype  = GetFrameSubType(pframe);
+_func_enter_;			
+	frsubtype=frsubtype>>4;
+
+
+	_rtw_memset((void *)mic_iv, 0, 16);
+	_rtw_memset((void *)mic_header1, 0, 16);
+	_rtw_memset((void *)mic_header2, 0, 16);
+	_rtw_memset((void *)ctr_preload, 0, 16);
+	_rtw_memset((void *)chain_buffer, 0, 16);
+	_rtw_memset((void *)aes_out, 0, 16);
+	_rtw_memset((void *)padded_buffer, 0, 16);
+
+	//start to decrypt the payload
+
+	num_blocks = (plen-8) / 16; //(plen including llc, payload_length and mic )
+
+	payload_remainder = (plen-8) % 16;
+
+	pn_vector[0]  = pframe[hdrlen];
+	pn_vector[1]  = pframe[hdrlen+1];
+	pn_vector[2]  = pframe[hdrlen+4];
+	pn_vector[3]  = pframe[hdrlen+5];
+	pn_vector[4]  = pframe[hdrlen+6];
+	pn_vector[5]  = pframe[hdrlen+7];
+
+	if ((hdrlen == WLAN_HDR_A3_LEN )||(hdrlen ==  WLAN_HDR_A3_QOS_LEN))
+		a4_exists = 0;
+	else
+		a4_exists = 1;
+
+	if (
+		(frtype == WIFI_DATA_CFACK) ||
+		(frtype == WIFI_DATA_CFPOLL)||
+		(frtype == WIFI_DATA_CFACKPOLL))
+		{
+			qc_exists = 1;
+					if(hdrlen !=  WLAN_HDR_A3_QOS_LEN){
+				 
+					hdrlen += 2;
+			}
+		}
+	else if (
+		(frsubtype == 0x08) ||
+		(frsubtype == 0x09)||
+		(frsubtype == 0x0a)||
+		(frsubtype == 0x0b))
+		{
+			if(hdrlen !=  WLAN_HDR_A3_QOS_LEN){
+				 
+					hdrlen += 2;
+			}
+			qc_exists = 1;
+		}
+	else
+		qc_exists = 0;
+
+
+	// now, decrypt pframe with hdrlen offset and plen long
+
+	payload_index = hdrlen + 8; // 8 is for extiv
+	
+	for (i=0; i< num_blocks; i++)
+    {
+        construct_ctr_preload(
+                                ctr_preload,
+                                a4_exists,
+                                qc_exists,
+                                pframe,
+                                pn_vector,
+                                i+1
+                            );
+
+        aes128k128d(key, ctr_preload, aes_out);
+        bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+
+        for (j=0; j<16;j++) pframe[payload_index++] = chain_buffer[j];
+    }
+
+    if (payload_remainder > 0)          /* If there is a short final block, then pad it,*/
+    {                                   /* encrypt it and copy the unpadded part back   */
+        construct_ctr_preload(
+                                ctr_preload,
+                                a4_exists,
+                                qc_exists,
+                                pframe,
+                                pn_vector,
+                                num_blocks+1
+                            );
+
+        for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
+        for (j = 0; j < payload_remainder; j++)
+        {
+            padded_buffer[j] = pframe[payload_index+j];
+        }
+        aes128k128d(key, ctr_preload, aes_out);
+        bitwise_xor(aes_out, padded_buffer, chain_buffer);
+        for (j=0; j<payload_remainder;j++) pframe[payload_index++] = chain_buffer[j];
+    }
+
+	//start to calculate the mic	
+	if((hdrlen +plen+8) <= MAX_MSG_SIZE)
+		_rtw_memcpy((void *)message, pframe, (hdrlen +plen+8)); //8 is for ext iv len
+
+
+	pn_vector[0]=pframe[hdrlen];
+	pn_vector[1]=pframe[hdrlen+1];
+	pn_vector[2]=pframe[hdrlen+4];
+	pn_vector[3]=pframe[hdrlen+5];
+	pn_vector[4]=pframe[hdrlen+6];
+	pn_vector[5]=pframe[hdrlen+7];
+
+
+	
+	construct_mic_iv(
+                        mic_iv,
+                        qc_exists,
+                        a4_exists,
+                        message,
+                        plen-8,
+                        pn_vector
+                        );
+
+    construct_mic_header1(
+                            mic_header1,
+                            hdrlen,
+                            message
+                            );
+    construct_mic_header2(
+                            mic_header2,
+                            message,
+                            a4_exists,
+                            qc_exists
+                            );
+
+
+	payload_remainder = (plen-8) % 16;
+    num_blocks = (plen-8) / 16;
+
+    /* Find start of payload */
+    payload_index = (hdrlen + 8);
+
+    /* Calculate MIC */
+    aes128k128d(key, mic_iv, aes_out);
+    bitwise_xor(aes_out, mic_header1, chain_buffer);
+    aes128k128d(key, chain_buffer, aes_out);
+    bitwise_xor(aes_out, mic_header2, chain_buffer);
+    aes128k128d(key, chain_buffer, aes_out);
+
+	for (i = 0; i < num_blocks; i++)
+    {
+        bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+
+        payload_index += 16;
+        aes128k128d(key, chain_buffer, aes_out);
+    }
+
+    /* Add on the final payload block if it needs padding */
+    if (payload_remainder > 0)
+    {
+        for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
+        for (j = 0; j < payload_remainder; j++)
+        {
+            padded_buffer[j] = message[payload_index++];
+        }
+        bitwise_xor(aes_out, padded_buffer, chain_buffer);
+        aes128k128d(key, chain_buffer, aes_out);
+
+    }
+
+    for (j = 0 ; j < 8; j++) mic[j] = aes_out[j];
+
+    /* Insert MIC into payload */
+    for (j = 0; j < 8; j++)
+    	message[payload_index+j] = mic[j];
+
+	payload_index = hdrlen + 8;
+	for (i=0; i< num_blocks; i++)
+    {
+        construct_ctr_preload(
+                                ctr_preload,
+                                a4_exists,
+                                qc_exists,
+                                message,
+                                pn_vector,
+                                i+1);
+        aes128k128d(key, ctr_preload, aes_out);
+        bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+        for (j=0; j<16;j++) message[payload_index++] = chain_buffer[j];
+    }
+
+    if (payload_remainder > 0)          /* If there is a short final block, then pad it,*/
+    {                                   /* encrypt it and copy the unpadded part back   */
+        construct_ctr_preload(
+                                ctr_preload,
+                                a4_exists,
+                                qc_exists,
+                                message,
+                                pn_vector,
+                                num_blocks+1);
+
+        for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
+        for (j = 0; j < payload_remainder; j++)
+        {
+            padded_buffer[j] = message[payload_index+j];
+        }
+        aes128k128d(key, ctr_preload, aes_out);
+        bitwise_xor(aes_out, padded_buffer, chain_buffer);
+        for (j=0; j<payload_remainder;j++) message[payload_index++] = chain_buffer[j];
+    }
+
+    /* Encrypt the MIC */
+    construct_ctr_preload(
+                        ctr_preload,
+                        a4_exists,
+                        qc_exists,
+                        message,
+                        pn_vector,
+                        0);
+
+    for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
+    for (j = 0; j < 8; j++)
+    {
+        padded_buffer[j] = message[j+hdrlen+8+plen-8];
+    }
+
+    aes128k128d(key, ctr_preload, aes_out);
+    bitwise_xor(aes_out, padded_buffer, chain_buffer);
+    for (j=0; j<8;j++) message[payload_index++] = chain_buffer[j];
+
+	//compare the mic
+	for(i=0;i<8;i++){
+		if(pframe[hdrlen+8+plen-8+i] != message[hdrlen+8+plen-8+i])
+		{
+			RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x) \n",
+						i,pframe[hdrlen+8+plen-8+i],message[hdrlen+8+plen-8+i]));
+			DBG_871X("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x) \n",
+						i,pframe[hdrlen+8+plen-8+i],message[hdrlen+8+plen-8+i]);
+			res = _FAIL;
+		}
+	}
+_func_exit_;	
+	return res;
+}
+
+u32	rtw_aes_decrypt(_adapter *padapter, u8 *precvframe)
+{	// exclude ICV
+
+
+	/*static*/ 
+//	unsigned char	message[MAX_MSG_SIZE];
+
+
+    	/* Intermediate Buffers */
+
+
+	sint 		length;
+	u8	*pframe,*prwskey;	//, *payload,*iv
+	struct	sta_info		*stainfo;
+	struct	rx_pkt_attrib	 *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
+	struct 	security_priv	*psecuritypriv=&padapter->securitypriv;
+//	struct	recv_priv		*precvpriv=&padapter->recvpriv;
+	u32	res=_SUCCESS;
+_func_enter_;	 
+	pframe=(unsigned char *)((union recv_frame*)precvframe)->u.hdr.rx_data;
+	//4 start to encrypt each fragment
+	if((prxattrib->encrypt==_AES_)){
+
+		stainfo=rtw_get_stainfo(&padapter->stapriv ,&prxattrib->ta[0] );
+		if (stainfo!=NULL){
+			RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_aes_decrypt: stainfo!=NULL!!!\n"));
+
+			if(IS_MCAST(prxattrib->ra))
+			{
+				//in concurrent we should use sw descrypt in group key, so we remove this message			
+				//DBG_871X("rx bc/mc packets, to perform sw rtw_aes_decrypt\n");
+				//prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+				if(psecuritypriv->binstallGrpkey==_FALSE)
+				{
+					res=_FAIL;				
+					DBG_8192C("%s:rx bc/mc packets,but didn't install group key!!!!!!!!!!\n",__FUNCTION__);
+					goto exit;
+				}
+				prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+				if(psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index)
+				{
+					DBG_871X("not match packet_index=%d, install_index=%d \n"
+					, prxattrib->key_index, psecuritypriv->dot118021XGrpKeyid);
+					res=_FAIL;
+					goto exit;
+				}
+			}
+			else
+			{
+				prwskey=&stainfo->dot118021x_UncstKey.skey[0];
+			}
+	
+			length= ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
+				
+			res= aes_decipher(prwskey,prxattrib->hdrlen,pframe, length);
+
+
+		}
+		else{
+			RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_aes_encrypt: stainfo==NULL!!!\n"));
+			res=_FAIL;
+		}
+						
+	}
+_func_exit_;	
+exit:
+	return res;
+}
+#ifndef PLATFORM_FREEBSD
+/* compress 512-bits */
+static int sha256_compress(struct sha256_state *md, unsigned char *buf)
+{
+	u32 S[8], W[64], t0, t1;
+	u32 t;
+	int i;
+
+	/* copy state into S */
+	for (i = 0; i < 8; i++) {
+		S[i] = md->state[i];
+	}
+
+	/* copy the state into 512-bits into W[0..15] */
+	for (i = 0; i < 16; i++)
+		W[i] = WPA_GET_BE32(buf + (4 * i));
+
+	/* fill W[16..63] */
+	for (i = 16; i < 64; i++) {
+		W[i] = Gamma1(W[i - 2]) + W[i - 7] + Gamma0(W[i - 15]) +
+			W[i - 16];
+	}        
+
+	/* Compress */
+#define RND(a,b,c,d,e,f,g,h,i)                          \
+	t0 = h + Sigma1(e) + Ch(e, f, g) + K[i] + W[i];	\
+	t1 = Sigma0(a) + Maj(a, b, c);			\
+	d += t0;					\
+	h  = t0 + t1;
+
+	for (i = 0; i < 64; ++i) {
+		RND(S[0], S[1], S[2], S[3], S[4], S[5], S[6], S[7], i);
+		t = S[7]; S[7] = S[6]; S[6] = S[5]; S[5] = S[4]; 
+		S[4] = S[3]; S[3] = S[2]; S[2] = S[1]; S[1] = S[0]; S[0] = t;
+	}
+
+	/* feedback */
+	for (i = 0; i < 8; i++) {
+		md->state[i] = md->state[i] + S[i];
+	}
+	return 0;
+}
+
+/* Initialize the hash state */
+static void sha256_init(struct sha256_state *md)
+{
+	md->curlen = 0;
+	md->length = 0;
+	md->state[0] = 0x6A09E667UL;
+	md->state[1] = 0xBB67AE85UL;
+	md->state[2] = 0x3C6EF372UL;
+	md->state[3] = 0xA54FF53AUL;
+	md->state[4] = 0x510E527FUL;
+	md->state[5] = 0x9B05688CUL;
+	md->state[6] = 0x1F83D9ABUL;
+	md->state[7] = 0x5BE0CD19UL;
+}
+
+/**
+   Process a block of memory though the hash
+   @param md     The hash state
+   @param in     The data to hash
+   @param inlen  The length of the data (octets)
+   @return CRYPT_OK if successful
+*/
+static int sha256_process(struct sha256_state *md, unsigned char *in,
+			  unsigned long inlen)
+{
+	unsigned long n;
+#define block_size 64
+
+	if (md->curlen > sizeof(md->buf))
+		return -1;
+
+	while (inlen > 0) {
+		if (md->curlen == 0 && inlen >= block_size) {
+			if (sha256_compress(md, (unsigned char *) in) < 0)
+				return -1;
+			md->length += block_size * 8;
+			in += block_size;
+			inlen -= block_size;
+		} else {
+			n = MIN(inlen, (block_size - md->curlen));
+			_rtw_memcpy(md->buf + md->curlen, in, n);
+			md->curlen += n;
+			in += n;
+			inlen -= n;
+			if (md->curlen == block_size) {
+				if (sha256_compress(md, md->buf) < 0)
+					return -1;
+				md->length += 8 * block_size;
+				md->curlen = 0;
+			}
+		}
+	}
+
+	return 0;
+}
+
+
+/**
+   Terminate the hash to get the digest
+   @param md  The hash state
+   @param out [out] The destination of the hash (32 bytes)
+   @return CRYPT_OK if successful
+*/
+static int sha256_done(struct sha256_state *md, unsigned char *out)
+{
+	int i;
+
+	if (md->curlen >= sizeof(md->buf))
+		return -1;
+
+	/* increase the length of the message */
+	md->length += md->curlen * 8;
+
+	/* append the '1' bit */
+	md->buf[md->curlen++] = (unsigned char) 0x80;
+
+	/* if the length is currently above 56 bytes we append zeros
+	 * then compress.  Then we can fall back to padding zeros and length
+	 * encoding like normal.
+	 */
+	if (md->curlen > 56) {
+		while (md->curlen < 64) {
+			md->buf[md->curlen++] = (unsigned char) 0;
+		}
+		sha256_compress(md, md->buf);
+		md->curlen = 0;
+	}
+
+	/* pad upto 56 bytes of zeroes */
+	while (md->curlen < 56) {
+		md->buf[md->curlen++] = (unsigned char) 0;
+	}
+
+	/* store length */
+	WPA_PUT_BE64(md->buf + 56, md->length);
+	sha256_compress(md, md->buf);
+
+	/* copy output */
+	for (i = 0; i < 8; i++)
+		WPA_PUT_BE32(out + (4 * i), md->state[i]);
+
+	return 0;
+}
+
+/**
+ * sha256_vector - SHA256 hash for data vector
+ * @num_elem: Number of elements in the data vector
+ * @addr: Pointers to the data areas
+ * @len: Lengths of the data blocks
+ * @mac: Buffer for the hash
+ * Returns: 0 on success, -1 of failure
+ */
+static int sha256_vector(size_t num_elem, u8 *addr[], size_t *len,
+		  u8 *mac)
+{
+	struct sha256_state ctx;
+	size_t i;
+
+	sha256_init(&ctx);
+	for (i = 0; i < num_elem; i++)
+		if (sha256_process(&ctx, addr[i], len[i]))
+			return -1;
+	if (sha256_done(&ctx, mac))
+		return -1;
+	return 0;
+}
+
+static u8 os_strlen(const char *s)
+{
+	const char *p = s;
+	while (*p)
+		p++;
+	return p - s;
+}
+
+static int os_memcmp(void *s1, void *s2, u8 n)
+{
+	unsigned char *p1 = s1, *p2 = s2;
+
+	if (n == 0)
+		return 0;
+
+	while (*p1 == *p2) {
+		p1++;
+		p2++;
+		n--;
+		if (n == 0)
+			return 0;
+	}
+
+	return *p1 - *p2;
+}
+
+/**
+ * hmac_sha256_vector - HMAC-SHA256 over data vector (RFC 2104)
+ * @key: Key for HMAC operations
+ * @key_len: Length of the key in bytes
+ * @num_elem: Number of elements in the data vector
+ * @addr: Pointers to the data areas
+ * @len: Lengths of the data blocks
+ * @mac: Buffer for the hash (32 bytes)
+ */
+static void hmac_sha256_vector(u8 *key, size_t key_len, size_t num_elem,
+			u8 *addr[], size_t *len, u8 *mac)
+{
+	unsigned char k_pad[64]; /* padding - key XORd with ipad/opad */
+	unsigned char tk[32];
+	u8 *_addr[6];
+	size_t _len[6], i;
+
+	if (num_elem > 5) {
+		/*
+		 * Fixed limit on the number of fragments to avoid having to
+		 * allocate memory (which could fail).
+		 */
+		return;
+	}
+
+        /* if key is longer than 64 bytes reset it to key = SHA256(key) */
+        if (key_len > 64) {
+		sha256_vector(1, &key, &key_len, tk);
+		key = tk;
+		key_len = 32;
+        }
+
+	/* the HMAC_SHA256 transform looks like:
+	 *
+	 * SHA256(K XOR opad, SHA256(K XOR ipad, text))
+	 *
+	 * where K is an n byte key
+	 * ipad is the byte 0x36 repeated 64 times
+	 * opad is the byte 0x5c repeated 64 times
+	 * and text is the data being protected */
+
+	/* start out by storing key in ipad */
+	_rtw_memset(k_pad, 0, sizeof(k_pad));
+	_rtw_memcpy(k_pad, key, key_len);
+	/* XOR key with ipad values */
+	for (i = 0; i < 64; i++)
+		k_pad[i] ^= 0x36;
+
+	/* perform inner SHA256 */
+	_addr[0] = k_pad;
+	_len[0] = 64;
+	for (i = 0; i < num_elem; i++) {
+		_addr[i + 1] = addr[i];
+		_len[i + 1] = len[i];
+	}
+	sha256_vector(1 + num_elem, _addr, _len, mac);
+
+	_rtw_memset(k_pad, 0, sizeof(k_pad));
+	_rtw_memcpy(k_pad, key, key_len);
+	/* XOR key with opad values */
+	for (i = 0; i < 64; i++)
+		k_pad[i] ^= 0x5c;
+
+	/* perform outer SHA256 */
+	_addr[0] = k_pad;
+	_len[0] = 64;
+	_addr[1] = mac;
+	_len[1] = 32;
+	sha256_vector(2, _addr, _len, mac);
+}
+#endif //PLATFORM_FREEBSD
+/**
+ * sha256_prf - SHA256-based Pseudo-Random Function (IEEE 802.11r, 8.5.1.5.2)
+ * @key: Key for PRF
+ * @key_len: Length of the key in bytes
+ * @label: A unique label for each purpose of the PRF
+ * @data: Extra data to bind into the key
+ * @data_len: Length of the data
+ * @buf: Buffer for the generated pseudo-random key
+ * @buf_len: Number of bytes of key to generate
+ *
+ * This function is used to derive new, cryptographically separate keys from a
+ * given key.
+ */
+#ifndef PLATFORM_FREEBSD //Baron
+static void sha256_prf(u8 *key, size_t key_len, char *label,
+		u8 *data, size_t data_len, u8 *buf, size_t buf_len)
+{
+	u16 counter = 1;
+	size_t pos, plen;
+	u8 hash[SHA256_MAC_LEN];
+	u8 *addr[4];
+	size_t len[4];
+	u8 counter_le[2], length_le[2];
+
+	addr[0] = counter_le;
+	len[0] = 2;
+	addr[1] = (u8 *) label;
+	len[1] = os_strlen(label);
+	addr[2] = data;
+	len[2] = data_len;
+	addr[3] = length_le;
+	len[3] = sizeof(length_le);
+
+	WPA_PUT_LE16(length_le, buf_len * 8);
+	pos = 0;
+	while (pos < buf_len) {
+		plen = buf_len - pos;
+		WPA_PUT_LE16(counter_le, counter);
+		if (plen >= SHA256_MAC_LEN) {
+			hmac_sha256_vector(key, key_len, 4, addr, len,
+					   &buf[pos]);
+			pos += SHA256_MAC_LEN;
+		} else {
+			hmac_sha256_vector(key, key_len, 4, addr, len, hash);
+			_rtw_memcpy(&buf[pos], hash, plen);
+			break;
+		}
+		counter++;
+	}
+}
+#endif //PLATFORM_FREEBSD Baron
+
+/* AES tables*/
+const u32 Te0[256] = {
+    0xc66363a5U, 0xf87c7c84U, 0xee777799U, 0xf67b7b8dU,
+    0xfff2f20dU, 0xd66b6bbdU, 0xde6f6fb1U, 0x91c5c554U,
+    0x60303050U, 0x02010103U, 0xce6767a9U, 0x562b2b7dU,
+    0xe7fefe19U, 0xb5d7d762U, 0x4dababe6U, 0xec76769aU,
+    0x8fcaca45U, 0x1f82829dU, 0x89c9c940U, 0xfa7d7d87U,
+    0xeffafa15U, 0xb25959ebU, 0x8e4747c9U, 0xfbf0f00bU,
+    0x41adadecU, 0xb3d4d467U, 0x5fa2a2fdU, 0x45afafeaU,
+    0x239c9cbfU, 0x53a4a4f7U, 0xe4727296U, 0x9bc0c05bU,
+    0x75b7b7c2U, 0xe1fdfd1cU, 0x3d9393aeU, 0x4c26266aU,
+    0x6c36365aU, 0x7e3f3f41U, 0xf5f7f702U, 0x83cccc4fU,
+    0x6834345cU, 0x51a5a5f4U, 0xd1e5e534U, 0xf9f1f108U,
+    0xe2717193U, 0xabd8d873U, 0x62313153U, 0x2a15153fU,
+    0x0804040cU, 0x95c7c752U, 0x46232365U, 0x9dc3c35eU,
+    0x30181828U, 0x379696a1U, 0x0a05050fU, 0x2f9a9ab5U,
+    0x0e070709U, 0x24121236U, 0x1b80809bU, 0xdfe2e23dU,
+    0xcdebeb26U, 0x4e272769U, 0x7fb2b2cdU, 0xea75759fU,
+    0x1209091bU, 0x1d83839eU, 0x582c2c74U, 0x341a1a2eU,
+    0x361b1b2dU, 0xdc6e6eb2U, 0xb45a5aeeU, 0x5ba0a0fbU,
+    0xa45252f6U, 0x763b3b4dU, 0xb7d6d661U, 0x7db3b3ceU,
+    0x5229297bU, 0xdde3e33eU, 0x5e2f2f71U, 0x13848497U,
+    0xa65353f5U, 0xb9d1d168U, 0x00000000U, 0xc1eded2cU,
+    0x40202060U, 0xe3fcfc1fU, 0x79b1b1c8U, 0xb65b5bedU,
+    0xd46a6abeU, 0x8dcbcb46U, 0x67bebed9U, 0x7239394bU,
+    0x944a4adeU, 0x984c4cd4U, 0xb05858e8U, 0x85cfcf4aU,
+    0xbbd0d06bU, 0xc5efef2aU, 0x4faaaae5U, 0xedfbfb16U,
+    0x864343c5U, 0x9a4d4dd7U, 0x66333355U, 0x11858594U,
+    0x8a4545cfU, 0xe9f9f910U, 0x04020206U, 0xfe7f7f81U,
+    0xa05050f0U, 0x783c3c44U, 0x259f9fbaU, 0x4ba8a8e3U,
+    0xa25151f3U, 0x5da3a3feU, 0x804040c0U, 0x058f8f8aU,
+    0x3f9292adU, 0x219d9dbcU, 0x70383848U, 0xf1f5f504U,
+    0x63bcbcdfU, 0x77b6b6c1U, 0xafdada75U, 0x42212163U,
+    0x20101030U, 0xe5ffff1aU, 0xfdf3f30eU, 0xbfd2d26dU,
+    0x81cdcd4cU, 0x180c0c14U, 0x26131335U, 0xc3ecec2fU,
+    0xbe5f5fe1U, 0x359797a2U, 0x884444ccU, 0x2e171739U,
+    0x93c4c457U, 0x55a7a7f2U, 0xfc7e7e82U, 0x7a3d3d47U,
+    0xc86464acU, 0xba5d5de7U, 0x3219192bU, 0xe6737395U,
+    0xc06060a0U, 0x19818198U, 0x9e4f4fd1U, 0xa3dcdc7fU,
+    0x44222266U, 0x542a2a7eU, 0x3b9090abU, 0x0b888883U,
+    0x8c4646caU, 0xc7eeee29U, 0x6bb8b8d3U, 0x2814143cU,
+    0xa7dede79U, 0xbc5e5ee2U, 0x160b0b1dU, 0xaddbdb76U,
+    0xdbe0e03bU, 0x64323256U, 0x743a3a4eU, 0x140a0a1eU,
+    0x924949dbU, 0x0c06060aU, 0x4824246cU, 0xb85c5ce4U,
+    0x9fc2c25dU, 0xbdd3d36eU, 0x43acacefU, 0xc46262a6U,
+    0x399191a8U, 0x319595a4U, 0xd3e4e437U, 0xf279798bU,
+    0xd5e7e732U, 0x8bc8c843U, 0x6e373759U, 0xda6d6db7U,
+    0x018d8d8cU, 0xb1d5d564U, 0x9c4e4ed2U, 0x49a9a9e0U,
+    0xd86c6cb4U, 0xac5656faU, 0xf3f4f407U, 0xcfeaea25U,
+    0xca6565afU, 0xf47a7a8eU, 0x47aeaee9U, 0x10080818U,
+    0x6fbabad5U, 0xf0787888U, 0x4a25256fU, 0x5c2e2e72U,
+    0x381c1c24U, 0x57a6a6f1U, 0x73b4b4c7U, 0x97c6c651U,
+    0xcbe8e823U, 0xa1dddd7cU, 0xe874749cU, 0x3e1f1f21U,
+    0x964b4bddU, 0x61bdbddcU, 0x0d8b8b86U, 0x0f8a8a85U,
+    0xe0707090U, 0x7c3e3e42U, 0x71b5b5c4U, 0xcc6666aaU,
+    0x904848d8U, 0x06030305U, 0xf7f6f601U, 0x1c0e0e12U,
+    0xc26161a3U, 0x6a35355fU, 0xae5757f9U, 0x69b9b9d0U,
+    0x17868691U, 0x99c1c158U, 0x3a1d1d27U, 0x279e9eb9U,
+    0xd9e1e138U, 0xebf8f813U, 0x2b9898b3U, 0x22111133U,
+    0xd26969bbU, 0xa9d9d970U, 0x078e8e89U, 0x339494a7U,
+    0x2d9b9bb6U, 0x3c1e1e22U, 0x15878792U, 0xc9e9e920U,
+    0x87cece49U, 0xaa5555ffU, 0x50282878U, 0xa5dfdf7aU,
+    0x038c8c8fU, 0x59a1a1f8U, 0x09898980U, 0x1a0d0d17U,
+    0x65bfbfdaU, 0xd7e6e631U, 0x844242c6U, 0xd06868b8U,
+    0x824141c3U, 0x299999b0U, 0x5a2d2d77U, 0x1e0f0f11U,
+    0x7bb0b0cbU, 0xa85454fcU, 0x6dbbbbd6U, 0x2c16163aU,
+};
+const u32 Td0[256] = {
+    0x51f4a750U, 0x7e416553U, 0x1a17a4c3U, 0x3a275e96U,
+    0x3bab6bcbU, 0x1f9d45f1U, 0xacfa58abU, 0x4be30393U,
+    0x2030fa55U, 0xad766df6U, 0x88cc7691U, 0xf5024c25U,
+    0x4fe5d7fcU, 0xc52acbd7U, 0x26354480U, 0xb562a38fU,
+    0xdeb15a49U, 0x25ba1b67U, 0x45ea0e98U, 0x5dfec0e1U,
+    0xc32f7502U, 0x814cf012U, 0x8d4697a3U, 0x6bd3f9c6U,
+    0x038f5fe7U, 0x15929c95U, 0xbf6d7aebU, 0x955259daU,
+    0xd4be832dU, 0x587421d3U, 0x49e06929U, 0x8ec9c844U,
+    0x75c2896aU, 0xf48e7978U, 0x99583e6bU, 0x27b971ddU,
+    0xbee14fb6U, 0xf088ad17U, 0xc920ac66U, 0x7dce3ab4U,
+    0x63df4a18U, 0xe51a3182U, 0x97513360U, 0x62537f45U,
+    0xb16477e0U, 0xbb6bae84U, 0xfe81a01cU, 0xf9082b94U,
+    0x70486858U, 0x8f45fd19U, 0x94de6c87U, 0x527bf8b7U,
+    0xab73d323U, 0x724b02e2U, 0xe31f8f57U, 0x6655ab2aU,
+    0xb2eb2807U, 0x2fb5c203U, 0x86c57b9aU, 0xd33708a5U,
+    0x302887f2U, 0x23bfa5b2U, 0x02036abaU, 0xed16825cU,
+    0x8acf1c2bU, 0xa779b492U, 0xf307f2f0U, 0x4e69e2a1U,
+    0x65daf4cdU, 0x0605bed5U, 0xd134621fU, 0xc4a6fe8aU,
+    0x342e539dU, 0xa2f355a0U, 0x058ae132U, 0xa4f6eb75U,
+    0x0b83ec39U, 0x4060efaaU, 0x5e719f06U, 0xbd6e1051U,
+    0x3e218af9U, 0x96dd063dU, 0xdd3e05aeU, 0x4de6bd46U,
+    0x91548db5U, 0x71c45d05U, 0x0406d46fU, 0x605015ffU,
+    0x1998fb24U, 0xd6bde997U, 0x894043ccU, 0x67d99e77U,
+    0xb0e842bdU, 0x07898b88U, 0xe7195b38U, 0x79c8eedbU,
+    0xa17c0a47U, 0x7c420fe9U, 0xf8841ec9U, 0x00000000U,
+    0x09808683U, 0x322bed48U, 0x1e1170acU, 0x6c5a724eU,
+    0xfd0efffbU, 0x0f853856U, 0x3daed51eU, 0x362d3927U,
+    0x0a0fd964U, 0x685ca621U, 0x9b5b54d1U, 0x24362e3aU,
+    0x0c0a67b1U, 0x9357e70fU, 0xb4ee96d2U, 0x1b9b919eU,
+    0x80c0c54fU, 0x61dc20a2U, 0x5a774b69U, 0x1c121a16U,
+    0xe293ba0aU, 0xc0a02ae5U, 0x3c22e043U, 0x121b171dU,
+    0x0e090d0bU, 0xf28bc7adU, 0x2db6a8b9U, 0x141ea9c8U,
+    0x57f11985U, 0xaf75074cU, 0xee99ddbbU, 0xa37f60fdU,
+    0xf701269fU, 0x5c72f5bcU, 0x44663bc5U, 0x5bfb7e34U,
+    0x8b432976U, 0xcb23c6dcU, 0xb6edfc68U, 0xb8e4f163U,
+    0xd731dccaU, 0x42638510U, 0x13972240U, 0x84c61120U,
+    0x854a247dU, 0xd2bb3df8U, 0xaef93211U, 0xc729a16dU,
+    0x1d9e2f4bU, 0xdcb230f3U, 0x0d8652ecU, 0x77c1e3d0U,
+    0x2bb3166cU, 0xa970b999U, 0x119448faU, 0x47e96422U,
+    0xa8fc8cc4U, 0xa0f03f1aU, 0x567d2cd8U, 0x223390efU,
+    0x87494ec7U, 0xd938d1c1U, 0x8ccaa2feU, 0x98d40b36U,
+    0xa6f581cfU, 0xa57ade28U, 0xdab78e26U, 0x3fadbfa4U,
+    0x2c3a9de4U, 0x5078920dU, 0x6a5fcc9bU, 0x547e4662U,
+    0xf68d13c2U, 0x90d8b8e8U, 0x2e39f75eU, 0x82c3aff5U,
+    0x9f5d80beU, 0x69d0937cU, 0x6fd52da9U, 0xcf2512b3U,
+    0xc8ac993bU, 0x10187da7U, 0xe89c636eU, 0xdb3bbb7bU,
+    0xcd267809U, 0x6e5918f4U, 0xec9ab701U, 0x834f9aa8U,
+    0xe6956e65U, 0xaaffe67eU, 0x21bccf08U, 0xef15e8e6U,
+    0xbae79bd9U, 0x4a6f36ceU, 0xea9f09d4U, 0x29b07cd6U,
+    0x31a4b2afU, 0x2a3f2331U, 0xc6a59430U, 0x35a266c0U,
+    0x744ebc37U, 0xfc82caa6U, 0xe090d0b0U, 0x33a7d815U,
+    0xf104984aU, 0x41ecdaf7U, 0x7fcd500eU, 0x1791f62fU,
+    0x764dd68dU, 0x43efb04dU, 0xccaa4d54U, 0xe49604dfU,
+    0x9ed1b5e3U, 0x4c6a881bU, 0xc12c1fb8U, 0x4665517fU,
+    0x9d5eea04U, 0x018c355dU, 0xfa877473U, 0xfb0b412eU,
+    0xb3671d5aU, 0x92dbd252U, 0xe9105633U, 0x6dd64713U,
+    0x9ad7618cU, 0x37a10c7aU, 0x59f8148eU, 0xeb133c89U,
+    0xcea927eeU, 0xb761c935U, 0xe11ce5edU, 0x7a47b13cU,
+    0x9cd2df59U, 0x55f2733fU, 0x1814ce79U, 0x73c737bfU,
+    0x53f7cdeaU, 0x5ffdaa5bU, 0xdf3d6f14U, 0x7844db86U,
+    0xcaaff381U, 0xb968c43eU, 0x3824342cU, 0xc2a3405fU,
+    0x161dc372U, 0xbce2250cU, 0x283c498bU, 0xff0d9541U,
+    0x39a80171U, 0x080cb3deU, 0xd8b4e49cU, 0x6456c190U,
+    0x7bcb8461U, 0xd532b670U, 0x486c5c74U, 0xd0b85742U,
+};
+const u8 Td4s[256] = {
+    0x52U, 0x09U, 0x6aU, 0xd5U, 0x30U, 0x36U, 0xa5U, 0x38U,
+    0xbfU, 0x40U, 0xa3U, 0x9eU, 0x81U, 0xf3U, 0xd7U, 0xfbU,
+    0x7cU, 0xe3U, 0x39U, 0x82U, 0x9bU, 0x2fU, 0xffU, 0x87U,
+    0x34U, 0x8eU, 0x43U, 0x44U, 0xc4U, 0xdeU, 0xe9U, 0xcbU,
+    0x54U, 0x7bU, 0x94U, 0x32U, 0xa6U, 0xc2U, 0x23U, 0x3dU,
+    0xeeU, 0x4cU, 0x95U, 0x0bU, 0x42U, 0xfaU, 0xc3U, 0x4eU,
+    0x08U, 0x2eU, 0xa1U, 0x66U, 0x28U, 0xd9U, 0x24U, 0xb2U,
+    0x76U, 0x5bU, 0xa2U, 0x49U, 0x6dU, 0x8bU, 0xd1U, 0x25U,
+    0x72U, 0xf8U, 0xf6U, 0x64U, 0x86U, 0x68U, 0x98U, 0x16U,
+    0xd4U, 0xa4U, 0x5cU, 0xccU, 0x5dU, 0x65U, 0xb6U, 0x92U,
+    0x6cU, 0x70U, 0x48U, 0x50U, 0xfdU, 0xedU, 0xb9U, 0xdaU,
+    0x5eU, 0x15U, 0x46U, 0x57U, 0xa7U, 0x8dU, 0x9dU, 0x84U,
+    0x90U, 0xd8U, 0xabU, 0x00U, 0x8cU, 0xbcU, 0xd3U, 0x0aU,
+    0xf7U, 0xe4U, 0x58U, 0x05U, 0xb8U, 0xb3U, 0x45U, 0x06U,
+    0xd0U, 0x2cU, 0x1eU, 0x8fU, 0xcaU, 0x3fU, 0x0fU, 0x02U,
+    0xc1U, 0xafU, 0xbdU, 0x03U, 0x01U, 0x13U, 0x8aU, 0x6bU,
+    0x3aU, 0x91U, 0x11U, 0x41U, 0x4fU, 0x67U, 0xdcU, 0xeaU,
+    0x97U, 0xf2U, 0xcfU, 0xceU, 0xf0U, 0xb4U, 0xe6U, 0x73U,
+    0x96U, 0xacU, 0x74U, 0x22U, 0xe7U, 0xadU, 0x35U, 0x85U,
+    0xe2U, 0xf9U, 0x37U, 0xe8U, 0x1cU, 0x75U, 0xdfU, 0x6eU,
+    0x47U, 0xf1U, 0x1aU, 0x71U, 0x1dU, 0x29U, 0xc5U, 0x89U,
+    0x6fU, 0xb7U, 0x62U, 0x0eU, 0xaaU, 0x18U, 0xbeU, 0x1bU,
+    0xfcU, 0x56U, 0x3eU, 0x4bU, 0xc6U, 0xd2U, 0x79U, 0x20U,
+    0x9aU, 0xdbU, 0xc0U, 0xfeU, 0x78U, 0xcdU, 0x5aU, 0xf4U,
+    0x1fU, 0xddU, 0xa8U, 0x33U, 0x88U, 0x07U, 0xc7U, 0x31U,
+    0xb1U, 0x12U, 0x10U, 0x59U, 0x27U, 0x80U, 0xecU, 0x5fU,
+    0x60U, 0x51U, 0x7fU, 0xa9U, 0x19U, 0xb5U, 0x4aU, 0x0dU,
+    0x2dU, 0xe5U, 0x7aU, 0x9fU, 0x93U, 0xc9U, 0x9cU, 0xefU,
+    0xa0U, 0xe0U, 0x3bU, 0x4dU, 0xaeU, 0x2aU, 0xf5U, 0xb0U,
+    0xc8U, 0xebU, 0xbbU, 0x3cU, 0x83U, 0x53U, 0x99U, 0x61U,
+    0x17U, 0x2bU, 0x04U, 0x7eU, 0xbaU, 0x77U, 0xd6U, 0x26U,
+    0xe1U, 0x69U, 0x14U, 0x63U, 0x55U, 0x21U, 0x0cU, 0x7dU,
+};
+const u8 rcons[] = {
+	0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1B, 0x36
+	/* for 128-bit blocks, Rijndael never uses more than 10 rcon values */
+};
+
+/**
+ * Expand the cipher key into the encryption key schedule.
+ *
+ * @return	the number of rounds for the given cipher key size.
+ */
+#ifndef PLATFORM_FREEBSD //Baron
+static void rijndaelKeySetupEnc(u32 rk[/*44*/], const u8 cipherKey[])
+{
+	int i;
+	u32 temp;
+
+	rk[0] = GETU32(cipherKey     );
+	rk[1] = GETU32(cipherKey +  4);
+	rk[2] = GETU32(cipherKey +  8);
+	rk[3] = GETU32(cipherKey + 12);
+	for (i = 0; i < 10; i++) {
+		temp  = rk[3];
+		rk[4] = rk[0] ^
+			TE421(temp) ^ TE432(temp) ^ TE443(temp) ^ TE414(temp) ^
+			RCON(i);
+		rk[5] = rk[1] ^ rk[4];
+		rk[6] = rk[2] ^ rk[5];
+		rk[7] = rk[3] ^ rk[6];
+		rk += 4;
+	}
+}
+
+static void rijndaelEncrypt(u32 rk[/*44*/], u8 pt[16], u8 ct[16])
+{
+	u32 s0, s1, s2, s3, t0, t1, t2, t3;
+	int Nr = 10;
+#ifndef FULL_UNROLL
+	int r;
+#endif /* ?FULL_UNROLL */
+
+	/*
+	 * map byte array block to cipher state
+	 * and add initial round key:
+	 */
+	s0 = GETU32(pt     ) ^ rk[0];
+	s1 = GETU32(pt +  4) ^ rk[1];
+	s2 = GETU32(pt +  8) ^ rk[2];
+	s3 = GETU32(pt + 12) ^ rk[3];
+
+#define ROUND(i,d,s) \
+d##0 = TE0(s##0) ^ TE1(s##1) ^ TE2(s##2) ^ TE3(s##3) ^ rk[4 * i]; \
+d##1 = TE0(s##1) ^ TE1(s##2) ^ TE2(s##3) ^ TE3(s##0) ^ rk[4 * i + 1]; \
+d##2 = TE0(s##2) ^ TE1(s##3) ^ TE2(s##0) ^ TE3(s##1) ^ rk[4 * i + 2]; \
+d##3 = TE0(s##3) ^ TE1(s##0) ^ TE2(s##1) ^ TE3(s##2) ^ rk[4 * i + 3]
+
+#ifdef FULL_UNROLL
+
+	ROUND(1,t,s);
+	ROUND(2,s,t);
+	ROUND(3,t,s);
+	ROUND(4,s,t);
+	ROUND(5,t,s);
+	ROUND(6,s,t);
+	ROUND(7,t,s);
+	ROUND(8,s,t);
+	ROUND(9,t,s);
+
+	rk += Nr << 2;
+
+#else  /* !FULL_UNROLL */
+
+	/* Nr - 1 full rounds: */
+	r = Nr >> 1;
+	for (;;) {
+		ROUND(1,t,s);
+		rk += 8;
+		if (--r == 0)
+			break;
+		ROUND(0,s,t);
+	}
+
+#endif /* ?FULL_UNROLL */
+
+#undef ROUND
+
+	/*
+	 * apply last round and
+	 * map cipher state to byte array block:
+	 */
+	s0 = TE41(t0) ^ TE42(t1) ^ TE43(t2) ^ TE44(t3) ^ rk[0];
+	PUTU32(ct     , s0);
+	s1 = TE41(t1) ^ TE42(t2) ^ TE43(t3) ^ TE44(t0) ^ rk[1];
+	PUTU32(ct +  4, s1);
+	s2 = TE41(t2) ^ TE42(t3) ^ TE43(t0) ^ TE44(t1) ^ rk[2];
+	PUTU32(ct +  8, s2);
+	s3 = TE41(t3) ^ TE42(t0) ^ TE43(t1) ^ TE44(t2) ^ rk[3];
+	PUTU32(ct + 12, s3);
+}
+
+static void * aes_encrypt_init(u8 *key, size_t len)
+{
+	u32 *rk;
+	if (len != 16)
+		return NULL;
+	rk = (u32*)rtw_malloc(AES_PRIV_SIZE);
+	if (rk == NULL)
+		return NULL;
+	rijndaelKeySetupEnc(rk, key);
+	return rk;
+}
+
+static void aes_128_encrypt(void *ctx, u8 *plain, u8 *crypt)
+{
+	rijndaelEncrypt(ctx, plain, crypt);
+}
+
+
+static void gf_mulx(u8 *pad)
+{
+	int i, carry;
+
+	carry = pad[0] & 0x80;
+	for (i = 0; i < AES_BLOCK_SIZE - 1; i++)
+		pad[i] = (pad[i] << 1) | (pad[i + 1] >> 7);
+	pad[AES_BLOCK_SIZE - 1] <<= 1;
+	if (carry)
+		pad[AES_BLOCK_SIZE - 1] ^= 0x87;
+}
+
+static void aes_encrypt_deinit(void *ctx)
+{
+	_rtw_memset(ctx, 0, AES_PRIV_SIZE);
+	rtw_mfree(ctx, AES_PRIV_SIZE);
+}
+
+
+/**
+ * omac1_aes_128_vector - One-Key CBC MAC (OMAC1) hash with AES-128
+ * @key: 128-bit key for the hash operation
+ * @num_elem: Number of elements in the data vector
+ * @addr: Pointers to the data areas
+ * @len: Lengths of the data blocks
+ * @mac: Buffer for MAC (128 bits, i.e., 16 bytes)
+ * Returns: 0 on success, -1 on failure
+ *
+ * This is a mode for using block cipher (AES in this case) for authentication.
+ * OMAC1 was standardized with the name CMAC by NIST in a Special Publication
+ * (SP) 800-38B.
+ */
+static int omac1_aes_128_vector(u8 *key, size_t num_elem,
+							 u8 *addr[], size_t *len, u8 *mac)
+{
+	void *ctx;
+	u8 cbc[AES_BLOCK_SIZE], pad[AES_BLOCK_SIZE];
+	u8 *pos, *end;
+	size_t i, e, left, total_len;
+
+	ctx = aes_encrypt_init(key, 16);
+	if (ctx == NULL)
+		return -1;
+	_rtw_memset(cbc, 0, AES_BLOCK_SIZE);
+
+	total_len = 0;
+	for (e = 0; e < num_elem; e++)
+		total_len += len[e];
+	left = total_len;
+
+	e = 0;
+	pos = addr[0];
+	end = pos + len[0];
+
+	while (left >= AES_BLOCK_SIZE) {
+		for (i = 0; i < AES_BLOCK_SIZE; i++) {
+			cbc[i] ^= *pos++;
+			if (pos >= end) {
+				e++;
+				pos = addr[e];
+				end = pos + len[e];
+			}
+		}
+		if (left > AES_BLOCK_SIZE)
+			aes_128_encrypt(ctx, cbc, cbc);
+		left -= AES_BLOCK_SIZE;
+	}
+
+	_rtw_memset(pad, 0, AES_BLOCK_SIZE);
+	aes_128_encrypt(ctx, pad, pad);
+	gf_mulx(pad);
+
+	if (left || total_len == 0) {
+		for (i = 0; i < left; i++) {
+			cbc[i] ^= *pos++;
+			if (pos >= end) {
+				e++;
+				pos = addr[e];
+				end = pos + len[e];
+			}
+		}
+		cbc[left] ^= 0x80;
+		gf_mulx(pad);
+	}
+
+	for (i = 0; i < AES_BLOCK_SIZE; i++)
+		pad[i] ^= cbc[i];
+	aes_128_encrypt(ctx, pad, mac);
+	aes_encrypt_deinit(ctx);
+	return 0;
+}
+
+
+/**
+ * omac1_aes_128 - One-Key CBC MAC (OMAC1) hash with AES-128 (aka AES-CMAC)
+ * @key: 128-bit key for the hash operation
+ * @data: Data buffer for which a MAC is determined
+ * @data_len: Length of data buffer in bytes
+ * @mac: Buffer for MAC (128 bits, i.e., 16 bytes)
+ * Returns: 0 on success, -1 on failure
+ *
+ * This is a mode for using block cipher (AES in this case) for authentication.
+ * OMAC1 was standardized with the name CMAC by NIST in a Special Publication
+ * (SP) 800-38B.
+ */
+static int omac1_aes_128(u8 *key, u8 *data, size_t data_len, u8 *mac)
+{
+	return omac1_aes_128_vector(key, 1, &data, &data_len, mac);
+}
+#endif //PLATFORM_FREEBSD Baron
+
+#ifdef CONFIG_TDLS
+void wpa_tdls_generate_tpk(_adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	u8 *SNonce = psta->SNonce;
+	u8 *ANonce = psta->ANonce;
+
+	u8 key_input[SHA256_MAC_LEN];
+	u8 *nonce[2];
+	size_t len[2];
+	u8 data[3 * ETH_ALEN];
+
+	/* IEEE Std 802.11z-2010 8.5.9.1:
+	 * TPK-Key-Input = SHA-256(min(SNonce, ANonce) || max(SNonce, ANonce))
+	 */
+	len[0] = 32;
+	len[1] = 32;
+	if (os_memcmp(SNonce, ANonce, 32) < 0) {
+		nonce[0] = SNonce;
+		nonce[1] = ANonce;
+	} else {
+		nonce[0] = ANonce;
+		nonce[1] = SNonce;
+	}
+
+	sha256_vector(2, nonce, len, key_input);
+
+	/*
+	 * TPK-Key-Data = KDF-N_KEY(TPK-Key-Input, "TDLS PMK",
+	 *	min(MAC_I, MAC_R) || max(MAC_I, MAC_R) || BSSID || N_KEY)
+	 * TODO: is N_KEY really included in KDF Context and if so, in which
+	 * presentation format (little endian 16-bit?) is it used? It gets
+	 * added by the KDF anyway..
+	 */
+
+	if (os_memcmp(myid(&(padapter->eeprompriv)), psta->hwaddr, ETH_ALEN) < 0) {
+		_rtw_memcpy(data, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		_rtw_memcpy(data + ETH_ALEN, psta->hwaddr, ETH_ALEN);
+	} else {
+		_rtw_memcpy(data, psta->hwaddr, ETH_ALEN);
+		_rtw_memcpy(data + ETH_ALEN, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	}
+	_rtw_memcpy(data + 2 * ETH_ALEN, get_bssid(pmlmepriv), ETH_ALEN);
+
+	sha256_prf(key_input, SHA256_MAC_LEN, "TDLS PMK", data, sizeof(data), (u8 *) &psta->tpk, sizeof(psta->tpk));	
+
+
+}
+
+/**
+ * wpa_tdls_ftie_mic - Calculate TDLS FTIE MIC
+ * @kck: TPK-KCK
+ * @lnkid: Pointer to the beginning of Link Identifier IE
+ * @rsnie: Pointer to the beginning of RSN IE used for handshake
+ * @timeoutie: Pointer to the beginning of Timeout IE used for handshake
+ * @ftie: Pointer to the beginning of FT IE
+ * @mic: Pointer for writing MIC
+ *
+ * Calculate MIC for TDLS frame.
+ */
+int wpa_tdls_ftie_mic(u8 *kck, u8 trans_seq, 
+							u8 *lnkid, u8 *rsnie, u8 *timeoutie, u8 *ftie,
+							u8 *mic)
+{
+	u8 *buf, *pos;
+	struct wpa_tdls_ftie *_ftie;
+	struct wpa_tdls_lnkid *_lnkid;
+	int ret;
+	int len = 2 * ETH_ALEN + 1 + 2 + lnkid[1] + 2 + rsnie[1] +
+		2 + timeoutie[1] + 2 + ftie[1];
+	buf = rtw_zmalloc(len);
+	if (!buf) {
+		DBG_871X("TDLS: No memory for MIC calculation\n");
+		return -1;
+	}
+
+	pos = buf;
+	_lnkid = (struct wpa_tdls_lnkid *) lnkid;
+	/* 1) TDLS initiator STA MAC address */
+	_rtw_memcpy(pos, _lnkid->init_sta, ETH_ALEN);
+	pos += ETH_ALEN;
+	/* 2) TDLS responder STA MAC address */
+	_rtw_memcpy(pos, _lnkid->resp_sta, ETH_ALEN);
+	pos += ETH_ALEN;
+	/* 3) Transaction Sequence number */
+	*pos++ = trans_seq;
+	/* 4) Link Identifier IE */
+	_rtw_memcpy(pos, lnkid, 2 + lnkid[1]);
+	pos += 2 + lnkid[1];
+	/* 5) RSN IE */
+	_rtw_memcpy(pos, rsnie, 2 + rsnie[1]);
+	pos += 2 + rsnie[1];
+	/* 6) Timeout Interval IE */
+	_rtw_memcpy(pos, timeoutie, 2 + timeoutie[1]);
+	pos += 2 + timeoutie[1];
+	/* 7) FTIE, with the MIC field of the FTIE set to 0 */
+	_rtw_memcpy(pos, ftie, 2 + ftie[1]);
+	_ftie = (struct wpa_tdls_ftie *) pos;
+	_rtw_memset(_ftie->mic, 0, TDLS_MIC_LEN);
+	pos += 2 + ftie[1];
+ 
+	ret = omac1_aes_128(kck, buf, pos - buf, mic);
+	rtw_mfree(buf, len);
+	return ret;
+
+}
+
+int tdls_verify_mic(u8 *kck, u8 trans_seq,
+							u8 *lnkid, u8 *rsnie, u8 *timeoutie, u8 *ftie)
+{
+	u8 *buf, *pos;
+	int len;
+	u8 mic[16];
+	int ret;
+	u8 *rx_ftie, *tmp_ftie;
+
+	if (lnkid == NULL || rsnie == NULL ||
+	    timeoutie == NULL || ftie == NULL){
+		return 0;
+	}
+	
+	len = 2 * ETH_ALEN + 1 + 2 + 18 + 2 + *(rsnie+1) + 2 + *(timeoutie+1) + 2 + *(ftie+1);
+
+	buf = rtw_zmalloc(len);
+	if (buf == NULL)
+		return 0;
+
+	pos = buf;
+	/* 1) TDLS initiator STA MAC address */
+	_rtw_memcpy(pos, lnkid + ETH_ALEN + 2, ETH_ALEN);
+	pos += ETH_ALEN;
+	/* 2) TDLS responder STA MAC address */
+	_rtw_memcpy(pos, lnkid + 2 * ETH_ALEN + 2, ETH_ALEN);
+	pos += ETH_ALEN;
+	/* 3) Transaction Sequence number */
+	*pos++ = trans_seq;
+	/* 4) Link Identifier IE */
+	_rtw_memcpy(pos, lnkid, 2 + 18);
+	pos += 2 + 18;
+	/* 5) RSN IE */
+	_rtw_memcpy(pos, rsnie, 2 + *(rsnie+1));
+	pos += 2 + *(rsnie+1);
+	/* 6) Timeout Interval IE */
+	_rtw_memcpy(pos, timeoutie, 2 + *(timeoutie+1));
+	pos += 2 + *(timeoutie+1);
+	/* 7) FTIE, with the MIC field of the FTIE set to 0 */
+	_rtw_memcpy(pos, ftie, 2 + *(ftie+1));
+	pos += 2;
+	tmp_ftie = (u8 *) (pos+2);
+	_rtw_memset(tmp_ftie, 0, 16);
+	pos += *(ftie+1);
+
+	ret = omac1_aes_128(kck, buf, pos - buf, mic);
+	rtw_mfree(buf, len);
+	if (ret)
+		return 0;
+	rx_ftie = ftie+4;
+
+	if (os_memcmp(mic, rx_ftie, 16) == 0) {
+		//Valid MIC
+		return 1;
+	}
+
+	//Invalid MIC
+	DBG_871X( "[%s] Invalid MIC\n", __FUNCTION__);
+	return 0;
+
+}
+#endif //CONFIG_TDLS
+
+#ifdef PLATFORM_WINDOWS
+void rtw_use_tkipkey_handler (
+	IN	PVOID					SystemSpecific1,
+	IN	PVOID					FunctionContext,
+	IN	PVOID					SystemSpecific2,
+	IN	PVOID					SystemSpecific3
+	)
+#endif
+#ifdef PLATFORM_LINUX
+void rtw_use_tkipkey_handler(void *FunctionContext)
+#endif
+#ifdef PLATFORM_FREEBSD
+void rtw_use_tkipkey_handler(void *FunctionContext)
+#endif
+{
+        _adapter *padapter = (_adapter *)FunctionContext;
+
+
+_func_enter_;			
+
+	RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("^^^rtw_use_tkipkey_handler ^^^\n"));
+	
+/*
+	if(padapter->bDriverStopped ||padapter->bSurpriseRemoved){
+			RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("^^^rtw_use_tkipkey_handler (padapter->bDriverStopped %d)(padapter->bSurpriseRemoved %d)^^^\n",padapter->bDriverStopped,padapter->bSurpriseRemoved));
+
+		return;
+	}
+	*/
+	
+	padapter->securitypriv.busetkipkey=_TRUE;
+
+	RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("^^^rtw_use_tkipkey_handler padapter->securitypriv.busetkipkey=%d^^^\n",padapter->securitypriv.busetkipkey));
+
+_func_exit_;	
+
+}
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_sreset.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_sreset.c
new file mode 100755
index 00000000..b67fb758
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_sreset.c
@@ -0,0 +1,350 @@
+/******************************************************************************

+ *

+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.

+ *                                        

+ * This program is free software; you can redistribute it and/or modify it

+ * under the terms of version 2 of the GNU General Public License as

+ * published by the Free Software Foundation.

+ *

+ * 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.,

+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA

+ *

+ *

+ ******************************************************************************/

+

+#include<rtw_sreset.h>

+

+void sreset_init_value(_adapter *padapter)

+{

+#if defined(DBG_CONFIG_ERROR_DETECT)

+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);

+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;

+

+	_rtw_mutex_init(&psrtpriv->silentreset_mutex);

+	psrtpriv->silent_reset_inprogress = _FALSE;

+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;

+	psrtpriv->last_tx_time =0;

+	psrtpriv->last_tx_complete_time =0;

+#endif

+}

+void sreset_reset_value(_adapter *padapter)

+{

+#if defined(DBG_CONFIG_ERROR_DETECT)

+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);

+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;

+

+	psrtpriv->silent_reset_inprogress = _FALSE;

+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;

+	psrtpriv->last_tx_time =0;

+	psrtpriv->last_tx_complete_time =0;

+#endif

+}

+

+u8 sreset_get_wifi_status(_adapter *padapter)

+{

+#if defined(DBG_CONFIG_ERROR_DETECT)

+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);

+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;

+

+	u8 status = WIFI_STATUS_SUCCESS;

+	u32 val32 = 0;

+	_irqL irqL;

+	if(psrtpriv->silent_reset_inprogress == _TRUE)

+        {

+		return status;

+	}

+	val32 =rtw_read32(padapter,REG_TXDMA_STATUS);

+	if(val32==0xeaeaeaea){

+		psrtpriv->Wifi_Error_Status = WIFI_IF_NOT_EXIST;

+	}

+	else if(val32!=0){

+		DBG_8192C("txdmastatu(%x)\n",val32);

+		psrtpriv->Wifi_Error_Status = WIFI_MAC_TXDMA_ERROR;

+	}

+

+	if(WIFI_STATUS_SUCCESS !=psrtpriv->Wifi_Error_Status)

+	{

+		DBG_8192C("==>%s error_status(0x%x) \n",__FUNCTION__,psrtpriv->Wifi_Error_Status);

+		status = (psrtpriv->Wifi_Error_Status &( ~(USB_READ_PORT_FAIL|USB_WRITE_PORT_FAIL)));

+	}

+	DBG_8192C("==> %s wifi_status(0x%x)\n",__FUNCTION__,status);

+

+	//status restore

+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;

+

+	return status;

+#else

+	return WIFI_STATUS_SUCCESS;

+#endif

+}

+

+void sreset_set_wifi_error_status(_adapter *padapter, u32 status)

+{

+#if defined(DBG_CONFIG_ERROR_DETECT)

+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);

+	pHalData->srestpriv.Wifi_Error_Status = status;

+#endif

+}

+

+void sreset_set_trigger_point(_adapter *padapter, s32 tgp)

+{

+#if defined(DBG_CONFIG_ERROR_DETECT)

+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);

+	pHalData->srestpriv.dbg_trigger_point = tgp;

+#endif

+}

+

+bool sreset_inprogress(_adapter *padapter)

+{

+#if defined(DBG_CONFIG_ERROR_RESET)

+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);

+	return pHalData->srestpriv.silent_reset_inprogress;

+#else

+	return _FALSE;

+#endif

+}

+

+void sreset_restore_security_station(_adapter *padapter)

+{

+	u8 EntryId = 0;

+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;

+	struct sta_priv * pstapriv = &padapter->stapriv;

+	struct sta_info *psta;

+	struct security_priv* psecuritypriv=&(padapter->securitypriv);

+	struct mlme_ext_info	*pmlmeinfo = &padapter->mlmeextpriv.mlmext_info;

+

+	{

+		u8 val8;

+

+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) {

+			val8 = 0xcc;

+		#ifdef CONFIG_WAPI_SUPPORT

+		} else if (padapter->wapiInfo.bWapiEnable && pmlmeinfo->auth_algo == dot11AuthAlgrthm_WAPI) {

+			//Disable TxUseDefaultKey, RxUseDefaultKey, RxBroadcastUseDefaultKey.

+			val8 = 0x4c;

+		#endif

+		} else {

+			val8 = 0xcf;

+		}

+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));

+	}

+

+	#if 0

+	if (	( padapter->securitypriv.dot11PrivacyAlgrthm == _WEP40_ ) ||

+		( padapter->securitypriv.dot11PrivacyAlgrthm == _WEP104_ ))

+	{

+

+		for(EntryId=0; EntryId<4; EntryId++)

+		{

+			if(EntryId == psecuritypriv->dot11PrivacyKeyIndex)

+				rtw_set_key(padapter,&padapter->securitypriv, EntryId, 1);

+			else

+				rtw_set_key(padapter,&padapter->securitypriv, EntryId, 0);

+		}

+

+	}

+	else

+	#endif

+	if((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||

+		(padapter->securitypriv.dot11PrivacyAlgrthm == _AES_))

+	{

+		psta = rtw_get_stainfo(pstapriv, get_bssid(mlmepriv));

+		if (psta == NULL) {

+			//DEBUG_ERR( ("Set wpa_set_encryption: Obtain Sta_info fail \n"));

+		}

+		else

+		{

+			//pairwise key

+			rtw_setstakey_cmd(padapter, (unsigned char *)psta, _TRUE);

+			//group key

+			rtw_set_key(padapter,&padapter->securitypriv,padapter->securitypriv.dot118021XGrpKeyid, 0);

+		}

+	}

+}

+

+void sreset_restore_network_station(_adapter *padapter)

+{

+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;

+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+

+	#if 0

+	{

+	//=======================================================

+	// reset related register of Beacon control

+

+	//set MSR to nolink

+	Set_MSR(padapter, _HW_STATE_NOLINK_);		

+	// reject all data frame

+	rtw_write16(padapter, REG_RXFLTMAP2,0x00);

+	//reset TSF

+	rtw_write8(padapter, REG_DUAL_TSF_RST, (BIT(0)|BIT(1)));

+

+	// disable update TSF

+	SetBcnCtrlReg(padapter, BIT(4), 0);

+

+	//=======================================================

+	}

+	#endif

+	

+	rtw_setopmode_cmd(padapter, Ndis802_11Infrastructure);

+

+	{

+		u8 threshold;

+		#ifdef CONFIG_USB_HCI

+		// TH=1 => means that invalidate usb rx aggregation

+		// TH=0 => means that validate usb rx aggregation, use init value.

+		if(mlmepriv->htpriv.ht_option) {

+			if(padapter->registrypriv.wifi_spec==1)		

+				threshold = 1;

+			else

+				threshold = 0;

+			rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));

+		} else {

+			threshold = 1;

+			rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));

+		}

+		#endif

+	}

+

+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);

+

+	//disable dynamic functions, such as high power, DIG

+	//Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);

+	

+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);

+

+	{

+		u8	join_type = 0;

+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));

+	}

+

+	Set_MSR(padapter, (pmlmeinfo->state & 0x3));

+

+	mlmeext_joinbss_event_callback(padapter, 1);

+	//restore Sequence No.

+	rtw_write8(padapter,0x4dc,padapter->xmitpriv.nqos_ssn);

+

+	sreset_restore_security_station(padapter);

+}

+

+void sreset_restore_network_status(_adapter *padapter)

+{

+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;

+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+

+	if (check_fwstate(mlmepriv, WIFI_STATION_STATE)) {

+		DBG_871X(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_STATION_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));

+		sreset_restore_network_station(padapter);

+	} else if (check_fwstate(mlmepriv, WIFI_AP_STATE)) {

+		DBG_871X(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_AP_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));

+		rtw_ap_restore_network(padapter);

+	} else if (check_fwstate(mlmepriv, WIFI_ADHOC_STATE)) {

+		DBG_871X(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_ADHOC_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));

+	} else {

+		DBG_871X(FUNC_ADPT_FMT" fwstate:0x%08x - ???\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));

+	}

+}

+

+void sreset_stop_adapter(_adapter *padapter)

+{

+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);

+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;

+

+	if (padapter == NULL)

+		return;

+

+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));

+

+	if (!rtw_netif_queue_stopped(padapter->pnetdev))

+		rtw_netif_stop_queue(padapter->pnetdev);

+

+	rtw_cancel_all_timer(padapter);

+

+	/* TODO: OS and HCI independent */

+	#if defined(PLATFORM_LINUX) && defined(CONFIG_USB_HCI)

+	tasklet_kill(&pxmitpriv->xmit_tasklet);

+	#endif

+

+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))

+		rtw_scan_abort(padapter);

+

+	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))

+		_rtw_join_timeout_handler(padapter);

+

+}

+

+void sreset_start_adapter(_adapter *padapter)

+{

+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);

+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;

+

+	if (padapter == NULL)

+		return;

+

+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));

+

+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {

+		sreset_restore_network_status(padapter);

+	}

+

+	/* TODO: OS and HCI independent */

+	#if defined(PLATFORM_LINUX) && defined(CONFIG_USB_HCI)

+	tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);

+	#endif

+

+	_set_timer(&padapter->mlmepriv.dynamic_chk_timer, 2000);

+

+	if (rtw_netif_queue_stopped(padapter->pnetdev))

+		rtw_netif_wake_queue(padapter->pnetdev);

+

+}

+

+void sreset_reset(_adapter *padapter)

+{

+#ifdef DBG_CONFIG_ERROR_RESET

+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);

+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;

+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;

+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);

+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;

+	_irqL irqL;

+	u32 start = rtw_get_current_time();

+

+	DBG_871X("%s\n", __FUNCTION__);

+

+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;

+

+	_enter_critical_mutex(&psrtpriv->silentreset_mutex, &irqL);

+	psrtpriv->silent_reset_inprogress = _TRUE;

+	pwrpriv->change_rfpwrstate = rf_off;

+

+	sreset_stop_adapter(padapter);

+	#ifdef CONFIG_CONCURRENT_MODE

+	sreset_stop_adapter(padapter->pbuddy_adapter);

+	#endif

+

+	#ifdef CONFIG_IPS

+	ips_enter(padapter);

+	ips_leave(padapter);

+	#endif

+

+	sreset_start_adapter(padapter);

+	#ifdef CONFIG_CONCURRENT_MODE

+	sreset_start_adapter(padapter->pbuddy_adapter);

+	#endif

+

+	psrtpriv->silent_reset_inprogress = _FALSE;

+	_exit_critical_mutex(&psrtpriv->silentreset_mutex, &irqL);

+

+	DBG_871X("%s done in %d ms\n", __FUNCTION__, rtw_get_passing_time_ms(start));

+#endif

+}

+

diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_sta_mgt.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_sta_mgt.c
new file mode 100755
index 00000000..3c1e4c30
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_sta_mgt.c
@@ -0,0 +1,852 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_STA_MGT_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <recv_osdep.h>
+#include <xmit_osdep.h>
+#include <mlme_osdep.h>
+
+
+#if defined (PLATFORM_LINUX) && defined (PLATFORM_WINDOWS)
+
+#error "Shall be Linux or Windows, but not both!\n"
+
+#endif
+
+#include <sta_info.h>
+
+void _rtw_init_stainfo(struct sta_info *psta);
+void _rtw_init_stainfo(struct sta_info *psta)
+{
+
+_func_enter_;
+
+	_rtw_memset((u8 *)psta, 0, sizeof (struct sta_info));
+
+	 _rtw_spinlock_init(&psta->lock);
+	_rtw_init_listhead(&psta->list);
+	_rtw_init_listhead(&psta->hash_list);
+	//_rtw_init_listhead(&psta->asoc_list);
+	//_rtw_init_listhead(&psta->sleep_list);
+	//_rtw_init_listhead(&psta->wakeup_list);	
+
+	_rtw_init_queue(&psta->sleep_q);
+	psta->sleepq_len = 0;
+
+	_rtw_init_sta_xmit_priv(&psta->sta_xmitpriv);
+	_rtw_init_sta_recv_priv(&psta->sta_recvpriv);
+	
+#ifdef CONFIG_AP_MODE
+
+	_rtw_init_listhead(&psta->asoc_list);
+
+	_rtw_init_listhead(&psta->auth_list);
+	
+	psta->expire_to = 0;
+	
+	psta->flags = 0;
+	
+	psta->capability = 0;
+
+	psta->bpairwise_key_installed = _FALSE;
+
+
+#ifdef CONFIG_NATIVEAP_MLME
+	psta->nonerp_set = 0;
+	psta->no_short_slot_time_set = 0;
+	psta->no_short_preamble_set = 0;
+	psta->no_ht_gf_set = 0;
+	psta->no_ht_set = 0;
+	psta->ht_20mhz_set = 0;
+#endif	
+
+#ifdef CONFIG_TX_MCAST2UNI
+	psta->under_exist_checking = 0;
+#endif	// CONFIG_TX_MCAST2UNI
+	
+	psta->keep_alive_trycnt = 0;
+
+#endif	// CONFIG_AP_MODE	
+	
+_func_exit_;	
+
+}
+
+u32	_rtw_init_sta_priv(struct	sta_priv *pstapriv)
+{
+	struct sta_info *psta;
+	s32 i;
+
+_func_enter_;	
+
+	pstapriv->pallocated_stainfo_buf = rtw_zvmalloc (sizeof(struct sta_info) * NUM_STA+ 4);
+	
+	if(!pstapriv->pallocated_stainfo_buf)
+		return _FAIL;
+
+	pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 - 
+		((SIZE_PTR)(pstapriv->pallocated_stainfo_buf ) & 3);
+
+	_rtw_init_queue(&pstapriv->free_sta_queue);
+
+	_rtw_spinlock_init(&pstapriv->sta_hash_lock);
+	
+	//_rtw_init_queue(&pstapriv->asoc_q);
+	pstapriv->asoc_sta_count = 0;
+	_rtw_init_queue(&pstapriv->sleep_q);
+	_rtw_init_queue(&pstapriv->wakeup_q);
+
+	psta = (struct sta_info *)(pstapriv->pstainfo_buf);
+
+		
+	for(i = 0; i < NUM_STA; i++)
+	{
+		_rtw_init_stainfo(psta);
+
+		_rtw_init_listhead(&(pstapriv->sta_hash[i]));
+
+		rtw_list_insert_tail(&psta->list, get_list_head(&pstapriv->free_sta_queue));
+
+		psta++;
+	}
+
+	
+
+#ifdef CONFIG_AP_MODE
+
+	pstapriv->sta_dz_bitmap = 0;
+	pstapriv->tim_bitmap = 0;
+
+	_rtw_init_listhead(&pstapriv->asoc_list);
+	_rtw_init_listhead(&pstapriv->auth_list);
+	_rtw_spinlock_init(&pstapriv->asoc_list_lock);
+	_rtw_spinlock_init(&pstapriv->auth_list_lock);
+	pstapriv->asoc_list_cnt = 0;
+	pstapriv->auth_list_cnt = 0;
+
+	pstapriv->auth_to = 3; // 3*2 = 6 sec 
+	pstapriv->assoc_to = 3;
+	//pstapriv->expire_to = 900;// 900*2 = 1800 sec = 30 min, expire after no any traffic.
+	//pstapriv->expire_to = 30;// 30*2 = 60 sec = 1 min, expire after no any traffic.
+#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+	pstapriv->expire_to = 3; // 3*2 = 6 sec
+#else
+	pstapriv->expire_to = 60;// 60*2 = 120 sec = 2 min, expire after no any traffic.
+#endif	
+	pstapriv->max_num_sta = NUM_STA;
+		
+#endif
+	
+_func_exit_;		
+
+	return _SUCCESS;
+	
+}
+
+inline int rtw_stainfo_offset(struct sta_priv *stapriv, struct sta_info *sta)
+{
+	int offset = (((u8 *)sta) - stapriv->pstainfo_buf)/sizeof(struct sta_info);
+
+	if (!stainfo_offset_valid(offset))
+		DBG_871X("%s invalid offset(%d), out of range!!!", __func__, offset);
+
+	return offset;
+}
+
+inline struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int offset)
+{
+	if (!stainfo_offset_valid(offset))
+		DBG_871X("%s invalid offset(%d), out of range!!!", __func__, offset);
+
+	return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info));
+}
+
+void	_rtw_free_sta_xmit_priv_lock(struct sta_xmit_priv *psta_xmitpriv);
+void	_rtw_free_sta_xmit_priv_lock(struct sta_xmit_priv *psta_xmitpriv)
+{
+_func_enter_;
+
+	_rtw_spinlock_free(&psta_xmitpriv->lock);
+
+	_rtw_spinlock_free(&(psta_xmitpriv->be_q.sta_pending.lock));
+	_rtw_spinlock_free(&(psta_xmitpriv->bk_q.sta_pending.lock));
+	_rtw_spinlock_free(&(psta_xmitpriv->vi_q.sta_pending.lock));
+	_rtw_spinlock_free(&(psta_xmitpriv->vo_q.sta_pending.lock));
+_func_exit_;	
+}
+
+static void	_rtw_free_sta_recv_priv_lock(struct sta_recv_priv *psta_recvpriv)
+{
+_func_enter_;	
+
+	_rtw_spinlock_free(&psta_recvpriv->lock);
+
+	_rtw_spinlock_free(&(psta_recvpriv->defrag_q.lock));
+
+_func_exit_;
+
+}
+
+void rtw_mfree_stainfo(struct sta_info *psta);
+void rtw_mfree_stainfo(struct sta_info *psta)
+{
+_func_enter_;
+
+	if(&psta->lock != NULL)
+		 _rtw_spinlock_free(&psta->lock);
+
+	_rtw_free_sta_xmit_priv_lock(&psta->sta_xmitpriv);
+	_rtw_free_sta_recv_priv_lock(&psta->sta_recvpriv);
+	
+_func_exit_;	
+}
+
+
+// this function is used to free the memory of lock || sema for all stainfos
+void rtw_mfree_all_stainfo(struct sta_priv *pstapriv );
+void rtw_mfree_all_stainfo(struct sta_priv *pstapriv )
+{
+	_irqL	 irqL;
+	_list	*plist, *phead;
+	struct sta_info *psta = NULL;
+	
+_func_enter_;	
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	phead = get_list_head(&pstapriv->free_sta_queue);
+	plist = get_next(phead);
+		
+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
+	{
+		psta = LIST_CONTAINOR(plist, struct sta_info ,list);
+		plist = get_next(plist);
+
+		rtw_mfree_stainfo(psta);
+	}
+	
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+_func_exit_;	
+
+}
+
+void rtw_mfree_sta_priv_lock(struct	sta_priv *pstapriv);
+void rtw_mfree_sta_priv_lock(struct	sta_priv *pstapriv)
+{
+#ifdef CONFIG_AP_MODE
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+#endif
+
+	 rtw_mfree_all_stainfo(pstapriv); //be done before free sta_hash_lock
+
+	_rtw_spinlock_free(&pstapriv->free_sta_queue.lock);
+
+	_rtw_spinlock_free(&pstapriv->sta_hash_lock);
+	_rtw_spinlock_free(&pstapriv->wakeup_q.lock);
+	_rtw_spinlock_free(&pstapriv->sleep_q.lock);
+
+#ifdef CONFIG_AP_MODE
+	_rtw_spinlock_free(&pstapriv->asoc_list_lock);
+	_rtw_spinlock_free(&pstapriv->auth_list_lock);
+	_rtw_spinlock_free(&pacl_list->acl_node_q.lock);
+#endif
+
+}
+
+u32	_rtw_free_sta_priv(struct	sta_priv *pstapriv)
+{
+	_irqL 	irqL;
+	_list	*phead, *plist;
+	struct sta_info *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int 	index;
+
+_func_enter_;
+	if(pstapriv){
+
+		/*	delete all reordering_ctrl_timer		*/ 
+		_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+		for(index = 0; index < NUM_STA; index++)
+		{
+			phead = &(pstapriv->sta_hash[index]);
+			plist = get_next(phead);
+			
+			while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
+			{
+				int i;	
+				psta = LIST_CONTAINOR(plist, struct sta_info ,hash_list);
+				plist = get_next(plist);
+
+				for(i=0; i < 16 ; i++)
+				{
+					preorder_ctrl = &psta->recvreorder_ctrl[i];
+					_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);	
+				}
+			}
+		}
+		_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+		/*===============================*/
+		
+		rtw_mfree_sta_priv_lock(pstapriv);
+
+		if(pstapriv->pallocated_stainfo_buf) {
+			rtw_vmfree(pstapriv->pallocated_stainfo_buf, sizeof(struct sta_info)*NUM_STA+4);
+		}
+	}
+	
+_func_exit_;
+	return _SUCCESS;
+}
+
+
+//struct	sta_info *rtw_alloc_stainfo(_queue *pfree_sta_queue, unsigned char *hwaddr)
+struct	sta_info *rtw_alloc_stainfo(struct	sta_priv *pstapriv, u8 *hwaddr) 
+{	
+	_irqL irqL, irqL2;
+	uint tmp_aid;
+	s32	index;
+	_list	*phash_list;
+	struct sta_info	*psta;
+	_queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int i = 0;
+	u16  wRxSeqInitialValue = 0xffff;
+	
+_func_enter_;	
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+	
+	//_enter_critical_bh(&(pfree_sta_queue->lock), &irqL);
+	_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+
+	if (_rtw_queue_empty(pfree_sta_queue) == _TRUE)
+	{
+		//_exit_critical_bh(&(pfree_sta_queue->lock), &irqL);
+		_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+		psta = NULL;
+	}
+	else
+	{
+		psta = LIST_CONTAINOR(get_next(&pfree_sta_queue->queue), struct sta_info, list);
+		
+		rtw_list_delete(&(psta->list));
+
+		//_exit_critical_bh(&(pfree_sta_queue->lock), &irqL);
+		
+		tmp_aid = psta->aid;	
+	
+		_rtw_init_stainfo(psta);
+
+		psta->padapter = pstapriv->padapter;
+
+		_rtw_memcpy(psta->hwaddr, hwaddr, ETH_ALEN);
+
+		index = wifi_mac_hash(hwaddr);
+
+		RT_TRACE(_module_rtl871x_sta_mgt_c_,_drv_info_,("rtw_alloc_stainfo: index  = %x", index));
+
+		if(index >= NUM_STA){
+			RT_TRACE(_module_rtl871x_sta_mgt_c_,_drv_err_,("ERROR=> rtw_alloc_stainfo: index >= NUM_STA"));
+			psta= NULL;	
+			goto exit;
+		}
+		phash_list = &(pstapriv->sta_hash[index]);
+
+		//_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+
+		rtw_list_insert_tail(&psta->hash_list, phash_list);
+
+		pstapriv->asoc_sta_count ++ ;
+
+		//_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+
+// Commented by Albert 2009/08/13
+// For the SMC router, the sequence number of first packet of WPS handshake will be 0.
+// In this case, this packet will be dropped by recv_decache function if we use the 0x00 as the default value for tid_rxseq variable.
+// So, we initialize the tid_rxseq variable as the 0xffff.
+
+		for( i = 0; i < 16; i++ )
+		{
+                     _rtw_memcpy( &psta->sta_recvpriv.rxcache.tid_rxseq[ i ], &wRxSeqInitialValue, 2 );
+		}
+
+		RT_TRACE(_module_rtl871x_sta_mgt_c_,_drv_info_,("alloc number_%d stainfo  with hwaddr = %x %x %x %x %x %x  \n", 
+		pstapriv->asoc_sta_count , hwaddr[0], hwaddr[1], hwaddr[2],hwaddr[3],hwaddr[4],hwaddr[5]));
+
+		init_addba_retry_timer(pstapriv->padapter, psta);
+
+#ifdef CONFIG_TDLS
+		psta->padapter = pstapriv->padapter;
+		init_TPK_timer(pstapriv->padapter, psta);
+		init_ch_switch_timer(pstapriv->padapter, psta);
+		init_base_ch_timer(pstapriv->padapter, psta);
+		init_off_ch_timer(pstapriv->padapter, psta);
+		init_handshake_timer(pstapriv->padapter, psta);
+		init_tdls_alive_timer(pstapriv->padapter, psta);
+#endif //CONFIG_TDLS
+
+		//for A-MPDU Rx reordering buffer control
+		for(i=0; i < 16 ; i++)
+		{
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+			preorder_ctrl->padapter = pstapriv->padapter;
+		
+			preorder_ctrl->enable = _FALSE;
+		
+			preorder_ctrl->indicate_seq = 0xffff;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d\n", __FUNCTION__, __LINE__,
+				preorder_ctrl->indicate_seq);
+			#endif
+			preorder_ctrl->wend_b= 0xffff;       
+			//preorder_ctrl->wsize_b = (NR_RECVBUFF-2);
+			preorder_ctrl->wsize_b = 64;//64;
+
+			_rtw_init_queue(&preorder_ctrl->pending_recvframe_queue);
+
+			rtw_init_recv_timer(preorder_ctrl);
+		}
+
+
+		//init for DM
+		psta->rssi_stat.UndecoratedSmoothedPWDB = (-1);
+		psta->rssi_stat.UndecoratedSmoothedCCK = (-1);
+		
+		/* init for the sequence number of received management frame */
+		psta->RxMgmtFrameSeqNum = 0xffff;
+	}
+	
+exit:
+
+	_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+
+_func_exit_;
+
+	return psta;
+
+
+}
+
+
+// using pstapriv->sta_hash_lock to protect
+u32	rtw_free_stainfo(_adapter *padapter , struct sta_info *psta)
+{	
+	int i;
+	_irqL irqL0;
+	_queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct	sta_xmit_priv	*pstaxmitpriv;
+	struct	xmit_priv	*pxmitpriv= &padapter->xmitpriv;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct hw_xmit *phwxmit;
+
+
+_func_enter_;	
+	
+	if (psta == NULL)
+		goto exit;
+
+
+	_enter_critical_bh(&psta->lock, &irqL0);
+	psta->state &= ~_FW_LINKED;
+	_exit_critical_bh(&psta->lock, &irqL0);
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+
+
+	pstaxmitpriv = &psta->sta_xmitpriv;
+	
+	//rtw_list_delete(&psta->sleep_list);
+	
+	//rtw_list_delete(&psta->wakeup_list);
+	
+	_enter_critical_bh(&pxmitpriv->lock, &irqL0);
+	
+	rtw_free_xmitframe_queue(pxmitpriv, &psta->sleep_q);
+	psta->sleepq_len = 0;
+	
+	//vo
+	//_enter_critical_bh(&(pxmitpriv->vo_pending.lock), &irqL0);
+	rtw_free_xmitframe_queue( pxmitpriv, &pstaxmitpriv->vo_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->vo_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits;
+	phwxmit->accnt -= pstaxmitpriv->vo_q.qcnt;
+	pstaxmitpriv->vo_q.qcnt = 0;
+	//_exit_critical_bh(&(pxmitpriv->vo_pending.lock), &irqL0);
+
+	//vi
+	//_enter_critical_bh(&(pxmitpriv->vi_pending.lock), &irqL0);
+	rtw_free_xmitframe_queue( pxmitpriv, &pstaxmitpriv->vi_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->vi_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits+1;
+	phwxmit->accnt -= pstaxmitpriv->vi_q.qcnt;
+	pstaxmitpriv->vi_q.qcnt = 0;
+	//_exit_critical_bh(&(pxmitpriv->vi_pending.lock), &irqL0);
+
+	//be
+	//_enter_critical_bh(&(pxmitpriv->be_pending.lock), &irqL0);
+	rtw_free_xmitframe_queue( pxmitpriv, &pstaxmitpriv->be_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->be_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits+2;
+	phwxmit->accnt -= pstaxmitpriv->be_q.qcnt;
+	pstaxmitpriv->be_q.qcnt = 0;
+	//_exit_critical_bh(&(pxmitpriv->be_pending.lock), &irqL0);
+	
+	//bk
+	//_enter_critical_bh(&(pxmitpriv->bk_pending.lock), &irqL0);
+	rtw_free_xmitframe_queue( pxmitpriv, &pstaxmitpriv->bk_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->bk_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits+3;
+	phwxmit->accnt -= pstaxmitpriv->bk_q.qcnt;
+	pstaxmitpriv->bk_q.qcnt = 0;
+	//_exit_critical_bh(&(pxmitpriv->bk_pending.lock), &irqL0);
+	
+	_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+	
+	rtw_list_delete(&psta->hash_list);
+	RT_TRACE(_module_rtl871x_sta_mgt_c_,_drv_err_,("\n free number_%d stainfo  with hwaddr = 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x  \n",pstapriv->asoc_sta_count , psta->hwaddr[0], psta->hwaddr[1], psta->hwaddr[2],psta->hwaddr[3],psta->hwaddr[4],psta->hwaddr[5]));
+	pstapriv->asoc_sta_count --;
+	
+	
+	// re-init sta_info; 20061114 // will be init in alloc_stainfo
+	//_rtw_init_sta_xmit_priv(&psta->sta_xmitpriv);
+	//_rtw_init_sta_recv_priv(&psta->sta_recvpriv);
+
+	_cancel_timer_ex(&psta->addba_retry_timer);
+
+#ifdef CONFIG_TDLS
+	_cancel_timer_ex(&psta->TPK_timer);
+	_cancel_timer_ex(&psta->option_timer);
+	_cancel_timer_ex(&psta->base_ch_timer);
+	_cancel_timer_ex(&psta->off_ch_timer);
+	_cancel_timer_ex(&psta->alive_timer1);
+	_cancel_timer_ex(&psta->alive_timer2);
+#endif //CONFIG_TDLS
+
+	//for A-MPDU Rx reordering buffer control, cancel reordering_ctrl_timer
+	for(i=0; i < 16 ; i++)
+	{
+		_irqL irqL;
+		_list	*phead, *plist;
+		union recv_frame *prframe;
+		_queue *ppending_recvframe_queue;
+		_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+	
+		preorder_ctrl = &psta->recvreorder_ctrl[i];
+		
+		_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);		
+
+		
+		ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+		_enter_critical_bh(&ppending_recvframe_queue->lock, &irqL);
+
+		phead = 	get_list_head(ppending_recvframe_queue);
+		plist = get_next(phead);
+		
+		while(!rtw_is_list_empty(phead))
+		{	
+			prframe = LIST_CONTAINOR(plist, union recv_frame, u);
+			
+			plist = get_next(plist);
+			
+			rtw_list_delete(&(prframe->u.hdr.list));
+
+			rtw_free_recvframe(prframe, pfree_recv_queue);
+		}
+
+		_exit_critical_bh(&ppending_recvframe_queue->lock, &irqL);
+		
+	}
+
+	if (!(psta->state & WIFI_AP_STATE))
+		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, _FALSE);
+			
+#ifdef CONFIG_AP_MODE
+
+/*
+	_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL0);
+	rtw_list_delete(&psta->asoc_list);	
+	_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL0);
+*/
+	_enter_critical_bh(&pstapriv->auth_list_lock, &irqL0);
+	if (!rtw_is_list_empty(&psta->auth_list)) {
+		rtw_list_delete(&psta->auth_list);
+		pstapriv->auth_list_cnt--;
+	}
+	_exit_critical_bh(&pstapriv->auth_list_lock, &irqL0);
+	
+	psta->expire_to = 0;
+	
+	psta->sleepq_ac_len = 0;
+	psta->qos_info = 0;
+
+	psta->max_sp_len = 0;
+	psta->uapsd_bk = 0;
+	psta->uapsd_be = 0;
+	psta->uapsd_vi = 0;
+	psta->uapsd_vo = 0;
+
+	psta->has_legacy_ac = 0;
+
+#ifdef CONFIG_NATIVEAP_MLME
+	
+	pstapriv->sta_dz_bitmap &=~BIT(psta->aid);
+	pstapriv->tim_bitmap &=~BIT(psta->aid);	
+
+	//rtw_indicate_sta_disassoc_event(padapter, psta);
+
+	if ((psta->aid >0)&&(pstapriv->sta_aid[psta->aid - 1] == psta))
+	{
+		pstapriv->sta_aid[psta->aid - 1] = NULL;
+		psta->aid = 0;
+	}	
+	
+#endif	// CONFIG_NATIVEAP_MLME	
+
+#ifdef CONFIG_TX_MCAST2UNI
+	psta->under_exist_checking = 0;
+#endif	// CONFIG_TX_MCAST2UNI
+
+#endif	// CONFIG_AP_MODE	
+
+	 _rtw_spinlock_free(&psta->lock);
+
+	//_enter_critical_bh(&(pfree_sta_queue->lock), &irqL0);
+	rtw_list_insert_tail(&psta->list, get_list_head(pfree_sta_queue));
+	//_exit_critical_bh(&(pfree_sta_queue->lock), &irqL0);
+
+exit:
+	
+_func_exit_;	
+
+	return _SUCCESS;
+	
+}
+
+// free all stainfo which in sta_hash[all]
+void rtw_free_all_stainfo(_adapter *padapter)
+{
+	_irqL	 irqL;
+	_list	*plist, *phead;
+	s32	index;
+	struct sta_info *psta = NULL;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info* pbcmc_stainfo =rtw_get_bcmc_stainfo( padapter);
+	
+_func_enter_;	
+
+	if(pstapriv->asoc_sta_count==1)
+		goto exit;
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	for(index=0; index< NUM_STA; index++)
+	{
+		phead = &(pstapriv->sta_hash[index]);
+		plist = get_next(phead);
+		
+		while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
+		{
+			psta = LIST_CONTAINOR(plist, struct sta_info ,hash_list);
+
+			plist = get_next(plist);
+
+			if(pbcmc_stainfo!=psta)					
+				rtw_free_stainfo(padapter , psta);
+			
+		}
+	}
+	
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+	
+exit:	
+	
+_func_exit_;	
+
+}
+
+/* any station allocated can be searched by hash list */
+struct sta_info *rtw_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
+{
+
+	_irqL	 irqL;
+
+	_list	*plist, *phead;
+
+	struct sta_info *psta = NULL;
+	
+	u32	index;
+
+	u8 *addr;
+
+	u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
+
+_func_enter_;
+
+	if(hwaddr==NULL)
+		return NULL;
+		
+	if(IS_MCAST(hwaddr))
+	{
+		addr = bc_addr;
+	}
+	else
+	{
+		addr = hwaddr;
+	}
+
+	index = wifi_mac_hash(addr);
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+	
+	phead = &(pstapriv->sta_hash[index]);
+	plist = get_next(phead);
+
+
+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
+	{
+	
+		psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+		
+		if ((_rtw_memcmp(psta->hwaddr, addr, ETH_ALEN))== _TRUE) 
+		{ // if found the matched address
+			break;
+		}
+		psta=NULL;
+		plist = get_next(plist);
+	}
+
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+_func_exit_;	
+	return psta;
+	
+}
+
+u32 rtw_init_bcmc_stainfo(_adapter* padapter)
+{
+
+	struct sta_info 	*psta;
+	struct tx_servq	*ptxservq;
+	u32 res=_SUCCESS;
+	NDIS_802_11_MAC_ADDRESS	bcast_addr= {0xff,0xff,0xff,0xff,0xff,0xff};
+	
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	//_queue	*pstapending = &padapter->xmitpriv.bm_pending; 
+	
+_func_enter_;
+
+	psta = rtw_alloc_stainfo(pstapriv, bcast_addr);
+	
+	if(psta==NULL){
+		res=_FAIL;
+		RT_TRACE(_module_rtl871x_sta_mgt_c_,_drv_err_,("rtw_alloc_stainfo fail"));
+		goto exit;
+	}
+
+	// default broadcast & multicast use macid 1
+	psta->mac_id = 1;
+
+	ptxservq= &(psta->sta_xmitpriv.be_q);
+
+/*
+	_enter_critical(&pstapending->lock, &irqL0);
+
+	if (rtw_is_list_empty(&ptxservq->tx_pending))
+		rtw_list_insert_tail(&ptxservq->tx_pending, get_list_head(pstapending));
+
+	_exit_critical(&pstapending->lock, &irqL0);
+*/
+	
+exit:
+_func_exit_;		
+	return _SUCCESS;
+
+}
+
+
+struct sta_info* rtw_get_bcmc_stainfo(_adapter* padapter)
+{
+	struct sta_info 	*psta;
+	struct sta_priv 	*pstapriv = &padapter->stapriv;
+	u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
+_func_enter_;
+	 psta = rtw_get_stainfo(pstapriv, bc_addr);
+_func_exit_;		 
+	return psta;
+
+}
+
+u8 rtw_access_ctrl(_adapter *padapter, u8 *mac_addr)
+{
+	u8 res = _TRUE;
+#ifdef  CONFIG_AP_MODE
+	_irqL irqL;
+	_list	*plist, *phead;
+	struct rtw_wlan_acl_node *paclnode;
+	u8 match = _FALSE;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+	_queue	*pacl_node_q =&pacl_list->acl_node_q;
+	
+	_enter_critical_bh(&(pacl_node_q->lock), &irqL);
+	phead = get_list_head(pacl_node_q);
+	plist = get_next(phead);		
+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
+	{
+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);
+		plist = get_next(plist);
+
+		if(_rtw_memcmp(paclnode->addr, mac_addr, ETH_ALEN))
+		{
+			if(paclnode->valid == _TRUE)
+			{
+				match = _TRUE;
+				break;
+			}
+		}		
+	}	
+	_exit_critical_bh(&(pacl_node_q->lock), &irqL);
+	
+
+	if(pacl_list->mode == 1)//accept unless in deny list
+	{
+		res = (match == _TRUE) ?  _FALSE:_TRUE;
+	}	
+	else if(pacl_list->mode == 2)//deny unless in accept list
+	{
+		res = (match == _TRUE) ?  _TRUE:_FALSE;
+	}
+	else
+	{
+		 res = _TRUE;
+	}		
+	
+#endif
+
+	return res;
+
+}
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_tdls.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_tdls.c
new file mode 100755
index 00000000..a73d761e
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_tdls.c
@@ -0,0 +1,2941 @@
+/******************************************************************************

+ *

+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.

+ *                                        

+ * This program is free software; you can redistribute it and/or modify it

+ * under the terms of version 2 of the GNU General Public License as

+ * published by the Free Software Foundation.

+ *

+ * 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.,

+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA

+ *

+ *

+ ******************************************************************************/

+#define _RTW_TDLS_C_

+

+#include <drv_types.h>

+#include <ethernet.h>

+#include <rtw_tdls.h>

+#include <wifi.h>

+

+#ifdef CONFIG_TDLS

+extern unsigned char MCS_rate_2R[16];

+extern unsigned char MCS_rate_1R[16];

+extern void process_wmmps_data(_adapter *padapter, union recv_frame *precv_frame);

+extern s32 rtw_dump_xframe(_adapter *padapter, struct xmit_frame *pxmitframe);

+

+void rtw_reset_tdls_info(_adapter* padapter)

+{

+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;

+

+	ptdlsinfo->ap_prohibited = _FALSE;

+	ptdlsinfo->setup_state = TDLS_STATE_NONE;

+	ptdlsinfo->sta_cnt = 0;

+	ptdlsinfo->sta_maximum = _FALSE;

+	ptdlsinfo->macid_index= 6;

+	ptdlsinfo->clear_cam= 0;

+	ptdlsinfo->ch_sensing = 0;

+	ptdlsinfo->cur_channel = 0;

+	ptdlsinfo->candidate_ch = 1;	//when inplement channel switching, default candidate channel is 1

+	ptdlsinfo->watchdog_count = 0;

+	ptdlsinfo->dev_discovered = 0;

+

+#ifdef CONFIG_WFD

+	ptdlsinfo->wfd_info = &padapter->wfd_info;

+#endif //CONFIG_WFD

+}

+

+int rtw_init_tdls_info(_adapter* padapter)

+{

+	int	res = _SUCCESS;

+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;

+

+	ptdlsinfo->enable = 1;

+	rtw_reset_tdls_info(padapter);

+

+	_rtw_spinlock_init(&ptdlsinfo->cmd_lock);

+	_rtw_spinlock_init(&ptdlsinfo->hdl_lock);

+

+	return res;

+

+}

+

+void rtw_free_tdls_info(struct tdls_info *ptdlsinfo)

+{

+	_rtw_spinlock_free(&ptdlsinfo->cmd_lock);

+	_rtw_spinlock_free(&ptdlsinfo->hdl_lock);

+

+	_rtw_memset(ptdlsinfo, 0, sizeof(struct tdls_info) );

+

+}

+

+void issue_nulldata_to_TDLS_peer_STA(_adapter *padapter, struct sta_info *ptdls_sta, unsigned int power_mode)

+{

+	struct xmit_frame			*pmgntframe;

+	struct pkt_attrib			*pattrib;

+	unsigned char					*pframe;

+	struct rtw_ieee80211_hdr	*pwlanhdr;

+	unsigned short				*fctrl;

+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);

+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+	update_mgntframe_attrib(padapter, pattrib);

+

+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);

+

+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;

+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;

+

+	fctrl = &(pwlanhdr->frame_ctl);

+	*(fctrl) = 0;

+//	SetToDs(fctrl);

+	if (power_mode)

+	{

+		SetPwrMgt(fctrl);

+	}

+

+	_rtw_memcpy(pwlanhdr->addr1, ptdls_sta->hwaddr, ETH_ALEN);

+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);

+	_rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);

+

+	ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority]++;

+	ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;

+	pattrib->seqnum = ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority];

+	SetSeqNum(pwlanhdr, pattrib->seqnum);

+

+	SetFrameSubType(pframe, WIFI_DATA_NULL);

+

+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);

+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);

+

+	pattrib->last_txcmdsz = pattrib->pktlen;

+	dump_mgntframe(padapter, pmgntframe);

+

+	return;

+}

+

+s32 update_tdls_attrib(_adapter *padapter, struct pkt_attrib *pattrib)

+{

+

+	struct sta_info *psta = NULL;

+	struct sta_priv		*pstapriv = &padapter->stapriv;

+	struct security_priv	*psecuritypriv = &padapter->securitypriv;

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	struct qos_priv		*pqospriv= &pmlmepriv->qospriv;

+

+	s32 res=_SUCCESS;

+	sint bmcast;

+

+	bmcast = IS_MCAST(pattrib->ra);

+	

+	psta = rtw_get_stainfo(pstapriv, pattrib->ra);

+	if (psta == NULL)	{ 

+		res =_FAIL;

+		goto exit;

+	}

+

+	pattrib->mac_id = psta->mac_id;

+				

+	pattrib->psta = psta;

+

+	pattrib->ack_policy = 0;

+	// get ether_hdr_len

+	pattrib->pkt_hdrlen = ETH_HLEN;//(pattrib->ether_type == 0x8100) ? (14 + 4 ): 14; //vlan tag

+

+	if (pqospriv->qos_option &&  psta->qos_option) {

+		pattrib->priority = 1;	//tdls management frame should be AC_BK

+		pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN;

+		pattrib->subtype = WIFI_QOS_DATA_TYPE;

+	} else {

+		pattrib->hdrlen = WLAN_HDR_A3_LEN;

+		pattrib->subtype = WIFI_DATA_TYPE;	

+		pattrib->priority = 0;

+	}

+

+	if (psta->ieee8021x_blocked == _TRUE)

+	{

+		pattrib->encrypt = 0;

+	}

+	else

+	{

+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast);

+

+		switch(psecuritypriv->dot11AuthAlgrthm)

+		{

+			case dot11AuthAlgrthm_Open:

+			case dot11AuthAlgrthm_Shared:

+			case dot11AuthAlgrthm_Auto:				

+				pattrib->key_idx = (u8)psecuritypriv->dot11PrivacyKeyIndex;

+				break;

+			case dot11AuthAlgrthm_8021X:

+				pattrib->key_idx = 0;

+				break;

+			default:

+				pattrib->key_idx = 0;

+				break;

+		}

+	}

+

+	switch (pattrib->encrypt)

+	{

+		case _WEP40_:

+		case _WEP104_:

+			pattrib->iv_len = 4;

+			pattrib->icv_len = 4;

+			break;

+		case _TKIP_:

+			pattrib->iv_len = 8;

+			pattrib->icv_len = 4;

+			if(padapter->securitypriv.busetkipkey==_FAIL)

+			{

+				res =_FAIL;

+				goto exit;

+			}

+			break;			

+		case _AES_:

+			pattrib->iv_len = 8;

+			pattrib->icv_len = 8;

+			break;

+		default:

+			pattrib->iv_len = 0;

+			pattrib->icv_len = 0;

+			break;

+	}

+

+	if (pattrib->encrypt &&

+	    ((padapter->securitypriv.sw_encrypt == _TRUE) || (psecuritypriv->hw_decrypted == _FALSE)))

+	{

+		pattrib->bswenc = _TRUE;

+	} else {

+		pattrib->bswenc = _FALSE;

+	}

+

+	//qos_en, ht_en, init rate, ,bw, ch_offset, sgi

+	pattrib->qos_en = psta->qos_option;

+	pattrib->ht_en = psta->htpriv.ht_option;

+	pattrib->raid = psta->raid;

+	pattrib->bwmode = psta->htpriv.bwmode;

+	pattrib->ch_offset = psta->htpriv.ch_offset;

+	pattrib->sgi= psta->htpriv.sgi;

+	pattrib->ampdu_en = _FALSE;

+	

+	//if(pattrib->ht_en && psta->htpriv.ampdu_enable)

+	//{

+	//	if(psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority))

+	//		pattrib->ampdu_en = _TRUE;

+	//}	

+

+exit:

+

+	return res;

+}

+

+void free_tdls_sta(_adapter *padapter, struct sta_info *ptdls_sta)

+{

+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;

+   	struct sta_priv *pstapriv = &padapter->stapriv;

+	_irqL irqL;

+	

+	//free peer sta_info

+	_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);

+	if(ptdlsinfo->sta_cnt != 0)

+		ptdlsinfo->sta_cnt--;

+	_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);

+	if( ptdlsinfo->sta_cnt < (NUM_STA - 2) )	// -2: AP + BC/MC sta

+	{

+		ptdlsinfo->sta_maximum = _FALSE;

+		_rtw_memset( &ptdlsinfo->ss_record, 0x00, sizeof(struct tdls_ss_record) );

+	}

+	//ready to clear cam

+	if(ptdls_sta->mac_id!=0){

+		ptdlsinfo->clear_cam=ptdls_sta->mac_id;

+		rtw_setstakey_cmd(padapter, (u8 *)ptdls_sta, _TRUE);

+	}

+

+	if(ptdlsinfo->sta_cnt==0){

+		rtw_tdls_cmd(padapter, myid(&(padapter->eeprompriv)), TDLS_RS_RCR);

+		ptdlsinfo->setup_state=TDLS_STATE_NONE;

+	}

+	else

+		DBG_871X("Remain tdls sta:%02x\n", ptdlsinfo->sta_cnt);

+

+	rtw_free_stainfo(padapter,  ptdls_sta);

+	

+}

+

+// cam entry will be the same as mac_id

+void rtw_tdls_set_mac_id(struct tdls_info *ptdlsinfo, struct sta_info *ptdls_sta)

+{

+	if(ptdls_sta->mac_id==0)

+	{

+		ptdls_sta->mac_id = ptdlsinfo->macid_index;

+		if( (++ptdlsinfo->macid_index) > (NUM_STA -2) )

+			ptdlsinfo->macid_index= TDLS_INI_MACID_ENTRY;

+	}

+}

+

+//TDLS encryption(if needed) will always be CCMP

+void rtw_tdls_set_key(_adapter *adapter, struct rx_pkt_attrib *prx_pkt_attrib, struct sta_info *ptdls_sta)

+{

+	if(prx_pkt_attrib->encrypt)

+	{

+		ptdls_sta->dot118021XPrivacy=_AES_;

+		rtw_setstakey_cmd(adapter, (u8*)ptdls_sta, _TRUE);

+	}

+}

+

+void rtw_tdls_process_ht_cap(_adapter *adapter, struct sta_info *ptdls_sta, u8 *data, u8 Length)

+{

+	/* save HT capabilities in the sta object */

+	_rtw_memset(&ptdls_sta->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));

+	if (data && Length >= sizeof(struct rtw_ieee80211_ht_cap) )

+	{

+		ptdls_sta->flags |= WLAN_STA_HT;

+		

+		ptdls_sta->flags |= WLAN_STA_WME;

+		

+		_rtw_memcpy(&ptdls_sta->htpriv.ht_cap, data, sizeof(struct rtw_ieee80211_ht_cap));			

+		

+	} else

+		ptdls_sta->flags &= ~WLAN_STA_HT;

+

+	if(ptdls_sta->flags & WLAN_STA_HT)

+	{

+		if(adapter->registrypriv.ht_enable == _TRUE)

+		{

+			ptdls_sta->htpriv.ht_option = _TRUE;

+		}

+		else

+		{

+			ptdls_sta->htpriv.ht_option = _FALSE;

+			ptdls_sta->stat_code = _STATS_FAILURE_;

+		}

+	}

+

+	//HT related cap

+	if(ptdls_sta->htpriv.ht_option)

+	{

+		//check if sta supports rx ampdu

+		if(adapter->registrypriv.ampdu_enable==1)

+			ptdls_sta->htpriv.ampdu_enable = _TRUE;

+

+		//check if sta support s Short GI

+		if(ptdls_sta->htpriv.ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SGI_20|IEEE80211_HT_CAP_SGI_40))

+		{

+			ptdls_sta->htpriv.sgi = _TRUE;

+		}

+

+		// bwmode would still followed AP's setting

+		if(ptdls_sta->htpriv.ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH))

+		{

+			ptdls_sta->htpriv.bwmode = adapter->mlmeextpriv.cur_bwmode;

+			ptdls_sta->htpriv.ch_offset = adapter->mlmeextpriv.cur_ch_offset;

+		}

+	}

+}

+

+u8 *rtw_tdls_set_ht_cap(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)

+{

+	struct rtw_ieee80211_ht_cap ht_capie;

+	u8 rf_type;

+

+	//HT capabilities

+	_rtw_memset(&ht_capie, 0, sizeof(struct rtw_ieee80211_ht_cap));

+

+	ht_capie.cap_info = IEEE80211_HT_CAP_SUP_WIDTH |IEEE80211_HT_CAP_SGI_20 |IEEE80211_HT_CAP_SM_PS |

+						IEEE80211_HT_CAP_SGI_40 | IEEE80211_HT_CAP_TX_STBC |IEEE80211_HT_CAP_DSSSCCK40;

+

+	{

+		u32 rx_packet_offset, max_recvbuf_sz;

+		padapter->HalFunc.GetHalDefVarHandler(padapter, HAL_DEF_RX_PACKET_OFFSET, &rx_packet_offset);

+		padapter->HalFunc.GetHalDefVarHandler(padapter, HAL_DEF_MAX_RECVBUF_SZ, &max_recvbuf_sz);

+		if(max_recvbuf_sz-rx_packet_offset>(8191-256))

+			ht_capie.cap_info = ht_capie.cap_info |IEEE80211_HT_CAP_MAX_AMSDU;

+	}

+	

+	ht_capie.ampdu_params_info = (IEEE80211_HT_CAP_AMPDU_FACTOR&0x03);

+

+	padapter->HalFunc.GetHwRegHandler(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));

+	switch(rf_type)

+	{

+		case RF_1T1R:

+			ht_capie.cap_info |= 0x0100;//RX STBC One spatial stream

+			_rtw_memcpy(ht_capie.supp_mcs_set, MCS_rate_1R, 16);

+			break;

+

+		case RF_2T2R:

+		case RF_1T2R:

+		default:

+			ht_capie.cap_info|= 0x0200;//RX STBC two spatial stream

+			_rtw_memcpy(ht_capie.supp_mcs_set, MCS_rate_2R, 16);

+			break;

+	}

+			

+	return(rtw_set_ie(pframe, _HT_CAPABILITY_IE_, 

+						sizeof(struct rtw_ieee80211_ht_cap), (unsigned char*)&ht_capie, &(pattrib->pktlen)));

+}

+

+u8 *rtw_tdls_set_sup_ch(struct mlme_ext_priv *pmlmeext, u8 *pframe, struct pkt_attrib *pattrib)

+{

+	u8 sup_ch[ 30 * 2 ] = { 0x00 }, sup_ch_idx = 0, idx_5g = 2;	//For supported channel

+	do{

+		if( pmlmeext->channel_set[sup_ch_idx].ChannelNum <= 14 )

+		{

+			sup_ch[0] = 1;	//First channel number

+			sup_ch[1] = pmlmeext->channel_set[sup_ch_idx].ChannelNum;	//Number of channel

+		}

+		else

+		{

+			sup_ch[idx_5g++] = pmlmeext->channel_set[sup_ch_idx].ChannelNum;

+			sup_ch[idx_5g++] = 1;

+		}

+		

+		sup_ch_idx++;

+	}

+	while( pmlmeext->channel_set[sup_ch_idx].ChannelNum != 0 );

+	return(rtw_set_ie(pframe, _SUPPORTED_CH_IE_, idx_5g, sup_ch, &(pattrib->pktlen)));

+}

+

+#ifdef CONFIG_WFD

+void rtw_tdls_process_wfd_ie(struct tdls_info *ptdlsinfo, u8 *ptr, u8 length)

+{

+	u8	wfd_ie[ 128 ] = { 0x00 };

+	u32	wfd_ielen = 0;

+	u32	wfd_offset = 0;

+	//	Try to get the TCP port information when receiving the negotiation response.

+	//

+

+	wfd_offset = 0;

+	wfd_offset = rtw_get_wfd_ie( ptr + wfd_offset, length - wfd_offset, wfd_ie, &wfd_ielen );

+	while( wfd_offset )

+	{

+		u8	attr_content[ 10 ] = { 0x00 };

+		u32	attr_contentlen = 0;

+		int	i;

+

+		DBG_871X( "[%s] WFD IE Found!!\n", __FUNCTION__ );

+		rtw_get_wfd_attr_content( wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);

+		if ( attr_contentlen )

+		{

+			ptdlsinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16( attr_content + 2 );

+			DBG_871X( "[%s] Peer PORT NUM = %d\n", __FUNCTION__, ptdlsinfo->wfd_info->peer_rtsp_ctrlport );

+		}

+

+		_rtw_memset( attr_content, 0x00, 10);

+		attr_contentlen = 0;

+		rtw_get_wfd_attr_content( wfd_ie, wfd_ielen, WFD_ATTR_LOCAL_IP_ADDR, attr_content, &attr_contentlen);

+		if ( attr_contentlen )

+		{

+			_rtw_memcpy(ptdlsinfo->wfd_info->peer_ip_address, ( attr_content + 1 ), 4);

+			DBG_871X( "[%s] Peer IP = %02u.%02u.%02u.%02u \n", __FUNCTION__, 

+				ptdlsinfo->wfd_info->peer_ip_address[0], ptdlsinfo->wfd_info->peer_ip_address[1],

+				ptdlsinfo->wfd_info->peer_ip_address[2], ptdlsinfo->wfd_info->peer_ip_address[3]

+				);

+		}

+		wfd_offset = rtw_get_wfd_ie( ptr + wfd_offset, length - wfd_offset, wfd_ie, &wfd_ielen );

+	}

+}

+

+void issue_tunneled_probe_req(_adapter *padapter)

+{

+	struct xmit_frame			*pmgntframe;

+	struct pkt_attrib			*pattrib;

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);

+	u8 baddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; 

+

+	DBG_871X("[%s]\n", __FUNCTION__);

+	

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+	

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+

+	pmgntframe->frame_tag = DATA_FRAMETAG;

+	pattrib->ether_type = 0x890d;

+	pattrib->pctrl =0;

+

+	_rtw_memcpy(pattrib->dst, baddr, ETH_ALEN);

+	

+	_rtw_memcpy(pattrib->src, myid(&(padapter->eeprompriv)), ETH_ALEN);

+

+	_rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);

+	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);

+

+	update_tdls_attrib(padapter, pattrib);

+	pattrib->qsel=pattrib->priority;

+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, TUNNELED_PROBE_REQ) != _SUCCESS) {

+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);

+		rtw_free_xmitframe(pxmitpriv, pmgntframe);

+		goto exit;

+	}

+	rtw_dump_xframe(padapter, pmgntframe);

+

+exit:

+

+	return;

+}

+

+void issue_tunneled_probe_rsp(_adapter *padapter, union recv_frame *precv_frame)

+{

+	struct xmit_frame			*pmgntframe;

+	struct pkt_attrib			*pattrib;

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);

+	struct rx_pkt_attrib	*rx_pkt_pattrib = &precv_frame->u.hdr.attrib;

+

+	DBG_871X("[%s]\n", __FUNCTION__);

+	

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+	

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+

+	pmgntframe->frame_tag = DATA_FRAMETAG;

+	pattrib->ether_type = 0x890d;

+	pattrib->pctrl =0;

+

+	_rtw_memcpy(pattrib->dst, rx_pkt_pattrib->src, ETH_ALEN);

+	

+	_rtw_memcpy(pattrib->src, myid(&(padapter->eeprompriv)), ETH_ALEN);

+

+	_rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);

+	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);

+

+	update_tdls_attrib(padapter, pattrib);

+	pattrib->qsel=pattrib->priority;

+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, TUNNELED_PROBE_RSP) != _SUCCESS) {

+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);

+		rtw_free_xmitframe(pxmitpriv, pmgntframe);

+		goto exit;

+	}

+	rtw_dump_xframe(padapter, pmgntframe);

+

+exit:

+

+	return;

+}

+#endif //CONFIG_WFD

+

+void issue_tdls_setup_req(_adapter *padapter, u8 *mac_addr)

+{

+	struct tdls_info	*ptdlsinfo = &padapter->tdlsinfo;

+	struct xmit_frame			*pmgntframe;

+	struct pkt_attrib			*pattrib;

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);

+   	struct sta_priv *pstapriv = &padapter->stapriv;

+	struct sta_info *ptdls_sta= NULL;

+	_irqL irqL;

+	static u8 dialogtoken = 0;

+	u32 timeout_interval= TPK_RESEND_COUNT * 1000;	//retry timer should set at least 301 sec, using TPK_count counting 301 times.

+

+	if(ptdlsinfo->ap_prohibited == _TRUE)

+		goto exit;

+

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+	

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+

+	pmgntframe->frame_tag = DATA_FRAMETAG;

+	pattrib->ether_type = 0x890d;

+	pattrib->pctrl =0;		

+

+	_rtw_memcpy(pattrib->dst, mac_addr, ETH_ALEN);

+	_rtw_memcpy(pattrib->src, myid(&(padapter->eeprompriv)), ETH_ALEN);

+

+	_rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);

+	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);

+

+	update_tdls_attrib(padapter, pattrib);

+

+	//init peer sta_info

+	ptdls_sta = rtw_get_stainfo(pstapriv, mac_addr);

+	if(ptdls_sta==NULL)

+	{

+		ptdls_sta = rtw_alloc_stainfo(pstapriv, mac_addr);

+		if(ptdls_sta)

+		{

+			_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);	

+			if(!(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE))

+				ptdlsinfo->sta_cnt++;

+			_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);	

+			if( ptdlsinfo->sta_cnt == (NUM_STA - 2) )	// -2: AP + BC/MC sta

+			{

+				ptdlsinfo->sta_maximum  = _TRUE;

+			}

+		}

+		else

+		{

+			rtw_free_xmitbuf(pxmitpriv,pmgntframe->pxmitbuf);

+			rtw_free_xmitframe(pxmitpriv, pmgntframe);

+			goto exit;

+		}

+	}

+	

+	if(ptdls_sta){	

+		ptdls_sta->tdls_sta_state |= TDLS_RESPONDER_STATE;

+		//for tdls; ptdls_sta->aid is used to fill dialogtoken

+		ptdls_sta->dialog = dialogtoken;

+		dialogtoken = (dialogtoken+1)%256;

+		ptdls_sta->TDLS_PeerKey_Lifetime = timeout_interval;

+		_set_timer( &ptdls_sta->handshake_timer, TDLS_HANDSHAKE_TIME );

+	}

+

+	pattrib->qsel=pattrib->priority;

+	if(rtw_xmit_tdls_coalesce(padapter, pmgntframe, TDLS_SETUP_REQUEST) !=_SUCCESS ){

+		rtw_free_xmitbuf(pxmitpriv,pmgntframe->pxmitbuf);

+		rtw_free_xmitframe(pxmitpriv, pmgntframe);

+		goto exit;

+	}

+	rtw_dump_xframe(padapter, pmgntframe);

+

+exit:

+

+	return;

+}

+

+void issue_tdls_teardown(_adapter *padapter, u8 *mac_addr)

+{

+	struct xmit_frame			*pmgntframe;

+	struct pkt_attrib			*pattrib;

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);

+	struct sta_priv *pstapriv = &padapter->stapriv;

+	struct sta_info	*ptdls_sta=NULL;

+	_irqL irqL;

+

+	ptdls_sta = rtw_get_stainfo(pstapriv, mac_addr);

+	if(ptdls_sta==NULL){

+		DBG_871X("issue tdls teardown unsuccessful\n");

+		return;

+	}else{

+		ptdls_sta->tdls_sta_state=TDLS_STATE_NONE;

+	}

+

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+	

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+

+	pmgntframe->frame_tag = DATA_FRAMETAG;

+	pattrib->ether_type = 0x890d;

+	pattrib->pctrl =0;

+

+	_rtw_memcpy(pattrib->dst, mac_addr, ETH_ALEN);

+	_rtw_memcpy(pattrib->src, myid(&(padapter->eeprompriv)), ETH_ALEN);

+

+	_rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);

+	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);

+

+	update_tdls_attrib(padapter, pattrib);

+	pattrib->qsel=pattrib->priority;

+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, TDLS_TEARDOWN) != _SUCCESS) {

+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);

+		rtw_free_xmitframe(pxmitpriv, pmgntframe);

+		goto exit;

+	}

+	rtw_dump_xframe(padapter, pmgntframe);

+

+	if(ptdls_sta->tdls_sta_state & TDLS_CH_SWITCH_ON_STATE){

+		rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CS_OFF);

+	}

+	

+	if( ptdls_sta->timer_flag == 1 )

+	{

+		_enter_critical_bh(&(padapter->tdlsinfo.hdl_lock), &irqL);

+		ptdls_sta->timer_flag = 2;

+		_exit_critical_bh(&(padapter->tdlsinfo.hdl_lock), &irqL);

+	}

+	else

+		rtw_tdls_cmd(padapter, mac_addr, TDLS_FREE_STA );

+

+

+exit:

+

+	return;

+}

+

+void issue_tdls_dis_req(_adapter *padapter, u8 *mac_addr)

+{

+	struct xmit_frame			*pmgntframe;

+	struct pkt_attrib			*pattrib;

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);

+	u8 baddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; 

+	

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+	

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+

+	pmgntframe->frame_tag = DATA_FRAMETAG;

+	pattrib->ether_type = 0x890d;

+	pattrib->pctrl =0;

+

+	if(mac_addr == NULL)

+		_rtw_memcpy(pattrib->dst, baddr, ETH_ALEN);

+	else

+		_rtw_memcpy(pattrib->dst, mac_addr, ETH_ALEN);

+	

+	_rtw_memcpy(pattrib->src, myid(&(padapter->eeprompriv)), ETH_ALEN);

+

+	_rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);

+	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);

+

+	update_tdls_attrib(padapter, pattrib);

+	pattrib->qsel=pattrib->priority;

+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, TDLS_DISCOVERY_REQUEST) != _SUCCESS) {

+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);

+		rtw_free_xmitframe(pxmitpriv, pmgntframe);

+		goto exit;

+	}

+	rtw_dump_xframe(padapter, pmgntframe);

+	DBG_871X("issue tdls dis req\n");

+

+exit:

+

+	return;

+}

+

+void issue_tdls_setup_rsp(_adapter *padapter, union recv_frame *precv_frame)

+{

+	struct xmit_frame			*pmgntframe;

+	struct pkt_attrib			*pattrib;

+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);

+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+	struct rx_pkt_attrib	*rx_pkt_pattrib = &precv_frame->u.hdr.attrib;

+	_irqL irqL;

+

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+	

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+

+	pmgntframe->frame_tag = DATA_FRAMETAG;

+	pattrib->ether_type = 0x890d;

+	pattrib->pctrl =0;

+

+	_rtw_memcpy(pattrib->dst, rx_pkt_pattrib->src, ETH_ALEN);

+	_rtw_memcpy(pattrib->src, myid(&(padapter->eeprompriv)), ETH_ALEN);

+

+	_rtw_memcpy(pattrib->ra, rx_pkt_pattrib->bssid, ETH_ALEN);

+	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);

+

+	update_tdls_attrib(padapter, pattrib);

+	pattrib->qsel=pattrib->priority;

+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, TDLS_SETUP_RESPONSE) != _SUCCESS) {

+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);

+		rtw_free_xmitframe(pxmitpriv, pmgntframe);

+		goto exit;

+	}

+	rtw_dump_xframe(padapter, pmgntframe);

+

+exit:

+

+	return;

+

+}

+

+void issue_tdls_setup_cfm(_adapter *padapter, union recv_frame *precv_frame)

+{

+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;

+	struct xmit_frame			*pmgntframe;

+	struct pkt_attrib			*pattrib;

+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);

+	struct sta_info		*ptdls_sta=NULL;

+	_irqL irqL;

+

+	struct rx_pkt_attrib	*rx_pkt_pattrib = & precv_frame->u.hdr.attrib;

+	

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+	

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+

+	pmgntframe->frame_tag = DATA_FRAMETAG;

+	pattrib->ether_type = 0x890d;

+	pattrib->pctrl =0;

+

+	_rtw_memcpy(pattrib->dst, rx_pkt_pattrib->src, ETH_ALEN);

+	_rtw_memcpy(pattrib->src, myid(&(padapter->eeprompriv)), ETH_ALEN);

+

+	_rtw_memcpy(pattrib->ra, rx_pkt_pattrib->bssid, ETH_ALEN);

+	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);

+

+	update_tdls_attrib(padapter, pattrib);

+	pattrib->qsel=pattrib->priority;

+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, TDLS_SETUP_CONFIRM) != _SUCCESS) {

+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);

+		rtw_free_xmitframe(pxmitpriv, pmgntframe);

+		goto exit;		

+	}

+

+	rtw_dump_xframe(padapter, pmgntframe);

+

+exit:

+

+	return;

+

+}

+

+//TDLS Discovery Response frame is a management action frame

+void issue_tdls_dis_rsp(_adapter *padapter, union recv_frame *precv_frame, u8 dialog)

+{

+	struct xmit_frame		*pmgntframe;

+	struct pkt_attrib		*pattrib;

+	unsigned char			*pframe;

+	struct rtw_ieee80211_hdr	*pwlanhdr;

+	unsigned short		*fctrl;

+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);

+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);

+

+	struct rx_pkt_attrib	*rx_pkt_pattrib = &precv_frame->u.hdr.attrib;

+

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+	update_mgntframe_attrib(padapter, pattrib);

+

+	_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);

+

+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;

+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;

+

+	fctrl = &(pwlanhdr->frame_ctl);

+	*(fctrl) = 0;

+

+	//	unicast probe request frame

+	_rtw_memcpy(pwlanhdr->addr1, rx_pkt_pattrib->src, ETH_ALEN);

+	_rtw_memcpy(pattrib->dst, pwlanhdr->addr1, ETH_ALEN);

+	

+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);

+	_rtw_memcpy(pattrib->src, pwlanhdr->addr2, ETH_ALEN);

+

+	_rtw_memcpy(pwlanhdr->addr3, rx_pkt_pattrib->bssid, ETH_ALEN);

+	_rtw_memcpy(pattrib->ra, pwlanhdr->addr3, ETH_ALEN);

+	

+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);

+	pmlmeext->mgnt_seq++;

+	SetFrameSubType(pframe, WIFI_ACTION);

+

+	pframe += sizeof (struct rtw_ieee80211_hdr_3addr);

+	pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);

+	

+	rtw_build_tdls_dis_rsp_ies(padapter, pmgntframe, pframe, dialog);

+

+	pattrib->nr_frags = 1;

+	pattrib->last_txcmdsz = pattrib->pktlen;

+

+	dump_mgntframe(padapter, pmgntframe);

+

+	return;

+}

+

+void issue_tdls_peer_traffic_indication(_adapter *padapter, struct sta_info *ptdls_sta)

+{

+	struct xmit_frame			*pmgntframe;

+	struct pkt_attrib			*pattrib;

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);

+

+	static u8 dialogtoken=0;

+	

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+	

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+

+	pmgntframe->frame_tag = DATA_FRAMETAG;

+	pattrib->ether_type = 0x890d;

+	pattrib->pctrl =0;

+

+	_rtw_memcpy(pattrib->dst, ptdls_sta->hwaddr, ETH_ALEN);

+	_rtw_memcpy(pattrib->src, myid(&(padapter->eeprompriv)), ETH_ALEN);

+

+	_rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);

+	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);

+

+	//for tdls; pattrib->nr_frags is used to fill dialogtoken

+	ptdls_sta->dialog = dialogtoken;

+	dialogtoken = (dialogtoken+1)%256;

+	//PTI frame's priority should be AC_VO

+	pattrib->priority = 7; 

+

+	update_tdls_attrib(padapter, pattrib);

+	pattrib->qsel=pattrib->priority;

+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, TDLS_PEER_TRAFFIC_INDICATION) != _SUCCESS) {

+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);

+		rtw_free_xmitframe(pxmitpriv, pmgntframe);

+		goto exit;

+	}

+	rtw_dump_xframe(padapter, pmgntframe);

+	

+exit:

+

+	return;

+}

+

+void issue_tdls_ch_switch_req(_adapter *padapter, u8 *mac_addr)

+{

+	struct xmit_frame	*pmgntframe;

+	struct pkt_attrib	*pattrib;

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);

+

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+	

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+

+	pmgntframe->frame_tag = DATA_FRAMETAG;

+	pattrib->ether_type = 0x890d;

+	pattrib->pctrl =0;		

+

+	_rtw_memcpy(pattrib->dst, mac_addr, ETH_ALEN);

+	_rtw_memcpy(pattrib->src, myid(&(padapter->eeprompriv)), ETH_ALEN);

+

+	_rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);

+	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);

+

+	update_tdls_attrib(padapter, pattrib);

+

+	pattrib->qsel=pattrib->priority;

+	if(rtw_xmit_tdls_coalesce(padapter, pmgntframe, TDLS_CHANNEL_SWITCH_REQUEST) !=_SUCCESS ){

+		rtw_free_xmitbuf(pxmitpriv,pmgntframe->pxmitbuf);

+		rtw_free_xmitframe(pxmitpriv, pmgntframe);

+		goto exit;

+	}

+	rtw_dump_xframe(padapter, pmgntframe);

+

+exit:

+

+	return;

+}

+

+void issue_tdls_ch_switch_rsp(_adapter *padapter, u8 *mac_addr)

+{

+	struct xmit_frame	*pmgntframe;

+	struct pkt_attrib	*pattrib;

+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;

+	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);

+

+        _irqL irqL;	

+		

+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)

+	{

+		return;

+	}

+	

+	//update attribute

+	pattrib = &pmgntframe->attrib;

+

+	pmgntframe->frame_tag = DATA_FRAMETAG;

+	pattrib->ether_type = 0x890d;

+	pattrib->pctrl =0;		

+

+	_rtw_memcpy(pattrib->dst, mac_addr, ETH_ALEN);

+	_rtw_memcpy(pattrib->src, myid(&(padapter->eeprompriv)), ETH_ALEN);

+

+	_rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);

+	_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);

+

+	update_tdls_attrib(padapter, pattrib);

+

+	pattrib->qsel=pattrib->priority;

+/*

+	_enter_critical_bh(&pxmitpriv->lock, &irqL);

+	if(xmitframe_enqueue_for_tdls_sleeping_sta(padapter, pmgntframe)==_TRUE){

+		_exit_critical_bh(&pxmitpriv->lock, &irqL);

+		return _FALSE;

+	}

+*/

+	if(rtw_xmit_tdls_coalesce(padapter, pmgntframe, TDLS_CHANNEL_SWITCH_RESPONSE) !=_SUCCESS ){

+		rtw_free_xmitbuf(pxmitpriv,pmgntframe->pxmitbuf);

+		rtw_free_xmitframe(pxmitpriv, pmgntframe);

+		goto exit;	

+	}

+	rtw_dump_xframe(padapter, pmgntframe);

+

+exit:

+

+	return;

+}

+

+sint On_TDLS_Dis_Rsp(_adapter *adapter, union recv_frame *precv_frame)

+{

+	struct sta_info *ptdls_sta = NULL, *psta = rtw_get_stainfo(&(adapter->stapriv), get_bssid(&(adapter->mlmepriv)));

+	struct recv_priv *precvpriv = &(adapter->recvpriv);

+	u8 *ptr = precv_frame->u.hdr.rx_data, *psa;

+	struct rx_pkt_attrib *pattrib = &(precv_frame->u.hdr.attrib);

+	struct tdls_info *ptdlsinfo = &(adapter->tdlsinfo);

+	u8 empty_addr[ETH_ALEN] = { 0x00 };

+	int UndecoratedSmoothedPWDB;

+	

+

+	//WFDTDLS: for sigma test, not to setup direct link automatically

+	ptdlsinfo->dev_discovered = 1;

+	

+#ifdef CONFIG_TDLS_AUTOSETUP	

+	psa = get_sa(ptr);

+	ptdls_sta = rtw_get_stainfo(&(adapter->stapriv), psa);

+

+	if(ptdls_sta != NULL)

+	{

+		ptdls_sta->tdls_sta_state |= TDLS_ALIVE_STATE;

+

+		//Record the tdls sta with lowest signal strength

+		if( (ptdlsinfo->sta_maximum == _TRUE) && (ptdls_sta->alive_count >= 1) )

+		{

+			if( _rtw_memcmp(ptdlsinfo->ss_record.macaddr, empty_addr, ETH_ALEN) )

+			{

+				_rtw_memcpy(ptdlsinfo->ss_record.macaddr, psa, ETH_ALEN);

+				ptdlsinfo->ss_record.RxPWDBAll = pattrib->RxPWDBAll;

+			}

+			else

+			{

+				if( ptdlsinfo->ss_record.RxPWDBAll < pattrib->RxPWDBAll )

+				{

+					_rtw_memcpy(ptdlsinfo->ss_record.macaddr, psa, ETH_ALEN);

+					ptdlsinfo->ss_record.RxPWDBAll = pattrib->RxPWDBAll;

+				}

+			}

+	}

+

+	}

+	else

+	{

+		if( ptdlsinfo->sta_maximum == _TRUE)

+		{

+			if( _rtw_memcmp( ptdlsinfo->ss_record.macaddr, empty_addr, ETH_ALEN ) )

+			{

+				//All traffics are busy, do not set up another direct link.

+				return _FAIL;

+			}

+			else

+			{

+				if( pattrib->RxPWDBAll > ptdlsinfo->ss_record.RxPWDBAll )

+				{

+					issue_tdls_teardown(adapter, ptdlsinfo->ss_record.macaddr);

+				}

+				else

+				{

+					return _FAIL;

+				}

+			}

+		}

+

+		adapter->HalFunc.GetHalDefVarHandler(adapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);

+

+		if( pattrib->RxPWDBAll + TDLS_SIGNAL_THRESH >= UndecoratedSmoothedPWDB);

+		{

+			DBG_871X("pattrib->RxPWDBAll=%d, pdmpriv->UndecoratedSmoothedPWDB=%d\n", pattrib->RxPWDBAll, UndecoratedSmoothedPWDB);

+			issue_tdls_setup_req(adapter, psa);

+		}

+	}

+#endif //CONFIG_TDLS_AUTOSETUP

+

+	return _SUCCESS;

+}

+

+sint On_TDLS_Setup_Req(_adapter *adapter, union recv_frame *precv_frame)

+{

+	struct tdls_info *ptdlsinfo = &adapter->tdlsinfo;

+	u8 *psa, *pmyid;

+	struct sta_info *ptdls_sta= NULL;

+   	struct sta_priv *pstapriv = &adapter->stapriv;

+	u8 *ptr = precv_frame->u.hdr.rx_data;

+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);

+	struct security_priv *psecuritypriv = &adapter->securitypriv;

+	_irqL irqL;

+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;

+	u8 *prsnie, *ppairwise_cipher;

+	u8 i, k, pairwise_count;

+	u8 ccmp_have=0, rsnie_have=0;

+	u16 j;

+	u8 SNonce[32];

+	u32 *timeout_interval;

+	sint parsing_length;	//frame body length, without icv_len

+	PNDIS_802_11_VARIABLE_IEs	pIE;

+	u8 FIXED_IE = 5;

+	unsigned char		supportRate[16];

+	int				supportRateNum = 0;

+

+	psa = get_sa(ptr);

+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);

+

+	pmyid=myid(&(adapter->eeprompriv));

+	ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len+LLC_HEADER_SIZE+TYPE_LENGTH_FIELD_SIZE+1;

+	parsing_length= ((union recv_frame *)precv_frame)->u.hdr.len

+			-prx_pkt_attrib->hdrlen

+			-prx_pkt_attrib->iv_len

+			-prx_pkt_attrib->icv_len

+			-LLC_HEADER_SIZE

+			-ETH_TYPE_LEN

+			-PAYLOAD_TYPE_LEN

+			-FIXED_IE;

+

+	if(ptdlsinfo->ap_prohibited == _TRUE)

+	{

+		goto exit;

+	}

+

+	if(ptdls_sta==NULL){

+		ptdls_sta = rtw_alloc_stainfo(pstapriv, psa);

+	}else{

+		if(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE){

+			//If the direct link is already set up

+			//Process as re-setup after tear down

+			DBG_871X("re-setup a direct link\n");

+		}

+		//already receiving TDLS setup request

+		else if(ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE){

+			DBG_871X("receive duplicated TDLS setup request frame in handshaking\n");

+			goto exit;

+		}

+		//When receiving and sending setup_req to the same link at the same time, STA with higher MAC_addr would be initiator

+		//following is to check out MAC_addr

+		else if(ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE){

+			DBG_871X("receive setup_req after sending setup_req\n");

+			for (i=0;i<6;i++){

+				if(*(pmyid+i)==*(psa+i)){

+				}

+				else if(*(pmyid+i)>*(psa+i)){

+					goto exit;

+				}else if(*(pmyid+i)<*(psa+i)){

+					ptdls_sta->tdls_sta_state=TDLS_INITIATOR_STATE;

+					break;

+				}

+			}

+		}

+	}

+

+	if(ptdls_sta) 

+	{

+		ptdls_sta->dialog = *(ptr+2);	//copy dialog token

+		ptdls_sta->stat_code = 0;

+

+		//parsing information element

+		for(j=FIXED_IE; j<parsing_length;){

+

+			pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr+ j);

+

+			switch (pIE->ElementID)

+			{

+				case _SUPPORTEDRATES_IE_:

+					_rtw_memcpy(supportRate, pIE->data, pIE->Length);

+					supportRateNum = pIE->Length;

+					break;

+				case _COUNTRY_IE_:

+					break;

+				case _EXT_SUPPORTEDRATES_IE_:

+					if(supportRateNum<=sizeof(supportRate))

+					{

+						_rtw_memcpy(supportRate+supportRateNum, pIE->data, pIE->Length);

+						supportRateNum += pIE->Length;

+					}

+					break;

+				case _SUPPORTED_CH_IE_:

+					break;

+				case _RSN_IE_2_:

+					rsnie_have=1;

+					if(prx_pkt_attrib->encrypt){

+						prsnie=(u8*)pIE;

+						//check whether initiator STA has CCMP pairwise_cipher.

+						ppairwise_cipher=prsnie+10;

+						_rtw_memcpy(&pairwise_count, (u16*)(ppairwise_cipher-2), 1);

+						for(k=0;k<pairwise_count;k++){

+							if(_rtw_memcmp( ppairwise_cipher+4*k, RSN_CIPHER_SUITE_CCMP, 4)==_TRUE)

+								ccmp_have=1;

+						}

+						if(ccmp_have==0){

+							//invalid contents of RSNIE

+							ptdls_sta->stat_code=72;

+						}

+					}

+					break;

+				case _EXT_CAP_IE_:

+					break;

+				case _VENDOR_SPECIFIC_IE_:

+					break;

+				case _FTIE_:

+					if(prx_pkt_attrib->encrypt)

+						_rtw_memcpy(SNonce, (ptr+j+52), 32);

+					break;

+				case _TIMEOUT_ITVL_IE_:

+					if(prx_pkt_attrib->encrypt)

+						timeout_interval = (u32 *)(ptr+j+3);

+					break;

+				case _RIC_Descriptor_IE_:

+					break;

+				case _HT_CAPABILITY_IE_:

+					rtw_tdls_process_ht_cap(adapter, ptdls_sta, pIE->data, pIE->Length);

+					break;

+				case EID_BSSCoexistence:

+					break;

+				case _LINK_ID_IE_:

+					if(_rtw_memcmp(get_bssid(pmlmepriv), pIE->data, 6) == _FALSE)

+					{

+						//not in the same BSS

+						ptdls_sta->stat_code=7;

+					}

+					break;

+				default:

+					break;

+			}

+

+			j += (pIE->Length + 2);

+			

+		}

+

+		//update station supportRate	

+		ptdls_sta->bssratelen = supportRateNum;

+		_rtw_memcpy(ptdls_sta->bssrateset, supportRate, supportRateNum);

+

+		//check status code

+		//if responder STA has/hasn't security on AP, but request hasn't/has RSNIE, it should reject

+		if(ptdls_sta->stat_code == 0 )

+		{

+			if(rsnie_have && (prx_pkt_attrib->encrypt==0)){

+				//security disabled

+				ptdls_sta->stat_code = 5;

+			}else if(rsnie_have==0 && (prx_pkt_attrib->encrypt)){

+				//request haven't RSNIE

+				ptdls_sta->stat_code = 38;

+			}

+

+#ifdef CONFIG_WFD

+			//WFD test plan version 0.18.2 test item 5.1.5

+			//SoUT does not use TDLS if AP uses weak security

+			if ( adapter->wdinfo.wfd_tdls_enable )

+			{

+				if(rsnie_have && (prx_pkt_attrib->encrypt != _AES_))

+				{

+					ptdls_sta->stat_code = 5;

+				}

+			}

+#endif //CONFIG_WFD

+		}

+

+		ptdls_sta->tdls_sta_state|= TDLS_INITIATOR_STATE;

+		if(prx_pkt_attrib->encrypt){

+			_rtw_memcpy(ptdls_sta->SNonce, SNonce, 32);

+			_rtw_memcpy(&(ptdls_sta->TDLS_PeerKey_Lifetime), timeout_interval, 4);

+		}

+		_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		

+		if(!(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE))

+			ptdlsinfo->sta_cnt++;

+		_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);	

+		if( ptdlsinfo->sta_cnt == (NUM_STA - 2) )	// -2: AP + BC/MC sta

+		{

+			ptdlsinfo->sta_maximum = _TRUE;

+		}

+

+#ifdef CONFIG_WFD

+		rtw_tdls_process_wfd_ie(ptdlsinfo, ptr + FIXED_IE, parsing_length - FIXED_IE);

+#endif // CONFIG_WFD

+

+	}

+	else

+	{

+		goto exit;

+	}

+

+ 	issue_tdls_setup_rsp(adapter, precv_frame);

+

+	if(ptdls_sta->stat_code==0)

+	{

+		_set_timer( &ptdls_sta->handshake_timer, TDLS_HANDSHAKE_TIME);

+	}

+	else		//status code!=0 ; setup unsuccess

+	{

+		free_tdls_sta(adapter, ptdls_sta);

+	}

+		

+exit:

+	

+	return _FAIL;

+}

+

+sint On_TDLS_Setup_Rsp(_adapter *adapter, union recv_frame *precv_frame)

+{

+	struct tdls_info *ptdlsinfo = &adapter->tdlsinfo;

+	struct sta_info *ptdls_sta= NULL;

+   	struct sta_priv *pstapriv = &adapter->stapriv;

+	u8 *ptr = precv_frame->u.hdr.rx_data;

+	_irqL irqL;

+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;

+	u8 *psa;

+	u16 stat_code;

+	sint parsing_length;	//frame body length, without icv_len

+	PNDIS_802_11_VARIABLE_IEs	pIE;

+	u8 FIXED_IE =7;

+	u8  *pftie, *ptimeout_ie, *plinkid_ie, *prsnie, *pftie_mic, *ppairwise_cipher;

+	u16 pairwise_count, j, k;

+	u8 verify_ccmp=0;

+	unsigned char		supportRate[16];

+	int				supportRateNum = 0;

+

+	psa = get_sa(ptr);

+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);

+

+	if ( NULL == ptdls_sta )

+	{

+		return _FAIL;

+	}

+

+	ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len+LLC_HEADER_SIZE+TYPE_LENGTH_FIELD_SIZE+1;

+	parsing_length= ((union recv_frame *)precv_frame)->u.hdr.len

+			-prx_pkt_attrib->hdrlen

+			-prx_pkt_attrib->iv_len

+			-prx_pkt_attrib->icv_len

+			-LLC_HEADER_SIZE

+			-TYPE_LENGTH_FIELD_SIZE

+			-1

+			-FIXED_IE;

+

+	_rtw_memcpy(&stat_code, ptr+2, 2);

+	

+	if(stat_code!=0)

+	{

+		DBG_871X( "[%s] status_code = %d, free_tdls_sta\n", __FUNCTION__, stat_code );

+		free_tdls_sta(adapter, ptdls_sta);

+		return _FAIL;

+	}

+

+	stat_code = 0;

+

+	//parsing information element

+	for(j=FIXED_IE; j<parsing_length;)

+	{

+		pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr+ j);

+

+		switch (pIE->ElementID)

+		{

+			case _SUPPORTEDRATES_IE_:

+				_rtw_memcpy(supportRate, pIE->data, pIE->Length);

+				supportRateNum = pIE->Length;

+				break;

+			case _COUNTRY_IE_:

+				break;

+			case _EXT_SUPPORTEDRATES_IE_:

+				if(supportRateNum<=sizeof(supportRate))

+				{

+					_rtw_memcpy(supportRate+supportRateNum, pIE->data, pIE->Length);

+					supportRateNum += pIE->Length;

+				}

+				break;

+			case _SUPPORTED_CH_IE_:

+				break;

+			case _RSN_IE_2_:

+				prsnie=(u8*)pIE;

+				//check whether responder STA has CCMP pairwise_cipher.

+				ppairwise_cipher=prsnie+10;

+				_rtw_memcpy(&pairwise_count, (u16*)(ppairwise_cipher-2), 2);

+				for(k=0;k<pairwise_count;k++){

+					if(_rtw_memcmp( ppairwise_cipher+4*k, RSN_CIPHER_SUITE_CCMP, 4)==_TRUE)

+						verify_ccmp=1;

+				}

+			case _EXT_CAP_IE_:

+				break;

+			case _VENDOR_SPECIFIC_IE_:

+				break;

+			case _FTIE_:

+				pftie=(u8*)pIE;

+				_rtw_memcpy(ptdls_sta->ANonce, (ptr+j+20), 32);

+				break;

+			case _TIMEOUT_ITVL_IE_:

+				ptimeout_ie=(u8*)pIE;

+				break;

+			case _RIC_Descriptor_IE_:

+				break;

+			case _HT_CAPABILITY_IE_:

+				rtw_tdls_process_ht_cap(adapter, ptdls_sta, pIE->data, pIE->Length);

+				break;

+			case EID_BSSCoexistence:

+				break;

+			case _LINK_ID_IE_:

+				plinkid_ie=(u8*)pIE;

+				break;

+			default:

+				break;

+		}

+

+		j += (pIE->Length + 2);

+		

+	}

+

+	//update station supportRate	

+	ptdls_sta->bssratelen = supportRateNum;

+	_rtw_memcpy(ptdls_sta->bssrateset, supportRate, supportRateNum);

+

+#ifdef CONFIG_WFD

+	rtw_tdls_process_wfd_ie(ptdlsinfo, ptr + FIXED_IE, parsing_length - FIXED_IE);

+#endif // CONFIG_WFD

+

+	if(stat_code != 0)

+	{

+		ptdls_sta->stat_code = stat_code;

+	}

+	else

+	{

+		if(prx_pkt_attrib->encrypt)

+		{

+			if(verify_ccmp==1)

+			{

+				wpa_tdls_generate_tpk(adapter, ptdls_sta);

+				ptdls_sta->stat_code=0;

+				if(tdls_verify_mic(ptdls_sta->tpk.kck, 2, plinkid_ie, prsnie, ptimeout_ie, pftie)==0)	//0: Invalid, 1: valid

+				{

+					free_tdls_sta(adapter, ptdls_sta);

+					return _FAIL;

+				}

+			}

+			else

+			{

+				ptdls_sta->stat_code=72;	//invalide contents of RSNIE

+			}

+

+		}else{

+			ptdls_sta->stat_code=0;

+		}

+	}

+

+	DBG_871X("issue_tdls_setup_cfm\n");

+	issue_tdls_setup_cfm(adapter, precv_frame);

+

+	if(ptdls_sta->stat_code==0)

+	{

+		ptdlsinfo->setup_state = TDLS_LINKED_STATE;

+

+		if( ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE )

+		{

+			ptdls_sta->tdls_sta_state |= TDLS_LINKED_STATE;

+			_cancel_timer_ex( &ptdls_sta->handshake_timer);

+#ifdef CONFIG_TDLS_AUTOCHECKALIVE

+			_set_timer( &ptdls_sta->alive_timer1, TDLS_ALIVE_TIMER_PH1);

+#endif //CONFIG_TDLS_AUTOSETUP

+		}

+

+		rtw_tdls_set_mac_id(ptdlsinfo, ptdls_sta);

+		rtw_tdls_set_key(adapter, prx_pkt_attrib, ptdls_sta);

+

+		rtw_tdls_cmd(adapter, ptdls_sta->hwaddr, TDLS_WRCR);

+

+	}

+	else //status code!=0 ; setup unsuccessful

+	{

+		free_tdls_sta(adapter, ptdls_sta);

+	}

+

+	return _FAIL;

+

+}

+

+sint On_TDLS_Setup_Cfm(_adapter *adapter, union recv_frame *precv_frame)

+{

+	struct tdls_info *ptdlsinfo = &adapter->tdlsinfo;

+	struct sta_info *ptdls_sta= NULL;

+   	struct sta_priv *pstapriv = &adapter->stapriv;

+	u8 *ptr = precv_frame->u.hdr.rx_data;

+	_irqL irqL;

+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;

+	u8 *psa; 

+	u16 stat_code;

+	sint parsing_length;

+	PNDIS_802_11_VARIABLE_IEs	pIE;

+	u8 FIXED_IE =5;

+	u8  *pftie, *ptimeout_ie, *plinkid_ie, *prsnie, *pftie_mic, *ppairwise_cipher;

+	u16 j, pairwise_count;

+

+	psa = get_sa(ptr);

+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);

+

+	ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len+LLC_HEADER_SIZE+TYPE_LENGTH_FIELD_SIZE+1;

+	parsing_length= ((union recv_frame *)precv_frame)->u.hdr.len

+			-prx_pkt_attrib->hdrlen

+			-prx_pkt_attrib->iv_len

+			-prx_pkt_attrib->icv_len

+			-LLC_HEADER_SIZE

+			-ETH_TYPE_LEN

+			-PAYLOAD_TYPE_LEN

+			-FIXED_IE;

+	_rtw_memcpy(&stat_code, ptr+2, 2);

+

+	if(stat_code!=0){

+		DBG_871X( "[%s] stat_code = %d\n, free_tdls_sta", __FUNCTION__, stat_code );

+		free_tdls_sta(adapter, ptdls_sta);

+		return _FAIL;

+	}

+

+	if(prx_pkt_attrib->encrypt){

+		//parsing information element

+		for(j=FIXED_IE; j<parsing_length;){

+

+			pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr+ j);

+

+			switch (pIE->ElementID)

+			{

+				case _RSN_IE_2_:

+					prsnie=(u8*)pIE;

+					break;

+				case _VENDOR_SPECIFIC_IE_:

+					break;

+	 			case _FTIE_:

+					pftie=(u8*)pIE;

+					break;

+				case _TIMEOUT_ITVL_IE_:

+					ptimeout_ie=(u8*)pIE;

+					break;

+				case _HT_EXTRA_INFO_IE_:

+					break;

+	 			case _LINK_ID_IE_:

+					plinkid_ie=(u8*)pIE;

+					break;

+				default:

+					break;

+			}

+

+			j += (pIE->Length + 2);

+			

+		}

+

+		//verify mic in FTIE MIC field

+		if(tdls_verify_mic(ptdls_sta->tpk.kck, 3, plinkid_ie, prsnie, ptimeout_ie, pftie)==0){	//0: Invalid, 1: Valid

+			free_tdls_sta(adapter, ptdls_sta);

+			return _FAIL;

+		}

+

+	}

+

+	ptdlsinfo->setup_state = TDLS_LINKED_STATE;

+	if( ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE )

+	{

+		ptdls_sta->tdls_sta_state|=TDLS_LINKED_STATE;

+		_cancel_timer_ex( &ptdls_sta->handshake_timer);

+#ifdef CONFIG_TDLS_AUTOCHECKALIVE

+		_set_timer( &ptdls_sta->alive_timer1, TDLS_ALIVE_TIMER_PH1);

+#endif //CONFIG_TDLS_AUTOCHECKALIVE

+	}

+

+	rtw_tdls_set_mac_id(ptdlsinfo, ptdls_sta);

+	rtw_tdls_set_key(adapter, prx_pkt_attrib, ptdls_sta);

+

+	rtw_tdls_cmd(adapter, ptdls_sta->hwaddr, TDLS_WRCR);

+	

+	return _FAIL;

+

+}

+

+sint On_TDLS_Dis_Req(_adapter *adapter, union recv_frame *precv_frame)

+{

+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;

+	struct sta_priv *pstapriv = &adapter->stapriv;

+	struct sta_info *psta_ap;

+	u8 *ptr = precv_frame->u.hdr.rx_data;

+	sint parsing_length;	//frame body length, without icv_len

+	PNDIS_802_11_VARIABLE_IEs	pIE;

+	u8 FIXED_IE = 3, *dst, *pdialog = NULL;

+	u16 j;

+

+	ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE+TYPE_LENGTH_FIELD_SIZE + 1;

+	pdialog=ptr+2;

+

+	parsing_length= ((union recv_frame *)precv_frame)->u.hdr.len

+			-prx_pkt_attrib->hdrlen

+			-prx_pkt_attrib->iv_len

+			-prx_pkt_attrib->icv_len

+			-LLC_HEADER_SIZE

+			-TYPE_LENGTH_FIELD_SIZE

+			-1

+			-FIXED_IE;

+

+	//parsing information element

+	for(j=FIXED_IE; j<parsing_length;){

+

+		pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr+ j);

+

+		switch (pIE->ElementID)

+		{

+			case _LINK_ID_IE_:

+				psta_ap = rtw_get_stainfo(pstapriv, pIE->data);

+				if(psta_ap == NULL)

+				{

+					goto exit;

+				}

+				dst = pIE->data + 12;

+				if( (MacAddr_isBcst(dst) == _FALSE) && (_rtw_memcmp(myid(&(adapter->eeprompriv)), dst, 6) == _FALSE) )

+				{

+					goto exit;

+				}

+				break;

+			default:

+				break;

+		}

+

+		j += (pIE->Length + 2);

+		

+	}

+

+	//check frame contents

+

+	issue_tdls_dis_rsp(adapter, precv_frame, *(pdialog) );

+

+exit:

+

+	return _FAIL;

+	

+}

+

+sint On_TDLS_Teardown(_adapter *adapter, union recv_frame *precv_frame)

+{

+	u8 *psa;

+	u8 *ptr = precv_frame->u.hdr.rx_data;

+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;

+	struct mlme_ext_priv	*pmlmeext = &(adapter->mlmeextpriv);	

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+	struct sta_priv 	*pstapriv = &adapter->stapriv;

+	struct sta_info *ptdls_sta= NULL;

+	_irqL irqL;

+

+	psa = get_sa(ptr);

+

+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);

+	if(ptdls_sta!=NULL){

+		if(ptdls_sta->tdls_sta_state & TDLS_CH_SWITCH_ON_STATE){

+			rtw_tdls_cmd(adapter, ptdls_sta->hwaddr, TDLS_CS_OFF);

+		}

+		free_tdls_sta(adapter, ptdls_sta);

+	}

+		

+	return _FAIL;

+	

+}

+

+u8 TDLS_check_ch_state(uint state){

+	if(	(state & TDLS_CH_SWITCH_ON_STATE) &&

+		(state & TDLS_AT_OFF_CH_STATE) &&

+		(state & TDLS_PEER_AT_OFF_STATE) ){

+

+		if(state & TDLS_PEER_SLEEP_STATE)

+			return 2;	//U-APSD + ch. switch

+		else

+			return 1;	//ch. switch

+	}else

+		return 0;

+}

+

+//we process buffered data for 1. U-APSD, 2. ch. switch, 3. U-APSD + ch. switch here

+sint On_TDLS_Peer_Traffic_Rsp(_adapter *adapter, union recv_frame *precv_frame)

+{

+	struct tdls_info *ptdlsinfo = &adapter->tdlsinfo;

+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;

+	struct rx_pkt_attrib	*pattrib = & precv_frame->u.hdr.attrib;

+	struct sta_priv *pstapriv = &adapter->stapriv;

+	//get peer sta infomation

+	struct sta_info *ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->src);

+	u8 wmmps_ac=0, state=TDLS_check_ch_state(ptdls_sta->tdls_sta_state);

+	int i;

+	

+	ptdls_sta->sta_stats.rx_data_pkts++;

+

+	//receive peer traffic response frame, sleeping STA wakes up

+	//ptdls_sta->tdls_sta_state &= ~(TDLS_PEER_SLEEP_STATE);

+	process_wmmps_data( adapter, precv_frame);

+

+	// if noticed peer STA wakes up by receiving peer traffic response

+	// and we want to do channel swtiching, then we will transmit channel switch request first

+	if(ptdls_sta->tdls_sta_state & TDLS_APSD_CHSW_STATE){

+		issue_tdls_ch_switch_req(adapter, pattrib->src);

+		ptdls_sta->tdls_sta_state &= ~(TDLS_APSD_CHSW_STATE);

+		return  _FAIL;

+	}

+

+	//check 4-AC queue bit

+	if(ptdls_sta->uapsd_vo || ptdls_sta->uapsd_vi || ptdls_sta->uapsd_be || ptdls_sta->uapsd_bk)

+		wmmps_ac=1;

+

+	//if it's a direct link and have buffered frame

+	if(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE){

+		if(wmmps_ac && state)

+		{

+			_irqL irqL;	 

+			_list	*xmitframe_plist, *xmitframe_phead;

+			struct xmit_frame *pxmitframe=NULL;

+		

+			_enter_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);	

+

+			xmitframe_phead = get_list_head(&ptdls_sta->sleep_q);

+			xmitframe_plist = get_next(xmitframe_phead);

+

+			//transmit buffered frames

+			while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)

+			{			

+				pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);

+				xmitframe_plist = get_next(xmitframe_plist);

+				rtw_list_delete(&pxmitframe->list);

+

+				ptdls_sta->sleepq_len--;

+				if(ptdls_sta->sleepq_len>0){

+					pxmitframe->attrib.mdata = 1;

+					pxmitframe->attrib.eosp = 0;

+				}else{

+					pxmitframe->attrib.mdata = 0;

+					pxmitframe->attrib.eosp = 1;

+				}

+				//pxmitframe->attrib.triggered = 1;	//maybe doesn't need in TDLS

+				if(adapter->HalFunc.hal_xmit(adapter, pxmitframe) == _TRUE)

+				{		

+					rtw_os_xmit_complete(adapter, pxmitframe);

+				}

+

+			}

+

+			if(ptdls_sta->sleepq_len==0)

+			{

+				DBG_871X("no buffered packets for tdls to xmit\n");

+				//on U-APSD + CH. switch state, when there is no buffered date to xmit,

+				// we should go back to base channel

+				if(state==2){

+					rtw_tdls_cmd(adapter, ptdls_sta->hwaddr, TDLS_CS_OFF);

+				}else if(ptdls_sta->tdls_sta_state&TDLS_SW_OFF_STATE){

+						ptdls_sta->tdls_sta_state &= ~(TDLS_SW_OFF_STATE);

+						ptdlsinfo->candidate_ch= pmlmeext->cur_channel;

+						issue_tdls_ch_switch_req(adapter, pattrib->src);

+						DBG_871X("issue tdls ch switch req back to base channel\n");

+				}

+				

+			}

+			else

+			{

+				DBG_871X("error!psta->sleepq_len=%d\n", ptdls_sta->sleepq_len);

+				ptdls_sta->sleepq_len=0;						

+			}

+

+			_exit_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);			

+		

+		}

+

+	}

+

+	return _FAIL;

+}

+

+sint On_TDLS_Ch_Switch_Req(_adapter *adapter, union recv_frame *precv_frame)

+{

+	struct sta_info *ptdls_sta= NULL;

+   	struct sta_priv *pstapriv = &adapter->stapriv;

+	u8 *ptr = precv_frame->u.hdr.rx_data;

+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;

+	u8 *psa; 

+	sint parsing_length;

+	PNDIS_802_11_VARIABLE_IEs	pIE;

+	u8 FIXED_IE =3;

+	u16 j;

+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;

+

+	psa = get_sa(ptr);

+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);

+	

+	ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len+LLC_HEADER_SIZE+TYPE_LENGTH_FIELD_SIZE+1;

+	parsing_length= ((union recv_frame *)precv_frame)->u.hdr.len

+			-prx_pkt_attrib->hdrlen

+			-prx_pkt_attrib->iv_len

+			-prx_pkt_attrib->icv_len

+			-LLC_HEADER_SIZE

+			-ETH_TYPE_LEN

+			-PAYLOAD_TYPE_LEN

+			-FIXED_IE;

+

+	ptdls_sta->off_ch = *(ptr+2);

+	

+	//parsing information element

+	for(j=FIXED_IE; j<parsing_length;){

+

+		pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr+ j);

+

+		switch (pIE->ElementID)

+		{

+			case _COUNTRY_IE_:

+				break;

+			case _CH_SWTICH_ANNOUNCE_:

+				break;

+ 			case _LINK_ID_IE_:

+				break;

+			case _CH_SWITCH_TIMING_:

+				_rtw_memcpy(&ptdls_sta->ch_switch_time, pIE->data, 2);

+				_rtw_memcpy(&ptdls_sta->ch_switch_timeout, pIE->data+2, 2);

+			default:

+				break;

+		}

+

+		j += (pIE->Length + 2);

+		

+	}

+

+	//todo: check status

+	ptdls_sta->stat_code=0;

+	ptdls_sta->tdls_sta_state |= TDLS_CH_SWITCH_ON_STATE;

+

+	issue_nulldata(adapter, NULL, 1, 0, 0);

+

+	issue_tdls_ch_switch_rsp(adapter, psa);

+

+	DBG_871X("issue tdls channel switch response\n");

+

+	if((ptdls_sta->tdls_sta_state & TDLS_CH_SWITCH_ON_STATE) && ptdls_sta->off_ch==pmlmeext->cur_channel){

+		DBG_871X("back to base channel %x\n", pmlmeext->cur_channel);

+		ptdls_sta->option=7;

+		rtw_tdls_cmd(adapter, ptdls_sta->hwaddr, TDLS_BASE_CH);

+	}else{		

+		ptdls_sta->option=6;

+		rtw_tdls_cmd(adapter, ptdls_sta->hwaddr, TDLS_OFF_CH);

+	}

+	return _FAIL;

+}

+

+sint On_TDLS_Ch_Switch_Rsp(_adapter *adapter, union recv_frame *precv_frame)

+{

+	struct sta_info *ptdls_sta= NULL;

+   	struct sta_priv *pstapriv = &adapter->stapriv;

+	u8 *ptr = precv_frame->u.hdr.rx_data;

+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;

+	u8 *psa; 

+	sint parsing_length;

+	PNDIS_802_11_VARIABLE_IEs	pIE;

+	u8 FIXED_IE =4;

+	u16 stat_code, j, switch_time, switch_timeout;

+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;

+

+	psa = get_sa(ptr);

+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);

+

+	//if channel switch is running and receiving Unsolicited TDLS Channel Switch Response,

+	//it will go back to base channel and terminate this channel switch procedure

+	if(ptdls_sta->tdls_sta_state & TDLS_CH_SWITCH_ON_STATE ){

+		if(pmlmeext->cur_channel==ptdls_sta->off_ch){

+			DBG_871X("back to base channel %x\n", pmlmeext->cur_channel);

+			ptdls_sta->option=7;

+			rtw_tdls_cmd(adapter, ptdls_sta->hwaddr, TDLS_OFF_CH);

+		}else{

+			DBG_871X("receive unsolicited channel switch response \n");

+			rtw_tdls_cmd(adapter, ptdls_sta->hwaddr, TDLS_CS_OFF);

+		}

+		return _FAIL;

+	}

+

+	//avoiding duplicated or unconditional ch. switch. rsp

+	if((ptdls_sta->tdls_sta_state & TDLS_CH_SW_INITIATOR_STATE) != TDLS_CH_SW_INITIATOR_STATE)

+		return _FAIL;

+	

+	ptr +=prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len+LLC_HEADER_SIZE+TYPE_LENGTH_FIELD_SIZE+1;

+	parsing_length= ((union recv_frame *)precv_frame)->u.hdr.len

+			-prx_pkt_attrib->hdrlen

+			-prx_pkt_attrib->iv_len

+			-prx_pkt_attrib->icv_len

+			-LLC_HEADER_SIZE

+			-ETH_TYPE_LEN

+			-PAYLOAD_TYPE_LEN

+			-FIXED_IE;

+

+	_rtw_memcpy(&stat_code, ptr+2, 2);

+

+	if(stat_code!=0){

+		return _FAIL;

+	}

+	

+	//parsing information element

+	for(j=FIXED_IE; j<parsing_length;){

+

+		pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr+ j);

+

+		switch (pIE->ElementID)

+		{

+ 			case _LINK_ID_IE_:

+				break;

+			case _CH_SWITCH_TIMING_:

+				_rtw_memcpy(&switch_time, pIE->data, 2);

+				if(switch_time > ptdls_sta->ch_switch_time)

+					_rtw_memcpy(&ptdls_sta->ch_switch_time, &switch_time, 2);

+

+				_rtw_memcpy(&switch_timeout, pIE->data+2, 2);

+				if(switch_timeout > ptdls_sta->ch_switch_timeout)

+					_rtw_memcpy(&ptdls_sta->ch_switch_timeout, &switch_timeout, 2);

+

+			default:

+				break;

+		}

+

+		j += (pIE->Length + 2);

+		

+	}

+

+	ptdls_sta->tdls_sta_state &= ~(TDLS_CH_SW_INITIATOR_STATE);

+	ptdls_sta->tdls_sta_state |=TDLS_CH_SWITCH_ON_STATE;

+

+	//goto set_channel_workitem_callback()

+	ptdls_sta->option=6;

+	rtw_tdls_cmd(adapter, ptdls_sta->hwaddr, TDLS_OFF_CH);

+

+	return _FAIL;	

+}

+

+#ifdef CONFIG_WFD

+void wfd_ie_tdls(_adapter * padapter, u8 *pframe, u32 *pktlen )

+{

+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;

+	struct wifi_display_info	*pwfd_info = padapter->tdlsinfo.wfd_info;

+	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };

+	u32 wfdielen = 0;

+

+	//	WFD OUI

+	wfdielen = 0;

+	wfdie[ wfdielen++ ] = 0x50;

+	wfdie[ wfdielen++ ] = 0x6F;

+	wfdie[ wfdielen++ ] = 0x9A;

+	wfdie[ wfdielen++ ] = 0x0A;	//	WFA WFD v1.0

+

+	//	Commented by Albert 20110825

+	//	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes

+	//	1. WFD Device Information

+	//	2. Associated BSSID ( Optional )

+	//	3. Local IP Adress ( Optional )

+

+	//	WFD Device Information ATTR

+	//	Type:

+	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;

+

+	//	Length:

+	//	Note: In the WFD specification, the size of length field is 2.

+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);

+	wfdielen += 2;

+

+	//	Value1:

+	//	WFD device information

+	//	available for WFD session + Preferred TDLS + WSD ( WFD Service Discovery )

+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL 

+								| WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_WSD);

+	wfdielen += 2;

+

+	//	Value2:

+	//	Session Management Control Port

+	//	Default TCP port for RTSP messages is 554

+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport );

+	wfdielen += 2;

+

+	//	Value3:

+	//	WFD Device Maximum Throughput

+	//	300Mbps is the maximum throughput

+	RTW_PUT_BE16(wfdie + wfdielen, 300);

+	wfdielen += 2;

+

+	//	Associated BSSID ATTR

+	//	Type:

+	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;

+

+	//	Length:

+	//	Note: In the WFD specification, the size of length field is 2.

+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);

+	wfdielen += 2;

+

+	//	Value:

+	//	Associated BSSID

+	if ( check_fwstate( pmlmepriv, _FW_LINKED) == _TRUE )

+	{

+		_rtw_memcpy( wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN );

+	}

+	else

+	{

+		_rtw_memset( wfdie + wfdielen, 0x00, ETH_ALEN );

+	}

+

+	//	Local IP Address ATTR

+	wfdie[ wfdielen++ ] = WFD_ATTR_LOCAL_IP_ADDR;

+

+	//	Length:

+	//	Note: In the WFD specification, the size of length field is 2.

+	RTW_PUT_BE16(wfdie + wfdielen, 0x0005);

+	wfdielen += 2;

+

+	//	Version:

+	//	0x01: Version1;IPv4

+	wfdie[ wfdielen++ ] = 0x01;	

+

+	//	IPv4 Address

+	_rtw_memcpy( wfdie + wfdielen, pwfd_info->ip_address, 4 );

+	wfdielen += 4;

+	

+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, pktlen);

+	

+}

+#endif //CONFIG_WFD

+

+void rtw_build_tdls_setup_req_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)

+{

+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;

+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;

+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;

+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;

+	struct sta_info *ptdls_sta=rtw_get_stainfo( (&padapter->stapriv) , pattrib->dst);

+

+	u8 payload_type = 0x02;

+	u8 category = RTW_WLAN_CATEGORY_TDLS;

+	u8 action = TDLS_SETUP_REQUEST;

+	u8 bssrate[NDIS_802_11_LENGTH_RATES_EX]; //Use NDIS_802_11_LENGTH_RATES_EX in order to call func.rtw_set_supported_rate

+ 	int	bssrate_len = 0, i = 0 ;

+	u8 more_supportedrates = 0;

+	unsigned int ie_len;

+	u8 *p;

+	struct mlme_priv 		*pmlmepriv = &padapter->mlmepriv;

+	u8 link_id_addr[18] = {0};

+	u8 iedata=0;

+	u8 sup_ch[ 30 * 2 ] = {0x00 }, sup_ch_idx = 0, idx_5g = 2;	//For supported channel

+	u8 timeout_itvl[5];	//set timeout interval to maximum value

+	u32 time;

+

+	//SNonce	

+	if(pattrib->encrypt){

+		for(i=0;i<8;i++){

+			time=rtw_get_current_time();

+			_rtw_memcpy(&ptdls_sta->SNonce[4*i], (u8 *)&time, 4);

+		}

+	}

+

+	//payload type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));

+	//category, action, dialog token

+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(ptdls_sta->dialog), &(pattrib->pktlen));

+

+	//capability

+	_rtw_memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);

+

+	if(pattrib->encrypt)

+		*pframe =*pframe | BIT(4);

+	pframe += 2;

+	pattrib->pktlen += 2;

+

+	//supported rates

+	rtw_set_supported_rate(bssrate, WIRELESS_11BG_24N);

+	bssrate_len = IEEE80211_CCK_RATE_LEN + IEEE80211_NUM_OFDM_RATESLEN;

+

+	if (bssrate_len > 8)

+	{

+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));

+		more_supportedrates = 1;

+	}

+	else

+	{

+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));

+	}

+

+	//country(optional)

+	//extended supported rates

+	if(more_supportedrates==1){

+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));

+	}

+

+	//supported channels

+	pframe = rtw_tdls_set_sup_ch(pmlmeext, pframe, pattrib);

+	

+	//	SRC IE

+	pframe = rtw_set_ie( pframe, _SRC_IE_, 16, TDLS_SRC, &(pattrib->pktlen));

+	

+	//RSNIE

+	if(pattrib->encrypt)

+		pframe = rtw_set_ie(pframe, _RSN_IE_2_, 20, TDLS_RSNIE, &(pattrib->pktlen));

+	

+	//extended capabilities

+	pframe = rtw_set_ie(pframe, _EXT_CAP_IE_ , 5, TDLS_EXT_CAPIE, &(pattrib->pktlen));

+

+	//QoS capability(WMM_IE)

+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 7, TDLS_WMMIE,  &(pattrib->pktlen));

+

+

+	if(pattrib->encrypt){

+		//FTIE

+		_rtw_memset(pframe, 0, 84);	//All fields except SNonce shall be set to 0

+		_rtw_memset(pframe, _FTIE_, 1);	//version

+		_rtw_memset((pframe+1), 82, 1);	//length

+		_rtw_memcpy((pframe+52), ptdls_sta->SNonce, 32);

+		pframe += 84;

+		pattrib->pktlen += 84;

+

+		//Timeout interval

+		timeout_itvl[0]=0x02;

+		_rtw_memcpy(timeout_itvl+1, (u8 *)(&ptdls_sta->TDLS_PeerKey_Lifetime), 4);

+		pframe = rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, 5, timeout_itvl,  &(pattrib->pktlen));

+	}

+

+	//Sup_reg_classes(optional)

+	//HT capabilities

+	pframe = rtw_tdls_set_ht_cap(padapter, pframe, pattrib);

+

+	//20/40 BSS coexistence

+	if(pmlmepriv->num_FortyMHzIntolerant>0)

+		iedata |= BIT(2);//20 MHz BSS Width Request

+	pframe = rtw_set_ie(pframe, EID_BSSCoexistence,  1, &iedata, &(pattrib->pktlen));

+	

+	//Link identifier

+	_rtw_memcpy(link_id_addr, pattrib->ra, 6);

+	_rtw_memcpy((link_id_addr+6), pattrib->src, 6);

+	_rtw_memcpy((link_id_addr+12), pattrib->dst, 6);

+	pframe = rtw_set_ie(pframe, _LINK_ID_IE_,  18, link_id_addr, &(pattrib->pktlen));

+

+#ifdef CONFIG_WFD

+	wfd_ie_tdls( padapter, pframe, &(pattrib->pktlen) );

+#endif //CONFIG_WFD

+

+}

+

+void rtw_build_tdls_setup_rsp_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)

+{

+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;

+	struct sta_info *ptdls_sta;

+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;

+

+	u8 payload_type = 0x02;	

+	unsigned char category = RTW_WLAN_CATEGORY_TDLS;

+	unsigned char action = TDLS_SETUP_RESPONSE;

+	unsigned char	bssrate[NDIS_802_11_LENGTH_RATES_EX];	

+	int	bssrate_len = 0;

+	u8 more_supportedrates = 0;

+	unsigned int ie_len;

+	unsigned char *p;

+	struct mlme_priv 		*pmlmepriv = &padapter->mlmepriv;

+	u8 link_id_addr[18] = {0};

+	u8 iedata=0;

+	u8 timeout_itvl[5];	//setup response timeout interval will copy from request

+	u8 ANonce[32];	//maybe it can put in ontdls_req

+	u8 k;		//for random ANonce

+	u8  *pftie, *ptimeout_ie, *plinkid_ie, *prsnie, *pftie_mic;

+	u32 time;

+

+	ptdls_sta = rtw_get_stainfo( &(padapter->stapriv) , pattrib->dst);

+

+	if(ptdls_sta == NULL )

+	{

+		DBG_871X("[%s] %d\n", __FUNCTION__, __LINE__);

+		return;

+	}

+

+	if(pattrib->encrypt){

+		for(k=0;k<8;k++){

+			time=rtw_get_current_time();

+			_rtw_memcpy(&ptdls_sta->ANonce[4*k], (u8*)&time, 4);

+		}

+	}

+

+	//payload type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));	

+	//category, action, status code

+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 2, (u8 *)&ptdls_sta->stat_code, &(pattrib->pktlen));

+

+	if(ptdls_sta->stat_code!=0)	//invalid setup request

+	{

+		DBG_871X("ptdls_sta->stat_code:%04x \n", ptdls_sta->stat_code);		

+		return;

+	}

+	

+	//dialog token

+	pframe = rtw_set_fixed_ie(pframe, 1, &(ptdls_sta->dialog), &(pattrib->pktlen));

+

+	//capability

+	_rtw_memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);

+

+	if(pattrib->encrypt )

+		*pframe =*pframe | BIT(4);

+	pframe += 2;

+	pattrib->pktlen += 2;

+

+	//supported rates

+	rtw_set_supported_rate(bssrate, WIRELESS_11BG_24N);

+	bssrate_len = IEEE80211_CCK_RATE_LEN + IEEE80211_NUM_OFDM_RATESLEN;

+

+	if (bssrate_len > 8)

+	{

+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));

+		more_supportedrates = 1;

+	}

+	else

+	{

+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));

+	}

+

+	//country(optional)

+	//extended supported rates

+	if(more_supportedrates==1){

+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));

+	}

+

+	//supported channels

+	pframe = rtw_tdls_set_sup_ch(pmlmeext, pframe, pattrib);

+	

+	// SRC IE

+	pframe = rtw_set_ie(pframe, _SRC_IE_ , 16, TDLS_SRC, &(pattrib->pktlen));

+

+	//RSNIE

+	if(pattrib->encrypt){

+		prsnie = pframe;

+		pframe = rtw_set_ie(pframe, _RSN_IE_2_, 20, TDLS_RSNIE, &(pattrib->pktlen));

+	}

+

+	//extended capabilities

+	pframe = rtw_set_ie(pframe, _EXT_CAP_IE_ , 5, TDLS_EXT_CAPIE, &(pattrib->pktlen));

+

+	//QoS capability(WMM_IE)

+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 7, TDLS_WMMIE,  &(pattrib->pktlen));

+

+	if(pattrib->encrypt){

+		wpa_tdls_generate_tpk(padapter, ptdls_sta);

+

+		//FTIE

+		pftie = pframe;

+		pftie_mic = pframe+4;

+		_rtw_memset(pframe, 0, 84);	//All fields except SNonce shall be set to 0

+		_rtw_memset(pframe, _FTIE_, 1);	//version

+		_rtw_memset((pframe+1), 82, 1);	//length

+		_rtw_memcpy((pframe+20), ptdls_sta->ANonce, 32);

+		_rtw_memcpy((pframe+52), ptdls_sta->SNonce, 32);

+		pframe += 84;

+		pattrib->pktlen += 84;

+

+		//Timeout interval

+		ptimeout_ie = pframe;

+		timeout_itvl[0]=0x02;

+		_rtw_memcpy(timeout_itvl+1, (u8 *)(&ptdls_sta->TDLS_PeerKey_Lifetime), 4);

+		pframe = rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, 5, timeout_itvl,  &(pattrib->pktlen));

+	}

+

+	//Sup_reg_classes(optional)

+	//HT capabilities

+	pframe = rtw_tdls_set_ht_cap(padapter, pframe, pattrib);

+

+	//20/40 BSS coexistence

+	if(pmlmepriv->num_FortyMHzIntolerant>0)

+		iedata |= BIT(2);//20 MHz BSS Width Request

+	pframe = rtw_set_ie(pframe, EID_BSSCoexistence,  1, &iedata, &(pattrib->pktlen));

+

+	//Link identifier

+	plinkid_ie = pframe;

+	_rtw_memcpy(link_id_addr, pattrib->ra, 6);

+	_rtw_memcpy((link_id_addr+6), pattrib->dst, 6);

+	_rtw_memcpy((link_id_addr+12), pattrib->src, 6);

+	pframe = rtw_set_ie(pframe, _LINK_ID_IE_,  18, link_id_addr, &(pattrib->pktlen));

+

+	//fill FTIE mic

+	if(pattrib->encrypt)

+		wpa_tdls_ftie_mic(ptdls_sta->tpk.kck, 2, plinkid_ie, prsnie, ptimeout_ie, pftie, pftie_mic);

+

+#ifdef CONFIG_WFD

+	wfd_ie_tdls( padapter, pframe, &(pattrib->pktlen) );

+#endif //CONFIG_WFD

+

+}

+

+void rtw_build_tdls_setup_cfm_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)

+{

+

+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);

+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);

+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;

+	struct sta_info *ptdls_sta=rtw_get_stainfo( (&padapter->stapriv) , pattrib->dst);

+

+	u8 payload_type = 0x02;	

+	unsigned char category = RTW_WLAN_CATEGORY_TDLS;

+	unsigned char action = TDLS_SETUP_CONFIRM;

+	u8 more_supportedrates = 0;

+	unsigned int ie_len;

+	unsigned char *p;

+	u8 timeout_itvl[5];	//set timeout interval to maximum value

+	struct mlme_priv 		*pmlmepriv = &padapter->mlmepriv;

+	u8	link_id_addr[18] = {0};

+	u8  *pftie, *ptimeout_ie, *plinkid_ie, *prsnie, *pftie_mic;

+

+	//payload type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));

+	//category, action, status code, dialog token

+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 2, (u8 *)&ptdls_sta->stat_code, &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(ptdls_sta->dialog), &(pattrib->pktlen));

+

+	if(ptdls_sta->stat_code!=0)	//invalid setup request

+		return;

+	

+	//RSNIE

+	if(pattrib->encrypt){

+		prsnie = pframe;

+		pframe = rtw_set_ie(pframe, _RSN_IE_2_, 20, TDLS_RSNIE, &(pattrib->pktlen));

+	}

+	

+	//EDCA param set; WMM param ele.

+	if(pattrib->encrypt){

+		//FTIE

+		pftie = pframe;

+		pftie_mic = pframe+4;

+		_rtw_memset(pframe, 0, 84);	//All fields except SNonce shall be set to 0

+		_rtw_memset(pframe, _FTIE_, 1);	//version

+		_rtw_memset((pframe+1), 82, 1);	//length

+		_rtw_memcpy((pframe+20), ptdls_sta->ANonce, 32);

+		_rtw_memcpy((pframe+52), ptdls_sta->SNonce, 32);

+		pframe += 84;

+		pattrib->pktlen += 84;

+

+		//Timeout interval

+		ptimeout_ie = pframe;

+		timeout_itvl[0]=0x02;

+		_rtw_memcpy(timeout_itvl+1, (u8 *)(&ptdls_sta->TDLS_PeerKey_Lifetime), 4);

+		ptdls_sta->TPK_count=0;

+		_set_timer(&ptdls_sta->TPK_timer, ptdls_sta->TDLS_PeerKey_Lifetime/TPK_RESEND_COUNT);

+		pframe = rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, 5, timeout_itvl,  &(pattrib->pktlen));

+	}

+

+	//HT operation; todo

+	//Link identifier

+	plinkid_ie = pframe;

+	_rtw_memcpy(link_id_addr, pattrib->ra, 6);

+	_rtw_memcpy((link_id_addr+6), pattrib->src, 6);

+	_rtw_memcpy((link_id_addr+12), pattrib->dst, 6);

+	pframe = rtw_set_ie(pframe, _LINK_ID_IE_,  18, link_id_addr, &(pattrib->pktlen));

+

+	//fill FTIE mic

+	if(pattrib->encrypt)

+		wpa_tdls_ftie_mic(ptdls_sta->tpk.kck, 3, plinkid_ie, prsnie, ptimeout_ie, pftie, pftie_mic);

+

+}

+

+void rtw_build_tdls_teardown_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)

+{

+

+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;

+	u8 payload_type = 0x02;

+	unsigned char category = RTW_WLAN_CATEGORY_TDLS;

+	unsigned char action = TDLS_TEARDOWN;

+	u8	link_id_addr[18] = {0};

+	

+	struct sta_info *ptdls_sta = rtw_get_stainfo( &(padapter->stapriv) , pattrib->dst);

+	struct sta_priv 	*pstapriv = &padapter->stapriv;

+

+	//payload type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));	

+	//category, action, reason code

+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, (u8 *)&ptdls_sta->stat_code, &(pattrib->pktlen));

+

+	//Link identifier

+	if(ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE){	

+		_rtw_memcpy(link_id_addr, pattrib->ra, 6);

+		_rtw_memcpy((link_id_addr+6), pattrib->src, 6);

+		_rtw_memcpy((link_id_addr+12), pattrib->dst, 6);

+	}else  if(ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE){

+		_rtw_memcpy(link_id_addr, pattrib->ra, 6);

+		_rtw_memcpy((link_id_addr+6), pattrib->dst, 6);

+		_rtw_memcpy((link_id_addr+12), pattrib->src, 6);

+	}

+	pframe = rtw_set_ie(pframe, _LINK_ID_IE_,  18, link_id_addr, &(pattrib->pktlen));

+	

+}

+

+void rtw_build_tdls_dis_req_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)

+{

+

+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;

+	u8 payload_type = 0x02;

+	u8 category = RTW_WLAN_CATEGORY_TDLS;

+	u8 action = TDLS_DISCOVERY_REQUEST;

+	u8	link_id_addr[18] = {0};

+	static u8 dialogtoken=0;

+

+	//payload type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));		

+	//category, action, reason code

+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogtoken), &(pattrib->pktlen));

+	dialogtoken = (dialogtoken+1)%256;

+

+	//Link identifier

+	_rtw_memcpy(link_id_addr, pattrib->ra, 6);

+	_rtw_memcpy((link_id_addr+6), pattrib->src, 6);

+	_rtw_memcpy((link_id_addr+12), pattrib->dst, 6);

+	pframe = rtw_set_ie(pframe, _LINK_ID_IE_,  18, link_id_addr, &(pattrib->pktlen));

+	

+}

+

+void rtw_build_tdls_dis_rsp_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, u8 dialog)

+{

+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;

+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;

+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;

+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;

+

+	u8 category = RTW_WLAN_CATEGORY_PUBLIC;

+	u8 action = TDLS_DISCOVERY_RESPONSE;

+	u8 bssrate[NDIS_802_11_LENGTH_RATES_EX];

+ 	int bssrate_len = 0;

+	u8 more_supportedrates = 0;

+	u8 *p;

+	struct mlme_priv 		*pmlmepriv = &padapter->mlmepriv;

+	u8 link_id_addr[18] = {0};

+	u8 iedata=0;

+	u8 timeout_itvl[5];	//set timeout interval to maximum value

+	u32 timeout_interval= TPK_RESEND_COUNT * 1000;

+	

+	//category, action, dialog token

+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialog), &(pattrib->pktlen));

+

+	//capability

+	_rtw_memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);

+

+	if(pattrib->encrypt)

+		*pframe =*pframe | BIT(4);

+	pframe += 2;

+	pattrib->pktlen += 2;

+

+	//supported rates

+	rtw_set_supported_rate(bssrate, WIRELESS_11BG_24N);

+	bssrate_len = IEEE80211_CCK_RATE_LEN + IEEE80211_NUM_OFDM_RATESLEN;

+

+	if (bssrate_len > 8)

+	{

+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));

+		more_supportedrates = 1;

+	}

+	else

+	{

+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));

+	}

+

+	//extended supported rates

+	if(more_supportedrates==1){

+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));

+	}

+

+	//supported channels

+	pframe = rtw_tdls_set_sup_ch(pmlmeext, pframe, pattrib);

+

+	//RSNIE

+	if(pattrib->encrypt)

+		pframe = rtw_set_ie(pframe, _RSN_IE_2_, 20, TDLS_RSNIE, &(pattrib->pktlen));

+	

+	//extended capability

+	pframe = rtw_set_ie(pframe, _EXT_CAP_IE_ , 5, TDLS_EXT_CAPIE, &(pattrib->pktlen));

+

+	if(pattrib->encrypt){

+		//FTIE

+		_rtw_memset(pframe, 0, 84);	//All fields shall be set to 0

+		_rtw_memset(pframe, _FTIE_, 1);	//version

+		_rtw_memset((pframe+1), 82, 1);	//length

+		pframe += 84;

+		pattrib->pktlen += 84;

+

+		//Timeout interval

+		timeout_itvl[0]=0x02;

+		_rtw_memcpy(timeout_itvl+1, &timeout_interval, 4);

+		pframe = rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, 5, timeout_itvl,  &(pattrib->pktlen));

+	}

+

+	//Sup_reg_classes(optional)

+	//HT capabilities

+	pframe = rtw_tdls_set_ht_cap(padapter, pframe, pattrib);

+

+	//20/40 BSS coexistence

+	if(pmlmepriv->num_FortyMHzIntolerant>0)

+		iedata |= BIT(2);//20 MHz BSS Width Request

+	pframe = rtw_set_ie(pframe, EID_BSSCoexistence, 1, &iedata, &(pattrib->pktlen));

+

+	//Link identifier

+	_rtw_memcpy(link_id_addr, pattrib->ra, 6);

+	_rtw_memcpy((link_id_addr+6), pattrib->dst, 6);

+	_rtw_memcpy((link_id_addr+12), pattrib->src, 6);

+	pframe = rtw_set_ie(pframe, _LINK_ID_IE_, 18, link_id_addr, &(pattrib->pktlen));

+	

+}

+

+void rtw_build_tdls_peer_traffic_indication_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)

+{

+

+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;

+	u8 payload_type = 0x02;

+	unsigned char category = RTW_WLAN_CATEGORY_TDLS;

+	unsigned char action = TDLS_PEER_TRAFFIC_INDICATION;

+

+	u8	link_id_addr[18] = {0};

+	u8 AC_queue=0;

+	struct sta_priv 	*pstapriv = &padapter->stapriv;	

+	struct sta_info *ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst);

+

+	//payload type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));	

+	//category, action, reason code

+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(ptdls_sta->dialog), &(pattrib->pktlen));

+

+	//Link identifier

+	_rtw_memcpy(link_id_addr, pattrib->ra, 6);

+	_rtw_memcpy((link_id_addr+6), pattrib->src, 6);

+	_rtw_memcpy((link_id_addr+12), pattrib->dst, 6);

+	pframe = rtw_set_ie(pframe, _LINK_ID_IE_,  18, link_id_addr, &(pattrib->pktlen));

+

+	//PTI control

+	//PU buffer status

+	if(ptdls_sta->uapsd_bk&BIT(1))

+		AC_queue=BIT(0);

+	if(ptdls_sta->uapsd_be&BIT(1))

+		AC_queue=BIT(1);

+	if(ptdls_sta->uapsd_vi&BIT(1))

+		AC_queue=BIT(2);

+	if(ptdls_sta->uapsd_vo&BIT(1))

+		AC_queue=BIT(3);

+	pframe = rtw_set_ie(pframe, _PTI_BUFFER_STATUS_, 1, &AC_queue, &(pattrib->pktlen));

+	

+}

+

+void rtw_build_tdls_ch_switch_req_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)

+{

+

+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;

+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;

+	u8 payload_type = 0x02;

+	unsigned char category = RTW_WLAN_CATEGORY_TDLS;

+	unsigned char action = TDLS_CHANNEL_SWITCH_REQUEST;

+	u8	link_id_addr[18] = {0};

+	struct sta_priv 	*pstapriv = &padapter->stapriv;	

+	struct sta_info *ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst);

+	u8 ch_switch_timing[4] = {0};

+	u16 switch_time= CH_SWITCH_TIME, switch_timeout=CH_SWITCH_TIMEOUT;	

+

+	//payload type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));	

+	//category, action, target_ch

+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(ptdlsinfo->candidate_ch), &(pattrib->pktlen));

+

+	//Link identifier

+	_rtw_memcpy(link_id_addr, pattrib->ra, 6);

+	_rtw_memcpy((link_id_addr+6), pattrib->src, 6);

+	_rtw_memcpy((link_id_addr+12), pattrib->dst, 6);

+	pframe = rtw_set_ie(pframe, _LINK_ID_IE_,  18, link_id_addr, &(pattrib->pktlen));

+

+	//ch switch timing

+	_rtw_memcpy(ch_switch_timing, &switch_time, 2);

+	_rtw_memcpy(ch_switch_timing+2, &switch_timeout, 2);

+	pframe = rtw_set_ie(pframe, _CH_SWITCH_TIMING_,  4, ch_switch_timing, &(pattrib->pktlen));

+

+	//update ch switch attrib to sta_info

+	ptdls_sta->off_ch=ptdlsinfo->candidate_ch;

+	ptdls_sta->ch_switch_time=switch_time;

+	ptdls_sta->ch_switch_timeout=switch_timeout;

+

+}

+

+void rtw_build_tdls_ch_switch_rsp_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)

+{

+

+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;

+	u8 payload_type = 0x02;

+	unsigned char category = RTW_WLAN_CATEGORY_TDLS;

+	unsigned char action = TDLS_CHANNEL_SWITCH_RESPONSE;

+	u8	link_id_addr[18] = {0};

+	struct sta_priv 	*pstapriv = &padapter->stapriv;	

+	struct sta_info *ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst);

+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;

+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;

+	u8 ch_switch_timing[4] = {0};

+

+	//payload type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));	

+	//category, action, status_code

+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 2, (u8 *)&ptdls_sta->stat_code, &(pattrib->pktlen));

+

+	//Link identifier

+	_rtw_memcpy(link_id_addr, pattrib->ra, 6);

+	_rtw_memcpy((link_id_addr+6), pattrib->src, 6);

+	_rtw_memcpy((link_id_addr+12), pattrib->dst, 6);

+	pframe = rtw_set_ie(pframe, _LINK_ID_IE_,  18, link_id_addr, &(pattrib->pktlen));

+

+	//ch switch timing

+	_rtw_memcpy(ch_switch_timing, &ptdls_sta->ch_switch_time, 2);

+	_rtw_memcpy(ch_switch_timing+2, &ptdls_sta->ch_switch_timeout, 2);

+	pframe = rtw_set_ie(pframe, _CH_SWITCH_TIMING_,  4, ch_switch_timing, &(pattrib->pktlen));

+

+}

+

+#ifdef CONFIG_WFD

+void rtw_build_tunneled_probe_req_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)

+{

+

+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;

+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;

+	struct wifidirect_info *pbuddy_wdinfo = &padapter->pbuddy_adapter->wdinfo;

+	u8 payload_type = 0x02;

+	u8 category = RTW_WLAN_CATEGORY_P2P;

+	u8 WFA_OUI[3] = { 0x50, 0x6f, 0x9a};

+	u8 probe_req = 4;

+	u8 wfdielen = 0;

+

+	//payload type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));		

+	//category, OUI, frame_body_type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 3, WFA_OUI, &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(probe_req), &(pattrib->pktlen));

+

+	if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))

+	{

+		wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe);

+		pframe += wfdielen;

+		pattrib->pktlen += wfdielen;

+	}

+	else if(!rtw_p2p_chk_state(pbuddy_wdinfo, P2P_STATE_NONE))

+	{

+		wfdielen = build_probe_req_wfd_ie(pbuddy_wdinfo, pframe);

+		pframe += wfdielen;

+		pattrib->pktlen += wfdielen;

+	}

+	

+}

+

+void rtw_build_tunneled_probe_rsp_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe)

+{

+

+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;

+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;

+	struct wifidirect_info *pbuddy_wdinfo = &padapter->pbuddy_adapter->wdinfo;

+	u8 payload_type = 0x02;

+	u8 category = RTW_WLAN_CATEGORY_P2P;

+	u8 WFA_OUI[3] = { 0x50, 0x6f, 0x9a};

+	u8 probe_rsp = 5;

+	u8 wfdielen = 0;

+

+	//payload type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));		

+	//category, OUI, frame_body_type

+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 3, WFA_OUI, &(pattrib->pktlen));

+	pframe = rtw_set_fixed_ie(pframe, 1, &(probe_rsp), &(pattrib->pktlen));

+

+	if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))

+	{

+		wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 1);

+		pframe += wfdielen;

+		pattrib->pktlen += wfdielen;

+	}

+	else if(!rtw_p2p_chk_state(pbuddy_wdinfo, P2P_STATE_NONE))

+	{

+		wfdielen = build_probe_resp_wfd_ie(pbuddy_wdinfo, pframe, 1);

+		pframe += wfdielen;

+		pattrib->pktlen += wfdielen;

+	}

+

+}

+#endif //CONFIG_WFD

+

+void _TPK_timer_hdl(void *FunctionContext)

+{

+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;

+

+	ptdls_sta->TPK_count++;

+	//TPK_timer set 1000 as default

+	//retry timer should set at least 301 sec.

+	if(ptdls_sta->TPK_count==TPK_RESEND_COUNT){

+		ptdls_sta->TPK_count=0;

+		issue_tdls_setup_req(ptdls_sta->padapter, ptdls_sta->hwaddr);

+	}

+	

+	_set_timer(&ptdls_sta->TPK_timer, ptdls_sta->TDLS_PeerKey_Lifetime/TPK_RESEND_COUNT);

+}

+

+void init_TPK_timer(_adapter *padapter, struct sta_info *psta)

+{

+	psta->padapter=padapter;

+

+	_init_timer(&psta->TPK_timer, padapter->pnetdev, _TPK_timer_hdl, psta);

+}

+

+// TDLS_DONE_CH_SEN: channel sensing and report candidate channel

+// TDLS_OFF_CH: first time set channel to off channel

+// TDLS_BASE_CH: when go back to the channel linked with AP, send null data to peer STA as an indication

+void _ch_switch_timer_hdl(void *FunctionContext)

+{

+

+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;

+	_adapter *padapter = ptdls_sta->padapter;

+	

+	if( ptdls_sta->option == TDLS_DONE_CH_SEN ){

+		rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_DONE_CH_SEN);

+	}else if( ptdls_sta->option == TDLS_OFF_CH ){

+		issue_nulldata_to_TDLS_peer_STA(ptdls_sta->padapter, ptdls_sta, 0);

+		_set_timer(&ptdls_sta->base_ch_timer, 500);

+	}else if( ptdls_sta->option == TDLS_BASE_CH){

+		issue_nulldata_to_TDLS_peer_STA(ptdls_sta->padapter, ptdls_sta, 0);

+	}

+}

+

+void init_ch_switch_timer(_adapter *padapter, struct sta_info *psta)

+{

+	psta->padapter=padapter;

+	_init_timer(&psta->option_timer, padapter->pnetdev, _ch_switch_timer_hdl, psta);

+}

+

+void _base_ch_timer_hdl(void *FunctionContext)

+{

+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;

+	rtw_tdls_cmd(ptdls_sta->padapter, ptdls_sta->hwaddr, TDLS_P_OFF_CH);

+}

+

+void init_base_ch_timer(_adapter *padapter, struct sta_info *psta)

+{

+	psta->padapter=padapter;

+	_init_timer(&psta->base_ch_timer, padapter->pnetdev, _base_ch_timer_hdl, psta);

+}

+

+void _off_ch_timer_hdl(void *FunctionContext)

+{

+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;

+	rtw_tdls_cmd(ptdls_sta->padapter, ptdls_sta->hwaddr, TDLS_P_BASE_CH );

+}

+	

+void init_off_ch_timer(_adapter *padapter, struct sta_info *psta)

+{

+	psta->padapter=padapter;

+	_init_timer(&psta->off_ch_timer, padapter->pnetdev, _off_ch_timer_hdl, psta);

+}

+

+void _tdls_handshake_timer_hdl(void *FunctionContext)

+{

+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;

+

+	if(ptdls_sta != NULL)

+	{

+		if( !(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) )

+		{

+			DBG_871X("tdls handshake time out\n");

+			free_tdls_sta(ptdls_sta->padapter, ptdls_sta);

+		}

+	}

+}

+

+void init_handshake_timer(_adapter *padapter, struct sta_info *psta)

+{

+	psta->padapter=padapter;

+	_init_timer(&psta->handshake_timer, padapter->pnetdev, _tdls_handshake_timer_hdl, psta);

+}

+

+//Check tdls peer sta alive.

+void _tdls_alive_timer_phase1_hdl(void *FunctionContext)

+{

+	_irqL irqL;

+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;

+	_adapter *padapter = ptdls_sta->padapter;

+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;

+	

+	_enter_critical_bh(&ptdlsinfo->hdl_lock, &irqL);

+	ptdls_sta->timer_flag = 1;

+	_exit_critical_bh(&ptdlsinfo->hdl_lock, &irqL);

+

+	ptdls_sta->tdls_sta_state &= (~TDLS_ALIVE_STATE);

+

+	DBG_871X("issue_tdls_dis_req to check alive\n");

+	issue_tdls_dis_req( padapter, ptdls_sta->hwaddr);

+	rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CKALV_PH1);

+	sta_update_last_rx_pkts(ptdls_sta);

+

+	if (	ptdls_sta->timer_flag == 2 )

+		rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_FREE_STA);		

+	else

+	{

+		_enter_critical_bh(&ptdlsinfo->hdl_lock, &irqL);

+		ptdls_sta->timer_flag = 0;

+		_exit_critical_bh(&ptdlsinfo->hdl_lock, &irqL);

+	}

+

+}

+

+void _tdls_alive_timer_phase2_hdl(void *FunctionContext)

+{

+	_irqL irqL;

+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;

+	_adapter *padapter = ptdls_sta->padapter;

+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;

+	

+	_enter_critical_bh(&(ptdlsinfo->hdl_lock), &irqL);

+	ptdls_sta->timer_flag = 1;

+	_exit_critical_bh(&ptdlsinfo->hdl_lock, &irqL);

+

+	if( (ptdls_sta->tdls_sta_state & TDLS_ALIVE_STATE) && 

+		(sta_last_rx_pkts(ptdls_sta) + 3 <= sta_rx_pkts(ptdls_sta)) )

+	{

+		DBG_871X("TDLS STA ALIVE, ptdls_sta->sta_stats.last_rx_pkts:%llu, ptdls_sta->sta_stats.rx_pkts:%llu\n",

+			sta_last_rx_pkts(ptdls_sta), sta_rx_pkts(ptdls_sta));

+

+		ptdls_sta->alive_count = 0;

+		rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CKALV_PH2);

+	}

+	else

+	{

+		if( !(ptdls_sta->tdls_sta_state & TDLS_ALIVE_STATE) )

+			DBG_871X("TDLS STA TOO FAR\n");

+		if( !(sta_last_rx_pkts(ptdls_sta) + 3 <= sta_rx_pkts(ptdls_sta)))

+			DBG_871X("TDLS LINK WITH LOW TRAFFIC, ptdls_sta->sta_stats.last_rx_pkts:%llu, ptdls_sta->sta_stats.rx_pkts:%llu\n",

+				sta_last_rx_pkts(ptdls_sta), sta_rx_pkts(ptdls_sta));

+

+		ptdls_sta->alive_count++;

+		if( ptdls_sta->alive_count == TDLS_ALIVE_COUNT )

+		{

+			ptdls_sta->stat_code = _RSON_TDLS_TEAR_TOOFAR_;

+			issue_tdls_teardown(padapter, ptdls_sta->hwaddr);

+		}

+		else

+		{

+			rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CKALV_PH2);

+		}

+	}

+

+	if (	ptdls_sta->timer_flag == 2 )

+		rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_FREE_STA);		

+	else

+	{

+		_enter_critical_bh(&(ptdlsinfo->hdl_lock), &irqL);

+		ptdls_sta->timer_flag = 0;

+		_exit_critical_bh(&ptdlsinfo->hdl_lock, &irqL);

+}

+

+}

+

+void init_tdls_alive_timer(_adapter *padapter, struct sta_info *psta)

+{

+	psta->padapter=padapter;

+	_init_timer(&psta->alive_timer1, padapter->pnetdev, _tdls_alive_timer_phase1_hdl, psta);

+	_init_timer(&psta->alive_timer2, padapter->pnetdev, _tdls_alive_timer_phase2_hdl, psta);

+}

+

+int update_sgi_tdls(_adapter *padapter, struct sta_info *psta)

+{

+	struct ht_priv	*psta_ht = NULL;

+	psta_ht = &psta->htpriv;

+

+	if(psta_ht->ht_option)

+	{

+		return psta_ht->sgi;

+	}

+	else

+		return _FALSE;

+}

+

+u32 update_mask_tdls(_adapter *padapter, struct sta_info *psta)

+{

+	int i;

+	u8 rf_type, id;

+	unsigned char sta_band = 0;

+	unsigned char limit;	

+	unsigned int tx_ra_bitmap=0;

+	struct ht_priv	*psta_ht = NULL;

+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);

+	WLAN_BSSID_EX *pcur_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;

+

+	psta_ht = &psta->htpriv;

+	//b/g mode ra_bitmap  

+	for (i=0; i<sizeof(psta->bssrateset); i++)

+	{

+		if (psta->bssrateset[i])

+			tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value(psta->bssrateset[i]&0x7f);

+	}

+

+	//n mode ra_bitmap

+	if(psta_ht->ht_option) 

+	{

+		padapter->HalFunc.GetHwRegHandler(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));

+		if(rf_type == RF_2T2R)

+			limit=16;// 2R

+		else

+			limit=8;//  1R

+

+		for (i=0; i<limit; i++) {

+			if (psta_ht->ht_cap.supp_mcs_set[i/8] & BIT(i%8))

+				tx_ra_bitmap |= BIT(i+12);

+		}

+	}

+

+	if ( pcur_network->Configuration.DSConfig > 14 ) {

+		// 5G band

+		if (tx_ra_bitmap & 0xffff000)

+			sta_band |= WIRELESS_11_5N | WIRELESS_11A;

+		else

+			sta_band |= WIRELESS_11A;

+	} else {

+		if (tx_ra_bitmap & 0xffff000)

+			sta_band |= WIRELESS_11_24N | WIRELESS_11G | WIRELESS_11B;

+		else if (tx_ra_bitmap & 0xff0)

+			sta_band |= WIRELESS_11G |WIRELESS_11B;

+		else

+			sta_band |= WIRELESS_11B;

+	}

+

+	id = networktype_to_raid(sta_band);

+	tx_ra_bitmap |= ((id<<28)&0xf0000000);

+	return tx_ra_bitmap;

+}

+

+#endif //CONFIG_TDLS

+

diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_wapi.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_wapi.c
new file mode 100755
index 00000000..6c6268c0
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_wapi.c
@@ -0,0 +1,1326 @@
+#ifdef CONFIG_WAPI_SUPPORT

+

+#include <linux/unistd.h>

+#include <linux/etherdevice.h>

+#include <drv_types.h>

+#include <rtw_wapi.h>

+

+

+u32 wapi_debug_component =

+//				WAPI_INIT	|

+//				WAPI_API    	|

+//				WAPI_TX    	|

+//				WAPI_RX    	|

+				WAPI_ERR ; //always open err flags on

+

+void WapiFreeAllStaInfo(_adapter *padapter)

+{

+	PRT_WAPI_T				pWapiInfo;

+	PRT_WAPI_STA_INFO		pWapiStaInfo;

+	PRT_WAPI_BKID			pWapiBkid;

+

+	WAPI_TRACE(WAPI_INIT, "===========> %s\n", __FUNCTION__);

+	pWapiInfo = &padapter->wapiInfo;

+

+	//Pust to Idle List

+	rtw_wapi_return_all_sta_info(padapter);

+

+	//Sta Info List

+	while(!list_empty(&(pWapiInfo->wapiSTAIdleList)))

+	{

+		pWapiStaInfo = (PRT_WAPI_STA_INFO)list_entry(pWapiInfo->wapiSTAIdleList.next, RT_WAPI_STA_INFO, list);

+		list_del_init(&pWapiStaInfo->list);

+	}

+

+	//BKID List

+	while(!list_empty(&(pWapiInfo->wapiBKIDIdleList)))

+	{

+		pWapiBkid = (PRT_WAPI_BKID)list_entry(pWapiInfo->wapiBKIDIdleList.next, RT_WAPI_BKID, list);

+		list_del_init(&pWapiBkid->list);

+	}

+	WAPI_TRACE(WAPI_INIT, "<=========== %s\n", __FUNCTION__);

+	return;

+}

+

+void WapiSetIE(_adapter *padapter)

+{

+	PRT_WAPI_T		pWapiInfo = &(padapter->wapiInfo);

+	//PRT_WAPI_BKID	pWapiBkid;

+	u16		protocolVer = 1;

+	u16		akmCnt = 1;

+	u16		suiteCnt = 1;

+	u16		capability = 0;

+	u8		OUI[3];

+

+	OUI[0] = 0x00;

+	OUI[1] = 0x14;

+	OUI[2] = 0x72;

+

+	pWapiInfo->wapiIELength = 0;

+//protocol version

+	memcpy(pWapiInfo->wapiIE+pWapiInfo->wapiIELength, &protocolVer, 2);

+	pWapiInfo->wapiIELength +=2;

+//akm

+	memcpy(pWapiInfo->wapiIE+pWapiInfo->wapiIELength, &akmCnt, 2);

+	pWapiInfo->wapiIELength +=2;

+

+	if(pWapiInfo->bWapiPSK){

+		memcpy(pWapiInfo->wapiIE+pWapiInfo->wapiIELength,OUI, 3);

+		pWapiInfo->wapiIELength +=3;

+		pWapiInfo->wapiIE[pWapiInfo->wapiIELength] = 0x2;

+		pWapiInfo->wapiIELength +=1;

+	}else{

+		memcpy(pWapiInfo->wapiIE+pWapiInfo->wapiIELength,OUI, 3);

+		pWapiInfo->wapiIELength +=3;

+		pWapiInfo->wapiIE[pWapiInfo->wapiIELength] = 0x1;

+		pWapiInfo->wapiIELength +=1;

+	}

+

+//usk

+	memcpy(pWapiInfo->wapiIE+pWapiInfo->wapiIELength, &suiteCnt, 2);

+	pWapiInfo->wapiIELength +=2;

+	memcpy(pWapiInfo->wapiIE+pWapiInfo->wapiIELength,OUI, 3);

+	pWapiInfo->wapiIELength +=3;

+	pWapiInfo->wapiIE[pWapiInfo->wapiIELength] = 0x1;

+	pWapiInfo->wapiIELength +=1;

+

+//msk

+	memcpy(pWapiInfo->wapiIE+pWapiInfo->wapiIELength,OUI, 3);

+	pWapiInfo->wapiIELength +=3;

+	pWapiInfo->wapiIE[pWapiInfo->wapiIELength] = 0x1;

+	pWapiInfo->wapiIELength +=1;

+

+//Capbility

+	memcpy(pWapiInfo->wapiIE+pWapiInfo->wapiIELength, &capability, 2);

+	pWapiInfo->wapiIELength +=2;

+}

+

+

+/*  PN1 > PN2, return 1,

+ *  else return 0.

+ */

+u32 WapiComparePN(u8 *PN1, u8 *PN2)

+{

+	char i;

+

+	if ((NULL == PN1) || (NULL == PN2))

+		return 1;

+

+	// overflow case

+	if ((PN2[15] - PN1[15]) & 0x80)

+		return 1;

+

+	for (i=16; i>0; i--)

+	{

+		if(PN1[i-1] == PN2[i-1])

+		    	continue;

+		else if(PN1[i-1] > PN2[i-1])

+			return 1;

+		else

+			return 0;

+	}

+

+	return 0;

+}

+

+u8

+WapiGetEntryForCamWrite(_adapter *padapter,u8 *pMacAddr,u8 KID,BOOLEAN IsMsk)

+{

+	PRT_WAPI_T		pWapiInfo=NULL;

+	//PRT_WAPI_CAM_ENTRY	pEntry=NULL;

+	u8 i=0;

+	u8 ret = 0xff;

+

+	WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__);

+

+	pWapiInfo =  &padapter->wapiInfo;

+

+	//exist?

+	for(i=0;i<WAPI_CAM_ENTRY_NUM;i++)

+	{

+		if(pWapiInfo->wapiCamEntry[i].IsUsed

+			&& (_rtw_memcmp(pMacAddr, pWapiInfo->wapiCamEntry[i].PeerMacAddr, ETH_ALEN) == _TRUE)

+			&& pWapiInfo->wapiCamEntry[i].keyidx == KID

+			&& pWapiInfo->wapiCamEntry[i].type == IsMsk)

+		{

+			ret = pWapiInfo->wapiCamEntry[i].entry_idx; //cover it

+			break;

+		}

+	}

+

+	if(i == WAPI_CAM_ENTRY_NUM) //not found

+	{

+		for(i=0;i<WAPI_CAM_ENTRY_NUM;i++)

+		{

+			if(pWapiInfo->wapiCamEntry[i].IsUsed == 0)

+			{

+				pWapiInfo->wapiCamEntry[i].IsUsed = 1;

+				pWapiInfo->wapiCamEntry[i].type = IsMsk;

+				pWapiInfo->wapiCamEntry[i].keyidx = KID;

+				_rtw_memcpy(pWapiInfo->wapiCamEntry[i].PeerMacAddr, pMacAddr,ETH_ALEN);

+				ret = pWapiInfo->wapiCamEntry[i].entry_idx;

+				break;

+			}

+		}

+	}

+

+	WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__);

+	return ret;

+

+/*

+	if(RTIsListEmpty(&pWapiInfo->wapiCamIdleList)){

+		RT_TRACE(COMP_SEC,DBG_LOUD,("No Entry for wapi!!!\n"));

+		return 0;

+	}

+

+	pEntry = (PRT_WAPI_CAM_ENTRY)RTRemoveHeadList(&pWapiInfo->wapiCamIdleList);

+	RTInsertTailList(&pWapiInfo->wapiCamUsedList, &pEntry->list);

+

+	RT_TRACE(COMP_SEC,DBG_LOUD,("<====WapiGetCamEntry(),Get Entry Idx:%d.but we just return 4 for test\n",pEntry->entry_idx));

+

+	return pEntry->entry_idx;*/

+}

+

+u8 WapiGetEntryForCamClear(_adapter *padapter,u8 *pPeerMac,u8 keyid,u8 IsMsk)

+{

+	PRT_WAPI_T		pWapiInfo=NULL;

+	u8		i=0;

+

+	WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__);

+

+	pWapiInfo =  &padapter->wapiInfo;

+

+	for(i=0;i<WAPI_CAM_ENTRY_NUM;i++)

+	{

+		if(pWapiInfo->wapiCamEntry[i].IsUsed

+			&& (_rtw_memcmp(pPeerMac, pWapiInfo->wapiCamEntry[i].PeerMacAddr, ETH_ALEN) == _TRUE)

+			&& pWapiInfo->wapiCamEntry[i].keyidx == keyid

+			&& pWapiInfo->wapiCamEntry[i].type == IsMsk)

+		{

+				pWapiInfo->wapiCamEntry[i].IsUsed = 0;

+				pWapiInfo->wapiCamEntry[i].keyidx = 2;

+				_rtw_memset(pWapiInfo->wapiCamEntry[i].PeerMacAddr,0,ETH_ALEN);

+

+				WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__);

+				return pWapiInfo->wapiCamEntry[i].entry_idx;

+		}

+	}

+

+	WAPI_TRACE(WAPI_API,"<====WapiGetReturnCamEntry(), No this cam entry.\n");

+	return 0xff;

+/*

+	if(RTIsListEmpty(&pWapiInfo->wapiCamUsedList)){

+		RT_TRACE(COMP_SEC,DBG_LOUD,("No Entry for wapi!!!\n"));

+		return FALSE;

+	}

+

+	pList = &pWapiInfo->wapiCamUsedList;

+	while(pList->Flink != &pWapiInfo->wapiCamUsedList)

+	{

+		pEntry = (PRT_WAPI_CAM_ENTRY)pList->Flink;

+		if(PlatformCompareMemory(pPeerMac,pEntry->PeerMacAddr, ETHER_ADDRLEN)== 0

+			&& keyid == pEntry->keyidx)

+		{

+			RTRemoveEntryList(pList);

+			RTInsertHeadList(&pWapiInfo->wapiCamIdleList, pList);

+			return pEntry->entry_idx;

+		}

+		pList = pList->Flink;

+	}

+

+	return 0;

+*/

+}

+

+void

+WapiResetAllCamEntry(_adapter *padapter)

+{

+	PRT_WAPI_T		pWapiInfo;

+	int				i;

+

+	WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__);

+

+	pWapiInfo =  &padapter->wapiInfo;

+

+	for (i=0;i<WAPI_CAM_ENTRY_NUM;i++)

+	{

+		_rtw_memset(pWapiInfo->wapiCamEntry[i].PeerMacAddr, 0, ETH_ALEN);

+		pWapiInfo->wapiCamEntry[i].IsUsed = 0;

+	 	pWapiInfo->wapiCamEntry[i].keyidx = 2; //invalid

+	 	pWapiInfo->wapiCamEntry[i].entry_idx = 4+i*2;

+ 	}

+

+	WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__);

+

+	return;

+}

+

+u8 WapiWriteOneCamEntry(

+	_adapter 	*padapter,

+	u8 			*pMacAddr,

+	u8 			KeyId,

+	u8			EntryId,

+	u8 			EncAlg,

+	u8 			bGroupKey,

+	u8 			*pKey

+)

+{

+	u8 retVal = 0;

+	u16 usConfig = 0;

+

+	WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__);

+

+	if(EntryId >= 32)

+	{

+		WAPI_TRACE(WAPI_ERR, "<=== CamAddOneEntry(): ulKeyId exceed!\n");

+		return retVal;

+	}

+

+	usConfig=usConfig|(0x01<<15)|((u16)(EncAlg)<<2)|(KeyId);

+

+	if(EncAlg == _SMS4_ )

+	{

+		if(bGroupKey == 1)

+			usConfig |= (0x01<<6);

+		if((EntryId % 2)==1)  // ==0 sec key; == 1mic key

+			usConfig |= (0x01<<5);

+	}

+

+	write_cam(padapter, EntryId, usConfig, pMacAddr, pKey);

+

+	WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__);

+    	return 1;

+}

+

+void rtw_wapi_init(_adapter *padapter)

+{

+	PRT_WAPI_T		pWapiInfo;

+	int				i;

+

+	WAPI_TRACE(WAPI_INIT, "===========> %s\n", __FUNCTION__);

+	RT_ASSERT_RET(padapter);

+

+	if (!padapter->WapiSupport)

+	{

+		WAPI_TRACE(WAPI_INIT, "<========== %s, WAPI not supported!\n", __FUNCTION__);

+		return;

+	}

+

+	pWapiInfo =  &padapter->wapiInfo;

+	pWapiInfo->bWapiEnable = false;

+

+	//Init BKID List

+	INIT_LIST_HEAD(&pWapiInfo->wapiBKIDIdleList);

+	INIT_LIST_HEAD(&pWapiInfo->wapiBKIDStoreList);

+	for(i=0;i<WAPI_MAX_BKID_NUM;i++)

+	{

+		list_add_tail(&pWapiInfo->wapiBKID[i].list, &pWapiInfo->wapiBKIDIdleList);

+	}

+

+	//Init STA List

+	INIT_LIST_HEAD(&pWapiInfo->wapiSTAIdleList);

+	INIT_LIST_HEAD(&pWapiInfo->wapiSTAUsedList);

+	for(i=0;i<WAPI_MAX_STAINFO_NUM;i++)

+	{

+		list_add_tail(&pWapiInfo->wapiSta[i].list, &pWapiInfo->wapiSTAIdleList);

+	}

+

+	 for (i=0;i<WAPI_CAM_ENTRY_NUM;i++)

+	 {

+		pWapiInfo->wapiCamEntry[i].IsUsed = 0;

+	 	pWapiInfo->wapiCamEntry[i].keyidx = 2; //invalid

+	 	pWapiInfo->wapiCamEntry[i].entry_idx = 4+i*2;

+ 	}

+

+	WAPI_TRACE(WAPI_INIT, "<========== %s\n", __FUNCTION__);

+}

+

+void rtw_wapi_free(_adapter *padapter)

+{

+	WAPI_TRACE(WAPI_INIT, "===========> %s\n", __FUNCTION__);

+	RT_ASSERT_RET(padapter);

+

+	if (!padapter->WapiSupport)

+	{

+		WAPI_TRACE(WAPI_INIT, "<========== %s, WAPI not supported!\n", __FUNCTION__);

+		return;

+	}

+

+	WapiFreeAllStaInfo(padapter);

+

+	WAPI_TRACE(WAPI_INIT, "<========== %s\n", __FUNCTION__);

+}

+

+void rtw_wapi_disable_tx(_adapter *padapter)

+{

+	WAPI_TRACE(WAPI_INIT, "===========> %s\n", __FUNCTION__);

+	RT_ASSERT_RET(padapter);

+

+	if (!padapter->WapiSupport)

+	{

+		WAPI_TRACE(WAPI_INIT, "<========== %s, WAPI not supported!\n", __FUNCTION__);

+		return;

+	}

+

+	padapter->wapiInfo.wapiTxMsk.bTxEnable = false;

+	padapter->wapiInfo.wapiTxMsk.bSet = false;

+

+	WAPI_TRACE(WAPI_INIT, "<========== %s\n", __FUNCTION__);

+}

+

+u8 rtw_wapi_is_wai_packet(_adapter* padapter,u8 *pkt_data)

+{

+	PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo);

+	struct mlme_priv 	*pmlmepriv = &padapter->mlmepriv;

+	struct security_priv   *psecuritypriv = &padapter->securitypriv;

+	PRT_WAPI_STA_INFO pWapiSta = NULL;

+	u8 WaiPkt = 0, *pTaddr, bFind = false;

+	u8 Offset_TypeWAI = 0 ;	// (mac header len + llc length)

+	

+	WAPI_TRACE(WAPI_TX|WAPI_RX, "===========> %s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__);

+		return 0;

+	}

+

+	 Offset_TypeWAI = 24 + 6 ;

+

+	//YJ,add,091103. Data frame may also have skb->data[30]=0x88 and skb->data[31]=0xb4.

+	if ((pkt_data[1]&0x40) !=0)

+	{

+		DBG_871X("data is privacy \n");

+	    	return 0;

+	}

+	

+	pTaddr = GetAddr2Ptr(pkt_data);

+	if(list_empty(&pWapiInfo->wapiSTAUsedList)){

+		bFind = false;

+	}else{

+		list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list){

+			if (_rtw_memcmp(pTaddr, pWapiSta->PeerMacAddr, 6) == _TRUE) {

+				bFind = true;

+				break;

+			}

+		}

+	}

+

+	WAPI_TRACE(WAPI_TX|WAPI_RX, "%s: bFind=%d pTaddr="MAC_FMT"\n", __FUNCTION__, bFind, MAC_ARG(pTaddr));

+

+	if (pkt_data[0] == WIFI_QOS_DATA_TYPE)

+	{

+		Offset_TypeWAI += 2;

+	}

+

+	// 88b4?

+	if( (pkt_data[Offset_TypeWAI]==0x88) && (pkt_data[Offset_TypeWAI+1]==0xb4) ){

+		WaiPkt = pkt_data[Offset_TypeWAI+5];

+

+		psecuritypriv->hw_decrypted = _TRUE;

+	}else{

+		WAPI_TRACE(WAPI_TX|WAPI_RX, "%s(): non wai packet\n",__FUNCTION__);

+	}

+

+	WAPI_TRACE(WAPI_TX|WAPI_RX, "%s(): Recvd WAI frame. IsWAIPkt(%d)\n",__FUNCTION__, WaiPkt);

+

+	return	WaiPkt;

+}

+

+

+void rtw_wapi_update_info(_adapter *padapter, union recv_frame *precv_frame)

+{

+	PRT_WAPI_T     pWapiInfo = &(padapter->wapiInfo);

+	struct recv_frame_hdr *precv_hdr;

+	u8 	*ptr;

+	u8 	*pTA;

+	u8 	*pRecvPN;

+

+

+	WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_RX, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__);

+		return;

+	}

+

+	precv_hdr = &precv_frame->u.hdr;

+	ptr = precv_hdr->rx_data;

+

+	if (precv_hdr->attrib.qos == 1)

+	{

+		precv_hdr->UserPriority = GetTid(ptr);

+	}

+	else

+	{

+		precv_hdr->UserPriority = 0;

+	}

+

+	pTA = GetAddr2Ptr(ptr);

+	_rtw_memcpy((u8 *)precv_hdr->WapiSrcAddr, pTA, 6);

+	pRecvPN = ptr + precv_hdr->attrib.hdrlen + 2;

+	_rtw_memcpy((u8 *)precv_hdr->WapiTempPN, pRecvPN, 16);

+

+	WAPI_TRACE(WAPI_RX, "<========== %s\n", __FUNCTION__);

+}

+

+/****************************************************************************

+TRUE-----------------Drop

+FALSE---------------- handle

+add to support WAPI to N-mode

+*****************************************************************************/

+u8 rtw_wapi_check_for_drop(

+	_adapter *padapter,

+	union recv_frame *precv_frame

+)

+{

+	PRT_WAPI_T     pWapiInfo = &(padapter->wapiInfo);

+	u8			*pLastRecvPN = NULL;

+	u8			bFind = false;

+	PRT_WAPI_STA_INFO	pWapiSta = NULL;

+	u8 			bDrop = false;

+	struct recv_frame_hdr *precv_hdr = &precv_frame->u.hdr;

+	u8					WapiAEPNInitialValueSrc[16] = {0x37,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C} ;

+	u8 					WapiAEMultiCastPNInitialValueSrc[16] = {0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C} ;

+	u8 					*ptr = precv_frame->u.hdr.rx_data;

+	int					i;

+

+	WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_RX, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__);

+		return false;

+	}

+

+	if(precv_hdr->bIsWaiPacket !=0)

+	{

+		if(precv_hdr->bIsWaiPacket== 0x8)

+		{

+

+			DBG_871X("rtw_wapi_check_for_drop: dump packet \n");

+			for(i=0;i<50;i++)

+			{

+				DBG_871X("%02X  ",ptr[i]);

+				if((i+1) %8 ==0)

+					DBG_871X("\n");

+			}

+			DBG_871X("\n rtw_wapi_check_for_drop: dump packet \n");

+

+			for(i=0;i<16;i++)

+			{

+				if(ptr[i+27] !=0)

+					break;

+			}

+

+			if(i== 16)

+			{

+				WAPI_TRACE(WAPI_RX,"rtw_wapi_check_for_drop: drop with zero BKID \n");

+				return true;

+			}

+			else

+			{

+				return false;

+			}

+		}

+			else

+				return false;

+	}

+

+	if(list_empty(&pWapiInfo->wapiSTAUsedList)){

+		bFind = false;

+	}else{

+		list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) {

+			if (_rtw_memcmp(precv_hdr->WapiSrcAddr, pWapiSta->PeerMacAddr, ETH_ALEN) == _TRUE) {

+				bFind = true;

+				break;

+			}

+		}

+	}

+	WAPI_TRACE(WAPI_RX, "%s: bFind=%d prxb->WapiSrcAddr="MAC_FMT"\n", __FUNCTION__, bFind, MAC_ARG(precv_hdr->WapiSrcAddr));

+

+	if(bFind)

+	{

+		if(IS_MCAST(precv_hdr->attrib.ra))

+		{

+			WAPI_TRACE(WAPI_RX,"rtw_wapi_check_for_drop: multicast case \n");

+			pLastRecvPN = pWapiSta->lastRxMulticastPN;

+		}

+		else

+		{

+			WAPI_TRACE(WAPI_RX,"rtw_wapi_check_for_drop: unicast case \n");

+			switch(precv_hdr->UserPriority)

+			{

+				case 0:

+				case 3:

+					pLastRecvPN = pWapiSta->lastRxUnicastPNBEQueue;

+					break;

+				case 1:

+				case 2:

+					pLastRecvPN = pWapiSta->lastRxUnicastPNBKQueue;

+					break;

+				case 4:

+				case 5:

+					pLastRecvPN = pWapiSta->lastRxUnicastPNVIQueue;

+					break;

+				case 6:

+				case 7:

+					pLastRecvPN = pWapiSta->lastRxUnicastPNVOQueue;

+					break;

+				default:

+					WAPI_TRACE(WAPI_ERR,"%s: Unknown TID \n",__FUNCTION__);

+					break;

+			}

+		}

+

+		if(!WapiComparePN(precv_hdr->WapiTempPN,pLastRecvPN))

+		{

+			WAPI_TRACE(WAPI_RX,"%s: Equal PN!!\n",__FUNCTION__);

+			if(IS_MCAST(precv_hdr->attrib.ra))

+				_rtw_memcpy(pLastRecvPN,WapiAEMultiCastPNInitialValueSrc,16);

+			else

+				_rtw_memcpy(pLastRecvPN,WapiAEPNInitialValueSrc,16);

+			bDrop = true;

+		}

+		else

+		{

+			_rtw_memcpy(pLastRecvPN,precv_hdr->WapiTempPN,16);

+		}

+	}

+

+	WAPI_TRACE(WAPI_RX, "<========== %s\n", __FUNCTION__);

+	return bDrop;

+}

+

+void rtw_build_probe_resp_wapi_ie(_adapter *padapter, unsigned char *pframe, struct pkt_attrib *pattrib)

+{

+	PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo);

+	u8 WapiIELength = 0;

+

+	WAPI_TRACE(WAPI_MLME, "===========> %s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport)  || (!pWapiInfo->bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported!\n", __FUNCTION__);

+		return;

+	}

+

+	WapiSetIE(padapter);

+	WapiIELength = pWapiInfo->wapiIELength;

+	pframe[0] = _WAPI_IE_;

+	pframe[1] = WapiIELength;

+	_rtw_memcpy(pframe+2, pWapiInfo->wapiIE, WapiIELength);

+	pframe += WapiIELength+2;

+	pattrib->pktlen += WapiIELength+2;

+

+	WAPI_TRACE(WAPI_MLME, "<========== %s\n", __FUNCTION__);

+}

+

+void rtw_build_beacon_wapi_ie(_adapter *padapter, unsigned char *pframe, struct pkt_attrib *pattrib)

+{

+	PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo);

+	u8 WapiIELength = 0;

+	WAPI_TRACE(WAPI_MLME, "===========> %s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport)  || (!pWapiInfo->bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported!\n", __FUNCTION__);

+		return;

+	}

+

+	WapiSetIE(padapter);

+	WapiIELength = pWapiInfo->wapiIELength;

+	pframe[0] = _WAPI_IE_;

+	pframe[1] = WapiIELength;

+	_rtw_memcpy(pframe+2, pWapiInfo->wapiIE, WapiIELength);

+	pframe += WapiIELength+2;

+	pattrib->pktlen += WapiIELength+2;

+

+	WAPI_TRACE(WAPI_MLME, "<========== %s\n", __FUNCTION__);

+}

+

+void rtw_build_assoc_req_wapi_ie(_adapter *padapter, unsigned char *pframe, struct pkt_attrib *pattrib)

+{

+	PRT_WAPI_BKID		pWapiBKID;

+	u16					bkidNum;

+	PRT_WAPI_T			pWapiInfo = &(padapter->wapiInfo);

+	u8					WapiIELength = 0;

+

+	WAPI_TRACE(WAPI_MLME, "===========> %s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported!\n", __FUNCTION__);

+		return;

+	}

+

+	WapiSetIE(padapter);

+	WapiIELength = pWapiInfo->wapiIELength;

+	bkidNum = 0;

+	if(!list_empty(&(pWapiInfo->wapiBKIDStoreList))){

+		list_for_each_entry(pWapiBKID, &pWapiInfo->wapiBKIDStoreList, list) {

+			bkidNum ++;

+			_rtw_memcpy(pWapiInfo->wapiIE+WapiIELength+2, pWapiBKID->bkid,16);

+			WapiIELength += 16;

+		}

+	}

+	_rtw_memcpy(pWapiInfo->wapiIE+WapiIELength, &bkidNum, 2);

+	WapiIELength += 2;

+

+	 pframe[0] = _WAPI_IE_;

+	 pframe[1] = WapiIELength;

+	_rtw_memcpy(pframe+2, pWapiInfo->wapiIE, WapiIELength);

+	pframe += WapiIELength+2;

+	pattrib->pktlen += WapiIELength+2;

+	WAPI_TRACE(WAPI_MLME, "<========== %s\n", __FUNCTION__);

+}

+

+void rtw_wapi_on_assoc_ok(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE)

+{

+	PRT_WAPI_T pWapiInfo = &(padapter->wapiInfo);

+	PRT_WAPI_STA_INFO pWapiSta;

+	u8 WapiAEPNInitialValueSrc[16] = {0x37,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C} ;

+	//u8 WapiASUEPNInitialValueSrc[16] = {0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C} ;

+	u8 WapiAEMultiCastPNInitialValueSrc[16] = {0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C} ;

+

+	WAPI_TRACE(WAPI_MLME, "===========> %s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__);

+		return;

+	}

+

+	pWapiSta =(PRT_WAPI_STA_INFO)list_entry(pWapiInfo->wapiSTAIdleList.next, RT_WAPI_STA_INFO, list);

+	list_del_init(&pWapiSta->list);

+	list_add_tail(&pWapiSta->list, &pWapiInfo->wapiSTAUsedList);

+	_rtw_memcpy(pWapiSta->PeerMacAddr,padapter->mlmeextpriv.mlmext_info.network.MacAddress,6);

+	_rtw_memcpy(pWapiSta->lastRxMulticastPN, WapiAEMultiCastPNInitialValueSrc, 16);

+	_rtw_memcpy(pWapiSta->lastRxUnicastPN, WapiAEPNInitialValueSrc, 16);

+

+	//For chenk PN error with Qos Data after s3: add by ylb 20111114

+	_rtw_memcpy(pWapiSta->lastRxUnicastPNBEQueue,WapiAEPNInitialValueSrc,16);

+	_rtw_memcpy(pWapiSta->lastRxUnicastPNBKQueue,WapiAEPNInitialValueSrc,16);

+	_rtw_memcpy(pWapiSta->lastRxUnicastPNVIQueue,WapiAEPNInitialValueSrc,16);

+	_rtw_memcpy(pWapiSta->lastRxUnicastPNVOQueue,WapiAEPNInitialValueSrc,16);

+

+	WAPI_TRACE(WAPI_MLME, "<========== %s\n", __FUNCTION__);

+}

+

+

+void rtw_wapi_return_one_sta_info(_adapter *padapter, u8 *MacAddr)

+{

+	PRT_WAPI_T				pWapiInfo;

+	PRT_WAPI_STA_INFO		pWapiStaInfo = NULL;

+	PRT_WAPI_BKID			pWapiBkid = NULL;

+	struct mlme_priv 	*pmlmepriv = &padapter->mlmepriv;

+

+	pWapiInfo = &padapter->wapiInfo;

+

+	WAPI_TRACE(WAPI_API, "==========> %s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__);

+		return;

+	}

+

+       if(check_fwstate(pmlmepriv, WIFI_STATION_STATE))

+       {

+             while(!list_empty(&(pWapiInfo->wapiBKIDStoreList)))

+                 {

+                        pWapiBkid = (PRT_WAPI_BKID)list_entry(pWapiInfo->wapiBKIDStoreList.next, RT_WAPI_BKID, list);

+                           list_del_init(&pWapiBkid->list);

+                           _rtw_memset(pWapiBkid->bkid,0,16);

+                        list_add_tail(&pWapiBkid->list, &pWapiInfo->wapiBKIDIdleList);

+                 }

+        }

+

+

+	WAPI_TRACE(WAPI_API, " %s: after clear bkid \n", __FUNCTION__);

+

+

+	//Remove STA info

+	if(list_empty(&(pWapiInfo->wapiSTAUsedList))){

+		WAPI_TRACE(WAPI_API, " %s: wapiSTAUsedList is null \n", __FUNCTION__);

+		return;

+	}else{

+

+		WAPI_TRACE(WAPI_API, " %s: wapiSTAUsedList is not null \n", __FUNCTION__);

+#if 0

+		pWapiStaInfo=(PRT_WAPI_STA_INFO)list_entry((pWapiInfo->wapiSTAUsedList.next),RT_WAPI_STA_INFO,list);

+

+		list_for_each_entry(pWapiStaInfo, &(pWapiInfo->wapiSTAUsedList), list) {

+

+			DBG_871X("MAC Addr %02x-%02x-%02x-%02x-%02x-%02x \n",MacAddr[0],MacAddr[1],MacAddr[2],MacAddr[3],MacAddr[4],MacAddr[5]);

+

+

+			DBG_871X("peer Addr %02x-%02x-%02x-%02x-%02x-%02x \n",pWapiStaInfo->PeerMacAddr[0],pWapiStaInfo->PeerMacAddr[1],pWapiStaInfo->PeerMacAddr[2],pWapiStaInfo->PeerMacAddr[3],pWapiStaInfo->PeerMacAddr[4],pWapiStaInfo->PeerMacAddr[5]);

+

+			if(pWapiStaInfo == NULL)

+			{

+				WAPI_TRACE(WAPI_API, " %s: pWapiStaInfo == NULL Case \n", __FUNCTION__);

+				return;

+			}

+

+			if(pWapiStaInfo->PeerMacAddr == NULL)

+			{

+				WAPI_TRACE(WAPI_API, " %s: pWapiStaInfo->PeerMacAddr == NULL Case \n", __FUNCTION__);

+				return;

+			}

+

+			if(MacAddr == NULL)

+			{

+				WAPI_TRACE(WAPI_API, " %s: MacAddr == NULL Case \n", __FUNCTION__);

+				return;

+			}

+

+			if (_rtw_memcmp(pWapiStaInfo->PeerMacAddr, MacAddr, ETH_ALEN) == _TRUE) {

+				pWapiStaInfo->bAuthenticateInProgress = false;

+				pWapiStaInfo->bSetkeyOk = false;

+				_rtw_memset(pWapiStaInfo->PeerMacAddr,0,ETH_ALEN);

+				list_del_init(&pWapiStaInfo->list);

+				list_add_tail(&pWapiStaInfo->list, &pWapiInfo->wapiSTAIdleList);

+				break;

+			}

+

+		}

+#endif

+

+		while(!list_empty(&(pWapiInfo->wapiSTAUsedList)))

+		{

+			pWapiStaInfo = (PRT_WAPI_STA_INFO)list_entry(pWapiInfo->wapiSTAUsedList.next, RT_WAPI_STA_INFO, list);

+

+			DBG_871X("peer Addr %02x-%02x-%02x-%02x-%02x-%02x \n",pWapiStaInfo->PeerMacAddr[0],pWapiStaInfo->PeerMacAddr[1],pWapiStaInfo->PeerMacAddr[2],pWapiStaInfo->PeerMacAddr[3],pWapiStaInfo->PeerMacAddr[4],pWapiStaInfo->PeerMacAddr[5]);

+

+			list_del_init(&pWapiStaInfo->list);

+			memset(pWapiStaInfo->PeerMacAddr,0,ETH_ALEN);

+			pWapiStaInfo->bSetkeyOk = 0;

+			list_add_tail(&pWapiStaInfo->list, &pWapiInfo->wapiSTAIdleList);

+		}

+

+	}

+

+        WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__);

+	return;

+}

+

+void rtw_wapi_return_all_sta_info(_adapter *padapter)

+{

+	PRT_WAPI_T				pWapiInfo;

+	PRT_WAPI_STA_INFO		pWapiStaInfo;

+	PRT_WAPI_BKID			pWapiBkid;

+	WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__);

+

+	pWapiInfo = &padapter->wapiInfo;

+

+	if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__);

+		return;

+	}

+

+	//Sta Info List

+	while(!list_empty(&(pWapiInfo->wapiSTAUsedList)))

+	{

+		pWapiStaInfo = (PRT_WAPI_STA_INFO)list_entry(pWapiInfo->wapiSTAUsedList.next, RT_WAPI_STA_INFO, list);

+		list_del_init(&pWapiStaInfo->list);

+		memset(pWapiStaInfo->PeerMacAddr,0,ETH_ALEN);

+		pWapiStaInfo->bSetkeyOk = 0;

+		list_add_tail(&pWapiStaInfo->list, &pWapiInfo->wapiSTAIdleList);

+	}

+

+	//BKID List

+	while(!list_empty(&(pWapiInfo->wapiBKIDStoreList)))

+	{

+		pWapiBkid = (PRT_WAPI_BKID)list_entry(pWapiInfo->wapiBKIDStoreList.next, RT_WAPI_BKID, list);

+		list_del_init(&pWapiBkid->list);

+		memset(pWapiBkid->bkid,0,16);

+		list_add_tail(&pWapiBkid->list, &pWapiInfo->wapiBKIDIdleList);

+	}

+	WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__);

+}

+

+void rtw_wapi_clear_cam_entry(_adapter *padapter, u8 *pMacAddr)

+{

+	u8 UcIndex = 0;

+

+	WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__);

+		return;

+	}

+

+	UcIndex = WapiGetEntryForCamClear(padapter, pMacAddr, 0, 0);

+	if(UcIndex != 0xff){

+		//CAM_mark_invalid(Adapter, UcIndex);

+		CAM_empty_entry(padapter, UcIndex);

+	}

+

+	UcIndex = WapiGetEntryForCamClear(padapter, pMacAddr, 1, 0);

+	if(UcIndex != 0xff){

+		//CAM_mark_invalid(Adapter, UcIndex);

+		CAM_empty_entry(padapter, UcIndex);

+	}

+

+	UcIndex = WapiGetEntryForCamClear(padapter, pMacAddr, 0, 1);

+	if(UcIndex != 0xff){

+		//CAM_mark_invalid(Adapter, UcIndex);

+		CAM_empty_entry(padapter, UcIndex);

+	}

+

+	UcIndex = WapiGetEntryForCamClear(padapter, pMacAddr, 1, 1);

+	if(UcIndex != 0xff){

+		//CAM_mark_invalid(padapter, UcIndex);

+		CAM_empty_entry(padapter, UcIndex);

+	}

+

+	WAPI_TRACE(WAPI_API, "<========== %s\n", __FUNCTION__);

+}

+

+void rtw_wapi_clear_all_cam_entry(_adapter *padapter)

+{

+	WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_MLME, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__);

+		return;

+	}

+

+	invalidate_cam_all(padapter); // is this ok?

+	WapiResetAllCamEntry(padapter);

+

+	WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__);

+}

+

+void rtw_wapi_set_key(_adapter *padapter, RT_WAPI_KEY *pWapiKey, RT_WAPI_STA_INFO *pWapiSta, u8 bGroupKey, u8 bUseDefaultKey)

+{

+	PRT_WAPI_T		pWapiInfo =  &padapter->wapiInfo;

+	u8				*pMacAddr = pWapiSta->PeerMacAddr;

+	u32 EntryId = 0;

+	BOOLEAN IsPairWise = false ;

+	u8 EncAlgo;

+

+	WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_API, "<========== %s, WAPI not supported or not enabled!\n", __FUNCTION__);

+		return;

+	}

+

+	EncAlgo = _SMS4_;

+

+	//For Tx bc/mc pkt,use defualt key entry

+	if(bUseDefaultKey)

+	{

+		// when WAPI update key, keyid will be 0 or 1 by turns.

+		if (pWapiKey->keyId == 0)

+			EntryId = 0;

+		else

+			EntryId = 2;

+	}

+	else

+	{

+		// tx/rx unicast pkt, or rx broadcast, find the key entry by peer's MacAddr

+		EntryId = WapiGetEntryForCamWrite(padapter,pMacAddr,pWapiKey->keyId,bGroupKey);

+	}

+

+	if(EntryId == 0xff){

+		WAPI_TRACE(WAPI_API, "===>No entry for WAPI setkey! !!\n");

+		return;

+	}

+

+	//EntryId is also used to diff Sec key and Mic key

+	//Sec Key

+	WapiWriteOneCamEntry(padapter,

+					pMacAddr,

+					pWapiKey->keyId, //keyid

+					EntryId,	//entry

+					EncAlgo, //type

+					bGroupKey, //pairwise or group key

+					pWapiKey->dataKey);

+	//MIC key

+	WapiWriteOneCamEntry(padapter,

+					pMacAddr,

+					pWapiKey->keyId, //keyid

+					EntryId+1,	//entry

+					EncAlgo, //type

+					bGroupKey, //pairwise or group key

+					pWapiKey->micKey);

+

+	WAPI_TRACE(WAPI_API, "Set Wapi Key :KeyId:%d,EntryId:%d,PairwiseKey:%d.\n",pWapiKey->keyId,EntryId,!bGroupKey);

+	WAPI_TRACE(WAPI_API, "===========> %s\n", __FUNCTION__);

+

+}

+

+#if 0

+//YJ,test,091013

+void wapi_test_set_key(struct _adapter *padapter, u8* buf)

+{ /*Data: keyType(1) + bTxEnable(1) + bAuthenticator(1) + bUpdate(1) + PeerAddr(6) + DataKey(16) + MicKey(16) + KeyId(1)*/

+	PRT_WAPI_T			pWapiInfo = &padapter->wapiInfo;

+	PRT_WAPI_BKID		pWapiBkid;

+	PRT_WAPI_STA_INFO	pWapiSta;

+	u8					data[43];

+	bool					bTxEnable;

+	bool					bUpdate;

+	bool					bAuthenticator;

+	u8					PeerAddr[6];

+	u8					WapiAEPNInitialValueSrc[16] = {0x37,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C} ;

+	u8					WapiASUEPNInitialValueSrc[16] = {0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C} ;

+	u8					WapiAEMultiCastPNInitialValueSrc[16] = {0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C,0x36,0x5C} ;

+

+	WAPI_TRACE(WAPI_INIT, "===========>%s\n", __FUNCTION__);

+

+	if (!padapter->WapiSupport){

+	    return;

+	}

+

+	copy_from_user(data, buf, 43);

+	bTxEnable = data[1];

+	bAuthenticator = data[2];

+	bUpdate = data[3];

+ 	memcpy(PeerAddr,data+4,6);

+

+	if(data[0] == 0x3){

+		if(!list_empty(&(pWapiInfo->wapiBKIDIdleList))){

+			pWapiBkid = (PRT_WAPI_BKID)list_entry(pWapiInfo->wapiBKIDIdleList.next, RT_WAPI_BKID, list);

+			list_del_init(&pWapiBkid->list);

+			memcpy(pWapiBkid->bkid, data+10, 16);

+			WAPI_DATA(WAPI_INIT, "SetKey - BKID", pWapiBkid->bkid, 16);

+			list_add_tail(&pWapiBkid->list, &pWapiInfo->wapiBKIDStoreList);

+		}

+	}else{

+		list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) {

+			if(!memcmp(pWapiSta->PeerMacAddr,PeerAddr,6)){

+				pWapiSta->bAuthenticatorInUpdata = false;

+				switch(data[0]){

+				case 1:              //usk

+					if(bAuthenticator){         //authenticator

+						memcpy(pWapiSta->lastTxUnicastPN,WapiAEPNInitialValueSrc,16);

+						if(!bUpdate) {     //first

+							WAPI_TRACE(WAPI_INIT,"AE fisrt set usk \n");

+							pWapiSta->wapiUsk.bSet = true;

+							memcpy(pWapiSta->wapiUsk.dataKey,data+10,16);

+							memcpy(pWapiSta->wapiUsk.micKey,data+26,16);

+							pWapiSta->wapiUsk.keyId = *(data+42);

+							pWapiSta->wapiUsk.bTxEnable = true;

+							WAPI_DATA(WAPI_INIT, "SetKey - AE USK Data Key", pWapiSta->wapiUsk.dataKey, 16);

+							WAPI_DATA(WAPI_INIT, "SetKey - AE USK Mic Key", pWapiSta->wapiUsk.micKey, 16);

+						}

+						else               //update

+						{

+							WAPI_TRACE(WAPI_INIT, "AE update usk \n");

+							pWapiSta->wapiUskUpdate.bSet = true;

+							pWapiSta->bAuthenticatorInUpdata = true;

+							memcpy(pWapiSta->wapiUskUpdate.dataKey,data+10,16);

+							memcpy(pWapiSta->wapiUskUpdate.micKey,data+26,16);

+							memcpy(pWapiSta->lastRxUnicastPNBEQueue,WapiASUEPNInitialValueSrc,16);

+							memcpy(pWapiSta->lastRxUnicastPNBKQueue,WapiASUEPNInitialValueSrc,16);

+							memcpy(pWapiSta->lastRxUnicastPNVIQueue,WapiASUEPNInitialValueSrc,16);

+							memcpy(pWapiSta->lastRxUnicastPNVOQueue,WapiASUEPNInitialValueSrc,16);

+							memcpy(pWapiSta->lastRxUnicastPN,WapiASUEPNInitialValueSrc,16);

+							pWapiSta->wapiUskUpdate.keyId = *(data+42);

+							pWapiSta->wapiUskUpdate.bTxEnable = true;

+						}

+					}

+					else{

+						if(!bUpdate){

+							WAPI_TRACE(WAPI_INIT,"ASUE fisrt set usk \n");

+							if(bTxEnable){

+								pWapiSta->wapiUsk.bTxEnable = true;

+								memcpy(pWapiSta->lastTxUnicastPN,WapiASUEPNInitialValueSrc,16);

+							}else{

+								pWapiSta->wapiUsk.bSet = true;

+								memcpy(pWapiSta->wapiUsk.dataKey,data+10,16);

+								memcpy(pWapiSta->wapiUsk.micKey,data+26,16);

+								pWapiSta->wapiUsk.keyId = *(data+42);

+								pWapiSta->wapiUsk.bTxEnable = false;

+							}

+						}else{

+							WAPI_TRACE(WAPI_INIT,"ASUE update usk \n");

+							if(bTxEnable){

+								pWapiSta->wapiUskUpdate.bTxEnable = true;

+								if(pWapiSta->wapiUskUpdate.bSet){

+									memcpy(pWapiSta->wapiUsk.dataKey,pWapiSta->wapiUskUpdate.dataKey,16);

+									memcpy(pWapiSta->wapiUsk.micKey,pWapiSta->wapiUskUpdate.micKey,16);

+									pWapiSta->wapiUsk.keyId=pWapiSta->wapiUskUpdate.keyId;

+									memcpy(pWapiSta->lastRxUnicastPNBEQueue,WapiASUEPNInitialValueSrc,16);

+									memcpy(pWapiSta->lastRxUnicastPNBKQueue,WapiASUEPNInitialValueSrc,16);

+									memcpy(pWapiSta->lastRxUnicastPNVIQueue,WapiASUEPNInitialValueSrc,16);

+									memcpy(pWapiSta->lastRxUnicastPNVOQueue,WapiASUEPNInitialValueSrc,16);

+									memcpy(pWapiSta->lastRxUnicastPN,WapiASUEPNInitialValueSrc,16);

+									pWapiSta->wapiUskUpdate.bTxEnable = false;

+									pWapiSta->wapiUskUpdate.bSet = false;

+								}

+								memcpy(pWapiSta->lastTxUnicastPN,WapiASUEPNInitialValueSrc,16);

+							}else{

+								pWapiSta->wapiUskUpdate.bSet = true;

+								memcpy(pWapiSta->wapiUskUpdate.dataKey,data+10,16);

+								memcpy(pWapiSta->wapiUskUpdate.micKey,data+26,16);

+								pWapiSta->wapiUskUpdate.keyId = *(data+42);

+								pWapiSta->wapiUskUpdate.bTxEnable = false;

+							}

+						}

+					}

+					break;

+				case 2:		//msk

+					if(bAuthenticator){          //authenticator

+						pWapiInfo->wapiTxMsk.bSet = true;

+						memcpy(pWapiInfo->wapiTxMsk.dataKey,data+10,16);

+						memcpy(pWapiInfo->wapiTxMsk.micKey,data+26,16);

+						pWapiInfo->wapiTxMsk.keyId = *(data+42);

+						pWapiInfo->wapiTxMsk.bTxEnable = true;

+						memcpy(pWapiInfo->lastTxMulticastPN,WapiAEMultiCastPNInitialValueSrc,16);

+

+						if(!bUpdate){      //first

+							WAPI_TRACE(WAPI_INIT, "AE fisrt set msk \n");

+							if(!pWapiSta->bSetkeyOk)

+								pWapiSta->bSetkeyOk = true;

+							pWapiInfo->bFirstAuthentiateInProgress= false;

+						}else{               //update

+							WAPI_TRACE(WAPI_INIT,"AE update msk \n");

+						}

+

+						WAPI_DATA(WAPI_INIT, "SetKey - AE MSK Data Key", pWapiInfo->wapiTxMsk.dataKey, 16);

+						WAPI_DATA(WAPI_INIT, "SetKey - AE MSK Mic Key", pWapiInfo->wapiTxMsk.micKey, 16);

+					}

+					else{

+						if(!bUpdate){

+							WAPI_TRACE(WAPI_INIT,"ASUE fisrt set msk \n");

+							pWapiSta->wapiMsk.bSet = true;

+							memcpy(pWapiSta->wapiMsk.dataKey,data+10,16);

+							memcpy(pWapiSta->wapiMsk.micKey,data+26,16);

+							pWapiSta->wapiMsk.keyId = *(data+42);

+							pWapiSta->wapiMsk.bTxEnable = false;

+							if(!pWapiSta->bSetkeyOk)

+								pWapiSta->bSetkeyOk = true;

+							pWapiInfo->bFirstAuthentiateInProgress= false;

+							WAPI_DATA(WAPI_INIT, "SetKey - ASUE MSK Data Key", pWapiSta->wapiMsk.dataKey, 16);

+							WAPI_DATA(WAPI_INIT, "SetKey - ASUE MSK Mic Key", pWapiSta->wapiMsk.micKey, 16);

+						}else{

+							WAPI_TRACE(WAPI_INIT,"ASUE update msk \n");

+							pWapiSta->wapiMskUpdate.bSet = true;

+							memcpy(pWapiSta->wapiMskUpdate.dataKey,data+10,16);

+							memcpy(pWapiSta->wapiMskUpdate.micKey,data+26,16);

+							pWapiSta->wapiMskUpdate.keyId = *(data+42);

+							pWapiSta->wapiMskUpdate.bTxEnable = false;

+						}

+					}

+					break;

+				default:

+					WAPI_TRACE(WAPI_ERR,"Unknown Flag \n");

+					break;

+				}

+			}

+		}

+	}

+	WAPI_TRACE(WAPI_INIT, "<===========%s\n", __FUNCTION__);

+}

+

+

+void wapi_test_init(struct _adapter *padapter)

+{

+	u8 keybuf[100];

+	u8 mac_addr[6]={0x00,0xe0,0x4c,0x72,0x04,0x70};

+	u8 UskDataKey[16]={0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f};

+	u8 UskMicKey[16]={0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,0x18,0x19,0x1a,0x1b,0x1c,0x1d,0x1e,0x1f};

+	u8 UskId = 0;

+	u8 MskDataKey[16]={0x20,0x21,0x22,0x23,0x24,0x25,0x26,0x27,0x28,0x29,0x2a,0x2b,0x2c,0x2d,0x2e,0x2f};

+	u8 MskMicKey[16]={0x30,0x31,0x32,0x33,0x34,0x35,0x36,0x37,0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f};

+	u8 MskId = 0;

+

+	WAPI_TRACE(WAPI_INIT, "===========>%s\n", __FUNCTION__);

+

+	//Enable Wapi

+	WAPI_TRACE(WAPI_INIT, "%s: Enable wapi!!!!\n", __FUNCTION__);

+	padapter->wapiInfo.bWapiEnable = true;

+	padapter->pairwise_key_type = KEY_TYPE_SMS4;

+	ieee->group_key_type = KEY_TYPE_SMS4;

+	padapter->wapiInfo.extra_prefix_len = WAPI_EXT_LEN;

+	padapter->wapiInfo.extra_postfix_len = SMS4_MIC_LEN;

+

+	//set usk

+	WAPI_TRACE(WAPI_INIT, "%s: Set USK!!!!\n", __FUNCTION__);

+	memset(keybuf,0,100);

+	keybuf[0] = 1;                           //set usk

+	keybuf[1] = 1; 				//enable tx

+	keybuf[2] = 1; 				//AE

+	keybuf[3] = 0; 				//not update

+

+	memcpy(keybuf+4,mac_addr,6);

+	memcpy(keybuf+10,UskDataKey,16);

+	memcpy(keybuf+26,UskMicKey,16);

+	keybuf[42]=UskId;

+	wapi_test_set_key(padapter, keybuf);

+

+	memset(keybuf,0,100);

+	keybuf[0] = 1;                           //set usk

+	keybuf[1] = 1; 				//enable tx

+	keybuf[2] = 0; 				//AE

+	keybuf[3] = 0; 				//not update

+

+	memcpy(keybuf+4,mac_addr,6);

+	memcpy(keybuf+10,UskDataKey,16);

+	memcpy(keybuf+26,UskMicKey,16);

+	keybuf[42]=UskId;

+	wapi_test_set_key(padapter, keybuf);

+

+	//set msk

+	WAPI_TRACE(WAPI_INIT, "%s: Set MSK!!!!\n", __FUNCTION__);

+	memset(keybuf,0,100);

+	keybuf[0] = 2;                                //set msk

+	keybuf[1] = 1;                               //Enable TX

+	keybuf[2] = 1; 				//AE

+	keybuf[3] = 0;                              //not update

+	memcpy(keybuf+4,mac_addr,6);

+	memcpy(keybuf+10,MskDataKey,16);

+	memcpy(keybuf+26,MskMicKey,16);

+	keybuf[42] = MskId;

+	wapi_test_set_key(padapter, keybuf);

+

+	memset(keybuf,0,100);

+	keybuf[0] = 2;                                //set msk

+	keybuf[1] = 1;                               //Enable TX

+	keybuf[2] = 0; 				//AE

+	keybuf[3] = 0;                              //not update

+	memcpy(keybuf+4,mac_addr,6);

+	memcpy(keybuf+10,MskDataKey,16);

+	memcpy(keybuf+26,MskMicKey,16);

+	keybuf[42] = MskId;

+	wapi_test_set_key(padapter, keybuf);

+	WAPI_TRACE(WAPI_INIT, "<===========%s\n", __FUNCTION__);

+}

+#endif

+

+void rtw_wapi_get_iv(_adapter *padapter,u8 *pRA, u8*IV)

+{

+	PWLAN_HEADER_WAPI_EXTENSION pWapiExt = NULL;

+       PRT_WAPI_T         pWapiInfo = &padapter->wapiInfo;

+	bool	bPNOverflow = false;

+	bool	bFindMatchPeer = false;

+	PRT_WAPI_STA_INFO  pWapiSta = NULL;

+

+	pWapiExt = (PWLAN_HEADER_WAPI_EXTENSION)IV;

+

+	WAPI_DATA(WAPI_RX,"wapi_get_iv: pra",pRA,6);

+

+	if(IS_MCAST(pRA)){

+		if(!pWapiInfo->wapiTxMsk.bTxEnable){

+			 WAPI_TRACE(WAPI_ERR,"%s: bTxEnable = 0!!\n",__FUNCTION__);

+			 return;

+		}

+

+		if(pWapiInfo->wapiTxMsk.keyId <= 1){

+			 pWapiExt->KeyIdx = pWapiInfo->wapiTxMsk.keyId;

+			 pWapiExt->Reserved = 0;

+			 bPNOverflow = WapiIncreasePN(pWapiInfo->lastTxMulticastPN, 1);

+			 memcpy(pWapiExt->PN, pWapiInfo->lastTxMulticastPN, 16);

+		}

+	}

+	else

+	{

+		if(list_empty(&pWapiInfo->wapiSTAUsedList)){

+			WAPI_TRACE(WAPI_RX,"rtw_wapi_get_iv: list is empty \n");

+			_rtw_memset(IV,10,18);

+			return;

+		}

+		else{

+				list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list){

+					WAPI_DATA(WAPI_RX,"rtw_wapi_get_iv: peermacaddr ",pWapiSta->PeerMacAddr,6);

+					if (_rtw_memcmp((u8*)pWapiSta->PeerMacAddr, pRA, 6) == _TRUE) {

+						bFindMatchPeer = true;

+						break;

+					}

+				}

+

+				WAPI_TRACE(WAPI_RX,"bFindMatchPeer: %d \n",bFindMatchPeer);

+				WAPI_DATA(WAPI_RX,"Addr",pRA,6);

+

+				if (bFindMatchPeer){

+					if((!pWapiSta->wapiUskUpdate.bTxEnable) && (!pWapiSta->wapiUsk.bTxEnable))

+						return;

+

+					if (pWapiSta->wapiUsk.keyId <= 1){

+						if(pWapiSta->wapiUskUpdate.bTxEnable)

+							pWapiExt->KeyIdx = pWapiSta->wapiUskUpdate.keyId;

+						else

+							pWapiExt->KeyIdx = pWapiSta->wapiUsk.keyId;

+

+						pWapiExt->Reserved = 0;

+						bPNOverflow = WapiIncreasePN(pWapiSta->lastTxUnicastPN, 2);

+						_rtw_memcpy(pWapiExt->PN, pWapiSta->lastTxUnicastPN, 16);

+

+					}

+				}

+		}

+

+	}

+

+}

+

+bool rtw_wapi_drop_for_key_absent(_adapter *padapter,u8 *pRA)

+{

+	PRT_WAPI_T         pWapiInfo = &padapter->wapiInfo;

+	bool				bFindMatchPeer = false;

+	bool				bDrop = false;

+	PRT_WAPI_STA_INFO  pWapiSta = NULL;

+	struct security_priv 		*psecuritypriv = &padapter->securitypriv;

+

+	WAPI_DATA(WAPI_RX,"rtw_wapi_drop_for_key_absent: ra ",pRA,6);

+

+	if(psecuritypriv->dot11PrivacyAlgrthm == _SMS4_)

+	{

+		if ((!padapter->WapiSupport) || (!pWapiInfo->bWapiEnable))

+			return true;

+

+		if(IS_MCAST(pRA)){

+			if(!pWapiInfo->wapiTxMsk.bTxEnable){

+				bDrop = true;

+				WAPI_TRACE(WAPI_RX,"rtw_wapi_drop_for_key_absent: multicast key is absent \n");

+				return bDrop;

+			}

+		}

+		else{

+				if(!list_empty(&pWapiInfo->wapiSTAUsedList)){

+					list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list){

+						WAPI_DATA(WAPI_RX,"rtw_wapi_drop_for_key_absent: pWapiSta->PeerMacAddr ",pWapiSta->PeerMacAddr,6);

+						if (_rtw_memcmp(pRA, pWapiSta->PeerMacAddr, 6) == _TRUE){

+							bFindMatchPeer = true;

+							break;

+						}

+					}

+					if (bFindMatchPeer)	{

+						if (!pWapiSta->wapiUsk.bTxEnable){

+							bDrop = true;

+							WAPI_TRACE(WAPI_RX,"rtw_wapi_drop_for_key_absent: unicast key is absent \n");

+							return bDrop;

+						}

+					}

+					else{

+						bDrop = true;

+						WAPI_TRACE(WAPI_RX,"rtw_wapi_drop_for_key_absent: no peer find \n");

+						return bDrop;

+					}

+

+				}

+				else{

+						bDrop = true;

+						WAPI_TRACE(WAPI_RX,"rtw_wapi_drop_for_key_absent: no sta  exist \n");

+						return bDrop;

+				}

+		}

+	}

+	else

+	{

+		return bDrop;

+	}

+

+	return bDrop;

+}

+

+#endif

diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_wapi_sms4.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_wapi_sms4.c
new file mode 100755
index 00000000..6126ed9a
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_wapi_sms4.c
@@ -0,0 +1,923 @@
+#ifdef CONFIG_WAPI_SUPPORT

+

+#include <linux/unistd.h>

+#include <linux/etherdevice.h>

+#include <drv_types.h>

+#include <rtw_wapi.h>

+

+

+#ifdef CONFIG_WAPI_SW_SMS4

+

+#define WAPI_LITTLE_ENDIAN

+//#define BIG_ENDIAN

+#define ENCRYPT  0

+#define DECRYPT  1

+

+

+/**********************************************************

+ **********************************************************/

+const u8 Sbox[256] = {

+0xd6,0x90,0xe9,0xfe,0xcc,0xe1,0x3d,0xb7,0x16,0xb6,0x14,0xc2,0x28,0xfb,0x2c,0x05,

+0x2b,0x67,0x9a,0x76,0x2a,0xbe,0x04,0xc3,0xaa,0x44,0x13,0x26,0x49,0x86,0x06,0x99,

+0x9c,0x42,0x50,0xf4,0x91,0xef,0x98,0x7a,0x33,0x54,0x0b,0x43,0xed,0xcf,0xac,0x62,

+0xe4,0xb3,0x1c,0xa9,0xc9,0x08,0xe8,0x95,0x80,0xdf,0x94,0xfa,0x75,0x8f,0x3f,0xa6,

+0x47,0x07,0xa7,0xfc,0xf3,0x73,0x17,0xba,0x83,0x59,0x3c,0x19,0xe6,0x85,0x4f,0xa8,

+0x68,0x6b,0x81,0xb2,0x71,0x64,0xda,0x8b,0xf8,0xeb,0x0f,0x4b,0x70,0x56,0x9d,0x35,

+0x1e,0x24,0x0e,0x5e,0x63,0x58,0xd1,0xa2,0x25,0x22,0x7c,0x3b,0x01,0x21,0x78,0x87,

+0xd4,0x00,0x46,0x57,0x9f,0xd3,0x27,0x52,0x4c,0x36,0x02,0xe7,0xa0,0xc4,0xc8,0x9e,

+0xea,0xbf,0x8a,0xd2,0x40,0xc7,0x38,0xb5,0xa3,0xf7,0xf2,0xce,0xf9,0x61,0x15,0xa1,

+0xe0,0xae,0x5d,0xa4,0x9b,0x34,0x1a,0x55,0xad,0x93,0x32,0x30,0xf5,0x8c,0xb1,0xe3,

+0x1d,0xf6,0xe2,0x2e,0x82,0x66,0xca,0x60,0xc0,0x29,0x23,0xab,0x0d,0x53,0x4e,0x6f,

+0xd5,0xdb,0x37,0x45,0xde,0xfd,0x8e,0x2f,0x03,0xff,0x6a,0x72,0x6d,0x6c,0x5b,0x51,

+0x8d,0x1b,0xaf,0x92,0xbb,0xdd,0xbc,0x7f,0x11,0xd9,0x5c,0x41,0x1f,0x10,0x5a,0xd8,

+0x0a,0xc1,0x31,0x88,0xa5,0xcd,0x7b,0xbd,0x2d,0x74,0xd0,0x12,0xb8,0xe5,0xb4,0xb0,

+0x89,0x69,0x97,0x4a,0x0c,0x96,0x77,0x7e,0x65,0xb9,0xf1,0x09,0xc5,0x6e,0xc6,0x84,

+0x18,0xf0,0x7d,0xec,0x3a,0xdc,0x4d,0x20,0x79,0xee,0x5f,0x3e,0xd7,0xcb,0x39,0x48

+};

+

+const u32 CK[32] = {

+	0x00070e15, 0x1c232a31, 0x383f464d, 0x545b6269,

+	0x70777e85, 0x8c939aa1, 0xa8afb6bd, 0xc4cbd2d9,

+	0xe0e7eef5, 0xfc030a11, 0x181f262d, 0x343b4249,

+	0x50575e65, 0x6c737a81, 0x888f969d, 0xa4abb2b9,

+	0xc0c7ced5, 0xdce3eaf1, 0xf8ff060d, 0x141b2229,

+	0x30373e45, 0x4c535a61, 0x686f767d, 0x848b9299,

+	0xa0a7aeb5, 0xbcc3cad1, 0xd8dfe6ed, 0xf4fb0209,

+	0x10171e25, 0x2c333a41, 0x484f565d, 0x646b7279 };

+

+#define Rotl(_x, _y) (((_x) << (_y)) | ((_x) >> (32 - (_y))))

+

+#define ByteSub(_A) (Sbox[(_A) >> 24 & 0xFF] << 24 | \

+                     Sbox[(_A) >> 16 & 0xFF] << 16 | \

+                     Sbox[(_A) >>  8 & 0xFF] <<  8 | \

+                     Sbox[(_A) & 0xFF])

+

+#define L1(_B) ((_B) ^ Rotl(_B, 2) ^ Rotl(_B, 10) ^ Rotl(_B, 18) ^ Rotl(_B, 24))

+#define L2(_B) ((_B) ^ Rotl(_B, 13) ^ Rotl(_B, 23))

+

+static void

+xor_block(void *dst, void *src1, void *src2)

+/* 128-bit xor: *dst = *src1 xor *src2. Pointers must be 32-bit aligned  */

+{

+    ((u32 *)dst)[0] = ((u32 *)src1)[0] ^ ((u32 *)src2)[0];

+    ((u32 *)dst)[1] = ((u32 *)src1)[1] ^ ((u32 *)src2)[1];

+    ((u32 *)dst)[2] = ((u32 *)src1)[2] ^ ((u32 *)src2)[2];

+    ((u32 *)dst)[3] = ((u32 *)src1)[3] ^ ((u32 *)src2)[3];

+}

+

+

+void SMS4Crypt(u8 *Input, u8 *Output, u32 *rk)

+{

+	 u32 r, mid, x0, x1, x2, x3, *p;

+	 p = (u32 *)Input;

+	 x0 = p[0];

+	 x1 = p[1];

+	 x2 = p[2];

+	 x3 = p[3];

+#ifdef WAPI_LITTLE_ENDIAN

+	 x0 = Rotl(x0, 16); x0 = ((x0 & 0x00FF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8);

+	 x1 = Rotl(x1, 16); x1 = ((x1 & 0x00FF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8);

+	 x2 = Rotl(x2, 16); x2 = ((x2 & 0x00FF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8);

+	 x3 = Rotl(x3, 16); x3 = ((x3 & 0x00FF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8);

+#endif

+	 for (r = 0; r < 32; r += 4)

+	 {

+		  mid = x1 ^ x2 ^ x3 ^ rk[r + 0];

+		  mid = ByteSub(mid);

+		  x0 ^= L1(mid);

+		  mid = x2 ^ x3 ^ x0 ^ rk[r + 1];

+		  mid = ByteSub(mid);

+		  x1 ^= L1(mid);

+		  mid = x3 ^ x0 ^ x1 ^ rk[r + 2];

+		  mid = ByteSub(mid);

+		  x2 ^= L1(mid);

+		  mid = x0 ^ x1 ^ x2 ^ rk[r + 3];

+		  mid = ByteSub(mid);

+		  x3 ^= L1(mid);

+	 }

+#ifdef WAPI_LITTLE_ENDIAN

+	 x0 = Rotl(x0, 16); x0 = ((x0 & 0x00FF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8);

+	 x1 = Rotl(x1, 16); x1 = ((x1 & 0x00FF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8);

+	 x2 = Rotl(x2, 16); x2 = ((x2 & 0x00FF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8);

+	 x3 = Rotl(x3, 16); x3 = ((x3 & 0x00FF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8);

+#endif

+	 p = (u32 *)Output;

+	 p[0] = x3;

+	 p[1] = x2;

+	 p[2] = x1;

+	 p[3] = x0;

+}

+

+

+

+void SMS4KeyExt(u8 *Key, u32 *rk, u32 CryptFlag)

+{

+	 u32 r, mid, x0, x1, x2, x3, *p;

+

+	 p = (u32 *)Key;

+	 x0 = p[0];

+	 x1 = p[1];

+	 x2 = p[2];

+	 x3 = p[3];

+#ifdef WAPI_LITTLE_ENDIAN

+	 x0 = Rotl(x0, 16); x0 = ((x0 & 0xFF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8);

+	 x1 = Rotl(x1, 16); x1 = ((x1 & 0xFF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8);

+	 x2 = Rotl(x2, 16); x2 = ((x2 & 0xFF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8);

+	 x3 = Rotl(x3, 16); x3 = ((x3 & 0xFF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8);

+#endif

+

+	 x0 ^= 0xa3b1bac6;

+	 x1 ^= 0x56aa3350;

+	 x2 ^= 0x677d9197;

+	 x3 ^= 0xb27022dc;

+	 for (r = 0; r < 32; r += 4)

+	 {

+		  mid = x1 ^ x2 ^ x3 ^ CK[r + 0];

+		  mid = ByteSub(mid);

+		  rk[r + 0] = x0 ^= L2(mid);

+		  mid = x2 ^ x3 ^ x0 ^ CK[r + 1];

+		  mid = ByteSub(mid);

+		  rk[r + 1] = x1 ^= L2(mid);

+		  mid = x3 ^ x0 ^ x1 ^ CK[r + 2];

+		  mid = ByteSub(mid);

+		  rk[r + 2] = x2 ^= L2(mid);

+		  mid = x0 ^ x1 ^ x2 ^ CK[r + 3];

+		  mid = ByteSub(mid);

+		  rk[r + 3] = x3 ^= L2(mid);

+	 }

+	 if (CryptFlag == DECRYPT)

+	 {

+	 	  for (r = 0; r < 16; r++)

+	 	  	 mid = rk[r], rk[r] = rk[31 - r], rk[31 - r] = mid;

+	 }

+}

+

+

+void WapiSMS4Cryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength,

+                                                u8 *Output, u16 *OutputLength, u32 CryptFlag)

+{

+	u32 blockNum,i,j, rk[32];

+	u16 remainder;

+	u8 blockIn[16],blockOut[16], tempIV[16], k;

+

+	*OutputLength = 0;

+	remainder = InputLength & 0x0F;

+	blockNum = InputLength >> 4;

+	if(remainder !=0)

+		blockNum++;

+	else

+		remainder = 16;

+

+	for(k=0;k<16;k++)

+		tempIV[k] = IV[15-k];

+

+	memcpy(blockIn, tempIV, 16);

+

+      SMS4KeyExt((u8 *)Key, rk,CryptFlag);

+

+	for(i=0; i<blockNum-1; i++)

+	{

+		SMS4Crypt((u8 *)blockIn, blockOut, rk);

+             xor_block(&Output[i*16], &Input[i*16], blockOut);

+		memcpy(blockIn,blockOut,16);

+	}

+

+	*OutputLength = i*16;

+

+	SMS4Crypt((u8 *)blockIn, blockOut, rk);

+

+	for(j=0; j<remainder; j++)

+	{

+		Output[i*16+j] = Input[i*16+j] ^ blockOut[j];

+	}

+      *OutputLength += remainder;

+

+}

+

+void WapiSMS4Encryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength,

+                                                    u8 *Output, u16 *OutputLength)

+{

+

+	WapiSMS4Cryption(Key, IV, Input, InputLength, Output, OutputLength, ENCRYPT);

+}

+

+void WapiSMS4Decryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength,

+                                                    u8 *Output, u16 *OutputLength)

+{

+	// OFB mode: is also ENCRYPT flag

+	WapiSMS4Cryption(Key, IV, Input, InputLength, Output, OutputLength, ENCRYPT);

+}

+

+void WapiSMS4CalculateMic(u8 *Key, u8 *IV, u8 *Input1, u8 Input1Length,

+                                                 u8 *Input2, u16 Input2Length, u8 *Output, u8 *OutputLength)

+{

+	u32 blockNum, i, remainder, rk[32];

+	u8 BlockIn[16], BlockOut[16], TempBlock[16], tempIV[16], k;

+

+	*OutputLength = 0;

+	remainder = Input1Length & 0x0F;

+	blockNum = Input1Length >> 4;

+

+	for(k=0;k<16;k++)

+		tempIV[k] = IV[15-k];

+

+	memcpy(BlockIn, tempIV, 16);

+

+	SMS4KeyExt((u8 *)Key, rk, ENCRYPT);

+

+	SMS4Crypt((u8 *)BlockIn, BlockOut, rk);

+

+	for(i=0; i<blockNum; i++){

+		xor_block(BlockIn, (Input1+i*16), BlockOut);

+		SMS4Crypt((u8 *)BlockIn, BlockOut, rk);

+	}

+

+	if(remainder !=0){

+		memset(TempBlock, 0, 16);

+		memcpy(TempBlock, (Input1+blockNum*16), remainder);

+

+		xor_block(BlockIn, TempBlock, BlockOut);

+		SMS4Crypt((u8 *)BlockIn, BlockOut, rk);

+      }

+

+	remainder = Input2Length & 0x0F;

+	blockNum = Input2Length >> 4;

+

+  	for(i=0; i<blockNum; i++){

+		xor_block(BlockIn, (Input2+i*16), BlockOut);

+		SMS4Crypt((u8 *)BlockIn, BlockOut, rk);

+	}

+

+	if(remainder !=0){

+		memset(TempBlock, 0, 16);

+		memcpy(TempBlock, (Input2+blockNum*16), remainder);

+

+		xor_block(BlockIn, TempBlock, BlockOut);

+		SMS4Crypt((u8 *)BlockIn, BlockOut, rk);

+	}

+

+	memcpy(Output, BlockOut, 16);

+	*OutputLength = 16;

+}

+

+void SecCalculateMicSMS4(

+	u8		KeyIdx,

+	u8        *MicKey,

+	u8        *pHeader,

+	u8        *pData,

+	u16       DataLen,

+	u8        *MicBuffer

+	)

+{

+#if 0

+	struct ieee80211_hdr_3addr_qos *header;

+	u8 TempBuf[34], TempLen = 32, MicLen, QosOffset, *IV;

+	u16 *pTemp, fc;

+

+	WAPI_TRACE(WAPI_TX|WAPI_RX, "=========>%s\n", __FUNCTION__);

+

+	header = (struct ieee80211_hdr_3addr_qos *)pHeader;

+	memset(TempBuf, 0, 34);

+	memcpy(TempBuf, pHeader, 2); //FrameCtrl

+	pTemp = (u16*)TempBuf;

+	*pTemp &= 0xc78f;       //bit4,5,6,11,12,13

+

+	memcpy((TempBuf+2), (pHeader+4), 12); //Addr1, Addr2

+	memcpy((TempBuf+14), (pHeader+22), 2); // SeqCtrl

+	pTemp = (u16*)(TempBuf + 14);

+	*pTemp &= 0x000f;

+

+	memcpy((TempBuf+16), (pHeader+16), 6); //Addr3

+

+	fc = le16_to_cpu(header->frame_ctl);

+

+

+

+	if (GetFrDs((u16*)&fc) && GetToDs((u16 *)&fc))

+	{

+		memcpy((TempBuf+22), (pHeader+24), 6);

+		QosOffset = 30;

+	}else{

+		memset((TempBuf+22), 0, 6);

+		QosOffset = 24;

+	}

+

+	if((fc & 0x0088) == 0x0088){

+		memcpy((TempBuf+28), (pHeader+QosOffset), 2);

+		TempLen += 2;

+		//IV = pHeader + QosOffset + 2 + SNAP_SIZE + sizeof(u16) + 2;

+		IV = pHeader + QosOffset + 2 + 2;

+	}else{

+		IV = pHeader + QosOffset + 2;

+		//IV = pHeader + QosOffset + SNAP_SIZE + sizeof(u16) + 2;

+	}

+

+	TempBuf[TempLen-1] = (u8)(DataLen & 0xff);

+	TempBuf[TempLen-2] = (u8)((DataLen & 0xff00)>>8);

+	TempBuf[TempLen-4] = KeyIdx;

+

+	WAPI_DATA(WAPI_TX, "CalculateMic - KEY", MicKey, 16);

+	WAPI_DATA(WAPI_TX, "CalculateMic - IV", IV, 16);

+	WAPI_DATA(WAPI_TX, "CalculateMic - TempBuf", TempBuf, TempLen);

+	WAPI_DATA(WAPI_TX, "CalculateMic - pData", pData, DataLen);

+

+	WapiSMS4CalculateMic(MicKey, IV, TempBuf, TempLen,

+	                                             pData, DataLen, MicBuffer, &MicLen);

+

+	if (MicLen != 16)

+		WAPI_TRACE(WAPI_ERR,"%s: MIC Length Error!!\n",__FUNCTION__);

+

+	WAPI_TRACE(WAPI_TX|WAPI_RX, "<=========%s\n", __FUNCTION__);

+#endif

+}

+

+/* AddCount: 1 or 2.

+ *  If overflow, return 1,

+ *  else return 0.

+ */

+u8 WapiIncreasePN(u8 *PN, u8 AddCount)

+{

+	u8  i;

+

+	if (NULL == PN)

+		return 1;

+	//YJ,test,091102

+	/*

+	if(AddCount == 2){

+		DBG_8192C("############################%s(): PN[0]=0x%x\n", __FUNCTION__, PN[0]);

+		if(PN[0] == 0x48){

+			PN[0] += AddCount;

+			return 1;

+		}else{

+			PN[0] += AddCount;

+			return 0;

+		}

+	}

+	*/

+	//YJ,test,091102,end

+

+	for (i=0; i<16; i++)

+	{

+		if (PN[i] + AddCount <= 0xff)

+		{

+			PN[i] += AddCount;

+			return 0;

+		}

+		else

+		{

+			PN[i] += AddCount;

+			AddCount = 1;

+		}

+	}

+	return 1;

+}

+

+

+void WapiGetLastRxUnicastPNForQoSData(

+	u8 			UserPriority,

+	PRT_WAPI_STA_INFO    pWapiStaInfo,

+	u8 *PNOut

+)

+{

+	WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__);

+	switch(UserPriority)

+	{

+		case 0:

+		case 3:

+			      memcpy(PNOut,pWapiStaInfo->lastRxUnicastPNBEQueue,16);

+			      break;

+		case 1:

+		case 2:

+			      memcpy(PNOut,pWapiStaInfo->lastRxUnicastPNBKQueue,16);

+			      break;

+		case 4:

+		case 5:

+			      memcpy(PNOut,pWapiStaInfo->lastRxUnicastPNVIQueue,16);

+			      break;

+		case 6:

+		case 7:

+			      memcpy(PNOut,pWapiStaInfo->lastRxUnicastPNVOQueue,16);

+			      break;

+		default:

+				WAPI_TRACE(WAPI_ERR, "%s: Unknown TID \n", __FUNCTION__);

+				break;

+	}

+	WAPI_TRACE(WAPI_RX, "<=========== %s\n", __FUNCTION__);

+}

+

+

+void WapiSetLastRxUnicastPNForQoSData(

+	u8 		UserPriority,

+	u8           *PNIn,

+	PRT_WAPI_STA_INFO    pWapiStaInfo

+)

+{

+	WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__);

+	switch(UserPriority)

+	{

+		case 0:

+		case 3:

+			      memcpy(pWapiStaInfo->lastRxUnicastPNBEQueue,PNIn,16);

+			      break;

+		case 1:

+		case 2:

+			      memcpy(pWapiStaInfo->lastRxUnicastPNBKQueue,PNIn,16);

+			      break;

+		case 4:

+		case 5:

+			      memcpy(pWapiStaInfo->lastRxUnicastPNVIQueue,PNIn,16);

+			      break;

+		case 6:

+		case 7:

+			      memcpy(pWapiStaInfo->lastRxUnicastPNVOQueue,PNIn,16);

+			      break;

+		default:

+				WAPI_TRACE(WAPI_ERR, "%s: Unknown TID \n", __FUNCTION__);

+				break;

+	}

+	WAPI_TRACE(WAPI_RX, "<=========== %s\n", __FUNCTION__);

+}

+

+

+/****************************************************************************

+ FALSE not RX-Reorder

+ TRUE do RX Reorder

+add to support WAPI to N-mode

+*****************************************************************************/

+u8 WapiCheckPnInSwDecrypt(

+	_adapter *padapter,

+	struct sk_buff *pskb

+)

+{

+	u8 				ret = false;

+

+#if 0

+	struct ieee80211_hdr_3addr_qos *header;

+	u16 				fc;

+	u8				*pDaddr, *pTaddr, *pRaddr;

+

+	header = (struct ieee80211_hdr_3addr_qos *)pskb->data;

+	pTaddr = header->addr2;

+	pRaddr = header->addr1;

+	fc = le16_to_cpu(header->frame_ctl);

+

+	if(GetToDs(&fc))

+		pDaddr = header->addr3;

+	else

+		pDaddr = header->addr1;

+

+	if ((_rtw_memcmp(pRaddr, padapter->pnetdev->dev_addr, ETH_ALEN) == 0)

+		&&	! (pDaddr)

+		&& (GetFrameType(&fc) == WIFI_QOS_DATA_TYPE))

+		//&& ieee->pHTInfo->bCurrentHTSupport &&

+		//ieee->pHTInfo->bCurRxReorderEnable)

+		ret = false;

+	else

+		ret = true;

+#endif

+	WAPI_TRACE(WAPI_RX, "%s: return %d\n", __FUNCTION__, ret);

+	return ret;

+}

+

+int SecSMS4HeaderFillIV(_adapter *padapter, u8 *pxmitframe)

+{

+	struct pkt_attrib *pattrib = &((struct xmit_frame*)pxmitframe)->attrib;

+	u8 * frame = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET;

+	u8 *pSecHeader = NULL, *pos = NULL, *pRA = NULL;

+	u8 bPNOverflow = false, bFindMatchPeer = false, hdr_len = 0;

+	PWLAN_HEADER_WAPI_EXTENSION pWapiExt = NULL;

+	PRT_WAPI_T         pWapiInfo = &padapter->wapiInfo;

+	PRT_WAPI_STA_INFO  pWapiSta = NULL;

+	int ret = 0;

+

+	WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__);

+

+	return ret;

+#if 0

+	hdr_len = sMacHdrLng;

+	if (GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE)

+	{

+		hdr_len += 2;

+	}

+	//hdr_len += SNAP_SIZE + sizeof(u16);

+

+	pos = skb_push(pskb, padapter->wapiInfo.extra_prefix_len);

+	memmove(pos, pos+padapter->wapiInfo.extra_prefix_len, hdr_len);

+

+	pSecHeader = pskb->data + hdr_len;

+	pWapiExt = (PWLAN_HEADER_WAPI_EXTENSION)pSecHeader;

+	pRA = pskb->data + 4;

+

+	WAPI_DATA(WAPI_TX, "FillIV - Before Fill IV", pskb->data, pskb->len);

+

+	//Address 1 is always receiver's address

+	if( IS_MCAST(pRA) ){

+		if(!pWapiInfo->wapiTxMsk.bTxEnable){

+			WAPI_TRACE(WAPI_ERR,"%s: bTxEnable = 0!!\n",__FUNCTION__);

+			return -2;

+		}

+		if(pWapiInfo->wapiTxMsk.keyId <= 1){

+			pWapiExt->KeyIdx = pWapiInfo->wapiTxMsk.keyId;

+			pWapiExt->Reserved = 0;

+			bPNOverflow = WapiIncreasePN(pWapiInfo->lastTxMulticastPN, 1);

+			memcpy(pWapiExt->PN, pWapiInfo->lastTxMulticastPN, 16);

+			if (bPNOverflow){

+				// Update MSK Notification.

+				WAPI_TRACE(WAPI_ERR,"===============>%s():multicast PN overflow\n",__FUNCTION__);

+				rtw_wapi_app_event_handler(padapter,NULL,0,pRA, false, false, true, 0, false);

+			}

+		}else{

+			WAPI_TRACE(WAPI_ERR,"%s: Invalid Wapi Multicast KeyIdx!!\n",__FUNCTION__);

+			ret = -3;

+		}

+	}

+    	else{

+		list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) {

+			if(!memcmp(pWapiSta->PeerMacAddr,pRA,6)){

+    				bFindMatchPeer = true;

+				break;

+			}

+		}

+		if (bFindMatchPeer){

+			if((!pWapiSta->wapiUskUpdate.bTxEnable) && (!pWapiSta->wapiUsk.bTxEnable)){

+				WAPI_TRACE(WAPI_ERR,"%s: bTxEnable = 0!!\n",__FUNCTION__);

+				return -4;

+			}

+			if (pWapiSta->wapiUsk.keyId <= 1){

+				if(pWapiSta->wapiUskUpdate.bTxEnable)

+					pWapiExt->KeyIdx = pWapiSta->wapiUskUpdate.keyId;

+				else

+					pWapiExt->KeyIdx = pWapiSta->wapiUsk.keyId;

+

+				pWapiExt->Reserved = 0;

+				bPNOverflow = WapiIncreasePN(pWapiSta->lastTxUnicastPN, 2);

+				memcpy(pWapiExt->PN, pWapiSta->lastTxUnicastPN, 16);

+				if (bPNOverflow){

+					// Update USK Notification.

+					WAPI_TRACE(WAPI_ERR,"===============>%s():unicast PN overflow\n",__FUNCTION__);

+					rtw_wapi_app_event_handler(padapter,NULL,0,pWapiSta->PeerMacAddr, false, true, false, 0, false);

+				}

+			}else{

+				WAPI_TRACE(WAPI_ERR,"%s: Invalid Wapi Unicast KeyIdx!!\n",__FUNCTION__);

+				ret = -5;

+			}

+		}

+		else{

+			WAPI_TRACE(WAPI_ERR,"%s: Can not find Peer Sta "MAC_FMT"!!\n",__FUNCTION__, MAC_ARG(pRA));

+			ret = -6;

+		}

+    	}

+

+	WAPI_DATA(WAPI_TX, "FillIV - After Fill IV", pskb->data, pskb->len);

+	WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__);

+	return ret;

+#endif

+}

+

+// WAPI SW Enc: must have done Coalesce!

+void SecSWSMS4Encryption(

+	_adapter *padapter,

+	u8 * pxmitframe

+	)

+{

+	PRT_WAPI_T		pWapiInfo = &padapter->wapiInfo;

+	PRT_WAPI_STA_INFO   pWapiSta = NULL;

+	u8 *pframe = ((struct xmit_frame*)pxmitframe)->buf_addr + TXDESC_SIZE;

+	struct pkt_attrib *pattrib = &((struct xmit_frame*)pxmitframe)->attrib;

+

+	u8 *SecPtr = NULL, *pRA, *pMicKey = NULL, *pDataKey = NULL, *pIV = NULL;

+	u8 IVOffset, DataOffset, bFindMatchPeer = false, KeyIdx = 0, MicBuffer[16];

+	u16 OutputLength;

+

+	WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__);

+

+	WAPI_TRACE(WAPI_TX,"hdrlen: %d \n",pattrib->hdrlen);

+

+	return;

+

+	DataOffset = pattrib->hdrlen + pattrib->iv_len;

+

+	pRA = pframe + 4;

+

+

+	if( IS_MCAST(pRA) ){

+		KeyIdx = pWapiInfo->wapiTxMsk.keyId;

+		pIV = pWapiInfo->lastTxMulticastPN;

+		pMicKey = pWapiInfo->wapiTxMsk.micKey;

+		pDataKey = pWapiInfo->wapiTxMsk.dataKey;

+	}else{

+		if (!list_empty(&(pWapiInfo->wapiSTAUsedList))){

+			list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) {

+				if (0 == memcmp(pWapiSta->PeerMacAddr, pRA, 6)){

+					bFindMatchPeer = true;

+					break;

+				}

+			}

+

+			if (bFindMatchPeer){

+				if (pWapiSta->wapiUskUpdate.bTxEnable){

+					KeyIdx = pWapiSta->wapiUskUpdate.keyId;

+					WAPI_TRACE(WAPI_TX, "%s(): Use update USK!! KeyIdx=%d\n", __FUNCTION__, KeyIdx);

+					pIV = pWapiSta->lastTxUnicastPN;

+					pMicKey = pWapiSta->wapiUskUpdate.micKey;

+					pDataKey = pWapiSta->wapiUskUpdate.dataKey;

+				}else{

+					KeyIdx = pWapiSta->wapiUsk.keyId;

+					WAPI_TRACE(WAPI_TX, "%s(): Use USK!! KeyIdx=%d\n", __FUNCTION__, KeyIdx);

+					pIV = pWapiSta->lastTxUnicastPN;

+					pMicKey = pWapiSta->wapiUsk.micKey;

+					pDataKey = pWapiSta->wapiUsk.dataKey;

+				}

+  			}else{

+				WAPI_TRACE(WAPI_ERR,"%s: Can not find Peer Sta!!\n",__FUNCTION__);

+				return;

+			}

+		}else{

+			WAPI_TRACE(WAPI_ERR,"%s: wapiSTAUsedList is empty!!\n",__FUNCTION__);

+			return;

+		}

+	}

+

+	SecPtr = pframe;

+	SecCalculateMicSMS4(KeyIdx, pMicKey, SecPtr, (SecPtr+DataOffset), pattrib->pktlen, MicBuffer);

+

+	WAPI_DATA(WAPI_TX, "Encryption - MIC", MicBuffer, padapter->wapiInfo.extra_postfix_len);

+

+	memcpy(pframe+pattrib->hdrlen+pattrib->iv_len+pattrib->pktlen-pattrib->icv_len,

+			(u8 *)MicBuffer,

+			padapter->wapiInfo.extra_postfix_len

+			);

+

+

+	WapiSMS4Encryption(pDataKey, pIV, (SecPtr+DataOffset),pattrib->pktlen+pattrib->icv_len, (SecPtr+DataOffset), &OutputLength);

+

+	WAPI_DATA(WAPI_TX, "Encryption - After SMS4 encryption",pframe,pattrib->hdrlen+pattrib->iv_len+pattrib->pktlen);

+

+	WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__);

+}

+

+u8 SecSWSMS4Decryption(

+	_adapter *padapter,

+	u8 		*precv_frame,

+	struct recv_priv *precv_priv

+	)

+{

+	PRT_WAPI_T pWapiInfo = &padapter->wapiInfo;

+	struct recv_frame_hdr *precv_hdr;

+	PRT_WAPI_STA_INFO   pWapiSta = NULL;

+	u8 IVOffset, DataOffset, bFindMatchPeer = false, bUseUpdatedKey = false;

+	u8 KeyIdx, MicBuffer[16], lastRxPNforQoS[16];

+	u8 *pRA, *pTA, *pMicKey, *pDataKey, *pLastRxPN, *pRecvPN, *pSecData, *pRecvMic, *pos;

+	u8 TID = 0;

+	u16 OutputLength, DataLen;

+	u8   bQosData;

+	struct sk_buff * 	pskb;

+

+	WAPI_TRACE(WAPI_RX, "=========>%s\n", __FUNCTION__);

+

+	return 0;

+

+	precv_hdr = &((union recv_frame*)precv_frame)->u.hdr;

+	pskb = (struct sk_buff *)(precv_hdr->rx_data);

+	precv_hdr->bWapiCheckPNInDecrypt = WapiCheckPnInSwDecrypt(padapter, pskb);

+	WAPI_TRACE(WAPI_RX, "=========>%s: check PN  %d\n", __FUNCTION__,precv_hdr->bWapiCheckPNInDecrypt);

+	WAPI_DATA(WAPI_RX, "Decryption - Before decryption", pskb->data, pskb->len);

+

+	IVOffset = sMacHdrLng;

+	bQosData = GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE;

+	if (bQosData){

+		IVOffset += 2;

+	}

+

+	//if(GetHTC())

+	//	IVOffset += 4;

+

+	//IVOffset += SNAP_SIZE + sizeof(u16);

+

+	DataOffset = IVOffset + padapter->wapiInfo.extra_prefix_len;

+

+	pRA = pskb->data + 4;

+	pTA = pskb->data + 10;

+	KeyIdx = *(pskb->data + IVOffset);

+	pRecvPN = pskb->data + IVOffset + 2;

+	pSecData = pskb->data + DataOffset;

+	DataLen = pskb->len - DataOffset;

+	pRecvMic = pskb->data + pskb->len - padapter->wapiInfo.extra_postfix_len;

+	TID = GetTid(pskb->data);

+

+	if (!list_empty(&(pWapiInfo->wapiSTAUsedList))){

+		list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) {

+			if (0 == memcmp(pWapiSta->PeerMacAddr, pTA, 6)){

+				bFindMatchPeer = true;

+				break;

+			}

+		}

+	}

+

+	if (!bFindMatchPeer){

+		WAPI_TRACE(WAPI_ERR, "%s: Can not find Peer Sta "MAC_FMT" for Key Info!!!\n", __FUNCTION__, MAC_ARG(pTA));

+		return false;

+	}

+

+	if( IS_MCAST(pRA) ){

+		WAPI_TRACE(WAPI_RX, "%s: Multicast decryption !!!\n", __FUNCTION__);

+		if (pWapiSta->wapiMsk.keyId == KeyIdx && pWapiSta->wapiMsk.bSet){

+			pLastRxPN = pWapiSta->lastRxMulticastPN;

+			if (!WapiComparePN(pRecvPN, pLastRxPN)){

+				WAPI_TRACE(WAPI_ERR, "%s: MSK PN is not larger than last, Dropped!!!\n", __FUNCTION__);

+				WAPI_DATA(WAPI_ERR, "pRecvPN:", pRecvPN, 16);

+				WAPI_DATA(WAPI_ERR, "pLastRxPN:", pLastRxPN, 16);

+				return false;

+			}

+

+			memcpy(pLastRxPN, pRecvPN, 16);

+			pMicKey = pWapiSta->wapiMsk.micKey;

+			pDataKey = pWapiSta->wapiMsk.dataKey;

+		}else if (pWapiSta->wapiMskUpdate.keyId == KeyIdx && pWapiSta->wapiMskUpdate.bSet){

+			WAPI_TRACE(WAPI_RX, "%s: Use Updated MSK for Decryption !!!\n", __FUNCTION__);

+			bUseUpdatedKey = true;

+			memcpy(pWapiSta->lastRxMulticastPN, pRecvPN, 16);

+			pMicKey = pWapiSta->wapiMskUpdate.micKey;

+			pDataKey = pWapiSta->wapiMskUpdate.dataKey;

+		}else{

+			WAPI_TRACE(WAPI_ERR, "%s: Can not find MSK with matched KeyIdx(%d), Dropped !!!\n", __FUNCTION__,KeyIdx);

+			return false;

+		}

+	}

+	else{

+		WAPI_TRACE(WAPI_RX, "%s: Unicast decryption !!!\n", __FUNCTION__);

+		if (pWapiSta->wapiUsk.keyId == KeyIdx && pWapiSta->wapiUsk.bSet){

+			WAPI_TRACE(WAPI_RX, "%s: Use USK for Decryption!!!\n", __FUNCTION__);

+			if(precv_hdr->bWapiCheckPNInDecrypt){

+				if(GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE){

+					WapiGetLastRxUnicastPNForQoSData(TID, pWapiSta, lastRxPNforQoS);

+					pLastRxPN = lastRxPNforQoS;

+				}else{

+					pLastRxPN = pWapiSta->lastRxUnicastPN;

+				}

+				if (!WapiComparePN(pRecvPN, pLastRxPN)){

+					return false;

+				}

+				if(bQosData){

+					WapiSetLastRxUnicastPNForQoSData(TID, pRecvPN, pWapiSta);

+				}else{

+					memcpy(pWapiSta->lastRxUnicastPN, pRecvPN, 16);

+				}

+			}else{

+				memcpy(precv_hdr->WapiTempPN,pRecvPN,16);

+			}

+

+			if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE))

+			{

+				if ((pRecvPN[0] & 0x1) == 0){

+					WAPI_TRACE(WAPI_ERR, "%s: Rx USK PN is not odd when Infra STA mode, Dropped !!!\n", __FUNCTION__);

+					return false;

+				}

+			}

+

+			pMicKey = pWapiSta->wapiUsk.micKey;

+			pDataKey = pWapiSta->wapiUsk.dataKey;

+		}

+		else if (pWapiSta->wapiUskUpdate.keyId == KeyIdx && pWapiSta->wapiUskUpdate.bSet ){

+			WAPI_TRACE(WAPI_RX, "%s: Use Updated USK for Decryption!!!\n", __FUNCTION__);

+			if(pWapiSta->bAuthenticatorInUpdata)

+				bUseUpdatedKey = true;

+			else

+				bUseUpdatedKey = false;

+

+			if(bQosData){

+				WapiSetLastRxUnicastPNForQoSData(TID, pRecvPN, pWapiSta);

+			}else{

+				memcpy(pWapiSta->lastRxUnicastPN, pRecvPN, 16);

+			}

+			pMicKey = pWapiSta->wapiUskUpdate.micKey;

+			pDataKey = pWapiSta->wapiUskUpdate.dataKey;

+		}else{

+			WAPI_TRACE(WAPI_ERR, "%s: No valid USK!!!KeyIdx=%d pWapiSta->wapiUsk.keyId=%d pWapiSta->wapiUskUpdate.keyId=%d\n", __FUNCTION__, KeyIdx, pWapiSta->wapiUsk.keyId, pWapiSta->wapiUskUpdate.keyId);

+			//dump_buf(pskb->data,pskb->len);

+			return false;

+		}

+	}

+

+	WAPI_DATA(WAPI_RX, "Decryption - DataKey", pDataKey, 16);

+	WAPI_DATA(WAPI_RX, "Decryption - IV", pRecvPN, 16);

+	WapiSMS4Decryption(pDataKey, pRecvPN, pSecData, DataLen, pSecData, &OutputLength);

+

+	if (OutputLength != DataLen)

+		WAPI_TRACE(WAPI_ERR, "%s:  Output Length Error!!!!\n", __FUNCTION__);

+

+	WAPI_DATA(WAPI_RX, "Decryption - After decryption", pskb->data, pskb->len);

+

+	DataLen -= padapter->wapiInfo.extra_postfix_len;

+

+	SecCalculateMicSMS4(KeyIdx, pMicKey, pskb->data, pSecData, DataLen, MicBuffer);

+

+	WAPI_DATA(WAPI_RX, "Decryption - MIC received", pRecvMic, SMS4_MIC_LEN);

+	WAPI_DATA(WAPI_RX, "Decryption - MIC calculated", MicBuffer, SMS4_MIC_LEN);

+

+	if (0 == memcmp(MicBuffer, pRecvMic, padapter->wapiInfo.extra_postfix_len)){

+		WAPI_TRACE(WAPI_RX, "%s: Check MIC OK!!\n", __FUNCTION__);

+		if (bUseUpdatedKey){

+			// delete the old key

+			if ( IS_MCAST(pRA) ){

+				WAPI_TRACE(WAPI_API, "%s(): AE use new update MSK!!\n", __FUNCTION__);

+				pWapiSta->wapiMsk.keyId = pWapiSta->wapiMskUpdate.keyId;

+				memcpy(pWapiSta->wapiMsk.dataKey, pWapiSta->wapiMskUpdate.dataKey, 16);

+				memcpy(pWapiSta->wapiMsk.micKey, pWapiSta->wapiMskUpdate.micKey, 16);

+				pWapiSta->wapiMskUpdate.bTxEnable = pWapiSta->wapiMskUpdate.bSet = false;

+			}else{

+				WAPI_TRACE(WAPI_API, "%s(): AE use new update USK!!\n", __FUNCTION__);

+				pWapiSta->wapiUsk.keyId = pWapiSta->wapiUskUpdate.keyId;

+				memcpy(pWapiSta->wapiUsk.dataKey, pWapiSta->wapiUskUpdate.dataKey, 16);

+				memcpy(pWapiSta->wapiUsk.micKey, pWapiSta->wapiUskUpdate.micKey, 16);

+				pWapiSta->wapiUskUpdate.bTxEnable = pWapiSta->wapiUskUpdate.bSet = false;

+			}

+		}

+	}else{

+		WAPI_TRACE(WAPI_ERR, "%s:  Check MIC Error, Dropped !!!!\n", __FUNCTION__);

+		return false;

+	}

+

+	pos = pskb->data;

+	memmove(pos+padapter->wapiInfo.extra_prefix_len, pos, IVOffset);

+	skb_pull(pskb, padapter->wapiInfo.extra_prefix_len);

+

+	WAPI_TRACE(WAPI_RX, "<=========%s\n", __FUNCTION__);

+

+	return true;

+}

+

+u32	rtw_sms4_encrypt(_adapter *padapter, u8 *pxmitframe)

+{

+

+	u8	*pframe;

+	u32 res = _SUCCESS;

+

+	WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_TX, "<========== %s, WAPI not supported or enabled!\n", __FUNCTION__);

+		return _FAIL;

+	}

+

+	if(((struct xmit_frame*)pxmitframe)->buf_addr==NULL)

+		return _FAIL;

+

+	pframe = ((struct xmit_frame*)pxmitframe)->buf_addr + TXDESC_OFFSET;

+

+	SecSWSMS4Encryption(padapter, pxmitframe);

+

+	WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__);

+	return res;

+}

+

+u32	rtw_sms4_decrypt(_adapter *padapter, u8 *precvframe)

+{

+	u8	*pframe;

+	u32 res = _SUCCESS;

+

+	WAPI_TRACE(WAPI_RX, "=========>%s\n", __FUNCTION__);

+

+	if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable))

+	{

+		WAPI_TRACE(WAPI_RX, "<========== %s, WAPI not supported or enabled!\n", __FUNCTION__);

+		return _FAIL;

+	}

+

+

+	//drop packet when hw decrypt fail

+       //return tempraily

+	return _FAIL;

+

+	//pframe=(unsigned char *)((union recv_frame*)precvframe)->u.hdr.rx_data;

+

+	if (false == SecSWSMS4Decryption(padapter, precvframe, &padapter->recvpriv))

+	{

+		WAPI_TRACE(WAPI_ERR, "%s():SMS4 decrypt frame error\n",__FUNCTION__);

+		return _FAIL;

+	}

+

+	WAPI_TRACE(WAPI_RX, "<=========%s\n", __FUNCTION__);

+	return res;

+}

+

+#else

+

+u32	rtw_sms4_encrypt(_adapter *padapter, u8 *pxmitframe)

+{

+	WAPI_TRACE(WAPI_TX, "=========>Dummy %s\n", __FUNCTION__);

+	WAPI_TRACE(WAPI_TX, "<=========Dummy %s\n", __FUNCTION__);

+	return _SUCCESS;

+}

+

+u32	rtw_sms4_decrypt(_adapter *padapter, u8 *precvframe)

+{

+	WAPI_TRACE(WAPI_RX, "=========>Dummy %s\n", __FUNCTION__);

+	WAPI_TRACE(WAPI_RX, "<=========Dummy %s\n", __FUNCTION__);

+	return _SUCCESS;

+}

+

+#endif

+

+#endif

diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_wlan_util.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_wlan_util.c
new file mode 100755
index 00000000..182865da
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_wlan_util.c
@@ -0,0 +1,2615 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_WLAN_UTIL_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <wifi.h>
+
+
+unsigned char ARTHEROS_OUI1[] = {0x00, 0x03, 0x7f};
+unsigned char ARTHEROS_OUI2[] = {0x00, 0x13, 0x74};
+
+unsigned char BROADCOM_OUI1[] = {0x00, 0x10, 0x18};
+unsigned char BROADCOM_OUI2[] = {0x00, 0x0a, 0xf7};
+unsigned char BROADCOM_OUI3[] = {0x00, 0x05, 0xb5};
+
+unsigned char CISCO_OUI[] = {0x00, 0x40, 0x96};
+unsigned char MARVELL_OUI[] = {0x00, 0x50, 0x43};
+unsigned char RALINK_OUI[] = {0x00, 0x0c, 0x43};
+unsigned char REALTEK_OUI[] = {0x00, 0xe0, 0x4c};
+unsigned char AIRGOCAP_OUI[] = {0x00, 0x0a, 0xf5};
+unsigned char EPIGRAM_OUI[] = {0x00, 0x90, 0x4c};
+
+unsigned char REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
+
+extern unsigned char	MCS_rate_2R[16];
+#ifdef CONFIG_DISABLE_MCS13TO15
+extern unsigned char	MCS_rate_2R_MCS13TO15_OFF[16];
+#endif //CONFIG_DISABLE_MCS13TO15
+extern unsigned char	MCS_rate_1R[16];
+extern unsigned char RTW_WPA_OUI[];
+extern unsigned char WPA_TKIP_CIPHER[4];
+extern unsigned char RSN_TKIP_CIPHER[4];
+
+#define R2T_PHY_DELAY	(0)
+
+//#define WAIT_FOR_BCN_TO_MIN	(3000)
+#define WAIT_FOR_BCN_TO_MIN	(6000)
+#define WAIT_FOR_BCN_TO_MAX	(20000)
+
+static u8 rtw_basic_rate_cck[4] = {
+	IEEE80211_CCK_RATE_1MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB|IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB|IEEE80211_BASIC_RATE_MASK
+};
+
+static u8 rtw_basic_rate_ofdm[3] = {
+	IEEE80211_OFDM_RATE_6MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB|IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_24MB|IEEE80211_BASIC_RATE_MASK
+};
+
+static u8 rtw_basic_rate_mix[7] = {
+	IEEE80211_CCK_RATE_1MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB|IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB|IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_6MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB|IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_24MB|IEEE80211_BASIC_RATE_MASK
+};
+
+
+int cckrates_included(unsigned char *rate, int ratelen)
+{
+	int	i;
+	
+	for(i = 0; i < ratelen; i++)
+	{
+		if  (  (((rate[i]) & 0x7f) == 2)	|| (((rate[i]) & 0x7f) == 4) ||
+			   (((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22) )
+		return _TRUE;	
+	}
+
+	return _FALSE;
+
+}
+
+int cckratesonly_included(unsigned char *rate, int ratelen)
+{
+	int	i;
+	
+	for(i = 0; i < ratelen; i++)
+	{
+		if  ( (((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
+			   (((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22) )
+		return _FALSE;	
+	}
+	
+	return _TRUE;
+}
+
+unsigned char networktype_to_raid(unsigned char network_type)
+{
+	unsigned char raid;
+
+	switch(network_type)
+	{
+		case WIRELESS_11B:
+			raid = RATR_INX_WIRELESS_B;
+			break;
+		case WIRELESS_11A:
+		case WIRELESS_11G:
+			raid = RATR_INX_WIRELESS_G;
+			break;
+		case WIRELESS_11BG:
+			raid = RATR_INX_WIRELESS_GB;
+			break;
+		case WIRELESS_11_24N:
+		case WIRELESS_11_5N:
+			raid = RATR_INX_WIRELESS_N;
+			break;
+		case WIRELESS_11A_5N:
+		case WIRELESS_11G_24N:
+			raid = RATR_INX_WIRELESS_NG;
+			break;
+		case WIRELESS_11BG_24N:
+			raid = RATR_INX_WIRELESS_NGB;
+			break;
+		default:
+			raid = RATR_INX_WIRELESS_GB;
+			break;	
+
+	}
+
+	return raid;
+	
+}
+
+u8 judge_network_type(_adapter *padapter, unsigned char *rate, int ratelen)
+{
+	u8 network_type = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	
+
+	if(pmlmeext->cur_channel > 14)
+	{
+		if (pmlmeinfo->HT_enable)
+		{
+			network_type = WIRELESS_11_5N;
+		}
+	
+		network_type |= WIRELESS_11A;
+	}
+	else
+	{
+		if (pmlmeinfo->HT_enable)
+		{
+			network_type = WIRELESS_11_24N;
+		}
+
+		if ((cckratesonly_included(rate, ratelen)) == _TRUE)
+		{
+			network_type |= WIRELESS_11B;
+		}
+		else if((cckrates_included(rate, ratelen)) == _TRUE)
+		{
+			network_type |= WIRELESS_11BG;
+		}
+		else
+		{
+			network_type |= WIRELESS_11G;
+		}
+	}
+		
+	return 	network_type;
+}
+
+unsigned char ratetbl_val_2wifirate(unsigned char rate);
+unsigned char ratetbl_val_2wifirate(unsigned char rate)
+{
+	unsigned char val = 0;
+
+	switch (rate & 0x7f) 
+	{
+		case 0:
+			val = IEEE80211_CCK_RATE_1MB;
+			break;
+
+		case 1:
+			val = IEEE80211_CCK_RATE_2MB;
+			break;
+
+		case 2:
+			val = IEEE80211_CCK_RATE_5MB;
+			break;
+
+		case 3:
+			val = IEEE80211_CCK_RATE_11MB;
+			break;
+			
+		case 4:
+			val = IEEE80211_OFDM_RATE_6MB;
+			break;
+
+		case 5:
+			val = IEEE80211_OFDM_RATE_9MB;
+			break;
+
+		case 6:
+			val = IEEE80211_OFDM_RATE_12MB;
+			break;
+			
+		case 7:
+			val = IEEE80211_OFDM_RATE_18MB;
+			break;
+
+		case 8:
+			val = IEEE80211_OFDM_RATE_24MB;
+			break;
+			
+		case 9:
+			val = IEEE80211_OFDM_RATE_36MB;
+			break;
+
+		case 10:
+			val = IEEE80211_OFDM_RATE_48MB;
+			break;
+		
+		case 11:
+			val = IEEE80211_OFDM_RATE_54MB;
+			break;
+
+	}
+
+	return val;
+
+}
+
+int is_basicrate(_adapter *padapter, unsigned char rate);
+int is_basicrate(_adapter *padapter, unsigned char rate)
+{
+	int i;
+	unsigned char val;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	
+	for(i = 0; i < NumRates; i++)
+	{
+		val = pmlmeext->basicrate[i];
+
+		if ((val != 0xff) && (val != 0xfe))
+		{
+			if (rate == ratetbl_val_2wifirate(val))
+			{
+				return _TRUE;
+			}
+		}
+	}
+	
+	return _FALSE;
+}
+
+unsigned int ratetbl2rateset(_adapter *padapter, unsigned char *rateset);
+unsigned int ratetbl2rateset(_adapter *padapter, unsigned char *rateset)
+{
+	int i;
+	unsigned char rate;
+	unsigned int	len = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	for (i = 0; i < NumRates; i++)
+	{
+		rate = pmlmeext->datarate[i];
+
+		switch (rate)
+		{
+			case 0xff:
+				return len;
+				
+			case 0xfe:
+				continue;
+				
+			default:
+				rate = ratetbl_val_2wifirate(rate);
+
+				if (is_basicrate(padapter, rate) == _TRUE)
+				{
+					rate |= IEEE80211_BASIC_RATE_MASK;
+				}
+				
+				rateset[len] = rate;
+				len++;
+				break;
+		}
+	}
+	return len;
+}
+
+void get_rate_set(_adapter *padapter, unsigned char *pbssrate, int *bssrate_len)
+{
+	unsigned char supportedrates[NumRates];
+
+	_rtw_memset(supportedrates, 0, NumRates);
+	*bssrate_len = ratetbl2rateset(padapter, supportedrates);
+	_rtw_memcpy(pbssrate, supportedrates, *bssrate_len);
+}
+
+void UpdateBrateTbl(
+	IN PADAPTER		Adapter,
+	IN u8			*mBratesOS
+)
+{
+	u8	i;
+	u8	rate;
+
+	// 1M, 2M, 5.5M, 11M, 6M, 12M, 24M are mandatory.
+	for(i=0;i<NDIS_802_11_LENGTH_RATES_EX;i++)
+	{
+		rate = mBratesOS[i] & 0x7f;
+		switch(rate)
+		{
+			case IEEE80211_CCK_RATE_1MB:
+			case IEEE80211_CCK_RATE_2MB:
+			case IEEE80211_CCK_RATE_5MB:
+			case IEEE80211_CCK_RATE_11MB:
+			case IEEE80211_OFDM_RATE_6MB:
+			case IEEE80211_OFDM_RATE_12MB:
+			case IEEE80211_OFDM_RATE_24MB:
+				mBratesOS[i] |= IEEE80211_BASIC_RATE_MASK;
+				break;
+		}
+	}
+
+}
+
+void UpdateBrateTblForSoftAP(u8 *bssrateset, u32 bssratelen)
+{
+	u8	i;
+	u8	rate;
+
+	for(i=0;i<bssratelen;i++)
+	{
+		rate = bssrateset[i] & 0x7f;
+		switch(rate)
+		{
+			case IEEE80211_CCK_RATE_1MB:
+			case IEEE80211_CCK_RATE_2MB:
+			case IEEE80211_CCK_RATE_5MB:
+			case IEEE80211_CCK_RATE_11MB:
+				bssrateset[i] |= IEEE80211_BASIC_RATE_MASK;
+				break;
+		}
+	}
+
+}
+
+void Save_DM_Func_Flag(_adapter *padapter)
+{
+	u8	bSaveFlag = _TRUE;
+
+#ifdef CONFIG_CONCURRENT_MODE	
+	_adapter *pbuddy_adapter = padapter->pbuddy_adapter;
+	if(pbuddy_adapter)
+	rtw_hal_set_hwreg(pbuddy_adapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
+#endif
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
+
+}
+
+void Restore_DM_Func_Flag(_adapter *padapter)
+{
+	u8	bSaveFlag = _FALSE;
+#ifdef CONFIG_CONCURRENT_MODE	
+	_adapter *pbuddy_adapter = padapter->pbuddy_adapter;
+	if(pbuddy_adapter)
+	rtw_hal_set_hwreg(pbuddy_adapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
+#endif
+	rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
+}
+
+void Switch_DM_Func(_adapter *padapter, u32 mode, u8 enable)
+{
+#ifdef CONFIG_CONCURRENT_MODE	
+	_adapter *pbuddy_adapter = padapter->pbuddy_adapter;
+#endif
+
+	if(enable == _TRUE)
+	{
+#ifdef CONFIG_CONCURRENT_MODE
+		if(pbuddy_adapter)
+		rtw_hal_set_hwreg(pbuddy_adapter, HW_VAR_DM_FUNC_SET, (u8 *)(&mode));
+#endif
+		rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_SET, (u8 *)(&mode));
+	}
+	else
+	{
+#ifdef CONFIG_CONCURRENT_MODE
+		if(pbuddy_adapter)
+		rtw_hal_set_hwreg(pbuddy_adapter, HW_VAR_DM_FUNC_CLR, (u8 *)(&mode));
+#endif
+		rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_CLR, (u8 *)(&mode));
+	}
+
+#if 0
+	u8 val8;
+
+	val8 = rtw_read8(padapter, FW_DYNAMIC_FUN_SWITCH);
+
+	if(enable == _TRUE)
+	{
+		rtw_write8(padapter, FW_DYNAMIC_FUN_SWITCH, (val8 | mode));
+	}
+	else
+	{
+		rtw_write8(padapter, FW_DYNAMIC_FUN_SWITCH, (val8 & mode));
+	}
+#endif
+
+}
+
+static void Set_NETYPE1_MSR(_adapter *padapter, u8 type)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_MEDIA_STATUS1, (u8 *)(&type));
+}
+
+static void Set_NETYPE0_MSR(_adapter *padapter, u8 type)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_MEDIA_STATUS, (u8 *)(&type));
+}
+
+void Set_MSR(_adapter *padapter, u8 type)
+{
+#ifdef CONFIG_CONCURRENT_MODE
+	if(padapter->iface_type == IFACE_PORT1)
+	{
+		Set_NETYPE1_MSR(padapter, type);
+	}
+	else
+#endif
+	{
+		Set_NETYPE0_MSR(padapter, type);
+	}
+}
+
+inline u8 rtw_get_oper_ch(_adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_channel;
+}
+
+inline void rtw_set_oper_ch(_adapter *adapter, u8 ch)
+{
+	adapter_to_dvobj(adapter)->oper_channel = ch;
+}
+
+inline u8 rtw_get_oper_bw(_adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_bwmode;
+}
+
+inline void rtw_set_oper_bw(_adapter *adapter, u8 bw)
+{
+	adapter_to_dvobj(adapter)->oper_bwmode = bw;
+}
+
+inline u8 rtw_get_oper_choffset(_adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_ch_offset;
+}
+
+inline void rtw_set_oper_choffset(_adapter *adapter, u8 offset)
+{
+	adapter_to_dvobj(adapter)->oper_ch_offset = offset;
+}
+
+void SelectChannel(_adapter *padapter, unsigned char channel)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;	
+
+#ifdef CONFIG_DUALMAC_CONCURRENT
+	//saved channel info
+	rtw_set_oper_ch(padapter, channel);
+	dc_SelectChannel(padapter, channel);
+#else //CONFIG_DUALMAC_CONCURRENT
+
+	_enter_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL);
+	
+	//saved channel info
+	rtw_set_oper_ch(padapter, channel);
+
+	rtw_hal_set_chan(padapter, channel);
+	
+	_exit_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL);
+		
+#endif // CONFIG_DUALMAC_CONCURRENT
+}
+
+void SetBWMode(_adapter *padapter, unsigned short bwmode, unsigned char channel_offset)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+#ifdef CONFIG_DUALMAC_CONCURRENT
+	//saved bw info
+	rtw_set_oper_bw(padapter, bwmode);
+	rtw_set_oper_choffset(padapter, channel_offset);
+	dc_SetBWMode(padapter, bwmode, channel_offset);
+#else //CONFIG_DUALMAC_CONCURRENT
+
+	_enter_critical_mutex(&(adapter_to_dvobj(padapter)->setbw_mutex), NULL);
+
+	//saved bw info
+	rtw_set_oper_bw(padapter, bwmode);
+	rtw_set_oper_choffset(padapter, channel_offset);
+
+	rtw_hal_set_bwmode(padapter, (HT_CHANNEL_WIDTH)bwmode, channel_offset);
+
+	_exit_critical_mutex(&(adapter_to_dvobj(padapter)->setbw_mutex), NULL);
+
+#endif // CONFIG_DUALMAC_CONCURRENT
+}
+
+void set_channel_bwmode(_adapter *padapter, unsigned char channel, unsigned char channel_offset, unsigned short bwmode)
+{
+	u8 center_ch;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if ( padapter->bNotifyChannelChange )
+	{
+		DBG_871X( "[%s] ch = %d, offset = %d, bwmode = %d\n", __FUNCTION__, channel, channel_offset, bwmode );
+	}
+
+	if((bwmode == HT_CHANNEL_WIDTH_20)||(channel_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE))
+	{
+		//SelectChannel(padapter, channel);
+		center_ch = channel;
+	}
+	else		
+	{
+		//switch to the proper channel
+		if (channel_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
+		{
+			//SelectChannel(padapter, channel + 2);
+			center_ch = channel + 2;
+		}
+		else
+		{
+			//SelectChannel(padapter, channel - 2);
+			center_ch = channel - 2;
+		}
+	}	
+
+	//set Channel
+#ifdef CONFIG_DUALMAC_CONCURRENT
+	//saved channel/bw info
+	rtw_set_oper_ch(padapter, channel);
+	rtw_set_oper_bw(padapter, bwmode);
+	rtw_set_oper_choffset(padapter, channel_offset);
+	dc_SelectChannel(padapter, center_ch);// set center channel
+#else //CONFIG_DUALMAC_CONCURRENT
+
+	_enter_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL);
+	
+	//saved channel/bw info
+	rtw_set_oper_ch(padapter, channel);
+	rtw_set_oper_bw(padapter, bwmode);
+	rtw_set_oper_choffset(padapter, channel_offset);
+
+	rtw_hal_set_chan(padapter, center_ch); // set center channel
+
+        _exit_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL);
+
+#endif // CONFIG_DUALMAC_CONCURRENT
+
+	
+	SetBWMode(padapter, bwmode, channel_offset);
+	
+}
+
+int get_bsstype(unsigned short capability)
+{
+	if (capability & BIT(0))
+	{
+		return WIFI_FW_AP_STATE;
+	}
+	else if (capability & BIT(1))
+	{
+		return WIFI_FW_ADHOC_STATE;
+	}
+	else
+	{
+		return 0;		
+	}
+}
+
+__inline u8 *get_my_bssid(WLAN_BSSID_EX *pnetwork)
+{	
+	return (pnetwork->MacAddress); 
+}
+
+u16 get_beacon_interval(WLAN_BSSID_EX *bss)
+{
+	unsigned short val;
+	_rtw_memcpy((unsigned char *)&val, rtw_get_beacon_interval_from_ie(bss->IEs), 2);
+
+	return le16_to_cpu(val);	
+
+}
+
+int is_client_associated_to_ap(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext;
+	struct mlme_ext_info	*pmlmeinfo;
+
+	if(!padapter)
+		return _FAIL;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+	
+	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE))
+	{
+		return _TRUE;
+	}
+	else
+	{
+		return _FAIL;
+	}
+}
+
+int is_client_associated_to_ibss(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	
+	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE))
+	{
+		return _TRUE;
+	}
+	else
+	{
+		return _FAIL;
+	}
+}
+
+int is_IBSS_empty(_adapter *padapter)
+{
+	unsigned int i;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	
+	for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
+	{
+		if (pmlmeinfo->FW_sta_info[i].status == 1)
+		{
+			return _FAIL;
+		}
+	}
+	
+	return _TRUE;
+	
+}
+
+unsigned int decide_wait_for_beacon_timeout(unsigned int bcn_interval)
+{
+	if ((bcn_interval << 2) < WAIT_FOR_BCN_TO_MIN)
+	{
+		return WAIT_FOR_BCN_TO_MIN;
+	} 
+	else if ((bcn_interval << 2) > WAIT_FOR_BCN_TO_MAX)
+	{
+		return WAIT_FOR_BCN_TO_MAX;
+	}	
+	else
+	{
+		return ((bcn_interval << 2));
+	}
+}
+
+void CAM_empty_entry(
+	PADAPTER     	Adapter,	
+	u8 			ucIndex
+)
+{
+	rtw_hal_set_hwreg(Adapter, HW_VAR_CAM_EMPTY_ENTRY, (u8 *)(&ucIndex));
+}
+
+void invalidate_cam_all(_adapter *padapter)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_CAM_INVALID_ALL, 0);
+}
+#if 0
+static u32 _ReadCAM(_adapter *padapter ,u32 addr)
+{
+	u32 count = 0, cmd;
+	cmd = CAM_POLLINIG |addr ;
+	rtw_write32(padapter, RWCAM, cmd);
+
+	do{
+		if(0 == (rtw_read32(padapter,REG_CAMCMD) & CAM_POLLINIG)){
+			break;
+		}
+	}while(count++ < 100);		
+
+	return rtw_read32(padapter,REG_CAMREAD);	
+}
+void read_cam(_adapter *padapter ,u8 entry)
+{
+	u32	j,count = 0, addr, cmd;
+	addr = entry << 3;
+
+	DBG_8192C("********* DUMP CAM Entry_#%02d***************\n",entry);
+	for (j = 0; j < 6; j++)
+	{	
+		cmd = _ReadCAM(padapter ,addr+j);
+		DBG_8192C("offset:0x%02x => 0x%08x \n",addr+j,cmd);
+	}
+	DBG_8192C("*********************************\n");
+}
+#endif
+
+void write_cam(_adapter *padapter, u8 entry, u16 ctrl, u8 *mac, u8 *key)
+{
+	unsigned int	i, val, addr;
+	//unsigned int    cmd;
+	int j;
+	u32	cam_val[2];
+
+	addr = entry << 3;
+
+	for (j = 5; j >= 0; j--)
+	{	
+		switch (j)
+		{
+			case 0:
+				val = (ctrl | (mac[0] << 16) | (mac[1] << 24) );
+				break;
+				
+			case 1:
+				val = (mac[2] | ( mac[3] << 8) | (mac[4] << 16) | (mac[5] << 24));
+				break;
+			
+			default:
+				i = (j - 2) << 2;
+				val = (key[i] | (key[i+1] << 8) | (key[i+2] << 16) | (key[i+3] << 24));
+				break;
+				
+		}
+
+		cam_val[0] = val;
+		cam_val[1] = addr + (unsigned int)j;
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_CAM_WRITE, (u8 *)cam_val);
+		
+		//rtw_write32(padapter, WCAMI, val);
+		
+		//cmd = CAM_POLLINIG | CAM_WRITE | (addr + j);
+		//rtw_write32(padapter, RWCAM, cmd);
+		
+		//DBG_871X("%s=> cam write: %x, %x\n",__FUNCTION__, cmd, val);
+		
+	}
+
+}
+
+void clear_cam_entry(_adapter *padapter, u8 entry)
+{	
+#if 0
+	u32	addr, val=0;
+	u32	cam_val[2];
+
+	addr = entry << 3;
+	
+
+	cam_val[0] = val;
+	cam_val[1] = addr + (unsigned int)0;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_CAM_WRITE, (u8 *)cam_val);
+
+
+
+	cam_val[0] = val;
+	cam_val[1] = addr + (unsigned int)1;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_CAM_WRITE, (u8 *)cam_val);
+#else
+
+	unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+	unsigned char null_key[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,0x00, 0x00, 0x00, 0x00,0x00, 0x00, 0x00, 0x00};
+
+	write_cam(padapter, entry, 0, null_sta, null_key);
+
+#endif
+}
+
+int allocate_fw_sta_entry(_adapter *padapter)
+{
+	unsigned int mac_id;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	
+	for (mac_id = IBSS_START_MAC_ID; mac_id < NUM_STA; mac_id++)
+	{
+		if (pmlmeinfo->FW_sta_info[mac_id].status == 0)
+		{
+			pmlmeinfo->FW_sta_info[mac_id].status = 1;
+			pmlmeinfo->FW_sta_info[mac_id].retry = 0;
+			break;
+		}
+	}
+	
+	return mac_id;
+}
+
+void flush_all_cam_entry(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+#ifdef CONFIG_CONCURRENT_MODE
+
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	//if(check_buddy_mlmeinfo_state(padapter, _HW_STATE_NOLINK_))	
+	if(check_buddy_fwstate(padapter, _FW_LINKED) == _FALSE)
+	{
+		rtw_hal_set_hwreg(padapter, HW_VAR_CAM_INVALID_ALL, 0);		
+	}
+	else
+	{
+		if(check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		{
+			struct sta_priv	*pstapriv = &padapter->stapriv;
+			struct sta_info	*psta;
+			u8 cam_id;//cam_entry
+
+			psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
+			if(psta) {
+				if(psta->state & WIFI_AP_STATE)
+				{}   //clear cam when ap free per sta_info        
+				else {
+					if(psta->mac_id==2)
+						cam_id = 5;
+					else
+						cam_id = 4;
+				}
+				//clear_cam_entry(padapter, cam_id);
+				rtw_clearstakey_cmd(padapter, (u8*)psta, cam_id, _FALSE);
+			}
+		}
+		else if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
+		{
+			//clear cam when ap free per sta_info 
+		}			
+	}
+#else //CONFIG_CONCURRENT_MODE
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_CAM_INVALID_ALL, 0);	
+
+#endif //CONFIG_CONCURRENT_MODE
+
+	_rtw_memset((u8 *)(pmlmeinfo->FW_sta_info), 0, sizeof(pmlmeinfo->FW_sta_info));
+	
+}
+
+#if defined(CONFIG_P2P) && defined(CONFIG_WFD)
+int WFD_info_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs	pIE)
+{
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo;	
+	u8	wfd_ie[ 128 ] = { 0x00 };
+	u32	wfd_ielen = 0;
+
+
+	pwdinfo = &padapter->wdinfo;
+	if ( rtw_get_wfd_ie( ( u8* ) pIE, pIE->Length, wfd_ie, &wfd_ielen ) )
+	{
+		u8	attr_content[ 10 ] = { 0x00 };
+		u32	attr_contentlen = 0;
+			
+		DBG_871X( "[%s] Found WFD IE\n", __FUNCTION__ );
+		rtw_get_wfd_attr_content( wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
+		if ( attr_contentlen )
+		{
+			pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16( attr_content + 2 );
+			DBG_8192C( "[%s] Peer PORT NUM = %d\n", __FUNCTION__, pwdinfo->wfd_info->peer_rtsp_ctrlport );
+			return( _TRUE );
+		}		
+	}
+	else
+	{
+		DBG_871X( "[%s] NO WFD IE\n", __FUNCTION__ );
+
+	}
+	return( _FAIL );
+}
+#endif
+
+int WMM_param_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs	pIE)
+{
+	//struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);	
+	
+	if(pmlmepriv->qospriv.qos_option==0)
+	{
+		pmlmeinfo->WMM_enable = 0;
+		return _FAIL;
+	}	
+
+	pmlmeinfo->WMM_enable = 1;
+	_rtw_memcpy(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element));
+	return _TRUE;
+
+	/*if (pregpriv->wifi_spec == 1)
+	{
+		if (pmlmeinfo->WMM_enable == 1)
+		{
+			//todo: compare the parameter set count & decide wheher to update or not
+			return _FAIL;
+		}
+		else
+		{
+			pmlmeinfo->WMM_enable = 1;
+			_rtw_rtw_memcpy(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element));
+			return _TRUE;
+		}
+	}
+	else
+	{
+		pmlmeinfo->WMM_enable = 0;
+		return _FAIL;
+	}*/
+	
+}
+
+void WMMOnAssocRsp(_adapter *padapter)
+{
+	u8	ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime;
+	u8	acm_mask;
+	u16	TXOP;
+	u32	acParm, i;
+	u32	edca[4], inx[4];
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+
+	if (pmlmeinfo->WMM_enable == 0)
+	{
+		padapter->mlmepriv.acm_mask = 0;
+		return;
+	}
+
+	acm_mask = 0;
+
+	if( pmlmeext->cur_wireless_mode == WIRELESS_11B)
+		aSifsTime = 10;
+	else
+		aSifsTime = 16;
+
+	for (i = 0; i < 4; i++)  
+	{
+		ACI = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 5) & 0x03;
+		ACM = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 4) & 0x01;
+
+		//AIFS = AIFSN * slot time + SIFS - r2t phy delay
+		AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) * pmlmeinfo->slotTime + aSifsTime;
+
+		ECWMin = (pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f);
+		ECWMax = (pmlmeinfo->WMM_param.ac_param[i].CW & 0xf0) >> 4;
+		TXOP = le16_to_cpu(pmlmeinfo->WMM_param.ac_param[i].TXOP_limit);
+
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+
+		switch (ACI)
+		{
+			case 0x0:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm));
+				acm_mask |= (ACM? BIT(1):0);
+				edca[XMIT_BE_QUEUE] = acParm;
+				break;
+
+			case 0x1:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm));
+				//acm_mask |= (ACM? BIT(0):0);
+				edca[XMIT_BK_QUEUE] = acParm;
+				break;
+
+			case 0x2:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm));
+				acm_mask |= (ACM? BIT(2):0);
+				edca[XMIT_VI_QUEUE] = acParm;
+				break;
+
+			case 0x3:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm));
+				acm_mask |= (ACM? BIT(3):0);
+				edca[XMIT_VO_QUEUE] = acParm;
+				break;							
+		}
+
+		DBG_871X("WMM(%x): %x, %x\n", ACI, ACM, acParm);
+	}
+
+	if(padapter->registrypriv.acm_method == 1)
+		rtw_hal_set_hwreg(padapter, HW_VAR_ACM_CTRL, (u8 *)(&acm_mask));
+	else
+		padapter->mlmepriv.acm_mask = acm_mask;
+
+	inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
+
+	if(pregpriv->wifi_spec==1)
+	{
+		u32	j, tmp, change_inx;
+
+		//entry indx: 0->vo, 1->vi, 2->be, 3->bk.
+		for(i=0; i<4; i++)
+		{
+			for(j=i+1; j<4; j++)
+			{
+				//compare CW and AIFS
+				if((edca[j] & 0xFFFF) < (edca[i] & 0xFFFF))
+				{
+					change_inx = _TRUE;
+				}
+				else if((edca[j] & 0xFFFF) == (edca[i] & 0xFFFF))
+				{
+					//compare TXOP
+					if((edca[j] >> 16) > (edca[i] >> 16))
+						change_inx = _TRUE;
+				}
+			
+				if(change_inx)
+				{
+					tmp = edca[i];
+					edca[i] = edca[j];
+					edca[j] = tmp;
+
+					tmp = inx[i];
+					inx[i] = inx[j];
+					inx[j] = tmp;
+
+					change_inx = _FALSE;
+				}
+			}
+		}
+	}
+
+	for(i=0; i<4; i++) {
+		pxmitpriv->wmm_para_seq[i] = inx[i];
+		DBG_871X("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]);
+	}
+
+	return;	
+}
+
+static void bwmode_update_check(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE)
+{
+#ifdef CONFIG_80211N_HT
+	unsigned char	 new_bwmode;
+	unsigned char  new_ch_offset;
+	struct HT_info_element	 *pHT_info;
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+
+	if(!pIE)
+		return;
+
+	if(phtpriv->ht_option == _FALSE)	return;
+
+	if(pIE->Length > sizeof(struct HT_info_element))
+		return;
+	
+	pHT_info = (struct HT_info_element *)pIE->data;
+
+	if((pHT_info->infos[0] & BIT(2)) && pregistrypriv->cbw40_enable )
+	{
+		new_bwmode = HT_CHANNEL_WIDTH_40;
+
+		switch (pHT_info->infos[0] & 0x3)
+		{
+			case 1:
+				new_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+				break;
+			
+			case 3:
+				new_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+				break;
+				
+			default:
+				new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+				break;
+		}
+	}
+	else
+	{
+		new_bwmode = HT_CHANNEL_WIDTH_20;
+		new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	}	
+
+	
+	if((new_bwmode!= pmlmeext->cur_bwmode) || (new_ch_offset!=pmlmeext->cur_ch_offset))
+	{
+		pmlmeinfo->bwmode_updated = _TRUE;
+		
+		pmlmeext->cur_bwmode = new_bwmode;
+		pmlmeext->cur_ch_offset = new_ch_offset;
+
+		//update HT info also
+		HT_info_handler(padapter, pIE);
+	}
+	else
+	{
+		pmlmeinfo->bwmode_updated = _FALSE;
+	}
+		
+
+	if(_TRUE == pmlmeinfo->bwmode_updated)
+	{
+		struct sta_info *psta;
+		WLAN_BSSID_EX 	*cur_network = &(pmlmeinfo->network);
+		struct sta_priv	*pstapriv = &padapter->stapriv;
+	
+		//set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+		
+		//update ap's stainfo
+		psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
+		if(psta)
+		{
+			struct ht_priv	*phtpriv_sta = &psta->htpriv;
+			
+			if(phtpriv_sta->ht_option)
+			{				
+				// bwmode				
+				phtpriv_sta->bwmode = pmlmeext->cur_bwmode;
+				phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;		
+			}
+			else
+			{
+				phtpriv_sta->bwmode = HT_CHANNEL_WIDTH_20;
+				phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			}
+			
+		}
+
+		//pmlmeinfo->bwmode_updated = _FALSE;//bwmode_updated done, reset it!
+		
+	}	
+#endif //CONFIG_80211N_HT
+}
+
+void HT_caps_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE)
+{
+#ifdef CONFIG_80211N_HT
+	unsigned int	i;
+	u8	rf_type;
+	u8	max_AMPDU_len, min_MPDU_spacing;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv 		*pmlmepriv = &padapter->mlmepriv;	
+	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+	struct registry_priv 	*pregistrypriv = &padapter->registrypriv;
+	
+	if(pIE==NULL) return;
+	
+	if(phtpriv->ht_option == _FALSE)	return;
+
+	pmlmeinfo->HT_caps_enable = 1;
+	
+	for (i = 0; i < (pIE->Length); i++)
+	{
+		if (i != 2)
+		{
+			//	Commented by Albert 2010/07/12
+			//	Got the endian issue here.
+			pmlmeinfo->HT_caps.u.HT_cap[i] &= (pIE->data[i]);
+		}
+		else
+		{
+			//modify from  fw by Thomas 2010/11/17
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (pIE->data[i] & 0x3))
+			{
+				max_AMPDU_len = (pIE->data[i] & 0x3);
+			}
+			else
+			{
+				max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3);
+			}
+			
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (pIE->data[i] & 0x1c))
+			{
+				min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c);
+			}
+			else
+			{
+				min_MPDU_spacing = (pIE->data[i] & 0x1c);
+			}
+
+			pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para = max_AMPDU_len | min_MPDU_spacing;
+		}
+	}
+
+	//	Commented by Albert 2010/07/12
+	//	Have to handle the endian issue after copying.
+	//	HT_ext_caps didn't be used yet.	
+	pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = le16_to_cpu( pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info );
+	pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps = le16_to_cpu( pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps );
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+	//update the MCS rates
+	for (i = 0; i < 16; i++)
+	{
+		if((rf_type == RF_1T1R) || (rf_type == RF_1T2R))
+		{
+			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i];
+		}
+		else
+		{
+			#ifdef CONFIG_DISABLE_MCS13TO15
+			if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40 && (pregistrypriv->wifi_spec!=1))
+				pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R_MCS13TO15_OFF[i];
+			else
+			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R[i];
+			#else
+			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R[i];
+			#endif //CONFIG_DISABLE_MCS13TO15
+		}
+	        #ifdef RTL8192C_RECONFIG_TO_1T1R
+		{
+			pmlmeinfo->HT_caps.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i];
+		}
+		#endif
+	}
+#endif //CONFIG_80211N_HT
+	return;
+}
+
+void HT_info_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE)
+{
+#ifdef CONFIG_80211N_HT
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv 		*pmlmepriv = &padapter->mlmepriv;	
+	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+
+	if(pIE==NULL) return;
+
+	if(phtpriv->ht_option == _FALSE)	return;
+
+
+	if(pIE->Length > sizeof(struct HT_info_element))
+		return;
+	
+	pmlmeinfo->HT_info_enable = 1;
+	_rtw_memcpy(&(pmlmeinfo->HT_info), pIE->data, pIE->Length);
+#endif //CONFIG_80211N_HT
+	return;
+}
+
+void HTOnAssocRsp(_adapter *padapter)
+{
+	unsigned char		max_AMPDU_len;
+	unsigned char		min_MPDU_spacing;
+	//struct registry_priv	 *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	
+	DBG_871X("%s\n", __FUNCTION__);
+
+	if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable))
+	{
+		pmlmeinfo->HT_enable = 1;
+	}
+	else
+	{
+		pmlmeinfo->HT_enable = 0;
+		//set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+		return;
+	}
+	
+	//handle A-MPDU parameter field
+	/* 	
+		AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+		AMPDU_para [4:2]:Min MPDU Start Spacing	
+	*/
+	max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;	
+	
+	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;	
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));
+
+#if 0 //move to rtw_update_ht_cap()
+	if ((pregpriv->cbw40_enable) &&
+		(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & BIT(1)) && 
+		(pmlmeinfo->HT_info.infos[0] & BIT(2)))
+	{
+		//switch to the 40M Hz mode accoring to the AP
+		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
+		switch ((pmlmeinfo->HT_info.infos[0] & 0x3))
+		{
+			case HT_EXTCHNL_OFFSET_UPPER:
+				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+				break;
+			
+			case HT_EXTCHNL_OFFSET_LOWER:
+				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+				break;
+				
+			default:
+				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+				break;
+		}
+		
+		//SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset);
+	}
+#endif
+
+	//set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+#if 0 //move to rtw_update_ht_cap()
+	//
+	// Config SM Power Save setting
+	//
+	pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2;
+	if(pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC)
+	{
+		/*u8 i;
+		//update the MCS rates
+		for (i = 0; i < 16; i++)
+		{
+			pmlmeinfo->HT_caps.HT_cap_element.MCS_rate[i] &= MCS_rate_1R[i];
+		}*/
+		DBG_871X("%s(): WLAN_HT_CAP_SM_PS_STATIC\n",__FUNCTION__);
+	}
+
+	//
+	// Config current HT Protection mode.
+	//
+	pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3;
+#endif
+	
+}
+
+void ERP_IE_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if(pIE->Length>1)
+		return;
+	
+	pmlmeinfo->ERP_enable = 1;
+	_rtw_memcpy(&(pmlmeinfo->ERP_IE), pIE->data, pIE->Length);
+}
+
+void VCS_update(_adapter *padapter, struct sta_info *psta)
+{
+	struct registry_priv	 *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	switch (pregpriv->vrtl_carrier_sense)/* 0:off 1:on 2:auto */
+	{
+		case 0: //off
+			psta->rtsen = 0;
+			psta->cts2self = 0;
+			break;
+			
+		case 1: //on
+			if (pregpriv->vcs_type == 1) /* 1:RTS/CTS 2:CTS to self */
+			{
+				psta->rtsen = 1;
+				psta->cts2self = 0;
+			}
+			else
+			{
+				psta->rtsen = 0;
+				psta->cts2self = 1;
+			}
+			break;
+			
+		case 2: //auto
+		default:
+			if ((pmlmeinfo->ERP_enable) && (pmlmeinfo->ERP_IE & BIT(1)))
+			{
+				if (pregpriv->vcs_type == 1)
+				{
+					psta->rtsen = 1;
+					psta->cts2self = 0;
+				}
+				else
+				{
+					psta->rtsen = 0;
+					psta->cts2self = 1;
+				}
+			}
+			else
+			{
+				psta->rtsen = 0;
+				psta->cts2self = 0;
+			}	
+			break;
+	}
+}
+
+#ifdef CONFIG_TDLS
+int check_ap_tdls_prohibited(u8 *pframe, u8 pkt_len)
+{
+	u8 tdls_prohibited_bit = 0x40; //bit(38); TDLS_prohibited
+
+	if(pkt_len < 5)
+	{
+		return _FALSE;
+	}
+
+	pframe += 4;
+	if( (*pframe) & tdls_prohibited_bit )
+		return _TRUE;
+
+	return _FALSE;
+}
+#endif //CONFIG_TDLS
+
+int rtw_check_bcn_info(ADAPTER *Adapter, u8 *pframe, u32 packet_len)
+{
+	unsigned int		len;
+	unsigned char		*p;
+	unsigned short	val16, subtype;
+	struct wlan_network *cur_network = &(Adapter->mlmepriv.cur_network);
+	//u8 wpa_ie[255],rsn_ie[255];
+	u16 wpa_len=0,rsn_len=0;
+	u8 encryp_protocol = 0;
+	WLAN_BSSID_EX *bssid;
+	int group_cipher = 0, pairwise_cipher = 0, is_8021x = 0;
+	unsigned char *pbuf;
+	u32 wpa_ielen = 0;
+	u8 *pbssid = GetAddr3Ptr(pframe);
+	u32 hidden_ssid = 0;
+	u8 cur_network_type, network_type=0;
+	struct HT_info_element *pht_info = NULL;
+	struct rtw_ieee80211_ht_cap *pht_cap = NULL;
+	u32 bcn_channel;
+	unsigned short 	ht_cap_info;
+	unsigned char	ht_info_infos_0;
+
+	if (is_client_associated_to_ap(Adapter) == _FALSE)
+		return _TRUE;
+
+	len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ) {
+		DBG_871X("%s IE too long for survey event\n", __func__);
+		return _FAIL;
+	}
+
+	if (_rtw_memcmp(cur_network->network.MacAddress, pbssid, 6) == _FALSE) {
+		DBG_871X("Oops: rtw_check_network_encrypt linked but recv other bssid bcn\n" MAC_FMT MAC_FMT,
+				MAC_ARG(pbssid), MAC_ARG(cur_network->network.MacAddress));
+		return _TRUE;
+	}
+
+	bssid = (WLAN_BSSID_EX *)rtw_zmalloc(sizeof(WLAN_BSSID_EX));
+
+	subtype = GetFrameSubType(pframe) >> 4;
+
+	if(subtype==WIFI_BEACON)
+		bssid->Reserved[0] = 1;
+
+	bssid->Length = sizeof(WLAN_BSSID_EX) - MAX_IE_SZ + len;
+
+	/* below is to copy the information element */
+	bssid->IELength = len;
+	_rtw_memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength);
+
+	/* check bw and channel offset */
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+	if(p && len>0) {
+			pht_cap = (struct rtw_ieee80211_ht_cap *)(p + 2);
+			ht_cap_info = pht_cap->cap_info;
+	} else {
+			ht_cap_info = 0;
+	}
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+	if(p && len>0) {
+			pht_info = (struct HT_info_element *)(p + 2);
+			ht_info_infos_0 = pht_info->infos[0];
+	} else {
+			ht_info_infos_0 = 0;
+	}
+	if (ht_cap_info != cur_network->BcnInfo.ht_cap_info ||
+		((ht_info_infos_0&0x03) != (cur_network->BcnInfo.ht_info_infos_0&0x03))) {
+			DBG_871X("%s bcn now: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
+						   	ht_cap_info, ht_info_infos_0);
+			DBG_871X("%s bcn link: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
+						   	cur_network->BcnInfo.ht_cap_info, cur_network->BcnInfo.ht_info_infos_0);
+			DBG_871X("%s bw mode change, disconnect\n", __func__);
+			{	
+				//bcn_info_update
+				cur_network->BcnInfo.ht_cap_info = ht_cap_info;
+				cur_network->BcnInfo.ht_info_infos_0 = ht_info_infos_0;
+				//to do : need to check that whether modify related register of BB or not 
+			}
+			//goto _mismatch;
+	}
+
+	/* Checking for channel */
+	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _DSSET_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+	if (p) {
+			bcn_channel = *(p + 2);
+	} else {/* In 5G, some ap do not have DSSET IE checking HT info for channel */
+			p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+			if(pht_info) {
+					bcn_channel = pht_info->primary_channel;
+			} else { /* we don't find channel IE, so don't check it */
+					DBG_871X("Oops: %s we don't find channel IE, so don't check it \n", __func__);
+					bcn_channel = Adapter->mlmeextpriv.cur_channel;
+			}
+	}
+	if (bcn_channel != Adapter->mlmeextpriv.cur_channel) {
+			DBG_871X("%s beacon channel:%d cur channel:%d disconnect\n", __func__,
+						   bcn_channel, Adapter->mlmeextpriv.cur_channel);
+			goto _mismatch;
+	}
+
+	/* checking SSID */
+	if ((p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _SSID_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_)) == NULL) {
+		DBG_871X("%s marc: cannot find SSID for survey event\n", __func__);
+		hidden_ssid = _TRUE;
+	} else {
+		hidden_ssid = _FALSE;
+	}
+
+	if((NULL != p) && (_FALSE == hidden_ssid && (*(p + 1)))) {
+		_rtw_memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
+		bssid->Ssid.SsidLength = *(p + 1);
+	} else {
+		bssid->Ssid.SsidLength = 0;
+		bssid->Ssid.Ssid[0] = '\0';
+	}
+
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("%s bssid.Ssid.Ssid:%s bssid.Ssid.SsidLength:%d "
+				"cur_network->network.Ssid.Ssid:%s len:%d\n", __func__, bssid->Ssid.Ssid,
+				bssid->Ssid.SsidLength, cur_network->network.Ssid.Ssid,
+				cur_network->network.Ssid.SsidLength));
+
+	if (_rtw_memcmp(bssid->Ssid.Ssid, cur_network->network.Ssid.Ssid, 32) == _FALSE ||
+			bssid->Ssid.SsidLength != cur_network->network.Ssid.SsidLength) {
+		if (bssid->Ssid.Ssid[0] != '\0' && bssid->Ssid.SsidLength != 0) { /* not hidden ssid */
+			DBG_871X("%s(), SSID is not match return FAIL\n", __func__);
+			goto _mismatch;
+		}
+	}
+
+	/* check encryption info */
+	val16 = rtw_get_capability((WLAN_BSSID_EX *)bssid);
+
+	if (val16 & BIT(4))
+		bssid->Privacy = 1;
+	else
+		bssid->Privacy = 0;
+
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+			("%s(): cur_network->network.Privacy is %d, bssid.Privacy is %d\n",
+			 __func__, cur_network->network.Privacy,bssid->Privacy));
+	if (cur_network->network.Privacy != bssid->Privacy) {
+		DBG_871X("%s(), privacy is not match return FAIL\n",__func__);
+		goto _mismatch;
+	}
+
+	rtw_get_sec_ie(bssid->IEs, bssid->IELength, NULL,&rsn_len,NULL,&wpa_len);
+
+	if (rsn_len > 0) {
+		encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+	} else if (wpa_len > 0) {
+		encryp_protocol = ENCRYP_PROTOCOL_WPA;
+	} else {
+		if (bssid->Privacy)
+			encryp_protocol = ENCRYP_PROTOCOL_WEP;
+	}
+
+	if (cur_network->BcnInfo.encryp_protocol != encryp_protocol) {
+		DBG_871X("%s(): enctyp is not match ,return FAIL\n",__func__);
+		goto _mismatch;
+	}
+
+	if (encryp_protocol == ENCRYP_PROTOCOL_WPA || encryp_protocol == ENCRYP_PROTOCOL_WPA2) {
+		pbuf = rtw_get_wpa_ie(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12);
+		if(pbuf && (wpa_ielen>0)) {
+			if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+						("%s pnetwork->pairwise_cipher: %d, group_cipher is %d, is_8021x is %d\n", __func__,
+						 pairwise_cipher, group_cipher, is_8021x));
+			}
+		} else {
+			pbuf = rtw_get_wpa2_ie(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12);
+
+			if(pbuf && (wpa_ielen>0)) {
+				if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
+					RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+							("%s pnetwork->pairwise_cipher: %d, pnetwork->group_cipher is %d, is_802x is %d\n",
+							 __func__, pairwise_cipher, group_cipher, is_8021x));
+				}
+			}
+		}
+
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+				("%s cur_network->group_cipher is %d: %d\n",__func__, cur_network->BcnInfo.group_cipher, group_cipher));
+		if (pairwise_cipher != cur_network->BcnInfo.pairwise_cipher || group_cipher != cur_network->BcnInfo.group_cipher) {
+			DBG_871X("%s pairwise_cipher(%x:%x) or group_cipher(%x:%x) is not match ,return FAIL\n",__func__,
+					pairwise_cipher, cur_network->BcnInfo.pairwise_cipher,
+					group_cipher, cur_network->BcnInfo.group_cipher);
+			goto _mismatch;
+		}
+
+		if (is_8021x != cur_network->BcnInfo.is_8021x) {
+			DBG_871X("%s authentication is not match ,return FAIL\n", __func__);
+			goto _mismatch;
+		}
+	}
+
+	rtw_mfree((u8 *)bssid, sizeof(WLAN_BSSID_EX));
+	return _SUCCESS;
+
+_mismatch:
+	rtw_mfree((u8 *)bssid, sizeof(WLAN_BSSID_EX));
+	return _FAIL;
+
+	_func_exit_;
+}
+
+void update_beacon_info(_adapter *padapter, u8 *pframe, uint pkt_len, struct sta_info *psta)
+{
+	unsigned int i;
+	unsigned int len;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+		
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	u8 tdls_prohibited[] = { 0x00, 0x00, 0x00, 0x00, 0x10 }; //bit(38): TDLS_prohibited
+#endif //CONFIG_TDLS
+		
+	len = pkt_len - (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN);
+
+	for (i = 0; i < len;)
+	{
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN) + i);
+		
+		switch (pIE->ElementID)
+		{
+#if 0			
+			case _VENDOR_SPECIFIC_IE_:		
+				//todo: to update WMM paramter set while receiving beacon			
+				if (_rtw_memcmp(pIE->data, WMM_PARA_OUI, 6))	//WMM
+				{
+					(WMM_param_handler(padapter, pIE))? WMMOnAssocRsp(padapter): 0;
+				}				
+				break;
+#endif
+
+			case _HT_EXTRA_INFO_IE_:	//HT info				
+				//HT_info_handler(padapter, pIE);
+				bwmode_update_check(padapter, pIE);
+				break;
+
+			case _ERPINFO_IE_:
+				ERP_IE_handler(padapter, pIE);
+				VCS_update(padapter, psta);
+				break;
+
+#ifdef CONFIG_TDLS
+			case _EXT_CAP_IE_:
+				if( check_ap_tdls_prohibited(pIE->data, pIE->Length) == _TRUE )
+					ptdlsinfo->ap_prohibited = _TRUE;
+				break;
+#endif //CONFIG_TDLS
+			default:
+				break;
+		}
+		
+		i += (pIE->Length + 2);
+	}
+}
+
+#ifdef CONFIG_DFS
+void process_csa_ie(_adapter *padapter, u8 *pframe, uint pkt_len)
+{
+	unsigned int i;
+	unsigned int len;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8 new_ch_no = 0; 
+		
+	len = pkt_len - (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN);
+
+	for (i = 0; i < len;)
+	{
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN) + i);
+		
+		switch (pIE->ElementID)
+		{
+			case _CH_SWTICH_ANNOUNCE_:
+				_rtw_memcpy(&new_ch_no, pIE->data+1, 1);
+				rtw_set_csa_cmd(padapter, new_ch_no);
+				break;
+
+			default:
+				break;
+		}
+		
+		i += (pIE->Length + 2);
+	}
+}
+#endif //CONFIG_DFS
+
+unsigned int is_ap_in_tkip(_adapter *padapter)
+{
+	u32 i;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX 		*cur_network = &(pmlmeinfo->network);
+
+	if (rtw_get_capability((WLAN_BSSID_EX *)cur_network) & WLAN_CAPABILITY_PRIVACY)
+	{
+		for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;)
+		{
+			pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i);
+		
+			switch (pIE->ElementID)
+			{
+				case _VENDOR_SPECIFIC_IE_:
+					if ((_rtw_memcmp(pIE->data, RTW_WPA_OUI, 4)) && (_rtw_memcmp((pIE->data + 12), WPA_TKIP_CIPHER, 4))) 
+					{
+						return _TRUE;
+					}
+					break;
+				
+				case _RSN_IE_2_:
+					if (_rtw_memcmp((pIE->data + 8), RSN_TKIP_CIPHER, 4)) 
+					{
+						return _TRUE;
+					}
+					
+				default:
+					break;
+			}
+		
+			i += (pIE->Length + 2);
+		}
+		
+		return _FALSE;
+	}
+	else
+	{
+		return _FALSE;
+	}
+	
+}
+
+unsigned int should_forbid_n_rate(_adapter * padapter)
+{
+	u32 i;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	WLAN_BSSID_EX  *cur_network = &pmlmepriv->cur_network.network;
+
+	if (rtw_get_capability((WLAN_BSSID_EX *)cur_network) & WLAN_CAPABILITY_PRIVACY)
+	{
+		for (i = sizeof(NDIS_802_11_FIXED_IEs); i < cur_network->IELength;)
+		{
+			pIE = (PNDIS_802_11_VARIABLE_IEs)(cur_network->IEs + i);
+
+			switch (pIE->ElementID)
+			{
+				case _VENDOR_SPECIFIC_IE_:
+					if (_rtw_memcmp(pIE->data, RTW_WPA_OUI, 4) &&
+						((_rtw_memcmp((pIE->data + 12), WPA_CIPHER_SUITE_CCMP, 4)) ||
+						  (_rtw_memcmp((pIE->data + 16), WPA_CIPHER_SUITE_CCMP, 4))))
+						return _FALSE;
+					break;
+
+				case _RSN_IE_2_:
+					if  ((_rtw_memcmp((pIE->data + 8), RSN_CIPHER_SUITE_CCMP, 4))  ||
+					       (_rtw_memcmp((pIE->data + 12), RSN_CIPHER_SUITE_CCMP, 4)))
+					return _FALSE;
+
+				default:
+					break;
+			}
+
+			i += (pIE->Length + 2);
+		}
+
+		return _TRUE;
+	}
+	else
+	{
+		return _FALSE;
+	}
+
+}
+
+
+unsigned int is_ap_in_wep(_adapter *padapter)
+{
+	u32 i;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX 		*cur_network = &(pmlmeinfo->network);
+
+	if (rtw_get_capability((WLAN_BSSID_EX *)cur_network) & WLAN_CAPABILITY_PRIVACY)
+	{
+		for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;)
+		{
+			pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i);
+
+			switch (pIE->ElementID)
+			{
+				case _VENDOR_SPECIFIC_IE_:
+					if (_rtw_memcmp(pIE->data, RTW_WPA_OUI, 4))
+						return _FALSE;
+					break;
+
+				case _RSN_IE_2_:
+					return _FALSE;
+
+				default:
+					break;
+			}
+
+			i += (pIE->Length + 2);
+		}
+
+		return _TRUE;
+	}
+	else
+	{
+		return _FALSE;
+	}
+
+}
+
+int wifirate2_ratetbl_inx(unsigned char rate);
+int wifirate2_ratetbl_inx(unsigned char rate)
+{
+	int	inx = 0;
+	rate = rate & 0x7f;
+
+	switch (rate) 
+	{
+		case 54*2:
+			inx = 11;
+			break;
+
+		case 48*2:
+			inx = 10;
+			break;
+
+		case 36*2:
+			inx = 9;
+			break;
+
+		case 24*2:
+			inx = 8;
+			break;
+			
+		case 18*2:
+			inx = 7;
+			break;
+
+		case 12*2:
+			inx = 6;
+			break;
+
+		case 9*2:
+			inx = 5;
+			break;
+			
+		case 6*2:
+			inx = 4;
+			break;
+
+		case 11*2:
+			inx = 3;
+			break;
+		case 11:
+			inx = 2;
+			break;
+
+		case 2*2:
+			inx = 1;
+			break;
+		
+		case 1*2:
+			inx = 0;
+			break;
+
+	}
+	return inx;	
+}
+
+unsigned int update_basic_rate(unsigned char *ptn, unsigned int ptn_sz)
+{
+	unsigned int i, num_of_rate;
+	unsigned int mask = 0;
+	
+	num_of_rate = (ptn_sz > NumRates)? NumRates: ptn_sz;
+		
+	for (i = 0; i < num_of_rate; i++)
+	{
+		if ((*(ptn + i)) & 0x80)
+		{
+			mask |= 0x1 << wifirate2_ratetbl_inx(*(ptn + i));
+		}
+	}
+	return mask;
+}
+
+unsigned int update_supported_rate(unsigned char *ptn, unsigned int ptn_sz)
+{
+	unsigned int i, num_of_rate;
+	unsigned int mask = 0;
+	
+	num_of_rate = (ptn_sz > NumRates)? NumRates: ptn_sz;
+		
+	for (i = 0; i < num_of_rate; i++)
+	{
+		mask |= 0x1 << wifirate2_ratetbl_inx(*(ptn + i));
+	}
+
+	return mask;
+}
+
+unsigned int update_MSC_rate(struct HT_caps_element *pHT_caps)
+{
+	unsigned int mask = 0;
+	
+	mask = ((pHT_caps->u.HT_cap_element.MCS_rate[0] << 12) | (pHT_caps->u.HT_cap_element.MCS_rate[1] << 20));
+						
+	return mask;
+}
+
+int support_short_GI(_adapter *padapter, struct HT_caps_element *pHT_caps)
+{
+	unsigned char					bit_offset;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	
+	if (!(pmlmeinfo->HT_enable))
+		return _FAIL;
+	
+	if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_RALINK))
+		return _FAIL;
+		
+	bit_offset = (pmlmeext->cur_bwmode & HT_CHANNEL_WIDTH_40)? 6: 5;
+	
+	if (pHT_caps->u.HT_cap_element.HT_caps_info & (0x1 << bit_offset))
+	{
+		return _SUCCESS;
+	}
+	else
+	{
+		return _FAIL;
+	}		
+}
+
+unsigned char get_highest_rate_idx(u32 mask)
+{
+	int i;
+	unsigned char rate_idx=0;
+
+	for(i=27; i>=0; i--)
+	{
+		if(mask & BIT(i))
+		{
+			rate_idx = i;
+			break;
+		}
+	}
+
+	return rate_idx;
+}
+
+unsigned char get_highest_mcs_rate(struct HT_caps_element *pHT_caps);
+unsigned char get_highest_mcs_rate(struct HT_caps_element *pHT_caps)
+{
+	int i, mcs_rate;
+	
+	mcs_rate = (pHT_caps->u.HT_cap_element.MCS_rate[0] | (pHT_caps->u.HT_cap_element.MCS_rate[1] << 8));
+	
+	for (i = 15; i >= 0; i--)
+	{
+		if (mcs_rate & (0x1 << i))
+		{
+			break;
+		}
+	}
+	
+	return i;
+}
+
+void Update_RA_Entry(_adapter *padapter, struct sta_info *psta)
+{
+	rtw_hal_update_ra_mask(psta, 0);
+}
+
+void enable_rate_adaptive(_adapter *padapter, struct sta_info *psta);
+void enable_rate_adaptive(_adapter *padapter, struct sta_info *psta)
+{
+	Update_RA_Entry(padapter, psta);
+}
+
+void set_sta_rate(_adapter *padapter, struct sta_info *psta)
+{
+	//rate adaptive	
+	enable_rate_adaptive(padapter, psta);
+}
+
+// Update RRSR and Rate for USERATE
+void update_tx_basic_rate(_adapter *padapter, u8 wirelessmode)
+{
+	NDIS_802_11_RATES_EX	supported_rates;
+#ifdef CONFIG_P2P
+	struct wifidirect_info*	pwdinfo = &padapter->wdinfo;
+
+	//	Added by Albert 2011/03/22
+	//	In the P2P mode, the driver should not support the b mode.
+	//	So, the Tx packet shouldn't use the CCK rate
+	if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+#endif //CONFIG_P2P
+#ifdef CONFIG_INTEL_WIDI
+	if (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_NONE)
+		return;
+#endif //CONFIG_INTEL_WIDI
+
+	_rtw_memset(supported_rates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+	if ((wirelessmode & WIRELESS_11B) && (wirelessmode == WIRELESS_11B)) {
+		_rtw_memcpy(supported_rates, rtw_basic_rate_cck, 4);
+	} else if (wirelessmode & WIRELESS_11B) {
+		_rtw_memcpy(supported_rates, rtw_basic_rate_mix, 7);
+	} else {
+		_rtw_memcpy(supported_rates, rtw_basic_rate_ofdm, 3);
+	}
+
+	if (wirelessmode & WIRELESS_11B)
+		update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB);
+	else
+		update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, supported_rates);
+}
+
+unsigned char check_assoc_AP(u8 *pframe, uint len)
+{
+	unsigned int	i;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8	epigram_vendor_flag;
+	u8	ralink_vendor_flag;
+	epigram_vendor_flag = 0;
+	ralink_vendor_flag = 0;
+
+	for (i = sizeof(NDIS_802_11_FIXED_IEs); i < len;)
+	{
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + i);
+		
+		switch (pIE->ElementID)
+		{
+			case _VENDOR_SPECIFIC_IE_:
+				if ((_rtw_memcmp(pIE->data, ARTHEROS_OUI1, 3)) || (_rtw_memcmp(pIE->data, ARTHEROS_OUI2, 3)))
+				{
+					DBG_871X("link to Artheros AP\n");
+					return HT_IOT_PEER_ATHEROS;
+				}
+				else if ((_rtw_memcmp(pIE->data, BROADCOM_OUI1, 3))
+							|| (_rtw_memcmp(pIE->data, BROADCOM_OUI2, 3))
+							|| (_rtw_memcmp(pIE->data, BROADCOM_OUI2, 3)))
+				{
+					DBG_871X("link to Broadcom AP\n");
+					return HT_IOT_PEER_BROADCOM;
+				}
+				else if (_rtw_memcmp(pIE->data, MARVELL_OUI, 3))
+				{
+					DBG_871X("link to Marvell AP\n");
+					return HT_IOT_PEER_MARVELL;
+				}
+				else if (_rtw_memcmp(pIE->data, RALINK_OUI, 3))
+				{
+					if (!ralink_vendor_flag) {
+						ralink_vendor_flag = 1;
+					} else {
+						DBG_871X("link to Ralink AP\n");
+						return HT_IOT_PEER_RALINK;
+					}
+				}
+				else if (_rtw_memcmp(pIE->data, CISCO_OUI, 3))
+				{
+					DBG_871X("link to Cisco AP\n");
+					return HT_IOT_PEER_CISCO;
+				}
+				else if (_rtw_memcmp(pIE->data, REALTEK_OUI, 3))
+				{
+					DBG_871X("link to Realtek 96B\n");
+					return HT_IOT_PEER_REALTEK;
+				}
+				else if (_rtw_memcmp(pIE->data, AIRGOCAP_OUI,3))
+				{
+					DBG_871X("link to Airgo Cap\n");
+					return HT_IOT_PEER_AIRGO;
+				}
+				else if (_rtw_memcmp(pIE->data, EPIGRAM_OUI, 3))
+				{
+					 epigram_vendor_flag = 1;
+					if(ralink_vendor_flag) {
+						DBG_871X("link to Tenda W311R AP\n");
+						 return HT_IOT_PEER_TENDA;
+					} else {
+						DBG_871X("Capture EPIGRAM_OUI\n");
+					}
+				}
+				else
+				{
+					break;
+				}
+						
+			default:
+				break;
+		}
+				
+		i += (pIE->Length + 2);
+	}
+	
+	if (ralink_vendor_flag && !epigram_vendor_flag) {
+		DBG_871X("link to Ralink AP\n");
+		return HT_IOT_PEER_RALINK;
+	} else if (ralink_vendor_flag && epigram_vendor_flag){
+		DBG_871X("link to Tenda W311R AP\n");
+		return HT_IOT_PEER_TENDA;
+	} else {
+		DBG_871X("link to new AP\n");
+		return HT_IOT_PEER_UNKNOWN;
+	}
+}
+
+void update_IOT_info(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	
+	switch (pmlmeinfo->assoc_AP_vendor)
+	{
+		case HT_IOT_PEER_MARVELL:
+			pmlmeinfo->turboMode_cts2self = 1;
+			pmlmeinfo->turboMode_rtsen = 0;
+			break;
+		
+		case HT_IOT_PEER_RALINK:
+			pmlmeinfo->turboMode_cts2self = 0;
+			pmlmeinfo->turboMode_rtsen = 1;
+			//disable high power			
+			Switch_DM_Func(padapter, (~DYNAMIC_BB_DYNAMIC_TXPWR), _FALSE);
+			break;
+		case HT_IOT_PEER_REALTEK:
+			//rtw_write16(padapter, 0x4cc, 0xffff);
+			//rtw_write16(padapter, 0x546, 0x01c0);
+			//disable high power			
+			Switch_DM_Func(padapter, (~DYNAMIC_BB_DYNAMIC_TXPWR), _FALSE);
+			break;
+		default:
+			pmlmeinfo->turboMode_cts2self = 0;
+			pmlmeinfo->turboMode_rtsen = 1;
+			break;	
+	}
+	
+}
+
+void update_capinfo(PADAPTER Adapter, u16 updateCap)
+{
+	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	BOOLEAN		ShortPreamble;
+
+	// Check preamble mode, 2005.01.06, by rcnjko.
+	// Mark to update preamble value forever, 2008.03.18 by lanhsin
+	//if( pMgntInfo->RegPreambleMode == PREAMBLE_AUTO )
+	{
+			
+		if(updateCap & cShortPreamble)
+		{ // Short Preamble
+			if(pmlmeinfo->preamble_mode != PREAMBLE_SHORT) // PREAMBLE_LONG or PREAMBLE_AUTO
+			{
+				ShortPreamble = _TRUE;
+				pmlmeinfo->preamble_mode = PREAMBLE_SHORT;
+				rtw_hal_set_hwreg( Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble );
+			}
+		}
+		else
+		{ // Long Preamble
+			if(pmlmeinfo->preamble_mode != PREAMBLE_LONG)  // PREAMBLE_SHORT or PREAMBLE_AUTO
+			{
+				ShortPreamble = _FALSE;
+				pmlmeinfo->preamble_mode = PREAMBLE_LONG;
+				rtw_hal_set_hwreg( Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble );
+			}
+		}
+	}
+
+	if ( updateCap & cIBSS ) {
+		//Filen: See 802.11-2007 p.91
+		pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+	}
+	else
+	{
+		//Filen: See 802.11-2007 p.90
+		if( pmlmeext->cur_wireless_mode & (WIRELESS_11G | WIRELESS_11_24N))
+		{
+			if( (updateCap & cShortSlotTime) /* && (!(pMgntInfo->pHTInfo->RT2RT_HT_Mode & RT_HT_CAP_USE_LONG_PREAMBLE)) */)
+			{ // Short Slot Time
+				if(pmlmeinfo->slotTime != SHORT_SLOT_TIME)
+				{
+					pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+				}
+			}
+			else
+			{ // Long Slot Time
+				if(pmlmeinfo->slotTime != NON_SHORT_SLOT_TIME)
+				{
+					pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+				}
+			}
+		}
+		else if( pmlmeext->cur_wireless_mode & (WIRELESS_11A | WIRELESS_11_5N))
+		{
+			pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+		}
+		else
+		{
+			//B Mode
+			pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+		}
+ 	}
+ 
+	rtw_hal_set_hwreg( Adapter, HW_VAR_SLOT_TIME, &pmlmeinfo->slotTime );
+
+}
+
+void update_wireless_mode(_adapter *padapter)
+{
+	int ratelen, network_type = 0;
+	u32 SIFS_Timer;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX 		*cur_network = &(pmlmeinfo->network);
+	unsigned char			*rate = cur_network->SupportedRates;
+
+	ratelen = rtw_get_rateset_len(cur_network->SupportedRates);
+
+	if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable))
+	{
+		pmlmeinfo->HT_enable = 1;
+	}
+
+	if(pmlmeext->cur_channel > 14)
+	{
+		if (pmlmeinfo->HT_enable)
+		{
+			network_type = WIRELESS_11_5N;
+		}
+
+		network_type |= WIRELESS_11A;
+	}
+	else
+	{
+		if (pmlmeinfo->HT_enable)
+		{
+			network_type = WIRELESS_11_24N;
+		}
+	
+		if ((cckratesonly_included(rate, ratelen)) == _TRUE)
+		{
+			network_type |= WIRELESS_11B;
+		}
+		else if((cckrates_included(rate, ratelen)) == _TRUE)
+		{
+			network_type |= WIRELESS_11BG;
+		}
+		else
+		{
+			network_type |= WIRELESS_11G;
+		}
+	}
+
+	pmlmeext->cur_wireless_mode = network_type & padapter->registrypriv.wireless_mode;
+/*
+	if((pmlmeext->cur_wireless_mode==WIRELESS_11G) ||
+		(pmlmeext->cur_wireless_mode==WIRELESS_11BG))//WIRELESS_MODE_G)
+		SIFS_Timer = 0x0a0a;//CCK
+	else
+		SIFS_Timer = 0x0e0e;//pHalData->SifsTime; //OFDM
+*/
+	
+	SIFS_Timer = 0x0a0a0808; //0x0808 -> for CCK, 0x0a0a -> for OFDM
+                             //change this value if having IOT issues.
+		
+	padapter->HalFunc.SetHwRegHandler( padapter, HW_VAR_RESP_SIFS,  (u8 *)&SIFS_Timer);
+
+	if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
+		update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB);
+	 else
+		update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB);
+}
+
+void fire_write_MAC_cmd(_adapter *padapter, unsigned int addr, unsigned int value);
+void fire_write_MAC_cmd(_adapter *padapter, unsigned int addr, unsigned int value)
+{
+#if 0
+	struct cmd_obj					*ph2c;
+	struct reg_rw_parm			*pwriteMacPara;
+	struct cmd_priv					*pcmdpriv = &(padapter->cmdpriv);
+
+	if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
+	{
+		return;
+	}	
+
+	if ((pwriteMacPara = (struct reg_rw_parm*)rtw_malloc(sizeof(struct reg_rw_parm))) == NULL) 
+	{		
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		return;
+	}
+	
+	pwriteMacPara->rw = 1;
+	pwriteMacPara->addr = addr;
+	pwriteMacPara->value = value;
+	
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pwriteMacPara, GEN_CMD_CODE(_Write_MACREG));
+	rtw_enqueue_cmd(pcmdpriv, ph2c);
+#endif	
+}
+
+void update_bmc_sta_support_rate(_adapter *padapter, u32 mac_id)
+{
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if(pmlmeext->cur_wireless_mode & WIRELESS_11B)
+	{
+		// Only B, B/G, and B/G/N AP could use CCK rate
+		_rtw_memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates), rtw_basic_rate_cck, 4);
+	}
+	else
+	{
+		_rtw_memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates), rtw_basic_rate_ofdm, 4);
+	}
+}
+
+int update_sta_support_rate(_adapter *padapter, u8* pvar_ie, uint var_ie_len, int cam_idx)
+{
+	unsigned int	ie_len;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	int	supportRateNum = 0;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	
+	pIE = (PNDIS_802_11_VARIABLE_IEs)rtw_get_ie(pvar_ie, _SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
+	if (pIE == NULL)
+	{
+		return _FAIL;
+	}
+	
+	_rtw_memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, pIE->data, ie_len);
+	supportRateNum = ie_len;
+				
+	pIE = (PNDIS_802_11_VARIABLE_IEs)rtw_get_ie(pvar_ie, _EXT_SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
+	if (pIE)
+	{
+		_rtw_memcpy((pmlmeinfo->FW_sta_info[cam_idx].SupportedRates + supportRateNum), pIE->data, ie_len);
+	}
+
+	return _SUCCESS;
+	
+}
+
+void process_addba_req(_adapter *padapter, u8 *paddba_req, u8 *addr)
+{
+	struct sta_info *psta;
+	u16 tid, start_seq, param;	
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct sta_priv *pstapriv = &padapter->stapriv;	
+	struct ADDBA_request	*preq = (struct ADDBA_request*)paddba_req;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	psta = rtw_get_stainfo(pstapriv, addr);
+
+	if(psta)
+	{
+		start_seq = le16_to_cpu(preq->BA_starting_seqctrl) >> 4;
+			
+		param = le16_to_cpu(preq->BA_para_set);
+		tid = (param>>2)&0x0f;
+		
+		preorder_ctrl = &psta->recvreorder_ctrl[tid];
+
+		#ifdef CONFIG_UPDATE_INDICATE_SEQ_WHILE_PROCESS_ADDBA_REQ
+		preorder_ctrl->indicate_seq = start_seq;
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, start_seq: %d\n", __FUNCTION__, __LINE__,
+			preorder_ctrl->indicate_seq, start_seq);
+		#endif
+		#else
+		preorder_ctrl->indicate_seq = 0xffff;
+		#endif
+		
+		preorder_ctrl->enable =(pmlmeinfo->bAcceptAddbaReq == _TRUE)? _TRUE :_FALSE;
+	}
+
+}
+
+void update_TSF(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len)
+{	
+	u8* pIE;
+	u32 *pbuf;
+		
+	pIE = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	pbuf = (u32*)pIE;
+
+	pmlmeext->TSFValue = le32_to_cpu(*(pbuf+1));
+	
+	pmlmeext->TSFValue = pmlmeext->TSFValue << 32;
+
+	pmlmeext->TSFValue |= le32_to_cpu(*pbuf);
+}
+
+void correct_TSF(_adapter *padapter, struct mlme_ext_priv *pmlmeext)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_CORRECT_TSF, 0);
+}
+
+void beacon_timing_control(_adapter *padapter)
+{
+	rtw_hal_bcn_related_reg_setting(padapter);
+}
+
+#if 0
+unsigned int setup_beacon_frame(_adapter *padapter, unsigned char *beacon_frame)
+{
+	unsigned short				ATIMWindow;
+	unsigned char					*pframe;
+	struct tx_desc 				*ptxdesc;
+	struct rtw_ieee80211_hdr 	*pwlanhdr;
+	unsigned short				*fctrl;
+	unsigned int					rate_len, len = 0;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX 		*cur_network = &(pmlmeinfo->network);
+	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	
+	_rtw_memset(beacon_frame, 0, 256);
+	
+	pframe = beacon_frame + TXDESC_SIZE;
+	
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;	
+	
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+	
+	_rtw_memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	_rtw_memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
+	
+	SetFrameSubType(pframe, WIFI_BEACON);
+	
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);	
+	len = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	//timestamp will be inserted by hardware
+	pframe += 8;
+	len += 8;
+
+	// beacon interval: 2 bytes
+	_rtw_memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2); 
+
+	pframe += 2;
+	len += 2;
+
+	// capability info: 2 bytes
+	_rtw_memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
+
+	pframe += 2;
+	len += 2;
+
+	// SSID
+	pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &len);
+
+	// supported rates...
+	rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
+	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8)? 8: rate_len), cur_network->SupportedRates, &len);
+
+	// DS parameter set
+	pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &len);
+
+	// IBSS Parameter Set...
+	//ATIMWindow = cur->Configuration.ATIMWindow;
+	ATIMWindow = 0;
+	pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &len);
+
+	//todo: ERP IE
+	
+	// EXTERNDED SUPPORTED RATE
+	if (rate_len > 8)
+	{
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &len);
+	}
+
+	if ((len + TXDESC_SIZE) > 256)
+	{
+		//DBG_871X("marc: beacon frame too large\n");
+		return 0;
+	}
+
+	//fill the tx descriptor
+	ptxdesc = (struct tx_desc *)beacon_frame;
+	
+	//offset 0	
+	ptxdesc->txdw0 |= cpu_to_le32(len & 0x0000ffff); 
+	ptxdesc->txdw0 |= cpu_to_le32(((TXDESC_SIZE + OFFSET_SZ) << OFFSET_SHT) & 0x00ff0000); //default = 32 bytes for TX Desc
+	
+	//offset 4	
+	ptxdesc->txdw1 |= cpu_to_le32((0x10 << QSEL_SHT) & 0x00001f00);
+	
+	//offset 8		
+	ptxdesc->txdw2 |= cpu_to_le32(BMC);
+	ptxdesc->txdw2 |= cpu_to_le32(BK);
+
+	//offset 16		
+	ptxdesc->txdw4 = 0x80000000;
+	
+	//offset 20
+	ptxdesc->txdw5 = 0x00000000; //1M	
+	
+	return (len + TXDESC_SIZE);
+}
+#endif
+
+static _adapter *pbuddy_padapter = NULL;
+
+int rtw_handle_dualmac(_adapter *adapter, bool init)
+{
+	int status = _SUCCESS;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+
+	if (init) {
+		#if 0
+		/* For SMSP on 92DU-VC, driver do not probe another Interface. */
+		if(dvobj->NumInterfaces == 2 && dvobj->InterfaceNumber != 0 &&
+			adapter->registrypriv.mac_phy_mode == 1) {
+			DBG_871X("%s(): Do not init another USB Interface because SMSP\n",__FUNCTION__);
+			status = _FAIL;
+			goto exit;
+		}
+		#endif
+		
+		if (pbuddy_padapter == NULL) {
+			pbuddy_padapter = adapter;
+			DBG_871X("%s(): pbuddy_padapter == NULL, Set pbuddy_padapter\n",__FUNCTION__);
+		} else {
+			adapter->pbuddy_adapter = pbuddy_padapter;
+			pbuddy_padapter->pbuddy_adapter = adapter;
+			// clear global value
+			pbuddy_padapter = NULL;
+			DBG_871X("%s(): pbuddy_padapter exist, Exchange Information\n",__FUNCTION__);
+		}
+#ifdef CONFIG_DUALMAC_CONCURRENT
+		if (dvobj->InterfaceNumber == 0) {
+			//set adapter_type/iface type
+			adapter->isprimary = _TRUE;
+			adapter->adapter_type = PRIMARY_ADAPTER;
+			adapter->iface_type = IFACE_PORT0;
+			DBG_871X("%s(): PRIMARY_ADAPTER\n",__FUNCTION__);
+		} else {
+			//set adapter_type/iface type
+			adapter->isprimary = _FALSE;
+			adapter->adapter_type = SECONDARY_ADAPTER;
+			adapter->iface_type = IFACE_PORT1;
+			DBG_871X("%s(): SECONDARY_ADAPTER\n",__FUNCTION__);
+		}
+#endif
+	}else {
+		pbuddy_padapter = NULL;
+	}
+exit:
+	return status;
+}
+
diff --git a/drivers/net/wireless/rtl8188E_8192E/core/rtw_xmit.c b/drivers/net/wireless/rtl8188E_8192E/core/rtw_xmit.c
new file mode 100755
index 00000000..0ba3bce8
--- /dev/null
+++ b/drivers/net/wireless/rtl8188E_8192E/core/rtw_xmit.c
@@ -0,0 +1,4179 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *                                        
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTW_XMIT_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <rtw_byteorder.h>
+#include <wifi.h>
+#include <osdep_intf.h>
+#include <circ_buf.h>
+#include <ip.h>
+
+#if defined (PLATFORM_LINUX) && defined (PLATFORM_WINDOWS)
+#error "Shall be Linux or Windows, but not both!\n"
+#endif
+
+#ifdef PLATFORM_WINDOWS
+#include <if_ether.h>
+#endif
+
+#ifdef CONFIG_USB_HCI
+#include <usb_ops.h>
+#endif
+
+
+static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
+static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
+
+static void _init_txservq(struct tx_servq *ptxservq)
+{
+_func_enter_;
+	_rtw_init_listhead(&ptxservq->tx_pending);
+	_rtw_init_queue(&ptxservq->sta_pending);
+	ptxservq->qcnt = 0;
+_func_exit_;		
+}
+
+
+void	_rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv)
+{	
+	
+_func_enter_;
+
+	_rtw_memset((unsigned char *)psta_xmitpriv, 0, sizeof (struct sta_xmit_priv));
+
+	_rtw_spinlock_init(&psta_xmitpriv->lock);
+	
+	//for(i = 0 ; i < MAX_NUMBLKS; i++)
+	//	_init_txservq(&(psta_xmitpriv->blk_q[i]));
+
+	_init_txservq(&psta_xmitpriv->be_q);
+	_init_txservq(&psta_xmitpriv->bk_q);
+	_init_txservq(&psta_xmitpriv->vi_q);
+	_init_txservq(&psta_xmitpriv->vo_q);
+	_rtw_init_listhead(&psta_xmitpriv->legacy_dz);
+	_rtw_init_listhead(&psta_xmitpriv->apsd);
+	
+_func_exit_;	
+
+}
+
+s32	_rtw_init_xmit_priv(struct xmit_priv *pxmitpriv, _adapter *padapter)
+{
+	int i;
+	struct xmit_buf *pxmitbuf;
+	struct xmit_frame *pxframe;
+	sint	res=_SUCCESS;   
+	u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+	u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
+
+#if defined(CONFIG_MP_INCLUDED) && defined(CONFIG_RTL8723A)
+		if (padapter->registrypriv.mp_mode) {
+			max_xmit_extbuf_size = 6000;
+			num_xmit_extbuf = 8;
+		}
+#endif
+
+_func_enter_;   	
+
+	// We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc().
+	//_rtw_memset((unsigned char *)pxmitpriv, 0, sizeof(struct xmit_priv));
+	
+	_rtw_spinlock_init(&pxmitpriv->lock);
+	_rtw_spinlock_init(&pxmitpriv->lock_sctx);
+	_rtw_init_sema(&pxmitpriv->xmit_sema, 0);
+	_rtw_init_sema(&pxmitpriv->terminate_xmitthread_sema, 0);
+
+	/* 
+	Please insert all the queue initializaiton using _rtw_init_queue below
+	*/
+
+	pxmitpriv->adapter = padapter;
+	
+	//for(i = 0 ; i < MAX_NUMBLKS; i++)
+	//	_rtw_init_queue(&pxmitpriv->blk_strms[i]);
+	
+	_rtw_init_queue(&pxmitpriv->be_pending);
+	_rtw_init_queue(&pxmitpriv->bk_pending);
+	_rtw_init_queue(&pxmitpriv->vi_pending);
+	_rtw_init_queue(&pxmitpriv->vo_pending);
+	_rtw_init_queue(&pxmitpriv->bm_pending);
+
+	//_rtw_init_queue(&pxmitpriv->legacy_dz_queue);
+	//_rtw_init_queue(&pxmitpriv->apsd_queue);
+
+	_rtw_init_queue(&pxmitpriv->free_xmit_queue);
+
+	/*	
+	Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME, 
+	and initialize free_xmit_frame below.
+	Please also apply  free_txobj to link_up all the xmit_frames...
+	*/
+
+	pxmitpriv->pallocated_frame_buf = rtw_zvmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
+	
+	if (pxmitpriv->pallocated_frame_buf  == NULL){
+		pxmitpriv->pxmit_frame_buf =NULL;
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("alloc xmit_frame fail!\n"));	
+		res= _FAIL;
+		goto exit;
+	}
+	pxmitpriv->pxmit_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_frame_buf), 4);
+	//pxmitpriv->pxmit_frame_buf = pxmitpriv->pallocated_frame_buf + 4 -
+	//						((SIZE_PTR) (pxmitpriv->pallocated_frame_buf) &3);
+
+	pxframe = (struct xmit_frame*) pxmitpriv->pxmit_frame_buf;
+
+	for (i = 0; i < NR_XMITFRAME; i++)
+	{
+		_rtw_init_listhead(&(pxframe->list));
+
+		pxframe->padapter = padapter;
+		pxframe->frame_tag = NULL_FRAMETAG;
+
+		pxframe->pkt = NULL;		
+
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+ 
+		rtw_list_insert_tail(&(pxframe->list), &(pxmitpriv->free_xmit_queue.queue));
+
+		pxframe++;
+	}
+
+	pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME;
+
+	pxmitpriv->frag_len = MAX_FRAG_THRESHOLD;
+
+
+	//init xmit_buf
+	_rtw_init_queue(&pxmitpriv->free_xmitbuf_queue);
+	_rtw_init_queue(&pxmitpriv->pending_xmitbuf_queue);
+
+	pxmitpriv->pallocated_xmitbuf = rtw_zvmalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
+	
+	if (pxmitpriv->pallocated_xmitbuf  == NULL){
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("alloc xmit_buf fail!\n"));
+		res= _FAIL;
+		goto exit;
+	}
+
+	pxmitpriv->pxmitbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmitbuf), 4);
+	//pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 -
+	//						((SIZE_PTR) (pxmitpriv->pallocated_xmitbuf) &3);
+
+	pxmitbuf = (struct xmit_buf*)pxmitpriv->pxmitbuf;
+
+	for (i = 0; i < NR_XMITBUFF; i++)
+	{
+		_rtw_init_listhead(&pxmitbuf->list);
+
+		pxmitbuf->priv_data = NULL;
+		pxmitbuf->padapter = padapter;
+		pxmitbuf->ext_tag = _FALSE;
+
+/*
+		pxmitbuf->pallocated_buf = rtw_zmalloc(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ);
+		if (pxmitbuf->pallocated_buf == NULL)
+		{
+			res = _FAIL;
+			goto exit;
+		}
+
+		pxmitbuf->pbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitbuf->pallocated_buf), XMITBUF_ALIGN_SZ);
+		//pxmitbuf->pbuf = pxmitbuf->pallocated_buf + XMITBUF_ALIGN_SZ -((SIZE_PTR) (pxmitbuf->pallocated_buf) &(XMITBUF_ALIGN_SZ-1));
+*/
+		/* Tx buf allocation may fail sometimes, so sleep and retry. */
+		if((res=rtw_os_xmit_resource_alloc(padapter, pxmitbuf,(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ))) == _FAIL) {
+			rtw_msleep_os(10);
+			res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf,(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
+			if (res == _FAIL) {
+				goto exit;
+			}
+		}
+
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+		pxmitbuf->phead = pxmitbuf->pbuf;
+		pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMITBUF_SZ;
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+#endif
+
+		pxmitbuf->flags = XMIT_VO_QUEUE;
+
+		rtw_list_insert_tail(&pxmitbuf->list, &(pxmitpriv->free_xmitbuf_queue.queue));
+		#ifdef DBG_XMIT_BUF
+		pxmitbuf->no=i;
+		#endif
+
+		pxmitbuf++;
+		
+	}
+
+	pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF;
+
+	/* init xframe_ext queue,  the same count as extbuf  */
+	_rtw_init_queue(&pxmitpriv->free_xframe_ext_queue);
+	
+	pxmitpriv->xframe_ext_alloc_addr = rtw_zvmalloc(num_xmit_extbuf * sizeof(struct xmit_frame) + 4);
+	
+	if (pxmitpriv->xframe_ext_alloc_addr  == NULL){
+		pxmitpriv->xframe_ext = NULL;
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("alloc xframe_ext fail!\n"));	
+		res= _FAIL;
+		goto exit;
+	}
+	pxmitpriv->xframe_ext = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->xframe_ext_alloc_addr), 4);
+	pxframe = (struct xmit_frame*)pxmitpriv->xframe_ext;
+
+	for (i = 0; i < num_xmit_extbuf; i++) {
+		_rtw_init_listhead(&(pxframe->list));
+
+		pxframe->padapter = padapter;
+		pxframe->frame_tag = NULL_FRAMETAG;
+
+		pxframe->pkt = NULL;		
+
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+		
+		pxframe->ext_tag = 1;
+ 
+		rtw_list_insert_tail(&(pxframe->list), &(pxmitpriv->free_xframe_ext_queue.queue));
+
+		pxframe++;
+	}
+	pxmitpriv->free_xframe_ext_cnt = num_xmit_extbuf;
+
+	// Init xmit extension buff
+	_rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue);
+
+	pxmitpriv->pallocated_xmit_extbuf = rtw_zvmalloc(num_xmit_extbuf * sizeof(struct xmit_buf) + 4);
+	
+	if (pxmitpriv->pallocated_xmit_extbuf  == NULL){
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("alloc xmit_extbuf fail!\n"));
+		res= _FAIL;
+		goto exit;
+	}
+
+	pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmit_extbuf), 4);
+
+	pxmitbuf = (struct xmit_buf*)pxmitpriv->pxmit_extbuf;
+
+	for (i = 0; i < num_xmit_extbuf; i++)
+	{
+		_rtw_init_listhead(&pxmitbuf->list);
+
+		pxmitbuf->priv_data = NULL;
+		pxmitbuf->padapter = padapter;
+		pxmitbuf->ext_tag = _TRUE;
+
+/*
+		pxmitbuf->pallocated_buf = rtw_zmalloc(max_xmit_extbuf_size);
+		if (pxmitbuf->pallocated_buf == NULL)
+		{
+			res = _FAIL;
+			goto exit;
+		}
+
+		pxmitbuf->pbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitbuf->pallocated_buf), 4);
+*/		
+
+		if((res=rtw_os_xmit_resource_alloc(padapter, pxmitbuf,max_xmit_extbuf_size + XMITBUF_ALIGN_SZ)) == _FAIL) {
+			res= _FAIL;
+			goto exit;
+		}
+		
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+		pxmitbuf->phead = pxmitbuf->pbuf;
+		pxmitbuf->pend = pxmitbuf->pbuf + max_xmit_extbuf_size;
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+#endif
+
+		rtw_list_insert_tail(&pxmitbuf->list, &(pxmitpriv->free_xmit_extbuf_queue.queue));
+		#ifdef DBG_XMIT_BUF_EXT
+		pxmitbuf->no=i;
+		#endif
+		pxmitbuf++;
+		
+	}
+
+	pxmitpriv->free_xmit_extbuf_cnt = num_xmit_extbuf;
+
+	rtw_alloc_hwxmits(padapter);
+	rtw_init_hwxmits(pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
+
+	for (i = 0; i < 4; i ++)
+	{
+		pxmitpriv->wmm_para_seq[i] = i;
+	}
+
+#ifdef CONFIG_USB_HCI
+	pxmitpriv->txirp_cnt=1;
+
+	_rtw_init_sema(&(pxmitpriv->tx_retevt), 0);
+
+	//per AC pending irp
+	pxmitpriv->beq_cnt = 0;
+	pxmitpriv->bkq_cnt = 0;
+	pxmitpriv->viq_cnt = 0;
+	pxmitpriv->voq_cnt = 0;
+#endif
+
+
+#ifdef CONFIG_XMIT_ACK
+	pxmitpriv->ack_tx = _FALSE;
+	_rtw_mutex_init(&pxmitpriv->ack_tx_mutex);
+	rtw_sctx_init(&pxmitpriv->ack_tx_ops, 0);	
+#endif
+
+	rtw_hal_init_xmit_priv(padapter);
+
+exit:
+
+_func_exit_;	
+
+	return res;
+}
+
+void  rtw_mfree_xmit_priv_lock (struct xmit_priv *pxmitpriv);
+void  rtw_mfree_xmit_priv_lock (struct xmit_priv *pxmitpriv)
+{
+	_rtw_spinlock_free(&pxmitpriv->lock);
+	_rtw_free_sema(&pxmitpriv->xmit_sema);
+	_rtw_free_sema(&pxmitpriv->terminate_xmitthread_sema);
+
+	_rtw_spinlock_free(&pxmitpriv->be_pending.lock);
+	_rtw_spinlock_free(&pxmitpriv->bk_pending.lock);
+	_rtw_spinlock_free(&pxmitpriv->vi_pending.lock);
+	_rtw_spinlock_free(&pxmitpriv->vo_pending.lock);
+	_rtw_spinlock_free(&pxmitpriv->bm_pending.lock);
+
+	//_rtw_spinlock_free(&pxmitpriv->legacy_dz_queue.lock);
+	//_rtw_spinlock_free(&pxmitpriv->apsd_queue.lock);
+
+	_rtw_spinlock_free(&pxmitpriv->free_xmit_queue.lock);
+	_rtw_spinlock_free(&pxmitpriv->free_xmitbuf_queue.lock);
+	_rtw_spinlock_free(&pxmitpriv->pending_xmitbuf_queue.lock);
+}
+
+
+void _rtw_free_xmit_priv (struct xmit_priv *pxmitpriv)
+{
+       int i;
+      _adapter *padapter = pxmitpriv->adapter;
+	struct xmit_frame	*pxmitframe = (struct xmit_frame*) pxmitpriv->pxmit_frame_buf;
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+	u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
+	u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
+	
+#if defined(CONFIG_MP_INCLUDED) && defined(CONFIG_RTL8723A)
+	if (padapter->registrypriv.mp_mode) {
+			max_xmit_extbuf_size = 6000;
+			num_xmit_extbuf = 8;
+	}
+#endif
+
+ _func_enter_;   
+
+	rtw_hal_free_xmit_priv(padapter);
+ 
+	rtw_mfree_xmit_priv_lock(pxmitpriv);
+ 
+ 	if(pxmitpriv->pxmit_frame_buf==NULL)
+		goto out;
+	
+	for(i=0; i<NR_XMITFRAME; i++)
+	{	
+		rtw_os_xmit_complete(padapter, pxmitframe);
+
+		pxmitframe++;
+	}		
+	
+	for(i=0; i<NR_XMITBUFF; i++)
+	{
+		rtw_os_xmit_resource_free(padapter, pxmitbuf,(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
+
+		//if(pxmitbuf->pallocated_buf)
+		//	rtw_mfree(pxmitbuf->pallocated_buf, MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ);
+		
+		pxmitbuf++;
+	}
+
+	if(pxmitpriv->pallocated_frame_buf) {
+		rtw_vmfree(pxmitpriv->pallocated_frame_buf, NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
+	}
+	
+
+	if(pxmitpriv->pallocated_xmitbuf) {
+		rtw_vmfree(pxmitpriv->pallocated_xmitbuf, NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
+	}
+
+	/* free xframe_ext queue,  the same count as extbuf  */
+	if ((pxmitframe = (struct xmit_frame*)pxmitpriv->xframe_ext)) {
+		for (i=0; i<num_xmit_extbuf; i++) {
+			rtw_os_xmit_complete(padapter, pxmitframe);
+			pxmitframe++;
+		}
+	}
+	if (pxmitpriv->xframe_ext_alloc_addr)
+		rtw_vmfree(pxmitpriv->xframe_ext_alloc_addr, num_xmit_extbuf * sizeof(struct xmit_frame) + 4);
+	_rtw_spinlock_free(&pxmitpriv->free_xframe_ext_queue.lock);
+
+	// free xmit extension buff
+	_rtw_spinlock_free(&pxmitpriv->free_xmit_extbuf_queue.lock);
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+	for(i=0; i<num_xmit_extbuf; i++)
+	{
+		rtw_os_xmit_resource_free(padapter, pxmitbuf,(max_xmit_extbuf_size + XMITBUF_ALIGN_SZ));
+
+		//if(pxmitbuf->pallocated_buf)
+		//	rtw_mfree(pxmitbuf->pallocated_buf, max_xmit_extbuf_size);
+		
+		pxmitbuf++;
+	}
+
+	if(pxmitpriv->pallocated_xmit_extbuf) {
+		rtw_vmfree(pxmitpriv->pallocated_xmit_extbuf, num_xmit_extbuf * sizeof(struct xmit_buf) + 4);
+	}
+
+	rtw_free_hwxmits(padapter);
+
+#ifdef CONFIG_XMIT_ACK	
+	_rtw_mutex_free(&pxmitpriv->ack_tx_mutex);	
+#endif	
+
+out:	
+
+_func_exit_;		
+
+}
+
+static void update_attrib_vcs_info(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	u32	sz;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_info	*psta = pattrib->psta;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+        if(pattrib->psta)
+	{
+		psta = pattrib->psta;
+	}
+	else
+	{
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		psta=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] );
+	}
+
+        if(psta==NULL)
+	{
+		DBG_871X("%s, psta==NUL\n", __func__);
+		return;
+	}
+
+	if(!(psta->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return;
+	}
+
+	if (pattrib->nr_frags != 1)
+	{
+		sz = padapter->xmitpriv.frag_len;
+	}
+	else //no frag
+	{
+		sz = pattrib->last_txcmdsz;
+	}
+
+	// (1) RTS_Threshold is compared to the MPDU, not MSDU.
+	// (2) If there are more than one frag in  this MSDU, only the first frag uses protection frame.
+	//		Other fragments are protected by previous fragment.
+	//		So we only need to check the length of first fragment.
+	if(pmlmeext->cur_wireless_mode < WIRELESS_11_24N  || padapter->registrypriv.wifi_spec)
+	{
+		if(sz > padapter->registrypriv.rts_thresh)
+		{
+			pattrib->vcs_mode = RTS_CTS;
+		}
+		else
+		{
+			if(psta->rtsen)
+				pattrib->vcs_mode = RTS_CTS;
+			else if(psta->cts2self)
+				pattrib->vcs_mode = CTS_TO_SELF;
+			else
+				pattrib->vcs_mode = NONE_VCS;
+		}
+	}
+	else
+	{
+		while (_TRUE)
+		{
+#if 0 //Todo
+			//check IOT action
+			if(pHTInfo->IOTAction & HT_IOT_ACT_FORCED_CTS2SELF)
+			{
+				pattrib->vcs_mode = CTS_TO_SELF;
+				pattrib->rts_rate = MGN_24M;
+				break;
+			}
+			else if(pHTInfo->IOTAction & (HT_IOT_ACT_FORCED_RTS|HT_IOT_ACT_PURE_N_MODE))
+			{
+				pattrib->vcs_mode = RTS_CTS;
+				pattrib->rts_rate = MGN_24M;
+				break;
+			}
+#endif
+
+			//IOT action
+			if((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS) && (pattrib->ampdu_en==_TRUE) &&
+				(padapter->securitypriv.dot11PrivacyAlgrthm == _AES_ ))
+			{
+				pattrib->vcs_mode = CTS_TO_SELF;
+				break;
+			}	
+			
+
+			//check ERP protection
+			if(psta->rtsen || psta->cts2self)
+			{
+				if(psta->rtsen)
+					pattrib->vcs_mode = RTS_CTS;
+				else if(psta->cts2self)
+					pattrib->vcs_mode = CTS_TO_SELF;
+
+				break;
+			}
+
+			//check HT op mode
+			if(pattrib->ht_en)
+			{
+				u8 HTOpMode = pmlmeinfo->HT_protection;
+				if((pmlmeext->cur_bwmode && (HTOpMode == 2 || HTOpMode == 3)) ||
+					(!pmlmeext->cur_bwmode && HTOpMode == 3) )
+				{
+					pattrib->vcs_mode = RTS_CTS;
+					break;
+				}
+			}
+
+			//check rts
+			if(sz > padapter->registrypriv.rts_thresh)
+			{
+				pattrib->vcs_mode = RTS_CTS;
+				break;
+			}
+
+			//to do list: check MIMO power save condition.
+
+			//check AMPDU aggregation for TXOP
+			if(pattrib->ampdu_en==_TRUE)
+			{
+				pattrib->vcs_mode = RTS_CTS;
+				break;
+			}
+
+			pattrib->vcs_mode = NONE_VCS;
+			break;
+		}
+	}
+}
+
+static void update_attrib_phy_info(struct pkt_attrib *pattrib, struct sta_info *psta)
+{
+	/*if(psta->rtsen)	
+		pattrib->vcs_mode = RTS_CTS;	
+	else if(psta->cts2self)	
+		pattrib->vcs_mode = CTS_TO_SELF;	
+	else
+		pattrib->vcs_mode = NONE_VCS;*/
+	
+	pattrib->mdata = 0;
+	pattrib->eosp = 0;
+	pattrib->triggered=0;
+	
+	//qos_en, ht_en, init rate, ,bw, ch_offset, sgi
+	pattrib->qos_en = psta->qos_option;
+	
+	pattrib->raid = psta->raid;
+#ifdef CONFIG_80211N_HT
+	pattrib->ht_en = psta->htpriv.ht_option;
+	pattrib->bwmode = psta->htpriv.bwmode;
+	pattrib->ch_offset = psta->htpriv.ch_offset;
+	pattrib->sgi= psta->htpriv.sgi;
+	pattrib->ampdu_en = _FALSE;
+#endif //CONFIG_80211N_HT
+	//if(pattrib->ht_en && psta->htpriv.ampdu_enable)
+	//{
+	//	if(psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority))
+	//		pattrib->ampdu_en = _TRUE;
+	//}	
+
+
+	pattrib->retry_ctrl = _FALSE;
+
+}
+
+u8	qos_acm(u8 acm_mask, u8 priority)
+{
+	u8	change_priority = priority;
+
+	switch (priority)
+	{
+		case 0:
+		case 3:
+			if(acm_mask & BIT(1))
+				change_priority = 1;
+			break;
+		case 1:
+		case 2:
+			break;
+		case 4:
+		case 5:
+			if(acm_mask & BIT(2))
+				change_priority = 0;
+			break;
+		case 6:
+		case 7:
+			if(acm_mask & BIT(3))
+				change_priority = 5;
+			break;
+		default:
+			DBG_871X("qos_acm(): invalid pattrib->priority: %d!!!\n", priority);
+			break;
+	}
+
+	return change_priority;
+}
+
+static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
+{
+	struct ethhdr etherhdr;
+	struct iphdr ip_hdr;
+	s32 UserPriority = 0;
+
+
+	_rtw_open_pktfile(ppktfile->pkt, ppktfile);
+	_rtw_pktfile_read(ppktfile, (unsigned char*)&etherhdr, ETH_HLEN);
+
+	// get UserPriority from IP hdr
+	if (pattrib->ether_type == 0x0800) {
+		_rtw_pktfile_read(ppktfile, (u8*)&ip_hdr, sizeof(ip_hdr));
+//		UserPriority = (ntohs(ip_hdr.tos) >> 5) & 0x3;
+		UserPriority = ip_hdr.tos >> 5;
+	} else if (pattrib->ether_type == 0x888e) {
+		// "When priority processing of data frames is supported,
+		// a STA's SME should send EAPOL-Key frames at the highest priority."
+		UserPriority = 7;
+	}
+
+	pattrib->priority = UserPriority;
+	pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN;
+	pattrib->subtype = WIFI_QOS_DATA_TYPE;
+}
+
+static s32 update_attrib(_adapter *padapter, _pkt *pkt, struct pkt_attrib *pattrib)
+{
+	uint i;
+	struct pkt_file pktfile;
+	struct sta_info *psta = NULL;
+	struct ethhdr etherhdr;
+
+	sint bmcast;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv		*pqospriv= &pmlmepriv->qospriv;
+	sint res = _SUCCESS;
+
+ _func_enter_;
+
+	_rtw_open_pktfile(pkt, &pktfile);
+	i = _rtw_pktfile_read(&pktfile, (u8*)&etherhdr, ETH_HLEN);
+
+	pattrib->ether_type = ntohs(etherhdr.h_proto);
+
+
+	_rtw_memcpy(pattrib->dst, &etherhdr.h_dest, ETH_ALEN);
+	_rtw_memcpy(pattrib->src, &etherhdr.h_source, ETH_ALEN);
+
+	pattrib->pctrl = 0;
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE)) {
+		_rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+	}
+	else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		_rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+		_rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+	}
+	else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		_rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		_rtw_memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN);
+	}
+
+	pattrib->pktlen = pktfile.pkt_len;
+
+	if (ETH_P_IP == pattrib->ether_type)
+	{
+		// The following is for DHCP and ARP packet, we use cck1M to tx these packets and let LPS awake some time 
+		// to prevent DHCP protocol fail
+		u8 tmp[24];
+		_rtw_pktfile_read(&pktfile, &tmp[0], 24);
+		pattrib->dhcp_pkt = 0;
+		if (pktfile.pkt_len > 282) {//MINIMUM_DHCP_PACKET_SIZE) {
+			if (ETH_P_IP == pattrib->ether_type) {// IP header
+				if (((tmp[21] == 68) && (tmp[23] == 67)) ||
+					((tmp[21] == 67) && (tmp[23] == 68))) {
+					// 68 : UDP BOOTP client
+					// 67 : UDP BOOTP server
+					RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("======================update_attrib: get DHCP Packet \n"));
+					// Use low rate to send DHCP packet.
+					//if(pMgntInfo->IOTAction & HT_IOT_ACT_WA_IOT_Broadcom) 
+					//{
+					//	tcb_desc->DataRate = MgntQuery_TxRateExcludeCCKRates(ieee);//0xc;//ofdm 6m
+					//	tcb_desc->bTxDisableRateFallBack = false;
+					//}
+					//else
+					//	pTcb->DataRate = Adapter->MgntInfo.LowestBasicRate; 
+					//RTPRINT(FDM, WA_IOT, ("DHCP TranslateHeader(), pTcb->DataRate = 0x%x\n", pTcb->DataRate)); 
+					pattrib->dhcp_pkt = 1;
+				}
+			}
+		}
+	} else if (0x888e == pattrib->ether_type) {
+		DBG_871X_LEVEL(_drv_always_, "send eapol packet\n");
+	}
+
+	if ( (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1) )
+	{
+		rtw_set_scan_deny(padapter, 3000);
+	}
+
+#ifdef CONFIG_LPS
+	// If EAPOL , ARP , OR DHCP packet, driver must be in active mode.
+#ifdef CONFIG_WAPI_SUPPORT
+	if ( (pattrib->ether_type == 0x88B4) || (pattrib->ether_type == 0x0806) || (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1) )
+#else
+	if ( (pattrib->ether_type == 0x0806) || (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1) )
+#endif
+	{
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SPECIAL_PACKET, 1);
+	}
+#endif
+
+	bmcast = IS_MCAST(pattrib->ra);
+	
+	// get sta_info
+	if (bmcast) {
+		psta = rtw_get_bcmc_stainfo(padapter);
+	} else {
+		psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+		if (psta == NULL)	{ // if we cannot get psta => drrp the pkt
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT"\n", MAC_ARG(pattrib->ra)));
+			#ifdef DBG_TX_DROP_FRAME
+			DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __FUNCTION__, MAC_ARG(pattrib->ra));
+			#endif
+			res =_FAIL;
+			goto exit;
+		}
+		else if((check_fwstate(pmlmepriv, WIFI_AP_STATE)==_TRUE)&&(!(psta->state & _FW_LINKED)))
+		{
+			res =_FAIL;
+			goto exit;
+		}
+	}
+
+	if (psta)
+	{
+		pattrib->mac_id = psta->mac_id;
+		//DBG_8192C("%s ==> mac_id(%d)\n",__FUNCTION__,pattrib->mac_id );
+		pattrib->psta = psta;
+	}
+	else
+	{
+		// if we cannot get psta => drop the pkt
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT "\n", MAC_ARG(pattrib->ra)));
+		#ifdef DBG_TX_DROP_FRAME
+		DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __FUNCTION__, MAC_ARG(pattrib->ra));
+		#endif
+		res = _FAIL;
+		goto exit;
+	}
+
+	pattrib->ack_policy = 0;
+	// get ether_hdr_len
+	pattrib->pkt_hdrlen = ETH_HLEN;//(pattrib->ether_type == 0x8100) ? (14 + 4 ): 14; //vlan tag
+
+	pattrib->hdrlen = WLAN_HDR_A3_LEN;
+	pattrib->subtype = WIFI_DATA_TYPE;	
+	pattrib->priority = 0;
+	
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE|WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE))
+	{
+		if(psta->qos_option)
+			set_qos(&pktfile, pattrib);
+	}
+	else
+	{
+		if(pqospriv->qos_option)
+		{
+			set_qos(&pktfile, pattrib);
+
+			if(pmlmepriv->acm_mask != 0)
+			{
+				pattrib->priority = qos_acm(pmlmepriv->acm_mask, pattrib->priority);
+			}
+		}
+	}
+
+	//pattrib->priority = 5; //force to used VI queue, for testing
+
+	if (psta->ieee8021x_blocked == _TRUE)
+	{
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("\n psta->ieee8021x_blocked == _TRUE \n"));
+
+		pattrib->encrypt = 0;
+
+		if((pattrib->ether_type != 0x888e) && (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _FALSE))
+		{
+			RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("\npsta->ieee8021x_blocked == _TRUE,  pattrib->ether_type(%.4x) != 0x888e\n",pattrib->ether_type));
+			#ifdef DBG_TX_DROP_FRAME
+			DBG_871X("DBG_TX_DROP_FRAME %s psta->ieee8021x_blocked == _TRUE,  pattrib->ether_type(%04x) != 0x888e\n", __FUNCTION__,pattrib->ether_type);
+			#endif
+			res = _FAIL;
+			goto exit;
+		}
+	}
+	else
+	{
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast);
+
+#ifdef CONFIG_WAPI_SUPPORT
+		if(pattrib->ether_type == 0x88B4)
+			pattrib->encrypt=_NO_PRIVACY_;
+#endif
+
+		switch(psecuritypriv->dot11AuthAlgrthm)
+		{
+			case dot11AuthAlgrthm_Open:
+			case dot11AuthAlgrthm_Shared:
+			case dot11AuthAlgrthm_Auto:				
+				pattrib->key_idx = (u8)psecuritypriv->dot11PrivacyKeyIndex;
+				break;
+			case dot11AuthAlgrthm_8021X:
+				if(bmcast)					
+					pattrib->key_idx = (u8)psecuritypriv->dot118021XGrpKeyid;	
+				else
+					pattrib->key_idx = 0;
+				break;
+			default:
+				pattrib->key_idx = 0;
+				break;
+		}
+		
+			
+	}
+
+	switch (pattrib->encrypt)
+	{
+		case _WEP40_:
+		case _WEP104_:
+			pattrib->iv_len = 4;
+			pattrib->icv_len = 4;
+			break;
+
+		case _TKIP_:
+			pattrib->iv_len = 8;
+			pattrib->icv_len = 4;
+			
+			if(padapter->securitypriv.busetkipkey==_FAIL)
+			{
+				RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("\npadapter->securitypriv.busetkipkey(%d)==_FAIL drop packet\n", padapter->securitypriv.busetkipkey));
+				#ifdef DBG_TX_DROP_FRAME
+				DBG_871X("DBG_TX_DROP_FRAME %s padapter->securitypriv.busetkipkey(%d)==_FAIL drop packet\n", __FUNCTION__, padapter->securitypriv.busetkipkey);
+				#endif
+				res =_FAIL;
+				goto exit;
+			}
+					
+			break;			
+		case _AES_:
+			RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("pattrib->encrypt=%d (_AES_)\n",pattrib->encrypt));
+			pattrib->iv_len = 8;
+			pattrib->icv_len = 8;
+			break;
+			
+#ifdef CONFIG_WAPI_SUPPORT
+		case _SMS4_:
+			pattrib->iv_len = 18;
+			pattrib->icv_len = 16;
+			break;
+#endif
+
+		default:
+			pattrib->iv_len = 0;
+			pattrib->icv_len = 0;
+			break;
+	}
+
+	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+		 ("update_attrib: encrypt=%d  securitypriv.sw_encrypt=%d\n",
+		  pattrib->encrypt, padapter->securitypriv.sw_encrypt));
+
+	if (pattrib->encrypt &&
+	    ((padapter->securitypriv.sw_encrypt == _TRUE) || (psecuritypriv->hw_decrypted == _FALSE)))
+	{
+		pattrib->bswenc = _TRUE;
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,
+			 ("update_attrib: encrypt=%d securitypriv.hw_decrypted=%d bswenc=_TRUE\n",
+			  pattrib->encrypt, padapter->securitypriv.sw_encrypt));
+	} else {
+		pattrib->bswenc = _FALSE;
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("update_attrib: bswenc=_FALSE\n"));
+	}
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if((pattrib->encrypt && bmcast) || (pattrib->encrypt ==_WEP40_) || (pattrib->encrypt ==_WEP104_))
+	{
+		pattrib->bswenc = _TRUE;//force using sw enc.
+	}
+#endif
+
+#ifdef CONFIG_WAPI_SUPPORT
+	if(pattrib->encrypt == _SMS4_)
+		pattrib->bswenc = _FALSE;
+#endif
+
+	rtw_set_tx_chksum_offload(pkt, pattrib);
+
+	update_attrib_phy_info(pattrib, psta);
+
+exit:
+
+_func_exit_;
+
+	return res;
+}
+
+static s32 xmitframe_addmic(_adapter *padapter, struct xmit_frame *pxmitframe){
+	sint 			curfragnum,length;
+	u8	*pframe, *payload,mic[8];
+	struct	mic_data		micdata;
+	struct	sta_info		*stainfo;
+	struct	qos_priv   *pqospriv= &(padapter->mlmepriv.qospriv);	
+	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
+	struct 	security_priv	*psecuritypriv=&padapter->securitypriv;
+	struct	xmit_priv		*pxmitpriv=&padapter->xmitpriv;
+	u8 priority[4]={0x0,0x0,0x0,0x0};
+	u8 hw_hdr_offset = 0;
+	sint bmcst = IS_MCAST(pattrib->ra);
+
+	if(pattrib->psta)
+	{
+		stainfo = pattrib->psta;
+	}
+	else
+	{
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]);
+	}	
+	
+	if(stainfo==NULL)
+	{
+		DBG_871X("%s, psta==NUL\n", __func__);
+		return _FAIL;
+	}
+
+	if(!(stainfo->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
+		return _FAIL;
+	}
+
+_func_enter_;
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+	hw_hdr_offset = TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);;	
+#else
+	#ifdef CONFIG_TX_EARLY_MODE
+	hw_hdr_offset = TXDESC_OFFSET+ EARLY_MODE_INFO_SIZE;
+	#else
+	hw_hdr_offset = TXDESC_OFFSET;
+	#endif
+#endif	
+	
+	if(pattrib->encrypt ==_TKIP_)//if(psecuritypriv->dot11PrivacyAlgrthm==_TKIP_PRIVACY_) 
+	{
+		//encode mic code
+		if(stainfo!= NULL){
+			u8 null_key[16]={0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0};
+
+			pframe = pxmitframe->buf_addr + hw_hdr_offset;
+			
+			if(bmcst)
+			{
+				if(_rtw_memcmp(psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey, null_key, 16)==_TRUE){
+					//DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey==0\n");
+					//rtw_msleep_os(10);
+					return _FAIL;
+				}				
+				//start to calculate the mic code
+				rtw_secmicsetkey(&micdata, psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey);
+			}
+			else
+			{
+				if(_rtw_memcmp(&stainfo->dot11tkiptxmickey.skey[0],null_key, 16)==_TRUE){
+					//DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey==0\n");
+					//rtw_msleep_os(10);
+					return _FAIL;
+				}
+				//start to calculate the mic code
+				rtw_secmicsetkey(&micdata, &stainfo->dot11tkiptxmickey.skey[0]);
+			}
+			
+			if(pframe[1]&1){   //ToDS==1
+				rtw_secmicappend(&micdata, &pframe[16], 6);  //DA
+				if(pframe[1]&2)  //From Ds==1
+					rtw_secmicappend(&micdata, &pframe[24], 6);
+				else
+				rtw_secmicappend(&micdata, &pframe[10], 6);		
+			}	
+			else{	//ToDS==0
+				rtw_secmicappend(&micdata, &pframe[4], 6);   //DA
+				if(pframe[1]&2)  //From Ds==1
+					rtw_secmicappend(&micdata, &pframe[16], 6);
+				else
+					rtw_secmicappend(&micdata, &pframe[10], 6);
+
+			}
+
+                    //if(pqospriv->qos_option==1)
+                    if(pattrib->qos_en)
+				priority[0]=(u8)pxmitframe->attrib.priority;
+
+			
+			rtw_secmicappend(&micdata, &priority[0], 4);
+	
+			payload=pframe;
+
+			for(curfragnum=0;curfragnum<pattrib->nr_frags;curfragnum++){
+				payload=(u8 *)RND4((SIZE_PTR)(payload));
+				RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("===curfragnum=%d, pframe= 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x,!!!\n",
+					curfragnum,*payload, *(payload+1),*(payload+2),*(payload+3),*(payload+4),*(payload+5),*(payload+6),*(payload+7)));
+
+				payload=payload+pattrib->hdrlen+pattrib->iv_len;
+				RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("curfragnum=%d pattrib->hdrlen=%d pattrib->iv_len=%d",curfragnum,pattrib->hdrlen,pattrib->iv_len));
+				if((curfragnum+1)==pattrib->nr_frags){
+					length=pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-( (pattrib->bswenc) ? pattrib->icv_len : 0);
+					rtw_secmicappend(&micdata, payload,length);
+					payload=payload+length;
+				}
+				else{
+					length=pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-( (pattrib->bswenc) ? pattrib->icv_len : 0);
+					rtw_secmicappend(&micdata, payload, length);
+					payload=payload+length+pattrib->icv_len;
+					RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("curfragnum=%d length=%d pattrib->icv_len=%d",curfragnum,length,pattrib->icv_len));
+				}
+			}
+			rtw_secgetmic(&micdata,&(mic[0]));
+			RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("xmitframe_addmic: before add mic code!!!\n"));
+			RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("xmitframe_addmic: pattrib->last_txcmdsz=%d!!!\n",pattrib->last_txcmdsz));
+			RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("xmitframe_addmic: mic[0]=0x%.2x ,mic[1]=0x%.2x ,mic[2]=0x%.2x ,mic[3]=0x%.2x \n\
+  mic[4]=0x%.2x ,mic[5]=0x%.2x ,mic[6]=0x%.2x ,mic[7]=0x%.2x !!!!\n",
+				mic[0],mic[1],mic[2],mic[3],mic[4],mic[5],mic[6],mic[7]));
+			//add mic code  and add the mic code length in last_txcmdsz
+
+			_rtw_memcpy(payload, &(mic[0]),8);
+			pattrib->last_txcmdsz+=8;
+			
+			RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("\n ========last pkt========\n"));
+			payload=payload-pattrib->last_txcmdsz+8;
+			for(curfragnum=0;curfragnum<pattrib->last_txcmdsz;curfragnum=curfragnum+8)
+					RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,(" %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x ",
+					*(payload+curfragnum), *(payload+curfragnum+1), *(payload+curfragnum+2),*(payload+curfragnum+3),
+					*(payload+curfragnum+4),*(payload+curfragnum+5),*(payload+curfragnum+6),*(payload+curfragnum+7)));
+			}
+			else{
+				RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("xmitframe_addmic: rtw_get_stainfo==NULL!!!\n"));
+			}
+	}
+	
+_func_exit_;
+
+	return _SUCCESS;
+}
+
+static s32 xmitframe_swencrypt(_adapter *padapter, struct xmit_frame *pxmitframe){
+
+	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
+	//struct 	security_priv	*psecuritypriv=&padapter->securitypriv;
+	
+_func_enter_;
+
+	//if((psecuritypriv->sw_encrypt)||(pattrib->bswenc))	
+	if(pattrib->bswenc)
+	{
+		//DBG_871X("start xmitframe_swencrypt\n");
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_alert_,("### xmitframe_swencrypt\n"));
+		switch(pattrib->encrypt){
+		case _WEP40_:
+		case _WEP104_:
+			rtw_wep_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		case _TKIP_:
+			rtw_tkip_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		case _AES_:
+			rtw_aes_encrypt(padapter, (u8 * )pxmitframe);
+			break;
+#ifdef CONFIG_WAPI_SUPPORT
+		case _SMS4_:
+			rtw_sms4_encrypt(padapter, (u8 * )pxmitframe);
+#endif
+		default:
+				break;
+		}
+
+	} else {
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_notice_,("### xmitframe_hwencrypt\n"));
+	}
+
+_func_exit_;
+
+	return _SUCCESS;
+}
+
+s32 rtw_make_wlanhdr (_adapter *padapter , u8 *hdr, struct pkt_attrib *pattrib)
+{
+	u16 *qc;
+
+	struct rtw_ieee80211_hdr *pwlanhdr = (struct rtw_ieee80211_hdr *)hdr;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	u8 qos_option = _FALSE;
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	struct sta_priv 	*pstapriv = &padapter->stapriv;
+	struct sta_info *ptdls_sta=NULL, *psta_backup=NULL;
+	u8 direct_link=0;
+#endif //CONFIG_TDLS
+
+	sint res = _SUCCESS;
+	u16 *fctrl = &pwlanhdr->frame_ctl;
+
+	struct sta_info *psta;
+
+	sint bmcst = IS_MCAST(pattrib->ra);
+
+_func_enter_;
+
+	if (pattrib->psta) {
+		psta = pattrib->psta;
+	} else {
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		if(bmcst) {
+			psta = rtw_get_bcmc_stainfo(padapter);
+		} else {
+			psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+		}
+	}
+
+	if(psta==NULL)
+	{
+		DBG_871X("%s, psta==NUL\n", __func__);
+		return _FAIL;
+	}
+
+	if(!(psta->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return _FAIL;
+	}
+
+	_rtw_memset(hdr, 0, WLANHDR_OFFSET);
+
+	SetFrameSubType(fctrl, pattrib->subtype);
+
+	if (pattrib->subtype & WIFI_DATA_TYPE)
+	{
+		if ((check_fwstate(pmlmepriv,  WIFI_STATION_STATE) == _TRUE)) {
+			//to_ds = 1, fr_ds = 0;
+#ifdef CONFIG_TDLS
+			if((ptdlsinfo->setup_state == TDLS_LINKED_STATE)){
+				ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst);
+				if((ptdls_sta!=NULL)&&(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)&&(pattrib->ether_type!=0x0806)){
+					//TDLS data transfer, ToDS=0, FrDs=0
+					_rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+					_rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+					_rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+					direct_link=1;
+				}else{
+					// 1.Data transfer to AP
+					// 2.Arp pkt will relayed by AP
+					SetToDs(fctrl);							
+					_rtw_memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
+					_rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+					_rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+				} 
+			}else
+#endif //CONFIG_TDLS
+			{
+				//Data transfer to AP
+				SetToDs(fctrl);							
+				_rtw_memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
+				_rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+				_rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+			} 
+
+			if (pqospriv->qos_option)
+				qos_option = _TRUE;
+
+		}
+		else if ((check_fwstate(pmlmepriv,  WIFI_AP_STATE) == _TRUE) ) {
+			//to_ds = 0, fr_ds = 1;
+			SetFrDs(fctrl);
+			_rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+			_rtw_memcpy(pwlanhdr->addr2, get_bssid(pmlmepriv), ETH_ALEN);
+			_rtw_memcpy(pwlanhdr->addr3, pattrib->src, ETH_ALEN);
+
+			if(psta->qos_option)			
+				qos_option = _TRUE;
+		}
+		else if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE)) {
+			_rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+			_rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+			_rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+
+			if(psta->qos_option)			
+				qos_option = _TRUE;
+		}
+		else {
+			RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("fw_state:%x is not allowed to xmit frame\n", get_fwstate(pmlmepriv)));
+			res = _FAIL;
+			goto exit;
+		}
+
+		if(pattrib->mdata)
+			SetMData(fctrl);
+
+		if (pattrib->encrypt)
+			SetPrivacy(fctrl);
+
+		if (qos_option)
+		{
+			qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
+
+			if (pattrib->priority)
+				SetPriority(qc, pattrib->priority);
+
+			SetEOSP(qc, pattrib->eosp);
+
+			SetAckpolicy(qc, pattrib->ack_policy);
+		}
+
+		//TODO: fill HT Control Field
+
+		//Update Seq Num will be handled by f/w
+		{
+			if(psta){
+#ifdef CONFIG_TDLS
+				if(direct_link==1)
+				{
+					psta_backup = psta;
+					psta = ptdls_sta;
+				}
+#endif //CONFIG_TDLS
+
+				psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
+				psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
+
+				pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
+
+				SetSeqNum(hdr, pattrib->seqnum);
+
+#ifdef CONFIG_80211N_HT
+				//check if enable ampdu
+				if(pattrib->ht_en && psta->htpriv.ampdu_enable)
+				{
+					if(psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority))
+					pattrib->ampdu_en = _TRUE;
+				}
+
+				//re-check if enable ampdu by BA_starting_seqctrl
+				if(pattrib->ampdu_en == _TRUE)
+				{					
+					u16 tx_seq;
+
+					tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f];
+		
+					//check BA_starting_seqctrl
+					if(SN_LESS(pattrib->seqnum, tx_seq))
+					{
+						//DBG_871X("tx ampdu seqnum(%d) < tx_seq(%d)\n", pattrib->seqnum, tx_seq);
+						pattrib->ampdu_en = _FALSE;//AGG BK
+					}
+					else if(SN_EQUAL(pattrib->seqnum, tx_seq))
+					{					
+						psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (tx_seq+1)&0xfff;
+					
+						pattrib->ampdu_en = _TRUE;//AGG EN
+					}
+					else
+					{
+						//DBG_871X("tx ampdu over run\n");
+						psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum+1)&0xfff;
+						pattrib->ampdu_en = _TRUE;//AGG EN
+					}
+
+				}
+#endif //CONFIG_80211N_HT
+#ifdef CONFIG_TDLS
+				if(direct_link==1)
+				{
+					if (pattrib->encrypt){
+						pattrib->encrypt= _AES_;
+						pattrib->iv_len=8;
+						pattrib->icv_len=8;
+					}
+
+					//qos_en, ht_en, init rate, ,bw, ch_offset, sgi
+					//pattrib->qos_en = ptdls_sta->qos_option;
+					
+					pattrib->raid = ptdls_sta->raid;
+#ifdef CONFIG_80211N_HT
+					pattrib->bwmode = ptdls_sta->htpriv.bwmode;
+					pattrib->ht_en = ptdls_sta->htpriv.ht_option;
+					pattrib->ch_offset = ptdls_sta->htpriv.ch_offset;
+					pattrib->sgi= ptdls_sta->htpriv.sgi;
+#endif //CONFIG_80211N_HT
+					pattrib->mac_id = ptdls_sta->mac_id;
+
+					psta = psta_backup;
+				}
+#endif //CONFIG_TDLS
+
+			}
+		}
+		
+	}
+	else
+	{
+
+	}
+
+exit:
+
+_func_exit_;
+
+	return res;
+}
+
+s32 rtw_txframes_pending(_adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	return ((_rtw_queue_empty(&pxmitpriv->be_pending) == _FALSE) || 
+			 (_rtw_queue_empty(&pxmitpriv->bk_pending) == _FALSE) || 
+			 (_rtw_queue_empty(&pxmitpriv->vi_pending) == _FALSE) ||
+			 (_rtw_queue_empty(&pxmitpriv->vo_pending) == _FALSE));
+}
+
+s32 rtw_txframes_sta_ac_pending(_adapter *padapter, struct pkt_attrib *pattrib)
+{	
+	struct sta_info *psta;
+	struct tx_servq *ptxservq;
+	int priority = pattrib->priority;
+
+	if(pattrib->psta)
+	{
+		psta = pattrib->psta;
+	}
+	else
+	{
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		psta=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]);
+	}	
+
+	if(psta==NULL)
+	{
+		DBG_871X("%s, psta==NUL\n", __func__);
+		return 0;
+	}
+
+	if(!(psta->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return 0;
+	}
+	
+	switch(priority) 
+	{
+			case 1:
+			case 2:
+				ptxservq = &(psta->sta_xmitpriv.bk_q);				
+				break;
+			case 4:
+			case 5:
+				ptxservq = &(psta->sta_xmitpriv.vi_q);				
+				break;
+			case 6:
+			case 7:
+				ptxservq = &(psta->sta_xmitpriv.vo_q);							
+				break;
+			case 0:
+			case 3:
+			default:
+				ptxservq = &(psta->sta_xmitpriv.be_q);							
+			break;
+	
+	}	
+
+	return ptxservq->qcnt;	
+}
+
+#ifdef CONFIG_TDLS
+
+int rtw_build_tdls_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, u8 action)
+{
+	int res=_SUCCESS;
+
+	switch(action){
+		case TDLS_SETUP_REQUEST:
+			rtw_build_tdls_setup_req_ies(padapter, pxmitframe, pframe);
+			break;
+		case TDLS_SETUP_RESPONSE:
+			rtw_build_tdls_setup_rsp_ies(padapter, pxmitframe, pframe);
+			break;
+		case TDLS_SETUP_CONFIRM:
+			rtw_build_tdls_setup_cfm_ies(padapter, pxmitframe, pframe);
+			break;
+		case TDLS_TEARDOWN:
+			rtw_build_tdls_teardown_ies(padapter, pxmitframe, pframe);
+			break;
+		case TDLS_DISCOVERY_REQUEST:
+			rtw_build_tdls_dis_req_ies(padapter, pxmitframe, pframe);
+			break;			
+		case TDLS_PEER_TRAFFIC_INDICATION:
+			rtw_build_tdls_peer_traffic_indication_ies(padapter, pxmitframe, pframe);
+			break;
+		case TDLS_CHANNEL_SWITCH_REQUEST:
+			rtw_build_tdls_ch_switch_req_ies(padapter, pxmitframe, pframe);
+			break;
+		case TDLS_CHANNEL_SWITCH_RESPONSE:
+			rtw_build_tdls_ch_switch_rsp_ies(padapter, pxmitframe, pframe);
+			break;
+#ifdef CONFIG_WFD			
+		case TUNNELED_PROBE_REQ:
+			rtw_build_tunneled_probe_req_ies(padapter, pxmitframe, pframe);
+			break;
+		case TUNNELED_PROBE_RSP:
+			rtw_build_tunneled_probe_rsp_ies(padapter, pxmitframe, pframe);
+			break;
+#endif //CONFIG_WFD
+		default:
+			res=_FAIL;
+			break;
+	}
+
+	return res;
+}
+
+s32 rtw_make_tdls_wlanhdr (_adapter *padapter , u8 *hdr, struct pkt_attrib *pattrib, u8 action)
+{
+	u16 *qc;
+	struct rtw_ieee80211_hdr *pwlanhdr = (struct rtw_ieee80211_hdr *)hdr;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	struct sta_priv 	*pstapriv = &padapter->stapriv;
+	struct sta_info *psta=NULL, *ptdls_sta=NULL;
+	u8 tdls_seq=0, baddr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+
+	sint res = _SUCCESS;
+	u16 *fctrl = &pwlanhdr->frame_ctl;
+
+_func_enter_;
+
+	_rtw_memset(hdr, 0, WLANHDR_OFFSET);
+
+	SetFrameSubType(fctrl, pattrib->subtype);
+
+	switch(action){
+		case TDLS_SETUP_REQUEST:
+		case TDLS_SETUP_RESPONSE:
+		case TDLS_SETUP_CONFIRM:
+		case TDLS_TEARDOWN:	//directly to peer STA or via AP
+		case TDLS_PEER_TRAFFIC_INDICATION:
+		case TDLS_PEER_PSM_REQUEST:	//directly to peer STA or via AP
+		case TUNNELED_PROBE_REQ:
+		case TUNNELED_PROBE_RSP:
+			SetToDs(fctrl);							
+			_rtw_memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
+			_rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+			_rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+			break;
+		case TDLS_CHANNEL_SWITCH_REQUEST:
+		case TDLS_CHANNEL_SWITCH_RESPONSE:
+		case TDLS_PEER_PSM_RESPONSE:
+		case TDLS_PEER_TRAFFIC_RESPONSE:
+			_rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+			_rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+			_rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+			tdls_seq=1;
+			break;
+		case TDLS_DISCOVERY_REQUEST:	//unicast: directly to peer sta, Bcast: via AP
+			if(_rtw_memcmp(pattrib->dst, baddr, ETH_ALEN) )
+			{
+				SetToDs(fctrl);							
+				_rtw_memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
+				_rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+				_rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+			}
+			else
+			{
+				_rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+				_rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+				_rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+				tdls_seq=1;
+			}
+			break;
+	}
+
+	if (pattrib->encrypt)
+		SetPrivacy(fctrl);
+
+	if (pqospriv->qos_option)
+	{
+		qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
+		if (pattrib->priority)
+			SetPriority(qc, pattrib->priority);
+		SetAckpolicy(qc, pattrib->ack_policy);
+	}
+
+	psta = pattrib->psta;
+ 
+	//  1. update seq_num per link by sta_info
+	//  2. rewrite encrypt to _AES_, also rewrite iv_len, icv_len
+	if(tdls_seq==1){
+		ptdls_sta=rtw_get_stainfo(pstapriv, pattrib->dst);
+		if(ptdls_sta){
+			ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
+			ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
+			pattrib->seqnum = ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority];
+			SetSeqNum(hdr, pattrib->seqnum);
+
+			if (pattrib->encrypt){
+				pattrib->encrypt= _AES_;
+				pattrib->iv_len=8;
+				pattrib->icv_len=8;
+			}
+		}else{
+			res=_FAIL;
+			goto exit;
+		}
+	}else if(psta){
+		psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
+		psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
+		pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
+		SetSeqNum(hdr, pattrib->seqnum);
+	}
+
+
+exit:
+
+_func_exit_;
+
+	return res;
+}
+
+s32 rtw_xmit_tdls_coalesce(_adapter * padapter, struct xmit_frame * pxmitframe, u8 action)
+{
+	s32 llc_sz;
+
+	u8 *pframe, *mem_start;
+
+	struct sta_info		*psta;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	u8 *pbuf_start;
+	s32 bmcst = IS_MCAST(pattrib->ra);
+	s32 res = _SUCCESS;
+	
+_func_enter_;
+
+	if (pattrib->psta) {
+		psta = pattrib->psta;
+	} else {	
+		if(bmcst) {
+			psta = rtw_get_bcmc_stainfo(padapter);
+		} else {
+			psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	        }
+	}
+
+	if(psta==NULL)
+		return _FAIL;
+
+	if (pxmitframe->buf_addr == NULL)
+		return _FAIL;
+
+	pbuf_start = pxmitframe->buf_addr;
+	mem_start = pbuf_start + TXDESC_OFFSET;
+
+	if (rtw_make_tdls_wlanhdr(padapter, mem_start, pattrib, action) == _FAIL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pframe = mem_start;
+	pframe += pattrib->hdrlen;
+
+	//adding icv, if necessary...
+	if (pattrib->iv_len)
+	{
+		if (psta != NULL)
+		{
+			switch(pattrib->encrypt)
+			{
+				case _WEP40_:
+				case _WEP104_:
+						WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);	
+					break;
+				case _TKIP_:			
+					if(bmcst)
+						TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+					else
+						TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
+					break;			
+				case _AES_:
+					if(bmcst)
+						AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+					else
+						AES_IV(pattrib->iv, psta->dot11txpn, 0);
+					break;
+			}
+		}
+
+		_rtw_memcpy(pframe, pattrib->iv, pattrib->iv_len);
+		pframe += pattrib->iv_len;
+
+	}
+
+	llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
+	pframe += llc_sz;
+
+	//pattrib->pktlen will be counted in rtw_build_tdls_ies
+	pattrib->pktlen = 0;
+
+	rtw_build_tdls_ies(padapter, pxmitframe, pframe, action);
+
+	if ((pattrib->icv_len >0 )&& (pattrib->bswenc)) {
+		pframe += pattrib->pktlen;
+		_rtw_memcpy(pframe, pattrib->icv, pattrib->icv_len); 
+		pframe += pattrib->icv_len;
+	}
+
+	pattrib->nr_frags = 1;
+	pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + llc_sz + 
+			((pattrib->bswenc) ? pattrib->icv_len : 0) + pattrib->pktlen;
+	
+	if (xmitframe_addmic(padapter, pxmitframe) == _FAIL)
+	{
+		goto exit;
+	}
+
+	xmitframe_swencrypt(padapter, pxmitframe);
+	
+	update_attrib_vcs_info(padapter, pxmitframe);
+	
+exit:	
+	
+_func_exit_;	
+
+	return res;
+}
+#endif //CONFIG_TDLS
+
+/*
+ * Calculate wlan 802.11 packet MAX size from pkt_attrib
+ * This function doesn't consider fragment case
+ */
+u32 rtw_calculate_wlan_pkt_size_by_attribue(struct pkt_attrib *pattrib)
+{
+	u32	len = 0;
+
+	len = pattrib->hdrlen + pattrib->iv_len; // WLAN Header and IV
+	len += SNAP_SIZE + sizeof(u16); // LLC
+	len += pattrib->pktlen;
+	if (pattrib->encrypt == _TKIP_) len += 8; // MIC
+	len += ((pattrib->bswenc) ? pattrib->icv_len : 0); // ICV
+
+	return len;
+}
+
+/*
+
+This sub-routine will perform all the following:
+
+1. remove 802.3 header.
+2. create wlan_header, based on the info in pxmitframe
+3. append sta's iv/ext-iv
+4. append LLC
+5. move frag chunk from pframe to pxmitframe->mem
+6. apply sw-encrypt, if necessary. 
+
+*/
+s32 rtw_xmitframe_coalesce(_adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe)
+{
+	struct pkt_file pktfile;
+
+	s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
+
+	SIZE_PTR addr;
+
+	u8 *pframe, *mem_start;
+	u8 hw_hdr_offset;
+
+	struct sta_info		*psta;
+	//struct sta_priv		*pstapriv = &padapter->stapriv;
+	//struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+
+	u8 *pbuf_start;
+
+	s32 bmcst = IS_MCAST(pattrib->ra);
+	s32 res = _SUCCESS;
+
+_func_enter_;
+
+	if (pattrib->psta)
+	{
+		psta = pattrib->psta;
+	} else
+	{	
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	}
+
+	if(psta==NULL)
+        {
+		
+		DBG_871X("%s, psta==NUL\n", __func__);
+		return _FAIL;
+	}
+
+
+	if(!(psta->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return _FAIL;
+	}
+
+	if (pxmitframe->buf_addr == NULL){
+		DBG_8192C("==> %s buf_addr==NULL \n",__FUNCTION__);
+		return _FAIL;
+	}
+
+	pbuf_start = pxmitframe->buf_addr;
+	
+#ifdef CONFIG_USB_TX_AGGREGATION
+	hw_hdr_offset =  TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
+#else
+	#ifdef CONFIG_TX_EARLY_MODE //for SDIO && Tx Agg
+	hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE;
+	#else
+	hw_hdr_offset = TXDESC_OFFSET;
+	#endif
+#endif
+
+	mem_start = pbuf_start +	hw_hdr_offset;
+
+	if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n"));
+		DBG_8192C("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n");
+		res = _FAIL;
+		goto exit;
+	}
+
+	_rtw_open_pktfile(pkt, &pktfile);
+	_rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen);
+
+	frg_inx = 0;
+	frg_len = pxmitpriv->frag_len - 4;//2346-4 = 2342
+
+	while (1)
+	{
+		llc_sz = 0;
+
+		mpdu_len = frg_len;
+
+		pframe = mem_start;
+
+		SetMFrag(mem_start);
+
+		pframe += pattrib->hdrlen;
+		mpdu_len -= pattrib->hdrlen;
+
+		//adding icv, if necessary...
+		if (pattrib->iv_len)
+		{
+			//if (check_fwstate(pmlmepriv, WIFI_MP_STATE))
+			//	psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
+			//else
+			//	psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+
+			if (psta != NULL)
+			{
+				switch(pattrib->encrypt)
+				{
+					case _WEP40_:
+					case _WEP104_:
+							WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);	
+						break;
+					case _TKIP_:			
+						if(bmcst)
+							TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+						else
+							TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
+						break;			
+					case _AES_:
+						if(bmcst)
+							AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+						else
+							AES_IV(pattrib->iv, psta->dot11txpn, 0);
+						break;
+#ifdef CONFIG_WAPI_SUPPORT
+					case _SMS4_:
+						rtw_wapi_get_iv(padapter,pattrib->ra,pattrib->iv);
+						break;
+#endif
+				}
+			}
+
+			_rtw_memcpy(pframe, pattrib->iv, pattrib->iv_len);
+
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
+				 ("rtw_xmitframe_coalesce: keyid=%d pattrib->iv[3]=%.2x pframe=%.2x %.2x %.2x %.2x\n",
+				  padapter->securitypriv.dot11PrivacyKeyIndex, pattrib->iv[3], *pframe, *(pframe+1), *(pframe+2), *(pframe+3)));
+
+			pframe += pattrib->iv_len;
+
+			mpdu_len -= pattrib->iv_len;
+		}
+
+		if (frg_inx == 0) {
+			llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
+			pframe += llc_sz;
+			mpdu_len -= llc_sz;
+		}
+
+		if ((pattrib->icv_len >0) && (pattrib->bswenc)) {
+			mpdu_len -= pattrib->icv_len;
+		}
+
+
+		if (bmcst) {
+			// don't do fragment to broadcat/multicast packets
+			mem_sz = _rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen);
+		} else {
+			mem_sz = _rtw_pktfile_read(&pktfile, pframe, mpdu_len);
+		}
+
+		pframe += mem_sz;
+
+		if ((pattrib->icv_len >0 )&& (pattrib->bswenc)) {
+			_rtw_memcpy(pframe, pattrib->icv, pattrib->icv_len); 
+			pframe += pattrib->icv_len;
+		}
+
+		frg_inx++;
+
+		if (bmcst || (rtw_endofpktfile(&pktfile) == _TRUE))
+		{
+			pattrib->nr_frags = frg_inx;
+
+			pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + ((pattrib->nr_frags==1)? llc_sz:0) + 
+					((pattrib->bswenc) ? pattrib->icv_len : 0) + mem_sz;
+			
+			ClearMFrag(mem_start);
+
+			break;
+		} else {
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("%s: There're still something in packet!\n", __FUNCTION__));
+		}
+
+		addr = (SIZE_PTR)(pframe);
+
+		mem_start = (unsigned char *)RND4(addr) + hw_hdr_offset;
+		_rtw_memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);	
+		
+	}
+
+	if (xmitframe_addmic(padapter, pxmitframe) == _FAIL)
+	{
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic(padapter, pxmitframe)==_FAIL\n"));
+		DBG_8192C("xmitframe_addmic(padapter, pxmitframe)==_FAIL\n");
+		res = _FAIL;
+		goto exit;
+	}
+
+	xmitframe_swencrypt(padapter, pxmitframe);
+
+	if(bmcst == _FALSE)
+		update_attrib_vcs_info(padapter, pxmitframe);
+	else
+		pattrib->vcs_mode = NONE_VCS;
+
+exit:	
+	
+_func_exit_;	
+
+	return res;
+}
+
+/* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
+ * IEEE LLC/SNAP header contains 8 octets
+ * First 3 octets comprise the LLC portion
+ * SNAP portion, 5 octets, is divided into two fields:
+ *	Organizationally Unique Identifier(OUI), 3 octets,
+ *	type, defined by that organization, 2 octets.
+ */
+s32 rtw_put_snap(u8 *data, u16 h_proto)
+{
+	struct ieee80211_snap_hdr *snap;
+	u8 *oui;
+
+_func_enter_;
+
+	snap = (struct ieee80211_snap_hdr *)data;
+	snap->dsap = 0xaa;
+	snap->ssap = 0xaa;
+	snap->ctrl = 0x03;
+
+	if (h_proto == 0x8137 || h_proto == 0x80f3)
+		oui = P802_1H_OUI;
+	else
+		oui = RFC1042_OUI;
+	
+	snap->oui[0] = oui[0];
+	snap->oui[1] = oui[1];
+	snap->oui[2] = oui[2];
+
+	*(u16 *)(data + SNAP_SIZE) = htons(h_proto);
+
+_func_exit_;
+
+	return SNAP_SIZE + sizeof(u16);
+}
+
+void rtw_update_protection(_adapter *padapter, u8 *ie, uint ie_len)
+{
+
+	uint	protection;
+	u8	*perp;
+	sint	 erp_len;
+	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct	registry_priv *pregistrypriv = &padapter->registrypriv;
+	
+_func_enter_;
+	
+	switch(pxmitpriv->vcs_setting)
+	{
+		case DISABLE_VCS:
+			pxmitpriv->vcs = NONE_VCS;
+			break;
+	
+		case ENABLE_VCS:
+			break;
+	
+		case AUTO_VCS:
+		default:
+			perp = rtw_get_ie(ie, _ERPINFO_IE_, &erp_len, ie_len);
+			if(perp == NULL)
+			{
+				pxmitpriv->vcs = NONE_VCS;
+			}
+			else
+			{
+				protection = (*(perp + 2)) & BIT(1);
+				if (protection)
+				{
+					if(pregistrypriv->vcs_type == RTS_CTS)
+						pxmitpriv->vcs = RTS_CTS;
+					else
+						pxmitpriv->vcs = CTS_TO_SELF;
+				}
+				else
+					pxmitpriv->vcs = NONE_VCS;
+			}
+
+			break;			
+	
+	}
+
+_func_exit_;
+
+}
+
+void rtw_count_tx_stats(PADAPTER padapter, struct xmit_frame *pxmitframe, int sz)
+{
+	struct sta_info *psta = NULL;
+	struct stainfo_stats *pstats = NULL;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+	if ((pxmitframe->frame_tag&0x0f) == DATA_FRAMETAG)
+	{
+		pxmitpriv->tx_bytes += sz;
+#if defined(CONFIG_USB_TX_AGGREGATION) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+		pmlmepriv->LinkDetectInfo.NumTxOkInPeriod += pxmitframe->agg_num;
+#else
+		pmlmepriv->LinkDetectInfo.NumTxOkInPeriod++;
+#endif
+
+		psta = pxmitframe->attrib.psta;
+		if (psta)
+		{
+			pstats = &psta->sta_stats;
+#if defined(CONFIG_USB_TX_AGGREGATION) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+			pstats->tx_pkts += pxmitframe->agg_num;
+#else
+			pstats->tx_pkts++;
+#endif
+			pstats->tx_bytes += sz;
+		}
+	}
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv)
+{
+	_irqL irqL;
+	struct xmit_buf *pxmitbuf =  NULL;
+	_list *plist, *phead;
+	_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+
+_func_enter_;
+
+	_enter_critical(&pfree_queue->lock, &irqL);
+
+	if(_rtw_queue_empty(pfree_queue) == _TRUE) {
+		pxmitbuf = NULL;
+	} else {
+
+		phead = get_list_head(pfree_queue);
+
+		plist = get_next(phead);
+
+		pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
+
+		rtw_list_delete(&(pxmitbuf->list));
+	}
+
+	if (pxmitbuf !=  NULL)
+	{
+		pxmitpriv->free_xmit_extbuf_cnt--;
+		#ifdef DBG_XMIT_BUF_EXT
+		DBG_871X("DBG_XMIT_BUF_EXT ALLOC no=%d,  free_xmit_extbuf_cnt=%d\n",pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt);
+		#endif
+		
+	
+		pxmitbuf->priv_data = NULL;
+		//pxmitbuf->ext_tag = _TRUE;
+
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+#endif
+#ifdef CONFIG_PCI_HCI
+		pxmitbuf->len = 0;
+#endif
+
+		if (pxmitbuf->sctx) {
+			DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+		}
+
+	}
+
+	_exit_critical(&pfree_queue->lock, &irqL);
+
+_func_exit_;
+
+	return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	_irqL irqL;
+	_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+
+_func_enter_;
+
+	if(pxmitbuf==NULL)
+	{
+		return _FAIL;
+	}
+
+	_enter_critical(&pfree_queue->lock, &irqL);
+
+	rtw_list_delete(&pxmitbuf->list);
+
+	rtw_list_insert_tail(&(pxmitbuf->list), get_list_head(pfree_queue));
+	pxmitpriv->free_xmit_extbuf_cnt++;
+	#ifdef DBG_XMIT_BUF_EXT
+	DBG_871X("DBG_XMIT_BUF_EXT FREE no=%d, free_xmit_extbuf_cnt=%d\n",pxmitbuf->no ,pxmitpriv->free_xmit_extbuf_cnt);
+	#endif
+
+	_exit_critical(&pfree_queue->lock, &irqL);
+
+_func_exit_;
+
+	return _SUCCESS;
+} 
+
+struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv)
+{
+	_irqL irqL;
+	struct xmit_buf *pxmitbuf =  NULL;
+	_list *plist, *phead;
+	_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+_func_enter_;
+
+	//DBG_871X("+rtw_alloc_xmitbuf\n");
+
+	_enter_critical(&pfree_xmitbuf_queue->lock, &irqL);
+
+	if(_rtw_queue_empty(pfree_xmitbuf_queue) == _TRUE) {
+		pxmitbuf = NULL;
+	} else {
+
+		phead = get_list_head(pfree_xmitbuf_queue);
+
+		plist = get_next(phead);
+
+		pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
+
+		rtw_list_delete(&(pxmitbuf->list));
+	}
+
+	if (pxmitbuf !=  NULL)
+	{
+		pxmitpriv->free_xmitbuf_cnt--;
+		#ifdef DBG_XMIT_BUF
+		DBG_871X("DBG_XMIT_BUF ALLOC no=%d,  free_xmitbuf_cnt=%d\n",pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt);
+		#endif
+		//DBG_871X("alloc, free_xmitbuf_cnt=%d\n", pxmitpriv->free_xmitbuf_cnt);
+
+		pxmitbuf->priv_data = NULL;
+		//pxmitbuf->ext_tag = _FALSE;
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+		pxmitbuf->agg_num = 0;
+		pxmitbuf->pg_num = 0;
+#endif
+#ifdef CONFIG_PCI_HCI
+		pxmitbuf->len = 0;
+#endif
+
+		if (pxmitbuf->sctx) {
+			DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+		}
+	}
+	#ifdef DBG_XMIT_BUF
+	else
+	{
+		DBG_871X("DBG_XMIT_BUF rtw_alloc_xmitbuf return NULL\n");
+	}
+	#endif
+
+	_exit_critical(&pfree_xmitbuf_queue->lock, &irqL);
+
+_func_exit_;
+
+	return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	_irqL irqL;
+	_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+_func_enter_;
+
+	//DBG_871X("+rtw_free_xmitbuf\n");
+
+	if(pxmitbuf==NULL)
+	{
+		return _FAIL;
+	}
+
+	if (pxmitbuf->sctx) {
+		DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
+		rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE);
+	}
+
+	if(pxmitbuf->ext_tag)
+	{
+		rtw_free_xmitbuf_ext(pxmitpriv, pxmitbuf);
+	}
+	else
+	{
+		_enter_critical(&pfree_xmitbuf_queue->lock, &irqL);
+
+		rtw_list_delete(&pxmitbuf->list);	
+
+		rtw_list_insert_tail(&(pxmitbuf->list), get_list_head(pfree_xmitbuf_queue));
+
+		pxmitpriv->free_xmitbuf_cnt++;
+		//DBG_871X("FREE, free_xmitbuf_cnt=%d\n", pxmitpriv->free_xmitbuf_cnt);
+		#ifdef DBG_XMIT_BUF
+		DBG_871X("DBG_XMIT_BUF FREE no=%d, free_xmitbuf_cnt=%d\n",pxmitbuf->no ,pxmitpriv->free_xmitbuf_cnt);
+		#endif
+		_exit_critical(&pfree_xmitbuf_queue->lock, &irqL);
+	}
+
+_func_exit_;	 
+
+	return _SUCCESS;	
+} 
+
+void rtw_init_xmitframe(struct xmit_frame *pxframe)
+{
+	if (pxframe !=  NULL)//default value setting
+	{
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+
+		_rtw_memset(&pxframe->attrib, 0, sizeof(struct pkt_attrib));
+		//pxframe->attrib.psta = NULL;
+
+		pxframe->frame_tag = DATA_FRAMETAG;
+
+#ifdef CONFIG_USB_HCI
+		pxframe->pkt = NULL;
+		pxframe->pkt_offset = 1;//default use pkt_offset to fill tx desc
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+		pxframe->agg_num = 1;
+#endif
+
+#endif //#ifdef CONFIG_USB_HCI
+
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+		pxframe->pg_num = 1;
+		pxframe->agg_num = 1;
+#endif
+
+#ifdef CONFIG_XMIT_ACK
+		pxframe->ack_report = 0;
+#endif
+
+	}
+}
+
+/*
+Calling context:
+1. OS_TXENTRY
+2. RXENTRY (rx_thread or RX_ISR/RX_CallBack)
+
+If we turn on USE_RXTHREAD, then, no need for critical section.
+Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
+
+Must be very very cautious...
+
+*/
+struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)//(_queue *pfree_xmit_queue)
+{
+	/*
+		Please remember to use all the osdep_service api,
+		and lock/unlock or _enter/_exit critical to protect 
+		pfree_xmit_queue
+	*/
+
+	_irqL irqL;
+	struct xmit_frame *pxframe = NULL;
+	_list *plist, *phead;
+	_queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
+#ifdef PLATFORM_LINUX
+	_adapter *padapter = pxmitpriv->adapter;
+#endif //PLATFORM_LINUX
+
+_func_enter_;
+
+	_enter_critical_bh(&pfree_xmit_queue->lock, &irqL);
+
+	if (_rtw_queue_empty(pfree_xmit_queue) == _TRUE) {
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_alloc_xmitframe:%d\n", pxmitpriv->free_xmitframe_cnt));
+		pxframe =  NULL;
+	} else {
+		phead = get_list_head(pfree_xmit_queue);
+
+		plist = get_next(phead);
+
+		pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		rtw_list_delete(&(pxframe->list));
+		pxmitpriv->free_xmitframe_cnt--;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe():free_xmitframe_cnt=%d\n", pxmitpriv->free_xmitframe_cnt));
+	}
+
+	_exit_critical_bh(&pfree_xmit_queue->lock, &irqL);
+
+	rtw_init_xmitframe(pxframe);
+
+_func_exit_;
+
+	return pxframe;
+}
+
+struct xmit_frame *rtw_alloc_xmitframe_ext(struct xmit_priv *pxmitpriv)
+{
+	_irqL irqL;
+	struct xmit_frame *pxframe = NULL;
+	_list *plist, *phead;
+	_queue *queue = &pxmitpriv->free_xframe_ext_queue;
+
+_func_enter_;
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	if (_rtw_queue_empty(queue) == _TRUE) {
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_alloc_xmitframe_ext:%d\n", pxmitpriv->free_xframe_ext_cnt));
+		pxframe =  NULL;
+	} else {
+		phead = get_list_head(queue);
+		plist = get_next(phead);
+		pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		rtw_list_delete(&(pxframe->list));
+		pxmitpriv->free_xframe_ext_cnt--;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe_ext():free_xmitframe_cnt=%d\n", pxmitpriv->free_xframe_ext_cnt));
+	}
+
+	_exit_critical_bh(&queue->lock, &irqL);
+
+	rtw_init_xmitframe(pxframe);
+
+_func_exit_;
+
+	return pxframe;
+}
+
+struct xmit_frame *rtw_alloc_xmitframe_once(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_frame *pxframe = NULL;
+	u8 *alloc_addr;
+
+	alloc_addr = rtw_zmalloc(sizeof(struct xmit_frame) + 4);
+	
+	if (alloc_addr == NULL)
+		goto exit;
+		
+	pxframe = (struct xmit_frame *)N_BYTE_ALIGMENT((SIZE_PTR)(alloc_addr), 4);
+	pxframe->alloc_addr = alloc_addr;
+
+	pxframe->padapter = pxmitpriv->adapter;
+	pxframe->frame_tag = NULL_FRAMETAG;
+
+	pxframe->pkt = NULL;
+
+	pxframe->buf_addr = NULL;
+	pxframe->pxmitbuf = NULL;
+
+	rtw_init_xmitframe(pxframe);
+
+	DBG_871X("################## %s ##################\n", __func__);
+
+exit:
+	return pxframe;
+}
+
+s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe)
+{	
+	_irqL irqL;
+	_queue *queue;
+	_adapter *padapter = pxmitpriv->adapter;
+	_pkt *pndis_pkt = NULL;
+
+_func_enter_;	
+
+	if (pxmitframe == NULL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("======rtw_free_xmitframe():pxmitframe==NULL!!!!!!!!!!\n"));
+		goto exit;
+	}
+
+	if (pxmitframe->pkt){
+		pndis_pkt = pxmitframe->pkt;
+		pxmitframe->pkt = NULL;
+	}
+
+	if (pxmitframe->alloc_addr) {
+		DBG_871X("################## %s with alloc_addr ##################\n", __func__);
+		rtw_mfree(pxmitframe->alloc_addr, sizeof(struct xmit_frame) + 4);
+		goto check_pkt_complete;
+	}
+
+	if (pxmitframe->ext_tag == 0)
+		queue = &pxmitpriv->free_xmit_queue;
+	else if(pxmitframe->ext_tag == 1)
+		queue = &pxmitpriv->free_xframe_ext_queue;
+	else
+	{}
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	rtw_list_delete(&pxmitframe->list);	
+	rtw_list_insert_tail(&pxmitframe->list, get_list_head(queue));
+	if (pxmitframe->ext_tag == 0) {
+		pxmitpriv->free_xmitframe_cnt++;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe():free_xmitframe_cnt=%d\n", pxmitpriv->free_xmitframe_cnt));
+	} else if(pxmitframe->ext_tag == 1) {
+		pxmitpriv->free_xframe_ext_cnt++;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe():free_xframe_ext_cnt=%d\n", pxmitpriv->free_xframe_ext_cnt));
+	} else {
+	}
+
+	_exit_critical_bh(&queue->lock, &irqL);
+
+check_pkt_complete:
+
+	if(pndis_pkt)
+		rtw_os_pkt_complete(padapter, pndis_pkt);
+
+exit:
+
+_func_exit_;
+
+	return _SUCCESS;
+}
+
+void rtw_free_xmitframe_queue(struct xmit_priv *pxmitpriv, _queue *pframequeue)
+{
+	_irqL irqL;
+	_list	*plist, *phead;
+	struct	xmit_frame 	*pxmitframe;
+
+_func_enter_;	
+
+	_enter_critical_bh(&(pframequeue->lock), &irqL);
+
+	phead = get_list_head(pframequeue);
+	plist = get_next(phead);
+	
+	while (rtw_end_of_queue_search(phead, plist) == _FALSE)
+	{
+			
+		pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		plist = get_next(plist); 
+		
+		rtw_free_xmitframe(pxmitpriv,pxmitframe);
+			
+	}
+	_exit_critical_bh(&(pframequeue->lock), &irqL);
+
+_func_exit_;
+}
+
+s32 rtw_xmitframe_enqueue(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	if (rtw_xmit_classifier(padapter, pxmitframe) == _FAIL)
+	{
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+			 ("rtw_xmitframe_enqueue: drop xmit pkt for classifier fail\n"));
+//		pxmitframe->pkt = NULL;
+		return _FAIL;
+	}
+
+	return _SUCCESS;
+}
+
+static struct xmit_frame *dequeue_one_xmitframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit, struct tx_servq *ptxservq, _queue *pframe_queue)
+{
+	_list	*xmitframe_plist, *xmitframe_phead;
+	struct	xmit_frame	*pxmitframe=NULL;
+
+	xmitframe_phead = get_list_head(pframe_queue);
+	xmitframe_plist = get_next(xmitframe_phead);
+
+	while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
+	{
+		pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+		xmitframe_plist = get_next(xmitframe_plist);
+
+/*#ifdef RTK_DMP_PLATFORM
+#ifdef CONFIG_USB_TX_AGGREGATION
+		if((ptxservq->qcnt>0) && (ptxservq->qcnt<=2))
+		{
+			pxmitframe = NULL;
+
+			tasklet_schedule(&pxmitpriv->xmit_tasklet);
+
+			break;
+		}
+#endif
+#endif*/
+		rtw_list_delete(&pxmitframe->list);
+
+		ptxservq->qcnt--;
+
+		//rtw_list_insert_tail(&pxmitframe->list, &phwxmit->pending);
+
+		//ptxservq->qcnt--;
+
+		break;		
+
+		pxmitframe = NULL;
+
+	}
+
+	return pxmitframe;
+}
+
+struct xmit_frame* rtw_dequeue_xframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit_i, sint entry)
+{
+	_irqL irqL0;
+	_list *sta_plist, *sta_phead;
+	struct hw_xmit *phwxmit;
+	struct tx_servq *ptxservq = NULL;
+	_queue *pframe_queue = NULL;
+	struct xmit_frame *pxmitframe = NULL;
+	_adapter *padapter = pxmitpriv->adapter;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	int i, inx[4];
+#ifdef CONFIG_USB_HCI
+//	int j, tmp, acirp_cnt[4];
+#endif
+
+_func_enter_;
+
+	inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
+
+	if(pregpriv->wifi_spec==1)
+	{
+		int j, tmp, acirp_cnt[4];
+#if 0
+		if(flags<XMIT_QUEUE_ENTRY)
+		{
+			//priority exchange according to the completed xmitbuf flags.
+			inx[flags] = 0;
+			inx[0] = flags;
+		}
+#endif	
+	
+#if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI)
+		for(j=0; j<4; j++)
+			inx[j] = pxmitpriv->wmm_para_seq[j];
+#endif
+	}
+
+	_enter_critical_bh(&pxmitpriv->lock, &irqL0);
+
+	for(i = 0; i < entry; i++) 
+	{
+		phwxmit = phwxmit_i + inx[i];
+
+		//_enter_critical_ex(&phwxmit->sta_queue->lock, &irqL0);
+
+		sta_phead = get_list_head(phwxmit->sta_queue);
+		sta_plist = get_next(sta_phead);
+
+		while ((rtw_end_of_queue_search(sta_phead, sta_plist)) == _FALSE)
+		{
+
+			ptxservq= LIST_CONTAINOR(sta_plist, struct tx_servq, tx_pending);
+
+			pframe_queue = &ptxservq->sta_pending;
+
+			pxmitframe = dequeue_one_xmitframe(pxmitpriv, phwxmit, ptxservq, pframe_queue);
+
+			if(pxmitframe)
+			{
+				phwxmit->accnt--;
+
+				//Remove sta node when there is no pending packets.
+				if(_rtw_queue_empty(pframe_queue)) //must be done after get_next and before break
+					rtw_list_delete(&ptxservq->tx_pending);
+
+				//_exit_critical_ex(&phwxmit->sta_queue->lock, &irqL0);
+
+				goto exit;
+			}
+
+			sta_plist = get_next(sta_plist);
+
+		}
+
+		//_exit_critical_ex(&phwxmit->sta_queue->lock, &irqL0);
+
+	}
+
+exit:
+
+	_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+
+_func_exit_;
+
+	return pxmitframe;
+}
+
+#if 1
+struct tx_servq *rtw_get_sta_pending(_adapter *padapter, struct sta_info *psta, sint up, u8 *ac)
+{
+	struct tx_servq *ptxservq=NULL;
+	
+_func_enter_;	
+
+	switch (up) 
+	{
+		case 1:
+		case 2:
+			ptxservq = &(psta->sta_xmitpriv.bk_q);
+			*(ac) = 3;
+			RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : BK \n"));
+			break;
+
+		case 4:
+		case 5:
+			ptxservq = &(psta->sta_xmitpriv.vi_q);
+			*(ac) = 1;
+			RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : VI\n"));
+			break;
+
+		case 6:
+		case 7:
+			ptxservq = &(psta->sta_xmitpriv.vo_q);
+			*(ac) = 0;
+			RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : VO \n"));			
+			break;
+
+		case 0:
+		case 3:
+		default:
+			ptxservq = &(psta->sta_xmitpriv.be_q);
+			*(ac) = 2;
+			RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : BE \n"));				
+		break;
+		
+	}
+
+_func_exit_;
+
+	return ptxservq;			
+}
+#else
+__inline static struct tx_servq *rtw_get_sta_pending
+	(_adapter *padapter, _queue **ppstapending, struct sta_info *psta, sint up)
+{
+	struct tx_servq *ptxservq;
+	struct hw_xmit *phwxmits =  padapter->xmitpriv.hwxmits;
+	
+_func_enter_;	
+
+#ifdef CONFIG_RTL8711
+
+	if(IS_MCAST(psta->hwaddr))
+	{
+		ptxservq = &(psta->sta_xmitpriv.be_q); // we will use be_q to queue bc/mc frames in BCMC_stainfo
+		*ppstapending = &padapter->xmitpriv.bm_pending; 
+	}
+	else
+#endif		
+	{
+		switch (up) 
+		{
+			case 1:
+			case 2:
+				ptxservq = &(psta->sta_xmitpriv.bk_q);
+				*ppstapending = &padapter->xmitpriv.bk_pending;
+				(phwxmits+3)->accnt++;
+				RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : BK \n"));
+				break;
+
+			case 4:
+			case 5:
+				ptxservq = &(psta->sta_xmitpriv.vi_q);
+				*ppstapending = &padapter->xmitpriv.vi_pending;
+				(phwxmits+1)->accnt++;
+				RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : VI\n"));
+				break;
+
+			case 6:
+			case 7:
+				ptxservq = &(psta->sta_xmitpriv.vo_q);
+				*ppstapending = &padapter->xmitpriv.vo_pending;
+				(phwxmits+0)->accnt++;
+				RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : VO \n"));			
+				break;
+
+			case 0:
+			case 3:
+			default:
+				ptxservq = &(psta->sta_xmitpriv.be_q);
+				*ppstapending = &padapter->xmitpriv.be_pending;
+				(phwxmits+2)->accnt++;
+				RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : BE \n"));				
+			break;
+			
+		}
+
+	}
+
+_func_exit_;
+
+	return ptxservq;			
+}
+#endif
+
+/*
+ * Will enqueue pxmitframe to the proper queue,
+ * and indicate it to xx_pending list.....
+ */
+s32 rtw_xmit_classifier(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	//_irqL irqL0;
+	u8	ac_index;
+	struct sta_info	*psta;
+	struct tx_servq	*ptxservq;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	struct hw_xmit	*phwxmits =  padapter->xmitpriv.hwxmits;
+	sint res = _SUCCESS;
+
+_func_enter_;
+
+	if (pattrib->psta) {
+		psta = pattrib->psta;		
+	} else {
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+	}
+
+	if (psta == NULL) {
+		res = _FAIL;
+		DBG_8192C("rtw_xmit_classifier: psta == NULL\n");
+		RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("rtw_xmit_classifier: psta == NULL\n"));
+		goto exit;
+	}
+
+	if(!(psta->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return _FAIL;
+	}
+
+	ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+	//_enter_critical(&pstapending->lock, &irqL0);
+
+	if (rtw_is_list_empty(&ptxservq->tx_pending)) {
+		rtw_list_insert_tail(&ptxservq->tx_pending, get_list_head(phwxmits[ac_index].sta_queue));
+	}
+
+	//_enter_critical(&ptxservq->sta_pending.lock, &irqL1);
+
+	rtw_list_insert_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending));
+	ptxservq->qcnt++;
+	phwxmits[ac_index].accnt++;
+
+	//_exit_critical(&ptxservq->sta_pending.lock, &irqL1);
+
+	//_exit_critical(&pstapending->lock, &irqL0);
+
+exit:
+
+_func_exit_;
+
+	return res;
+}
+
+void rtw_alloc_hwxmits(_adapter *padapter)
+{
+	struct hw_xmit *hwxmits;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
+
+	pxmitpriv->hwxmits = (struct hw_xmit *)rtw_zmalloc(sizeof (struct hw_xmit) * pxmitpriv->hwxmit_entry);	
+	
+	hwxmits = pxmitpriv->hwxmits;
+
+	if(pxmitpriv->hwxmit_entry == 5)
+	{
+		//pxmitpriv->bmc_txqueue.head = 0;
+		//hwxmits[0] .phwtxqueue = &pxmitpriv->bmc_txqueue;
+		hwxmits[0] .sta_queue = &pxmitpriv->bm_pending;
+	
+		//pxmitpriv->vo_txqueue.head = 0;
+		//hwxmits[1] .phwtxqueue = &pxmitpriv->vo_txqueue;
+		hwxmits[1] .sta_queue = &pxmitpriv->vo_pending;
+
+		//pxmitpriv->vi_txqueue.head = 0;
+		//hwxmits[2] .phwtxqueue = &pxmitpriv->vi_txqueue;
+		hwxmits[2] .sta_queue = &pxmitpriv->vi_pending;
+	
+		//pxmitpriv->bk_txqueue.head = 0;
+		//hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue;
+		hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
+
+      		//pxmitpriv->be_txqueue.head = 0;
+		//hwxmits[4] .phwtxqueue = &pxmitpriv->be_txqueue;
+		hwxmits[4] .sta_queue = &pxmitpriv->be_pending;
+		
+	}	
+	else if(pxmitpriv->hwxmit_entry == 4)
+	{
+
+		//pxmitpriv->vo_txqueue.head = 0;
+		//hwxmits[0] .phwtxqueue = &pxmitpriv->vo_txqueue;
+		hwxmits[0] .sta_queue = &pxmitpriv->vo_pending;
+
+		//pxmitpriv->vi_txqueue.head = 0;
+		//hwxmits[1] .phwtxqueue = &pxmitpriv->vi_txqueue;
+		hwxmits[1] .sta_queue = &pxmitpriv->vi_pending;
+
+		//pxmitpriv->be_txqueue.head = 0;
+		//hwxmits[2] .phwtxqueue = &pxmitpriv->be_txqueue;
+		hwxmits[2] .sta_queue = &pxmitpriv->be_pending;
+	
+		//pxmitpriv->bk_txqueue.head = 0;
+		//hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue;
+		hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
+	}
+	else
+	{
+		
+
+	}
+	
+
+}
+
+void rtw_free_hwxmits(_adapter *padapter)
+{
+	struct hw_xmit *hwxmits;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	hwxmits = pxmitpriv->hwxmits;
+	if(hwxmits)
+		rtw_mfree((u8 *)hwxmits, (sizeof (struct hw_xmit) * pxmitpriv->hwxmit_entry));
+}
+
+void rtw_init_hwxmits(struct hw_xmit *phwxmit, sint entry)
+{
+	sint i;
+_func_enter_;	
+	for(i = 0; i < entry; i++, phwxmit++)
+	{
+		//_rtw_spinlock_init(&phwxmit->xmit_lock);
+		//_rtw_init_listhead(&phwxmit->pending);		
+		//phwxmit->txcmdcnt = 0;
+		phwxmit->accnt = 0;
+	}
+_func_exit_;	
+}
+
+#ifdef CONFIG_BR_EXT
+int rtw_br_client_tx(_adapter *padapter, struct sk_buff **pskb)
+{
+	struct sk_buff *skb = *pskb;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	_irqL irqL;
+	//if(check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) == _TRUE)
+	{
+		void dhcp_flag_bcast(_adapter *priv, struct sk_buff *skb);
+		int res, is_vlan_tag=0, i, do_nat25=1;
+		unsigned short vlan_hdr=0;
+		void *br_port = NULL;
+
+		//mac_clone_handle_frame(priv, skb);
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+		br_port = padapter->pnetdev->br_port;
+#else   // (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+		rcu_read_lock();
+		br_port = rcu_dereference(padapter->pnetdev->rx_handler_data);
+		rcu_read_unlock();
+#endif  // (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+		_enter_critical_bh(&padapter->br_ext_lock, &irqL);
+		if (	!(skb->data[0] & 1) &&
+				br_port &&
+				memcmp(skb->data+MACADDRLEN, padapter->br_mac, MACADDRLEN) &&
+				*((unsigned short *)(skb->data+MACADDRLEN*2)) != __constant_htons(ETH_P_8021Q) &&
+				*((unsigned short *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_IP) &&
+				!memcmp(padapter->scdb_mac, skb->data+MACADDRLEN, MACADDRLEN) && padapter->scdb_entry) {
+			memcpy(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
+			padapter->scdb_entry->ageing_timer = jiffies;
+			_exit_critical_bh(&padapter->br_ext_lock, &irqL);
+		}
+		else
+		//if (!priv->pmib->ethBrExtInfo.nat25_disable) 		
+		{
+//			if (priv->dev->br_port &&
+//				 !memcmp(skb->data+MACADDRLEN, priv->br_mac, MACADDRLEN)) {
+#if 1
+			if (*((unsigned short *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_8021Q)) {
+				is_vlan_tag = 1;
+				vlan_hdr = *((unsigned short *)(skb->data+MACADDRLEN*2+2));
+				for (i=0; i<6; i++)
+					*((unsigned short *)(skb->data+MACADDRLEN*2+2-i*2)) = *((unsigned short *)(skb->data+MACADDRLEN*2-2-i*2));
+				skb_pull(skb, 4);
+			}
+			//if SA == br_mac && skb== IP  => copy SIP to br_ip ?? why
+			if (!memcmp(skb->data+MACADDRLEN, padapter->br_mac, MACADDRLEN) &&
+				(*((unsigned short *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_IP)))
+				memcpy(padapter->br_ip, skb->data+WLAN_ETHHDR_LEN+12, 4);
+
+			if (*((unsigned short *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_IP)) {
+				if (memcmp(padapter->scdb_mac, skb->data+MACADDRLEN, MACADDRLEN)) {
+					void *scdb_findEntry(_adapter *priv, unsigned char *macAddr, unsigned char *ipAddr);
+					
+					if ((padapter->scdb_entry = (struct nat25_network_db_entry *)scdb_findEntry(padapter,
+								skb->data+MACADDRLEN, skb->data+WLAN_ETHHDR_LEN+12)) != NULL) {
+						memcpy(padapter->scdb_mac, skb->data+MACADDRLEN, MACADDRLEN);
+						memcpy(padapter->scdb_ip, skb->data+WLAN_ETHHDR_LEN+12, 4);
+						padapter->scdb_entry->ageing_timer = jiffies;
+						do_nat25 = 0;
+					}
+				}
+				else {
+					if (padapter->scdb_entry) {
+						padapter->scdb_entry->ageing_timer = jiffies;
+						do_nat25 = 0;
+					}
+					else {
+						memset(padapter->scdb_mac, 0, MACADDRLEN);
+						memset(padapter->scdb_ip, 0, 4);
+					}
+				}
+			}
+			_exit_critical_bh(&padapter->br_ext_lock, &irqL);
+#endif // 1
+			if (do_nat25)
+			{
+				int nat25_db_handle(_adapter *priv, struct sk_buff *skb, int method);
+				if (nat25_db_handle(padapter, skb, NAT25_CHECK) == 0) {
+					struct sk_buff *newskb;
+
+					if (is_vlan_tag) {
+						skb_push(skb, 4);
+						for (i=0; i<6; i++)
+							*((unsigned short *)(skb->data+i*2)) = *((unsigned short *)(skb->data+4+i*2));
+						*((unsigned short *)(skb->data+MACADDRLEN*2)) = __constant_htons(ETH_P_8021Q);
+						*((unsigned short *)(skb->data+MACADDRLEN*2+2)) = vlan_hdr;
+					}
+
+					newskb = skb_copy(skb, GFP_ATOMIC);
+					if (newskb == NULL) {
+						//priv->ext_stats.tx_drops++;
+						DEBUG_ERR("TX DROP: skb_copy fail!\n");
+						//goto stop_proc;
+						return -1;
+					}
+					dev_kfree_skb_any(skb);
+
+					*pskb = skb = newskb;
+					if (is_vlan_tag) {
+						vlan_hdr = *((unsigned short *)(skb->data+MACADDRLEN*2+2));
+						for (i=0; i<6; i++)
+							*((unsigned short *)(skb->data+MACADDRLEN*2+2-i*2)) = *((unsigned short *)(skb->data+MACADDRLEN*2-2-i*2));
+						skb_pull(skb, 4);
+					}
+				}
+
+				if (skb_is_nonlinear(skb))
+					DEBUG_ERR("%s(): skb_is_nonlinear!!\n", __FUNCTION__);
+					
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+				res = skb_linearize(skb, GFP_ATOMIC);
+#else	// (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+				res = skb_linearize(skb);
+#endif	// (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+				if (res < 0) {				
+						DEBUG_ERR("TX DROP: skb_linearize fail!\n");
+						//goto free_and_stop;
+						return -1;
+				}
+				
+				res = nat25_db_handle(padapter, skb, NAT25_INSERT);
+				if (res < 0) {
+					if (res == -2) {
+						//priv->ext_stats.tx_drops++;
+						DEBUG_ERR("TX DROP: nat25_db_handle fail!\n");
+						//goto free_and_stop;
+						return -1;
+
+					}
+					// we just print warning message and let it go
+					//DEBUG_WARN("%s()-%d: nat25_db_handle INSERT Warning!\n", __FUNCTION__, __LINE__);
+					//return -1; // return -1 will cause system crash on 2011/08/30!
+					return 0;
+				}
+			}
+
+			memcpy(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
+
+			dhcp_flag_bcast(padapter, skb);
+
+			if (is_vlan_tag) {
+				skb_push(skb, 4);
+				for (i=0; i<6; i++)
+					*((unsigned short *)(skb->data+i*2)) = *((unsigned short *)(skb->data+4+i*2));
+				*((unsigned short *)(skb->data+MACADDRLEN*2)) = __constant_htons(ETH_P_8021Q);
+				*((unsigned short *)(skb->data+MACADDRLEN*2+2)) = vlan_hdr;
+			}
+		}
+#if 0		
+		else{
+			if (*((unsigned short *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_8021Q)) {
+				is_vlan_tag = 1;
+			}
+				
+			if(is_vlan_tag){
+				if(ICMPV6_MCAST_MAC(skb->data) && ICMPV6_PROTO1A_VALN(skb->data)){
+                                        memcpy(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
+				}
+			}else
+			{
+				if(ICMPV6_MCAST_MAC(skb->data) && ICMPV6_PROTO1A(skb->data)){
+                                        memcpy(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
+				}
+			}	
+		}
+#endif	// 0
+
+		// check if SA is equal to our MAC
+		if (memcmp(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN)) {
+			//priv->ext_stats.tx_drops++;
+			DEBUG_ERR("TX DROP: untransformed frame SA:%02X%02X%02X%02X%02X%02X!\n",
+				skb->data[6],skb->data[7],skb->data[8],skb->data[9],skb->data[10],skb->data[11]);
+			//goto free_and_stop;
+			return -1;
+		}
+	}
+	return 0;
+}
+#endif	// CONFIG_BR_EXT
+
+u32 rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe)
+{
+	u32 addr;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;	
+	
+	switch(pattrib->qsel)
+	{
+		case 0:
+		case 3:
+			addr = BE_QUEUE_INX;
+		 	break;
+		case 1:
+		case 2:
+			addr = BK_QUEUE_INX;
+			break;				
+		case 4:
+		case 5:
+			addr = VI_QUEUE_INX;
+			break;		
+		case 6:
+		case 7:
+			addr = VO_QUEUE_INX;
+			break;
+		case 0x10:
+			addr = BCN_QUEUE_INX;
+			break;
+		case 0x11://BC/MC in PS (HIQ)
+			addr = HIGH_QUEUE_INX;
+			break;
+		case 0x12:
+		default:
+			addr = MGT_QUEUE_INX;
+			break;		
+			
+	}
+
+	return addr;
+
+}
+
+static void do_queue_select(_adapter	*padapter, struct pkt_attrib *pattrib)
+{
+	u8 qsel;
+		
+	qsel = pattrib->priority;
+	RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("### do_queue_select priority=%d ,qsel = %d\n",pattrib->priority ,qsel));
+
+#ifdef CONFIG_CONCURRENT_MODE	
+//	if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == _TRUE)
+//		qsel = 7;//
+#endif
+	
+	pattrib->qsel = qsel;
+}
+
+/*
+ * The main transmit(tx) entry
+ *
+ * Return
+ *	1	enqueue
+ *	0	success, hardware will handle this xmit frame(packet)
+ *	<0	fail
+ */
+s32 rtw_xmit(_adapter *padapter, _pkt **ppkt)
+{
+	static u32 start = 0;
+	static u32 drop_cnt = 0;
+#ifdef CONFIG_AP_MODE
+	_irqL irqL0;
+#endif
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_frame *pxmitframe = NULL;
+#ifdef CONFIG_BR_EXT
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	void *br_port = NULL;
+#endif	// CONFIG_BR_EXT
+
+	s32 res;
+
+	if (start == 0)
+		start = rtw_get_current_time();
+
+	pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
+
+	if (rtw_get_passing_time_ms(start) > 2000) {
+		if (drop_cnt)
+			DBG_871X("DBG_TX_DROP_FRAME %s no more pxmitframe, drop_cnt:%u\n", __FUNCTION__, drop_cnt);
+		start = rtw_get_current_time();
+		drop_cnt = 0;
+	}
+
+	if (pxmitframe == NULL) {
+		drop_cnt ++;
+		RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: no more pxmitframe\n"));
+		return -1;
+	}
+
+#ifdef CONFIG_BR_EXT
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+	br_port = padapter->pnetdev->br_port;
+#else   // (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+	rcu_read_lock();
+	br_port = rcu_dereference(padapter->pnetdev->rx_handler_data);
+	rcu_read_unlock();
+#endif  // (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+
+	if( br_port && check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) == _TRUE)
+	{
+		res = rtw_br_client_tx(padapter, ppkt);
+		if (res == -1)
+		{
+			rtw_free_xmitframe(pxmitpriv, pxmitframe);
+			return -1;
+		}
+	}	
+
+#endif	// CONFIG_BR_EXT
+
+	res = update_attrib(padapter, *ppkt, &pxmitframe->attrib);
+
+#ifdef CONFIG_WAPI_SUPPORT
+	if(pxmitframe->attrib.ether_type != 0x88B4)
+	{
+		if(rtw_wapi_drop_for_key_absent(padapter, pxmitframe->attrib.ra))
+		{
+			WAPI_TRACE(WAPI_RX,"drop for key absend when tx \n");
+			res = _FAIL;
+		}
+	}
+#endif
+	if (res == _FAIL) {
+		RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: update attrib fail\n"));
+		#ifdef DBG_TX_DROP_FRAME
+		DBG_871X("DBG_TX_DROP_FRAME %s update attrib fail\n", __FUNCTION__);
+		#endif
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+		return -1;
+	}
+	pxmitframe->pkt = *ppkt;
+
+	rtw_led_control(padapter, LED_CTL_TX);
+
+	do_queue_select(padapter, &pxmitframe->attrib);
+
+#ifdef CONFIG_AP_MODE
+	_enter_critical_bh(&pxmitpriv->lock, &irqL0);
+	if(xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe) == _TRUE)
+	{
+		_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+		return 1;		
+	}
+	_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+#endif
+
+	if (rtw_hal_xmit(padapter, pxmitframe) == _FALSE)
+		return 1;
+
+	return 0;
+}
+
+#ifdef CONFIG_TDLS
+sint xmitframe_enqueue_for_tdls_sleeping_sta(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	sint ret=_FALSE;
+
+	_irqL irqL;
+	struct sta_info *ptdls_sta=NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	int i;
+	
+	ptdls_sta=rtw_get_stainfo(pstapriv, pattrib->dst);
+	if(ptdls_sta==NULL){
+		return ret;
+	}else if(ptdls_sta->tdls_sta_state&TDLS_LINKED_STATE){
+
+		if(pattrib->triggered==1)
+		{
+			ret = _TRUE;
+			return ret;
+			}
+
+		_enter_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);	
+		
+		if(ptdls_sta->state&WIFI_SLEEP_STATE)
+		{
+			rtw_list_delete(&pxmitframe->list);
+		
+			//_enter_critical_bh(&psta->sleep_q.lock, &irqL);	
+			
+			rtw_list_insert_tail(&pxmitframe->list, get_list_head(&ptdls_sta->sleep_q));
+			
+			ptdls_sta->sleepq_len++;
+			ptdls_sta->sleepq_ac_len++;
+
+			//indicate 4-AC queue bit in TDLS peer traffic indication
+			switch(pattrib->priority)
+			{
+				case 1:
+				case 2:
+					ptdls_sta->uapsd_bk = ptdls_sta->uapsd_bk | BIT(1);
+					break;
+				case 4:
+				case 5:
+					ptdls_sta->uapsd_vi = ptdls_sta->uapsd_vi | BIT(1);
+					break;
+				case 6:
+				case 7:
+					ptdls_sta->uapsd_vo = ptdls_sta->uapsd_vo | BIT(1);
+					break;
+				case 0:
+				case 3:
+				default:
+					ptdls_sta->uapsd_be = ptdls_sta->uapsd_be | BIT(1);
+					break;
+			}
+
+			if(ptdls_sta->sleepq_len==1)
+			{
+				//transmit TDLS PTI via AP
+				rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_SD_PTI);
+			}
+			ret = _TRUE;
+
+		}
+
+		_exit_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);	
+	}
+
+	return ret;
+	
+}
+#endif //CONFIG_TDLS
+
+#if defined(CONFIG_AP_MODE) || defined(CONFIG_TDLS)
+
+sint xmitframe_enqueue_for_sleeping_sta(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	_irqL irqL;
+	sint ret=_FALSE;
+	struct sta_info *psta=NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	sint bmcst = IS_MCAST(pattrib->ra);
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+
+	if( ptdlsinfo->setup_state == TDLS_LINKED_STATE )
+	{
+		ret = xmitframe_enqueue_for_tdls_sleeping_sta(padapter, pxmitframe);
+		return ret;
+	}
+#endif //CONFIG_TDLS
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == _FALSE)
+	    return ret;
+
+	if(pattrib->psta)
+	{
+		psta = pattrib->psta;
+	}
+	else
+	{
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		psta=rtw_get_stainfo(pstapriv, pattrib->ra);
+	}
+
+	if(psta==NULL)
+	{
+		DBG_871X("%s, psta==NUL\n", __func__);
+		return _FALSE;
+	}
+
+	if(!(psta->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return _FALSE;
+	}
+
+	if(pattrib->triggered==1)
+	{
+		//DBG_871X("directly xmit pspoll_triggered packet\n");
+
+		//pattrib->triggered=0;
+
+		if(bmcst)
+			pattrib->qsel = 0x11;//HIQ
+		
+
+		return ret;
+	}
+
+
+	if(bmcst)
+	{
+		_enter_critical_bh(&psta->sleep_q.lock, &irqL);	
+	
+		if(pstapriv->sta_dz_bitmap)//if anyone sta is in ps mode
+		{
+			//pattrib->qsel = 0x11;//HIQ
+			
+			rtw_list_delete(&pxmitframe->list);
+			
+			//_enter_critical_bh(&psta->sleep_q.lock, &irqL);	
+			
+			rtw_list_insert_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
+			
+			psta->sleepq_len++;
+
+			pstapriv->tim_bitmap |= BIT(0);//
+			pstapriv->sta_dz_bitmap |= BIT(0);
+			
+			//DBG_871X("enqueue, sq_len=%d, tim=%x\n", psta->sleepq_len, pstapriv->tim_bitmap);
+
+			update_beacon(padapter, _TIM_IE_, NULL, _FALSE);//tx bc/mc packets after upate bcn
+			
+			//_exit_critical_bh(&psta->sleep_q.lock, &irqL);				
+			
+			ret = _TRUE;			
+			
+		}
+		
+		_exit_critical_bh(&psta->sleep_q.lock, &irqL);	
+		
+		return ret;
+		
+	}
+	
+
+	_enter_critical_bh(&psta->sleep_q.lock, &irqL);	
+	
+	if(psta->state&WIFI_SLEEP_STATE)
+	{
+		u8 wmmps_ac=0;
+	
+		if(pstapriv->sta_dz_bitmap&BIT(psta->aid))	
+		{			
+			rtw_list_delete(&pxmitframe->list);
+		
+			//_enter_critical_bh(&psta->sleep_q.lock, &irqL);	
+			
+			rtw_list_insert_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
+			
+			psta->sleepq_len++;
+
+			switch(pattrib->priority)
+			{
+				case 1:
+				case 2:
+					wmmps_ac = psta->uapsd_bk&BIT(0);
+					break;
+				case 4:
+				case 5:
+					wmmps_ac = psta->uapsd_vi&BIT(0);
+					break;
+				case 6:
+				case 7:
+					wmmps_ac = psta->uapsd_vo&BIT(0);
+					break;
+				case 0:
+				case 3:
+				default:
+					wmmps_ac = psta->uapsd_be&BIT(0);
+					break;
+			}
+
+			if(wmmps_ac)
+				psta->sleepq_ac_len++;
+
+			if(((psta->has_legacy_ac) && (!wmmps_ac)) ||((!psta->has_legacy_ac)&&(wmmps_ac)))
+			{
+				pstapriv->tim_bitmap |= BIT(psta->aid);
+
+				//DBG_871X("enqueue, sq_len=%d, tim=%x\n", psta->sleepq_len, pstapriv->tim_bitmap);
+
+				if(psta->sleepq_len==1)
+				{
+					//DBG_871X("sleepq_len==1, update BCNTIM\n");
+					//upate BCN for TIM IE
+					update_beacon(padapter, _TIM_IE_, NULL, _FALSE);
+				}
+			}
+
+			//_exit_critical_bh(&psta->sleep_q.lock, &irqL);			
+
+			//if(psta->sleepq_len > (NR_XMITFRAME>>3))
+			//{
+			//	wakeup_sta_to_xmit(padapter, psta);
+			//}	
+
+			ret = _TRUE;
+
+		}
+
+	}
+
+	_exit_critical_bh(&psta->sleep_q.lock, &irqL);	
+
+	return ret;
+	
+}
+
+static void dequeue_xmitframes_to_sleeping_queue(_adapter *padapter, struct sta_info *psta, _queue *pframequeue)
+{
+	sint ret;
+	_list	*plist, *phead;
+	u8	ac_index;
+	struct tx_servq	*ptxservq;
+	struct pkt_attrib	*pattrib;
+	struct xmit_frame 	*pxmitframe;
+	struct hw_xmit *phwxmits =  padapter->xmitpriv.hwxmits;
+	
+	phead = get_list_head(pframequeue);
+	plist = get_next(phead);
+	
+	while (rtw_end_of_queue_search(phead, plist) == _FALSE)
+	{			
+		pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		plist = get_next(plist);	
+		
+		ret = xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe);	
+
+		if(_TRUE == ret)
+		{
+			pattrib = &pxmitframe->attrib;
+
+			ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+			ptxservq->qcnt--;
+			phwxmits[ac_index].accnt--;
+		}
+		else
+		{
+			//DBG_871X("xmitframe_enqueue_for_sleeping_sta return _FALSE\n");
+		}
+		
+	}
+	
+}
+
+void stop_sta_xmit(_adapter *padapter, struct sta_info *psta)
+{	
+	_irqL irqL0;	
+	struct sta_info *psta_bmc;
+	struct sta_xmit_priv *pstaxmitpriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;	
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;	
+	
+	pstaxmitpriv = &psta->sta_xmitpriv;
+
+	//for BC/MC Frames
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+	
+			
+	_enter_critical_bh(&pxmitpriv->lock, &irqL0);
+
+	psta->state |= WIFI_SLEEP_STATE;
+	
+#ifdef CONFIG_TDLS
+	if( !(psta->tdls_sta_state & TDLS_LINKED_STATE) )
+#endif //CONFIG_TDLS
+	pstapriv->sta_dz_bitmap |= BIT(psta->aid);
+	
+	
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vo_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->vo_q.tx_pending));
+
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vi_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->vi_q.tx_pending));
+
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->be_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->be_q.tx_pending));
+	
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->bk_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->bk_q.tx_pending));
+
+#ifdef CONFIG_TDLS
+	if( !(psta->tdls_sta_state & TDLS_LINKED_STATE) )
+	{
+		if( psta_bmc != NULL )
+		{
+#endif //CONFIG_TDLS
+
+
+	//for BC/MC Frames
+	pstaxmitpriv = &psta_bmc->sta_xmitpriv;
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->be_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->be_q.tx_pending));
+	
+
+#ifdef CONFIG_TDLS	
+		}
+	}
+#endif //CONFIG_TDLS	
+	_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+	
+
+}	
+
+void wakeup_sta_to_xmit(_adapter *padapter, struct sta_info *psta)
+{	 
+	_irqL irqL;	 
+	u8 update_mask=0, wmmps_ac=0;
+	struct sta_info *psta_bmc;
+	_list	*xmitframe_plist, *xmitframe_phead;
+	struct xmit_frame *pxmitframe=NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	//_enter_critical_bh(&psta->sleep_q.lock, &irqL);
+	_enter_critical_bh(&pxmitpriv->lock, &irqL);
+
+
+	xmitframe_phead = get_list_head(&psta->sleep_q);
+	xmitframe_plist = get_next(xmitframe_phead);
+
+	while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
+	{			
+		pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+		xmitframe_plist = get_next(xmitframe_plist);            
+
+		rtw_list_delete(&pxmitframe->list);
+
+		switch(pxmitframe->attrib.priority)
+		{
+			case 1:
+			case 2:
+				wmmps_ac = psta->uapsd_bk&BIT(1);
+				break;
+			case 4:
+			case 5:
+				wmmps_ac = psta->uapsd_vi&BIT(1);
+				break;
+			case 6:
+			case 7:
+				wmmps_ac = psta->uapsd_vo&BIT(1);
+				break;
+			case 0:
+			case 3:
+			default:
+				wmmps_ac = psta->uapsd_be&BIT(1);
+				break;	
+		}
+
+		psta->sleepq_len--;
+		if(psta->sleepq_len>0)
+			pxmitframe->attrib.mdata = 1;
+		else
+			pxmitframe->attrib.mdata = 0;
+
+		if(wmmps_ac)
+		{
+			psta->sleepq_ac_len--;
+			if(psta->sleepq_ac_len>0)
+			{
+				pxmitframe->attrib.mdata = 1;
+				pxmitframe->attrib.eosp = 0;
+			}
+			else
+			{
+				pxmitframe->attrib.mdata = 0;
+				pxmitframe->attrib.eosp = 1;
+			}
+		}
+		
+		pxmitframe->attrib.triggered = 1;
+
+/*
+		_exit_critical_bh(&psta->sleep_q.lock, &irqL);	
+		if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE)
+		{		
+			rtw_os_xmit_complete(padapter, pxmitframe);
+		}		
+		_enter_critical_bh(&psta->sleep_q.lock, &irqL);
+*/
+		rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+		
+	}	
+	
+	if(psta->sleepq_len==0)
+	{
+#ifdef CONFIG_TDLS
+		if( psta->tdls_sta_state & TDLS_LINKED_STATE )
+		{
+			if(psta->state&WIFI_SLEEP_STATE)
+				psta->state ^= WIFI_SLEEP_STATE;
+
+			//_exit_critical_bh(&psta->sleep_q.lock, &irqL);
+			_exit_critical_bh(&pxmitpriv->lock, &irqL);
+			return;
+		}
+#endif //CONFIG_TDLS
+		pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+		//DBG_871X("wakeup to xmit, qlen==0, update_BCNTIM, tim=%x\n", pstapriv->tim_bitmap);
+		//upate BCN for TIM IE
+		//update_BCNTIM(padapter);
+		update_mask = BIT(0);
+
+		if(psta->state&WIFI_SLEEP_STATE)
+			psta->state ^= WIFI_SLEEP_STATE;
+
+		if(psta->state & WIFI_STA_ALIVE_CHK_STATE)
+		{
+			psta->expire_to = pstapriv->expire_to;
+			psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+		}
+
+		pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+	}
+	
+	//_exit_critical_bh(&psta->sleep_q.lock, &irqL);
+	_exit_critical_bh(&pxmitpriv->lock, &irqL);
+
+
+	//for BC/MC Frames
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+	if(!psta_bmc)
+		return;
+
+	if((pstapriv->sta_dz_bitmap&0xfffe) == 0x0)//no any sta in ps mode
+	{
+		//_enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
+		_enter_critical_bh(&pxmitpriv->lock, &irqL);
+
+		xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+		xmitframe_plist = get_next(xmitframe_phead);
+
+		while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
+		{			
+			pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+			xmitframe_plist = get_next(xmitframe_plist);
+
+			rtw_list_delete(&pxmitframe->list);
+
+			psta_bmc->sleepq_len--;
+			if(psta_bmc->sleepq_len>0)
+				pxmitframe->attrib.mdata = 1;
+			else
+				pxmitframe->attrib.mdata = 0;
+			
+
+			pxmitframe->attrib.triggered = 1;
+/*
+			_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);	
+			if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE)
+			{		
+				rtw_os_xmit_complete(padapter, pxmitframe);
+			}
+			_enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);	
+
+*/
+			rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+		}	
+	
+		if(psta_bmc->sleepq_len==0)
+		{
+			pstapriv->tim_bitmap &= ~BIT(0);
+			pstapriv->sta_dz_bitmap &= ~BIT(0);
+
+			//DBG_871X("wakeup to xmit, qlen==0, update_BCNTIM, tim=%x\n", pstapriv->tim_bitmap);
+			//upate BCN for TIM IE
+			//update_BCNTIM(padapter);
+			update_mask |= BIT(1);
+		}		
+	
+		//_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);	
+		_exit_critical_bh(&pxmitpriv->lock, &irqL);
+
+	}	
+
+	
+	if(update_mask)
+	{
+		//update_BCNTIM(padapter);
+		//printk("%s => call update_beacon\n",__FUNCTION__);
+		update_beacon(padapter, _TIM_IE_, NULL, _FALSE);
+	}	
+	
+}
+
+void xmit_delivery_enabled_frames(_adapter *padapter, struct sta_info *psta)
+{
+	_irqL irqL;
+	u8 wmmps_ac=0;
+	_list	*xmitframe_plist, *xmitframe_phead;
+	struct xmit_frame *pxmitframe=NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+
+	//_enter_critical_bh(&psta->sleep_q.lock, &irqL);
+	_enter_critical_bh(&pxmitpriv->lock, &irqL);
+
+	xmitframe_phead = get_list_head(&psta->sleep_q);
+	xmitframe_plist = get_next(xmitframe_phead);
+
+	while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
+	{			
+		pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+		xmitframe_plist = get_next(xmitframe_plist);
+
+		switch(pxmitframe->attrib.priority)
+		{
+			case 1:
+			case 2:
+				wmmps_ac = psta->uapsd_bk&BIT(1);
+				break;
+			case 4:
+			case 5:
+				wmmps_ac = psta->uapsd_vi&BIT(1);
+				break;
+			case 6:
+			case 7:
+				wmmps_ac = psta->uapsd_vo&BIT(1);
+				break;
+			case 0:
+			case 3:
+			default:
+				wmmps_ac = psta->uapsd_be&BIT(1);
+				break;	
+		}
+		
+		if(!wmmps_ac)
+			continue;
+
+		rtw_list_delete(&pxmitframe->list);
+
+		psta->sleepq_len--;
+		psta->sleepq_ac_len--;
+
+		if(psta->sleepq_ac_len>0)
+		{
+			pxmitframe->attrib.mdata = 1;
+			pxmitframe->attrib.eosp = 0;
+		}
+		else
+		{
+			pxmitframe->attrib.mdata = 0;
+			pxmitframe->attrib.eosp = 1;
+		}
+
+		pxmitframe->attrib.triggered = 1;
+
+/*
+		if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE)
+		{		
+			rtw_os_xmit_complete(padapter, pxmitframe);
+		}
+*/
+		rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+		if((psta->sleepq_ac_len==0) && (!psta->has_legacy_ac) && (wmmps_ac))
+		{
+#ifdef CONFIG_TDLS
+			if(psta->tdls_sta_state & TDLS_LINKED_STATE )
+			{
+				//_exit_critical_bh(&psta->sleep_q.lock, &irqL);
+				_exit_critical_bh(&pxmitpriv->lock, &irqL);
+				return;
+			}
+#endif //CONFIG_TDLS
+			pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+			//DBG_871X("wakeup to xmit, qlen==0, update_BCNTIM, tim=%x\n", pstapriv->tim_bitmap);
+			//upate BCN for TIM IE
+			//update_BCNTIM(padapter);
+			update_beacon(padapter, _TIM_IE_, NULL, _FALSE);
+			//update_mask = BIT(0);
+		}
+	
+	}	
+	
+	//_exit_critical_bh(&psta->sleep_q.lock, &irqL);	
+	_exit_critical_bh(&pxmitpriv->lock, &irqL);
+
+}
+
+#endif
+
+#ifdef CONFIG_XMIT_THREAD_MODE
+void enqueue_pending_xmitbuf(
+	struct xmit_priv *pxmitpriv,
+	struct xmit_buf *pxmitbuf)
+{
+	_irqL irql;
+	_queue *pqueue;
+	_adapter *pri_adapter = pxmitpriv->adapter;
+
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	_enter_critical_bh(&pqueue->lock, &irql);
+	rtw_list_delete(&pxmitbuf->list);
+	rtw_list_insert_tail(&pxmitbuf->list, get_list_head(pqueue));
+	_exit_critical_bh(&pqueue->lock, &irql);
+
+
+
+#if defined(CONFIG_SDIO_HCI) && defined(CONFIG_CONCURRENT_MODE)
+	if (pri_adapter->adapter_type > PRIMARY_ADAPTER)
+		pri_adapter = pri_adapter->pbuddy_adapter;
+#endif  //SDIO_HCI + CONCURRENT
+	_rtw_up_sema(&(pri_adapter->xmitpriv.xmit_sema));
+
+}
+
+struct xmit_buf* dequeue_pending_xmitbuf(
+	struct xmit_priv *pxmitpriv)
+{
+	_irqL irql;
+	struct xmit_buf *pxmitbuf;
+	_queue *pqueue;
+
+
+	pxmitbuf = NULL;
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	_enter_critical_bh(&pqueue->lock, &irql);
+
+	if (_rtw_queue_empty(pqueue) == _FALSE)
+	{
+		_list *plist, *phead;
+
+		phead = get_list_head(pqueue);
+		plist = get_next(phead);
+		pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
+		rtw_list_delete(&pxmitbuf->list);
+	}
+
+	_exit_critical_bh(&pqueue->lock, &irql);
+
+	return pxmitbuf;
+}
+
+struct xmit_buf* dequeue_pending_xmitbuf_under_survey(
+	struct xmit_priv *pxmitpriv)
+{
+	_irqL irql;
+	struct xmit_buf *pxmitbuf;
+#ifdef CONFIG_USB_HCI	
+	struct xmit_frame *pxmitframe;
+#endif 
+	_queue *pqueue;
+
+
+	pxmitbuf = NULL;
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	_enter_critical_bh(&pqueue->lock, &irql);
+
+	if (_rtw_queue_empty(pqueue) == _FALSE)
+	{
+		_list *plist, *phead;
+		u8 type;
+
+		phead = get_list_head(pqueue);
+		plist = phead;
+		do {
+			plist = get_next(plist);
+				if (plist == phead) break;
+			
+			pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
+
+#ifdef CONFIG_USB_HCI
+			pxmitframe = (struct xmit_frame*)pxmitbuf->priv_data;
+			if(pxmitframe)
+			{
+				type = GetFrameSubType(pxmitbuf->pbuf + TXDESC_SIZE + pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
+			}
+			else
+			{
+				DBG_871X("%s, !!!ERROR!!! For USB, TODO ITEM \n", __FUNCTION__);
+			}
+#else
+			type = GetFrameSubType(pxmitbuf->pbuf + TXDESC_OFFSET);
+#endif
+
+			if ((type == WIFI_PROBEREQ) ||
+				(type == WIFI_DATA_NULL) ||
+				(type == WIFI_QOS_DATA_NULL))
+			{
+				rtw_list_delete(&pxmitbuf->list);
+				break;
+			}
+			pxmitbuf = NULL;
+		} while (1);
+	}
+
+	_exit_critical_bh(&pqueue->lock, &irql);
+
+	return pxmitbuf;
+}
+
+sint check_pending_xmitbuf(
+	struct xmit_priv *pxmitpriv)
+{
+	_queue *pqueue;
+
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	if(_rtw_queue_empty(pqueue) == _FALSE)
+		return _TRUE;
+	else
+		return _FALSE;
+}
+
+thread_return rtw_xmit_thread(thread_context context)
+{
+	s32 err;
+	PADAPTER padapter;
+
+
+	err = _SUCCESS;
+	padapter = (PADAPTER)context;
+
+	thread_enter("RTW_XMIT_THREAD");
+
+	do {
+		err = rtw_hal_xmit_thread_handler(padapter);
+		flush_signals_thread();
+	} while (_SUCCESS == err);
+
+	_rtw_up_sema(&padapter->xmitpriv.terminate_xmitthread_sema);
+
+	thread_exit();
+}
+#endif
+
+void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms)
+{
+	sctx->timeout_ms = timeout_ms;
+	sctx->submit_time= rtw_get_current_time();
+#ifdef PLATFORM_LINUX /* TODO: add condition wating interface for other os */
+	init_completion(&sctx->done);
+#endif
+	sctx->status = RTW_SCTX_SUBMITTED;
+}
+
+int rtw_sctx_wait(struct submit_ctx *sctx)
+{
+	int ret = _FAIL;
+	unsigned long expire; 
+	int status = 0;
+
+#ifdef PLATFORM_LINUX
+	expire= sctx->timeout_ms ? msecs_to_jiffies(sctx->timeout_ms) : MAX_SCHEDULE_TIMEOUT;
+	if (!wait_for_completion_timeout(&sctx->done, expire)) {
+		/* timeout, do something?? */
+		status = RTW_SCTX_DONE_TIMEOUT;
+		DBG_871X("%s timeout\n", __func__);	
+	} else {
+		status = sctx->status;
+	}
+#endif
+
+	if (status == RTW_SCTX_DONE_SUCCESS) {
+		ret = _SUCCESS;
+	}
+
+	return ret;
+}
+
+bool rtw_sctx_chk_waring_status(int status)
+{
+	switch(status) {
+	case RTW_SCTX_DONE_UNKNOWN:
+	case RTW_SCTX_DONE_BUF_ALLOC:
+	case RTW_SCTX_DONE_BUF_FREE:
+
+	case RTW_SCTX_DONE_DRV_STOP:
+	case RTW_SCTX_DONE_DEV_REMOVE:
+		return _TRUE;
+	default:
+		return _FALSE;
+	}
+}
+
+void rtw_sctx_done_err(struct submit_ctx **sctx, int status)
+{
+	if (*sctx) {
+		if (rtw_sctx_chk_waring_status(status))
+			DBG_871X("%s status:%d\n", __func__, status);
+		(*sctx)->status = status;
+		#ifdef PLATFORM_LINUX
+		complete(&((*sctx)->done));
+		#endif
+		*sctx = NULL;
+	}
+}
+
+void rtw_sctx_done(struct submit_ctx **sctx)
+{
+	rtw_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS);
+}
+
+#ifdef CONFIG_XMIT_ACK
+
+#ifdef CONFIG_XMIT_ACK_POLLING
+s32 c2h_evt_hdl(_adapter *adapter, struct c2h_evt_hdr *c2h_evt, c2h_id_filter filter);
+
+/**
+ * rtw_ack_tx_polling -
+ * @pxmitpriv: xmit_priv to address ack_tx_ops
+ * @timeout_ms: timeout msec
+ *
+ * Init ack_tx_ops and then do c2h_evt_hdl() and polling ack_tx_ops repeatedly
+ * till tx report or timeout
+ * Returns: _SUCCESS if TX report ok, _FAIL for others
+ */
+int rtw_ack_tx_polling(struct xmit_priv *pxmitpriv, u32 timeout_ms)
+{
+	int ret = _FAIL;
+	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+	_adapter *adapter = container_of(pxmitpriv, _adapter, xmitpriv);
+
+	pack_tx_ops->submit_time = rtw_get_current_time();
+	pack_tx_ops->timeout_ms = timeout_ms;
+	pack_tx_ops->status = RTW_SCTX_SUBMITTED;
+
+	do {
+		c2h_evt_hdl(adapter, NULL, rtw_hal_c2h_id_filter_ccx(adapter));
+		if (pack_tx_ops->status != RTW_SCTX_SUBMITTED)
+			break;
+
+		if (adapter->bDriverStopped) {
+			pack_tx_ops->status = RTW_SCTX_DONE_DRV_STOP;
+			break;
+		}
+		if (adapter->bSurpriseRemoved) {
+			pack_tx_ops->status = RTW_SCTX_DONE_DEV_REMOVE;
+			break;
+		}
+		
+		rtw_msleep_os(10);
+	} while (rtw_get_passing_time_ms(pack_tx_ops->submit_time) < timeout_ms);
+
+	if (pack_tx_ops->status == RTW_SCTX_SUBMITTED) {
+		pack_tx_ops->status = RTW_SCTX_DONE_TIMEOUT;
+		DBG_871X("%s timeout\n", __func__);
+	}
+
+	if (pack_tx_ops->status == RTW_SCTX_DONE_SUCCESS)
+		ret = _SUCCESS;
+
+	return ret;
+}
+#endif
+
+int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms)
+{
+#ifdef CONFIG_XMIT_ACK_POLLING
+	return rtw_ack_tx_polling(pxmitpriv, timeout_ms);
+#else
+	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+
+	pack_tx_ops->submit_time = rtw_get_current_time();
+	pack_tx_ops->timeout_ms = timeout_ms;
+	pack_tx_ops->status = RTW_SCTX_SUBMITTED;
+
+	return rtw_sctx_wait(pack_tx_ops);
+#endif
+}
+
+void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status)
+{
+	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+	
+	if (pxmitpriv->ack_tx) {
+		rtw_sctx_done_err(&pack_tx_ops, status);
+	} else {
+		DBG_871X("%s ack_tx not set\n", __func__);
+	}
+}
+#endif //CONFIG_XMIT_ACK
+