staging: r8188eu: Add files for new driver - part 4

This commit adds files core/rtw_led.c, core/rtw_mlme.c, core/rtw_mlme_ext.c,
core/rtw_mp.c, core/rtw_mp_ioctl.c, and core/rtw_p2p.c

Signed-off-by: Larry Finger <Larry.Finger@lwfinger.net>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

6 files changed, 17184 insertions, 0 deletions

diff --git a/drivers/staging/rtl8188eu/core/rtw_led.c b/drivers/staging/rtl8188eu/core/rtw_led.c
new file mode 100644
index 0000000..2db2380
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_led.c
@@ -0,0 +1,1692 @@
+/******************************************************************************
+ *
+ * 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>
+#include "rtw_led.h"
+
+/*  */
+/*	Description: */
+/*		Callback function of LED BlinkTimer, */
+/*		it just schedules to corresponding BlinkWorkItem/led_blink_hdl */
+/*  */
+void BlinkTimerCallback(void *data)
+{
+	struct LED_871x *pLed = (struct LED_871x *)data;
+	struct adapter *padapter = pLed->padapter;
+
+	if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped))
+		return;
+
+	_set_workitem(&(pLed->BlinkWorkItem));
+}
+
+/*  */
+/*	Description: */
+/*		Callback function of LED BlinkWorkItem. */
+/*		We dispatch acture LED blink action according to LedStrategy. */
+/*  */
+void BlinkWorkItemCallback(struct work_struct *work)
+{
+	struct LED_871x *pLed = container_of(work, struct LED_871x, BlinkWorkItem);
+	BlinkHandler(pLed);
+}
+
+/*  */
+/*	Description: */
+/*		Reset status of LED_871x object. */
+/*  */
+void ResetLedStatus(struct LED_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. */
+
+	pLed->bLedNoLinkBlinkInProgress = false;
+	pLed->bLedLinkBlinkInProgress = false;
+	pLed->bLedStartToLinkBlinkInProgress = false;
+	pLed->bLedScanBlinkInProgress = false;
+}
+
+/*Description: */
+/*		Initialize an LED_871x object. */
+void InitLed871x(struct adapter *padapter, struct LED_871x *pLed, enum LED_PIN_871x LedPin)
+{
+	pLed->padapter = padapter;
+	pLed->LedPin = LedPin;
+
+	ResetLedStatus(pLed);
+
+	_init_timer(&(pLed->BlinkTimer), padapter->pnetdev, BlinkTimerCallback, pLed);
+
+	_init_workitem(&(pLed->BlinkWorkItem), BlinkWorkItemCallback, pLed);
+}
+
+
+/*  */
+/*	Description: */
+/*		DeInitialize an LED_871x object. */
+/*  */
+void DeInitLed871x(struct LED_871x *pLed)
+{
+	_cancel_workitem_sync(&(pLed->BlinkWorkItem));
+	_cancel_timer_ex(&(pLed->BlinkTimer));
+	ResetLedStatus(pLed);
+}
+
+/*  */
+/*	Description: */
+/*		Implementation of LED blinking behavior. */
+/*		It toggle off LED and schedule corresponding timer if necessary. */
+/*  */
+
+static void SwLedBlink(struct LED_871x *pLed)
+{
+	struct 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)) && (!pLed->bLedOn)) {
+			SwLedOn(padapter, pLed);
+		} else if ((check_fwstate(pmlmepriv, _FW_LINKED)) &&  pLed->bLedOn) {
+			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;
+		}
+	}
+}
+
+static void SwLedBlink1(struct LED_871x *pLed)
+{
+	struct 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));
+	}
+
+	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)) {
+				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)) {
+				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)) {
+				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)) {
+				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;
+	}
+}
+
+static void SwLedBlink2(struct LED_871x *pLed)
+{
+	struct 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)) {
+				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)) {
+				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)) {
+				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)) {
+				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;
+	}
+}
+
+static void SwLedBlink3(struct LED_871x *pLed)
+{
+	struct 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)) {
+				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)) {
+				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)) {
+				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)) {
+				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;
+	}
+}
+
+static void SwLedBlink4(struct LED_871x *pLed)
+{
+	struct adapter *padapter = pLed->padapter;
+	struct led_priv *ledpriv = &(padapter->ledpriv);
+	struct LED_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));
+}
+
+static void SwLedBlink5(struct LED_871x *pLed)
+{
+	struct adapter *padapter = pLed->padapter;
+	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));
+}
+
+static void SwLedBlink6(struct LED_871x *pLed)
+{
+	struct adapter *padapter = pLed->padapter;
+
+	/*  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"));
+}
+
+ /* ALPHA, added by chiyoko, 20090106 */
+static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+	struct led_priv *ledpriv = &(padapter->ledpriv);
+	struct LED_871x *pLed = &(ledpriv->SwLed0);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	switch (LedAction) {
+	case LED_CTL_POWER_ON:
+	case LED_CTL_START_TO_LINK:
+	case LED_CTL_NO_LINK:
+		if (!pLed->bLedNoLinkBlinkInProgress) {
+			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedLinkBlinkInProgress) {
+				_cancel_timer_ex(&(pLed->BlinkTimer));
+				pLed->bLedLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedBlinkInProgress) {
+				_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) {
+			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				_cancel_timer_ex(&(pLed->BlinkTimer));
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedBlinkInProgress) {
+				_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))) {
+			;
+		} else if (!pLed->bLedScanBlinkInProgress) {
+			if (IS_LED_WPS_BLINKING(pLed))
+				return;
+			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;
+			}
+			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) {
+			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				_cancel_timer_ex(&(pLed->BlinkTimer));
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedLinkBlinkInProgress) {
+				_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) {
+			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->bLedScanBlinkInProgress) {
+				_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) {
+			_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->bLedScanBlinkInProgress) {
+			_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(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+	struct led_priv *ledpriv = &(padapter->ledpriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct LED_871x *pLed = &(ledpriv->SwLed0);
+
+	switch (LedAction) {
+	case LED_CTL_SITE_SURVEY:
+		if (pmlmepriv->LinkDetectInfo.bBusyTraffic) {
+		} else if (!pLed->bLedScanBlinkInProgress) {
+			if (IS_LED_WPS_BLINKING(pLed))
+				return;
+
+			if (pLed->bLedBlinkInProgress) {
+				_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) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
+			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) {
+			if (pLed->bLedBlinkInProgress) {
+				_cancel_timer_ex(&(pLed->BlinkTimer));
+				pLed->bLedBlinkInProgress = false;
+			}
+			if (pLed->bLedScanBlinkInProgress) {
+				_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(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+	struct led_priv *ledpriv = &(padapter->ledpriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct LED_871x *pLed = &(ledpriv->SwLed0);
+
+	switch (LedAction) {
+	case LED_CTL_SITE_SURVEY:
+		if (pmlmepriv->LinkDetectInfo.bBusyTraffic) {
+		} else if (!pLed->bLedScanBlinkInProgress) {
+			if (IS_LED_WPS_BLINKING(pLed))
+				return;
+
+			if (pLed->bLedBlinkInProgress) {
+				_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) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
+			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) {
+			if (pLed->bLedBlinkInProgress) {
+				_cancel_timer_ex(&(pLed->BlinkTimer));
+				pLed->bLedBlinkInProgress = false;
+			}
+			if (pLed->bLedScanBlinkInProgress) {
+				_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(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+	struct led_priv *ledpriv = &(padapter->ledpriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct LED_871x *pLed = &(ledpriv->SwLed0);
+	struct LED_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) {
+			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedBlinkInProgress) {
+				_cancel_timer_ex(&(pLed->BlinkTimer));
+				pLed->bLedBlinkInProgress = false;
+			}
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				_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) {
+			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedBlinkInProgress) {
+				_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))) {
+		} else if (!pLed->bLedScanBlinkInProgress) {
+			if (IS_LED_WPS_BLINKING(pLed))
+				return;
+
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				_cancel_timer_ex(&(pLed->BlinkTimer));
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedBlinkInProgress) {
+				_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) {
+			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
+				return;
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				_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) {
+			if (pLed->bLedNoLinkBlinkInProgress) {
+				_cancel_timer_ex(&(pLed->BlinkTimer));
+				pLed->bLedNoLinkBlinkInProgress = false;
+			}
+			if (pLed->bLedBlinkInProgress) {
+				_cancel_timer_ex(&(pLed->BlinkTimer));
+				pLed->bLedBlinkInProgress = false;
+			}
+			if (pLed->bLedScanBlinkInProgress) {
+				_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(
+	struct adapter *padapter,
+	enum LED_CTL_MODE LedAction
+)
+{
+	struct led_priv *ledpriv = &(padapter->ledpriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct LED_871x *pLed = &(ledpriv->SwLed0);
+
+	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))) {
+		} else if (!pLed->bLedScanBlinkInProgress) {
+			if (pLed->bLedBlinkInProgress) {
+				_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) {
+			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(
+	struct adapter *padapter,
+	enum LED_CTL_MODE LedAction
+)
+{
+	struct led_priv *ledpriv = &(padapter->ledpriv);
+	struct LED_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(struct LED_871x *pLed)
+{
+	struct adapter *padapter = pLed->padapter;
+	struct led_priv *ledpriv = &(padapter->ledpriv);
+
+	if ((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:
+		break;
+	}
+}
+
+void LedControl871x(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+	struct led_priv *ledpriv = &(padapter->ledpriv);
+
+       if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped) ||
+	   (!padapter->hw_init_completed))
+		return;
+
+	if (!ledpriv->bRegUseLed)
+		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:
+		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));
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme.c b/drivers/staging/rtl8188eu/core/rtw_mlme.c
new file mode 100644
index 0000000..ea66071
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme.c
@@ -0,0 +1,2442 @@
+/******************************************************************************
+ *
+ * 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 <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>
+#include <usb_osintf.h>
+
+extern unsigned char	MCS_rate_2R[16];
+extern unsigned char	MCS_rate_1R[16];
+
+int	_rtw_init_mlme_priv (struct adapter *padapter)
+{
+	int	i;
+	u8	*pbuf;
+	struct wlan_network	*pnetwork;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	int	res = _SUCCESS;
+
+_func_enter_;
+
+	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */
+
+	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(struct 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);
+
+	rtw_init_mlme_timer(padapter);
+
+exit:
+
+_func_exit_;
+
+	return res;
+}
+
+static 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));
+}
+
+#if defined (CONFIG_88EU_AP_MODE)
+static void rtw_free_mlme_ie_data(u8 **ppie, u32 *plen)
+{
+	kfree(*ppie);
+	*plen = 0;
+	*ppie = NULL;
+}
+
+void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
+{
+	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);
+}
+#else
+void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
+{
+}
+#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_;
+}
+
+int	_rtw_enqueue_network(struct __queue *queue, struct wlan_network *pnetwork)
+{
+	unsigned long 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(struct __queue *queue)
+{
+	unsigned long irql;
+
+	struct wlan_network *pnetwork;
+
+_func_enter_;
+
+	_enter_critical_bh(&queue->lock, &irql);
+
+	if (_rtw_queue_empty(queue)) {
+		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) */
+{
+	unsigned long	irql;
+	struct	wlan_network	*pnetwork;
+	struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+	struct list_head *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;
+	unsigned long irql;
+	struct __queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+_func_enter_;
+
+	if (pnetwork == NULL)
+		goto exit;
+
+	if (pnetwork->fixed)
+		goto exit;
+	curr_time = rtw_get_current_time();
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) ||
+	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)))
+		lifetime = 1;
+	if (!isfreeall) {
+		delta_time = (curr_time - pnetwork->last_scanned)/HZ;
+		if (delta_time < lifetime)/*  unit:sec */
+			goto exit;
+	}
+	_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--;
+	_exit_critical_bh(&free_queue->lock, &irql);
+
+exit:
+_func_exit_;
+}
+
+void _rtw_free_network_nolock(struct	mlme_priv *pmlmepriv, struct wlan_network *pnetwork)
+{
+	struct __queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+_func_enter_;
+	if (pnetwork == NULL)
+		goto exit;
+	if (pnetwork->fixed)
+		goto exit;
+	rtw_list_delete(&(pnetwork->list));
+	rtw_list_insert_tail(&(pnetwork->list), get_list_head(free_queue));
+	pmlmepriv->num_of_scanned--;
+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(struct __queue *scanned_queue, u8 *addr)
+{
+	struct list_head *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;
+	}
+	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:
+_func_exit_;
+	return pnetwork;
+}
+
+
+void _rtw_free_network_queue(struct adapter *padapter, u8 isfreeall)
+{
+	unsigned long irql;
+	struct list_head *phead, *plist;
+	struct wlan_network *pnetwork;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct __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_;
+}
+
+int rtw_if_up(struct adapter *padapter)
+{
+	int 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(struct wlan_bssid_ex *bss)
+{
+	__le16	val;
+_func_enter_;
+
+	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 (struct 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_;
+}
+
+static struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv)
+{
+	struct	wlan_network	*pnetwork;
+_func_enter_;
+	pnetwork = _rtw_alloc_network(pmlmepriv);
+_func_exit_;
+	return pnetwork;
+}
+
+static void rtw_free_network_nolock(struct mlme_priv *pmlmepriv,
+				    struct wlan_network *pnetwork)
+{
+_func_enter_;
+	_rtw_free_network_nolock(pmlmepriv, pnetwork);
+_func_exit_;
+}
+
+
+void rtw_free_network_queue(struct 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(struct __queue *scanned_queue, u8 *addr)
+{
+	struct	wlan_network *pnetwork = _rtw_find_network(scanned_queue, addr);
+
+	return pnetwork;
+}
+
+int rtw_is_same_ibss(struct 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;
+}
+
+static int is_same_ess(struct wlan_bssid_ex *a, struct wlan_bssid_ex *b)
+{
+	return (a->Ssid.SsidLength == b->Ssid.SsidLength) &&
+	       _rtw_memcmp(a->Ssid.Ssid, b->Ssid.Ssid, a->Ssid.SsidLength);
+}
+
+int is_same_network(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst)
+{
+	 u16 s_cap, d_cap;
+	__le16 le_scap, le_dcap;
+
+_func_enter_;
+	memcpy((u8 *)&le_scap, rtw_get_capability_from_ie(src->IEs), 2);
+	memcpy((u8 *)&le_dcap, rtw_get_capability_from_ie(dst->IEs), 2);
+
+
+	s_cap = le16_to_cpu(le_scap);
+	d_cap = le16_to_cpu(le_dcap);
+
+_func_exit_;
+
+	return ((src->Ssid.SsidLength == dst->Ssid.SsidLength) &&
+		((_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(struct __queue *scanned_queue)
+{
+	struct list_head *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) {
+			if (oldest == NULL || time_after(oldest->last_scanned, pwlan->last_scanned))
+				oldest = pwlan;
+		}
+
+		plist = get_next(plist);
+	}
+_func_exit_;
+	return oldest;
+}
+
+void update_network(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
+	struct adapter *padapter, bool update_ie)
+{
+	long rssi_ori = dst->Rssi;
+	u8 sq_smp = src->PhyInfo.SignalQuality;
+	u8 ss_final;
+	u8 sq_final;
+	long rssi_final;
+
+_func_enter_;
+	rtw_hal_antdiv_rssi_compared(padapter, dst, src); /* this will update src.Rssi, need consider again */
+
+	/* 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)
+		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;
+
+_func_exit_;
+}
+
+static void update_current_network(struct adapter *adapter, struct wlan_bssid_ex *pnetwork)
+{
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+
+_func_enter_;
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) &&
+	    (is_same_network(&(pmlmepriv->cur_network.network), pnetwork))) {
+		update_network(&(pmlmepriv->cur_network.network), pnetwork, adapter, true);
+		rtw_update_protection(adapter, (pmlmepriv->cur_network.network.IEs) + sizeof(struct ndis_802_11_fixed_ie),
+				      pmlmepriv->cur_network.network.IELength);
+	}
+_func_exit_;
+}
+
+/*
+Caller must hold pmlmepriv->lock first.
+*/
+void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *target)
+{
+	unsigned long irql;
+	struct list_head *plist, *phead;
+	u32	bssid_ex_sz;
+	struct mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+	struct __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 (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 */
+			pnetwork = oldest;
+
+			rtw_hal_get_def_var(adapter, HAL_DEF_CURRENT_ANTENNA, &(target->PhyInfo.Optimum_antenna));
+			memcpy(&(pnetwork->network), target,  get_wlan_bssid_ex_sz(target));
+			/*  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;
+			rtw_hal_get_def_var(adapter, HAL_DEF_CURRENT_ANTENNA, &(target->PhyInfo.Optimum_antenna));
+			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_;
+}
+
+static void rtw_add_network(struct adapter *adapter,
+			    struct wlan_bssid_ex *pnetwork)
+{
+_func_enter_;
+#if defined(CONFIG_88EU_P2P)
+	rtw_wlan_bssid_ex_remove_p2p_attr(pnetwork, P2P_ATTR_GROUP_INFO);
+#endif
+	update_current_network(adapter, pnetwork);
+	rtw_update_scanned_network(adapter, pnetwork);
+_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 */
+static int rtw_is_desired_network(struct 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_88E("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(struct 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(struct adapter	*adapter, u8 *pbuf)
+{
+	unsigned long  irql;
+	u32 len;
+	struct wlan_bssid_ex *pnetwork;
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+
+_func_enter_;
+
+	pnetwork = (struct wlan_bssid_ex *)pbuf;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_survey_event_callback, ssid=%s\n",  pnetwork->Ssid.Ssid));
+
+	len = get_wlan_bssid_ex_sz(pnetwork);
+	if (len > (sizeof(struct 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) {
+		if (_rtw_memcmp(&(pmlmepriv->cur_network.network.MacAddress), pnetwork->MacAddress, ETH_ALEN)) {
+			struct wlan_network *ibss_wlan = NULL;
+			unsigned long	irql;
+
+			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) {
+				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(struct adapter	*adapter, u8 *pbuf)
+{
+	unsigned long  irql;
+	struct	mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext;
+
+_func_enter_;
+	_enter_critical_bh(&pmlmepriv->lock, &irql);
+
+	if (pmlmepriv->wps_probe_req_ie) {
+		pmlmepriv->wps_probe_req_ie_len = 0;
+		kfree(pmlmepriv->wps_probe_req_ie);
+		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)));
+	}
+
+	rtw_set_signal_stat_timer(&adapter->recvpriv);
+
+	if (pmlmepriv->to_join) {
+		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 {
+					struct wlan_bssid_ex    *pdev_network = &(adapter->registrypriv.dev_network);
+					u8 *pibss = adapter->registrypriv.dev_network.MacAddress;
+
+					_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(struct ndis_802_11_ssid));
+					memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct 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;
+			s_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+			if (_SUCCESS == s_ret) {
+			     _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_88E("try_to_join, but select scanning queue fail, to_roaming:%d\n", pmlmepriv->to_roaming);
+				if (pmlmepriv->to_roaming != 0) {
+					if (--pmlmepriv->to_roaming == 0 ||
+					    _SUCCESS != rtw_sitesurvey_cmd(adapter, &pmlmepriv->assoc_ssid, 1, NULL, 0)) {
+						pmlmepriv->to_roaming = 0;
+						rtw_free_assoc_resources(adapter, 1);
+						rtw_indicate_disconnect(adapter);
+					} else {
+						pmlmepriv->to_join = true;
+					}
+				}
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+			}
+		}
+	}
+
+	indicate_wx_scan_complete_event(adapter);
+
+	_exit_critical_bh(&pmlmepriv->lock, &irql);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+		p2p_ps_wk_cmd(adapter, P2P_PS_SCAN_DONE, 0);
+
+	rtw_os_xmit_schedule(adapter);
+
+	pmlmeext = &adapter->mlmeextpriv;
+	if (pmlmeext->sitesurvey_res.bss_cnt == 0)
+		rtw_hal_sreset_reset(adapter);
+_func_exit_;
+}
+
+void rtw_dummy_event_callback(struct adapter *adapter , u8 *pbuf)
+{
+}
+
+void rtw_fwdbg_event_callback(struct adapter *adapter , u8 *pbuf)
+{
+}
+
+static void free_scanqueue(struct	mlme_priv *pmlmepriv)
+{
+	unsigned long irql, irql0;
+	struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+	struct __queue *scan_queue = &pmlmepriv->scanned_queue;
+	struct list_head *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(struct adapter *adapter, int lock_scanned_queue)
+{
+	unsigned long 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;
+
+_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=%pM ssid=%s\n",
+		 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);
+
+		_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)))
+		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(struct adapter *padapter)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+_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)) {
+		set_fwstate(pmlmepriv, _FW_LINKED);
+
+		rtw_led_control(padapter, LED_CTL_LINK);
+
+		rtw_os_indicate_connect(padapter);
+	}
+
+	pmlmepriv->to_roaming = 0;
+
+	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(struct adapter *padapter)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+_func_enter_;
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_disconnect\n"));
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING | WIFI_UNDER_WPS);
+
+
+	if (pmlmepriv->to_roaming > 0)
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) ||
+	    (pmlmepriv->to_roaming <= 0)) {
+		rtw_os_indicate_disconnect(padapter);
+
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+		rtw_led_control(padapter, LED_CTL_NO_LINK);
+		rtw_clear_scan_deny(padapter);
+	}
+	p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+
+	rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 1);
+
+_func_exit_;
+}
+
+inline void rtw_indicate_scan_done(struct adapter *padapter, bool aborted)
+{
+	rtw_os_indicate_scan_done(padapter, aborted);
+}
+
+void rtw_scan_abort(struct adapter *adapter)
+{
+	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_88E(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_88E(FUNC_NDEV_FMT"waiting for scan_abort time out!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+		rtw_indicate_scan_done(adapter, true);
+	}
+	pmlmeext->scan_abort = false;
+}
+
+static struct sta_info *rtw_joinbss_update_stainfo(struct 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_88E("%s\n", __func__);
+		psta->aid  = pnetwork->join_res;
+			psta->mac_id = 0;
+		/* 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;
+			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;
+				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(struct 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_88E("%s\n", __func__);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+		 ("\nfw_state:%x, BSSID:%pM\n",
+		 get_fwstate(pmlmepriv), pnetwork->network.MacAddress));
+
+
+	/*  why not use ptarget_wlan?? */
+	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;
+	memcpy(&cur_network->network.IEs[0], &ptarget_wlan->network.IEs[0], MAX_IE_SZ);
+
+	cur_network->aid = pnetwork->join_res;
+
+
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+	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);
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+
+	/* 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(struct ndis_802_11_fixed_ie),
+			      (cur_network->network.IELength));
+	rtw_update_ht_cap(padapter, cur_network->network.IEs, cur_network->network.IELength);
+}
+
+/* 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). */
+
+void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf)
+{
+	unsigned long irql, irql2;
+	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_;
+
+	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(struct wlan_bssid_ex)) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n ***joinbss_evt_callback return a wrong bss ***\n\n"));
+		goto ignore_nolock;
+	}
+
+	_enter_critical_bh(&pmlmepriv->lock, &irql);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\nrtw_joinbss_event_callback!! _enter_critical\n"));
+
+	if (pnetwork->join_res > 0) {
+		_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irql);
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+			/* s1. find ptarget_wlan */
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				if (the_same_macaddr) {
+					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);
+
+		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 */
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+	}
+
+ignore_joinbss_callback:
+	_exit_critical_bh(&pmlmepriv->lock, &irql);
+ignore_nolock:
+_func_exit_;
+}
+
+void rtw_joinbss_event_callback(struct 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);
+
+_func_exit_;
+}
+
+static u8 search_max_mac_id(struct adapter *padapter)
+{
+	u8 mac_id;
+#if defined (CONFIG_88EU_AP_MODE)
+	u8 aid;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+#endif
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+#if defined (CONFIG_88EU_AP_MODE)
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		for (aid = (pstapriv->max_num_sta); aid > 0; aid--) {
+			if (pstapriv->sta_aid[aid-1] != 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;
+		}
+	}
+	return mac_id;
+}
+
+/* FOR AP , AD-HOC mode */
+void rtw_stassoc_hw_rpt(struct adapter *adapter, struct sta_info *psta)
+{
+	u16 media_status;
+	u8 macid;
+
+	if (psta == NULL)
+		return;
+
+	macid = search_max_mac_id(adapter);
+	rtw_hal_set_hwreg(adapter, HW_VAR_TX_RPT_MAX_MACID, (u8 *)&macid);
+	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(struct adapter *adapter, u8 *pbuf)
+{
+	unsigned long 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_88EU_AP_MODE)
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+		if (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;
+	DBG_88E("%s\n", __func__);
+	/* 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)) ||
+	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))) {
+		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);
+exit:
+_func_exit_;
+}
+
+void rtw_stadel_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+	unsigned long irql, irql2;
+	int mac_id = -1;
+	struct sta_info *psta;
+	struct wlan_network *pwlan = NULL;
+	struct 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_88E("%s(mac_id=%d)=%pM\n", __func__, mac_id, 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))
+		return;
+
+	mlmeext_sta_del_event_callback(adapter);
+
+	_enter_critical_bh(&pmlmepriv->lock, &irql2);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		if (pmlmepriv->to_roaming > 0)
+			pmlmepriv->to_roaming--; /*  this stadel_event is caused by roaming, decrease to_roaming */
+		else if (pmlmepriv->to_roaming == 0)
+			pmlmepriv->to_roaming = adapter->registrypriv.max_roaming_times;
+
+		if (*((unsigned short *)(pstadel->rsvd)) != WLAN_REASON_EXPIRATION_CHK)
+			pmlmepriv->to_roaming = 0; /*  don't roam */
+
+		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);
+		_rtw_roaming(adapter, tgt_network);
+	}
+	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) */
+			_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irql);
+			/* free old ibss network */
+			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;
+
+			memcpy(pdev_network, &tgt_network->network, get_wlan_bssid_ex_sz(&tgt_network->network));
+
+			_rtw_memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
+			memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct 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(struct adapter *padapter, u8 *pbuf)
+{
+_func_enter_;
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_cpwm_event_callback !!!\n"));
+_func_exit_;
+}
+
+/*
+* _rtw_join_timeout_handler - Timeout/faliure handler for CMD JoinBss
+* @adapter: pointer to struct adapter structure
+*/
+void _rtw_join_timeout_handler (struct adapter *adapter)
+{
+	unsigned long irql;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	int do_join_r;
+
+_func_enter_;
+
+	DBG_88E("%s, fw_state=%x\n", __func__, get_fwstate(pmlmepriv));
+
+	if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+		return;
+
+
+	_enter_critical_bh(&pmlmepriv->lock, &irql);
+
+	if (pmlmepriv->to_roaming > 0) { /*  join timeout caused by roaming */
+		while (1) {
+			pmlmepriv->to_roaming--;
+			if (pmlmepriv->to_roaming != 0) { /* try another , */
+				DBG_88E("%s try another roaming\n", __func__);
+				do_join_r = rtw_do_join(adapter);
+				if (_SUCCESS != do_join_r) {
+					DBG_88E("%s roaming do_join return %d\n", __func__ , do_join_r);
+					continue;
+				}
+				break;
+			} else {
+				DBG_88E("%s We've try roaming but fail\n", __func__);
+				rtw_indicate_disconnect(adapter);
+				break;
+			}
+		}
+	} else {
+		rtw_indicate_disconnect(adapter);
+		free_scanqueue(pmlmepriv);/*  */
+	}
+	_exit_critical_bh(&pmlmepriv->lock, &irql);
+_func_exit_;
+}
+
+/*
+* rtw_scan_timeout_handler - Timeout/Faliure handler for CMD SiteSurvey
+* @adapter: pointer to struct adapter structure
+*/
+void rtw_scan_timeout_handler (struct adapter *adapter)
+{
+	unsigned long irql;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	DBG_88E(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(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	/* auto site survey per 60sec */
+	if (pmlmepriv->scan_interval > 0) {
+		pmlmepriv->scan_interval--;
+		if (pmlmepriv->scan_interval == 0) {
+			DBG_88E("%s\n", __func__);
+			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(struct adapter *adapter)
+{
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+
+	if (!adapter)
+		return;
+	if (!adapter->hw_init_completed)
+		return;
+	if ((adapter->bDriverStopped) || (adapter->bSurpriseRemoved))
+		return;
+	if (adapter->net_closed)
+		return;
+	rtw_dynamic_chk_wk_cmd(adapter);
+
+	if (pregistrypriv->wifi_spec == 1) {
+#ifdef CONFIG_88EU_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);
+		}
+	}
+
+	rcu_read_lock();
+
+	if (rcu_dereference(adapter->pnetdev->rx_handler_data) &&
+	    (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) == true)) {
+		/*  expire NAT2.5 entry */
+		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--;
+		}
+	}
+
+	rcu_read_unlock();
+}
+
+#define RTW_SCAN_RESULT_EXPIRE 2000
+
+/*
+* 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;
+	struct adapter *adapter = container_of(pmlmepriv, struct adapter, mlmepriv);
+
+
+	/* check bssid, if needed */
+	if (pmlmepriv->assoc_by_bssid) {
+		if (!_rtw_memcmp(competitor->network.MacAddress, pmlmepriv->assoc_bssid, ETH_ALEN))
+			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;
+
+	if (pmlmepriv->to_roaming) {
+		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;
+	}
+
+	if (*candidate == NULL || (*candidate)->network.Rssi < competitor->network.Rssi) {
+		*candidate = competitor;
+		updated = true;
+	}
+	if (updated) {
+		DBG_88E("[by_bssid:%u][assoc_ssid:%s]new candidate: %s(%pM rssi:%d\n",
+			pmlmepriv->assoc_by_bssid,
+			pmlmepriv->assoc_ssid.Ssid,
+			(*candidate)->network.Ssid.Ssid,
+			(*candidate)->network.MacAddress,
+			(int)(*candidate)->network.Rssi);
+		DBG_88E("[to_roaming:%u]\n", pmlmepriv->to_roaming);
+	}
+
+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)
+{
+	unsigned long	irql;
+	int ret;
+	struct list_head *phead;
+	struct adapter *adapter;
+	struct __queue *queue	= &(pmlmepriv->scanned_queue);
+	struct	wlan_network	*pnetwork = NULL;
+	struct	wlan_network	*candidate = NULL;
+	u8	supp_ant_div = false;
+
+_func_enter_;
+
+	_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irql);
+	phead = get_list_head(queue);
+	adapter = (struct 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", __func__));
+			ret = _FAIL;
+			goto exit;
+		}
+		pmlmepriv->pscanned = get_next(pmlmepriv->pscanned);
+		rtw_check_join_candidate(pmlmepriv, &candidate, pnetwork);
+	}
+	if (candidate == NULL) {
+		DBG_88E("%s: return _FAIL(candidate==NULL)\n", __func__);
+		ret = _FAIL;
+		goto exit;
+	} else {
+		DBG_88E("%s: candidate: %s(%pM ch:%u)\n", __func__,
+			candidate->network.Ssid.Ssid, candidate->network.MacAddress,
+			candidate->network.Configuration.DSConfig);
+	}
+
+
+	/*  check for situation of  _FW_LINKED */
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+		DBG_88E("%s: _FW_LINKED while ask_for_joinbss!!!\n", __func__);
+
+		rtw_disassoc_cmd(adapter, 0, true);
+		rtw_indicate_disconnect(adapter);
+		rtw_free_assoc_resources(adapter, 0);
+	}
+
+	rtw_hal_get_def_var(adapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &(supp_ant_div));
+	if (supp_ant_div) {
+		u8 cur_ant;
+		rtw_hal_get_def_var(adapter, HAL_DEF_CURRENT_ANTENNA, &(cur_ant));
+		DBG_88E("#### Opt_Ant_(%s), cur_Ant(%s)\n",
+			(2 == candidate->network.PhyInfo.Optimum_antenna) ? "A" : "B",
+			(2 == cur_ant) ? "A" : "B"
+		);
+	}
+
+	ret = rtw_joinbss_cmd(adapter, candidate);
+
+exit:
+	_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irql);
+
+_func_exit_;
+
+	return ret;
+}
+
+int rtw_set_auth(struct adapter *adapter, struct security_priv *psecuritypriv)
+{
+	struct	cmd_obj *pcmd;
+	struct	setauth_parm *psetauthparm;
+	struct	cmd_priv *pcmdpriv = &(adapter->cmdpriv);
+	int		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) {
+		kfree(pcmd);
+		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;
+}
+
+int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, int 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);
+	int	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) {
+		kfree(pcmd);
+		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;
+	pmlmepriv->key_mask |= BIT(psetkeyparm->keyid);
+	DBG_88E("==> 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;
+		memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen);
+		break;
+	case _WEP104_:
+		keylen = 13;
+		memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen);
+		break;
+	case _TKIP_:
+		keylen = 16;
+		memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen);
+		psetkeyparm->grpkey = 1;
+		break;
+	case _AES_:
+		keylen = 16;
+		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);
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+exit:
+_func_exit_;
+	return res;
+}
+
+/* adjust IEs for rtw_joinbss_cmd in WMM */
+int rtw_restruct_wmm_ie(struct 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 + 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(struct 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(struct 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++;
+		memcpy(&ie[ie_len], &psecuritypriv->PMKIDList[iEntry].PMKID, 16);
+
+		ie_len += 16;
+		ie[13] += 18;/* PMKID length = 2+16 */
+	}
+	return ie_len;
+}
+
+int rtw_restruct_sec_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len)
+{
+	u8 authmode;
+	uint	ielength;
+	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 */
+	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)) {
+		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 */
+		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);
+	}
+
+	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(struct adapter *adapter)
+{
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct eeprom_priv *peepriv = &adapter->eeprompriv;
+	struct wlan_bssid_ex    *pdev_network = &pregistrypriv->dev_network;
+	u8 *myhwaddr = myid(peepriv);
+
+_func_enter_;
+
+	memcpy(pdev_network->MacAddress, myhwaddr, ETH_ALEN);
+
+	memcpy(&pdev_network->Ssid, &pregistrypriv->ssid, sizeof(struct ndis_802_11_ssid));
+
+	pdev_network->Configuration.Length = sizeof(struct ndis_802_11_config);
+	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(struct adapter *adapter)
+{
+	int sz = 0;
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct wlan_bssid_ex    *pdev_network = &pregistrypriv->dev_network;
+	struct	security_priv *psecuritypriv = &adapter->securitypriv;
+	struct	wlan_network	*cur_network = &adapter->mlmepriv.cur_network;
+
+_func_enter_;
+
+	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 */
+
+	sz = rtw_generate_ie(pregistrypriv);
+	pdev_network->IELength = sz;
+	pdev_network->Length = get_wlan_bssid_ex_sz((struct 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(struct adapter *adapter)
+{
+_func_enter_;
+_func_exit_;
+}
+
+/* the fucntion is at passive_level */
+void rtw_joinbss_reset(struct adapter *padapter)
+{
+	u8	threshold;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+
+	/* todo: if you want to do something io/reg/hw setting before join_bss, please add code here */
+	pmlmepriv->num_FortyMHzIntolerant = 0;
+
+	pmlmepriv->num_sta_no_ht = 0;
+
+	phtpriv->ampdu_enable = false;/* reset to disabled */
+
+	/*  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));
+	}
+}
+
+/* the fucntion is >= passive_level */
+unsigned int rtw_restructure_ht_ie(struct adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len)
+{
+	u32 ielen, out_len;
+	enum ht_cap_ampdu_factor max_rx_ampdu_factor;
+	unsigned char *p;
+	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;
+	u32 rx_packet_offset, max_recvbuf_sz;
+
+
+	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;
+			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;
+
+		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);
+
+		/*
+		AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+		AMPDU_para [4:2]:Min MPDU Start Spacing
+		*/
+
+		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);
+
+
+		rtw_set_ie(out_ie+out_len, _HT_CAPABILITY_IE_,
+			   sizeof(struct rtw_ieee80211_ht_cap), (unsigned char *)&ht_capie, pout_len);
+
+		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;
+			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(struct adapter *padapter, u8 *pie, uint ie_len)
+{
+	u8 *p, max_ampdu_sz;
+	int len;
+	struct rtw_ieee80211_ht_cap *pht_capie;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	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_88E("+rtw_update_ht_cap()\n");
+
+	/* maybe needs check if ap supports rx ampdu. */
+	if ((!phtpriv->ampdu_enable) && (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(struct ndis_802_11_fixed_ie), _HT_CAPABILITY_IE_, &len, ie_len-sizeof(struct ndis_802_11_fixed_ie));
+	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); */
+		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
+	}
+	len = 0;
+	p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fixed_ie), _HT_ADD_INFO_IE_, &len, ie_len-sizeof(struct ndis_802_11_fixed_ie));
+
+	/* update cur_bwmode & cur_ch_offset */
+	if ((pregistrypriv->cbw40_enable) &&
+	    (le16_to_cpu(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
+				pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R[i];
+		}
+		/* 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 = (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & 0x0C) >> 2;
+	if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC)
+		DBG_88E("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__);
+
+	/*  Config current HT Protection mode. */
+	pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3;
+}
+
+void rtw_issue_addbareq_cmd(struct 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.NumTxOkInPeriod < 100))
+		return;
+
+	priority = pattrib->priority;
+
+	if (pattrib->psta)
+		psta = pattrib->psta;
+	else
+		psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+
+	if (psta == NULL)
+		return;
+
+	phtpriv = &psta->htpriv;
+
+	if ((phtpriv->ht_option) && (phtpriv->ampdu_enable)) {
+		issued = (phtpriv->agg_enable_bitmap>>priority)&0x1;
+		issued |= (phtpriv->candidate_tid_bitmap>>priority)&0x1;
+
+		if (0 == issued) {
+			DBG_88E("rtw_issue_addbareq_cmd, p=%d\n", priority);
+			psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority);
+			rtw_addbareq_cmd(padapter, (u8) priority, pattrib->ra);
+		}
+	}
+}
+
+void rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network)
+{
+	unsigned long 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(struct 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 < pmlmepriv->to_roaming) {
+		DBG_88E("roaming from %s(%pM length:%d\n",
+			pnetwork->network.Ssid.Ssid, pnetwork->network.MacAddress,
+			pnetwork->network.Ssid.SsidLength);
+		memcpy(&pmlmepriv->assoc_ssid, &pnetwork->network.Ssid, sizeof(struct ndis_802_11_ssid));
+
+		pmlmepriv->assoc_by_bssid = false;
+
+		while (1) {
+			do_join_r = rtw_do_join(padapter);
+			if (_SUCCESS == do_join_r) {
+				break;
+			} else {
+				DBG_88E("roaming do_join return %d\n", do_join_r);
+				pmlmepriv->to_roaming--;
+
+				if (0 < pmlmepriv->to_roaming) {
+					continue;
+				} else {
+					DBG_88E("%s(%d) -to roaming fail, indicate_disconnect\n", __func__, __LINE__);
+					rtw_indicate_disconnect(padapter);
+					break;
+				}
+			}
+		}
+	}
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
new file mode 100644
index 0000000..8b2ba26
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
@@ -0,0 +1,8481 @@
+/******************************************************************************
+ *
+ * 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 <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>
+
+static 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},
+};
+
+static 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},
+};
+
+
+static 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
+*********************************************************/
+unsigned char	MCS_rate_2R[16] = {0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+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 struct 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 struct rt_channel_plan_map	RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
+	/*  0x00 ~ 0x1F , Old Define ===== */
+	{0x02},	/* 0x00, RT_CHANNEL_DOMAIN_FCC */
+	{0x02},	/* 0x01, RT_CHANNEL_DOMAIN_IC */
+	{0x01},	/* 0x02, RT_CHANNEL_DOMAIN_ETSI */
+	{0x01},	/* 0x03, RT_CHANNEL_DOMAIN_SPAIN */
+	{0x01},	/* 0x04, RT_CHANNEL_DOMAIN_FRANCE */
+	{0x03},	/* 0x05, RT_CHANNEL_DOMAIN_MKK */
+	{0x03},	/* 0x06, RT_CHANNEL_DOMAIN_MKK1 */
+	{0x01},	/* 0x07, RT_CHANNEL_DOMAIN_ISRAEL */
+	{0x03},	/* 0x08, RT_CHANNEL_DOMAIN_TELEC */
+	{0x03},	/* 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN */
+	{0x00},	/* 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13 */
+	{0x02},	/* 0x0B, RT_CHANNEL_DOMAIN_TAIWAN */
+	{0x01},	/* 0x0C, RT_CHANNEL_DOMAIN_CHINA */
+	{0x02},	/* 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO */
+	{0x02},	/* 0x0E, RT_CHANNEL_DOMAIN_KOREA */
+	{0x02},	/* 0x0F, RT_CHANNEL_DOMAIN_TURKEY */
+	{0x01},	/* 0x10, RT_CHANNEL_DOMAIN_JAPAN */
+	{0x02},	/* 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS */
+	{0x01},	/* 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+	{0x00},	/* 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G */
+	{0x02},	/* 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS */
+	{0x00},	/* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */
+	{0x00},	/* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */
+	{0x03},	/* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+	{0x05},	/* 0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS */
+	{0x02},	/* 0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS */
+	{0x00},	/* 0x1A, */
+	{0x00},	/* 0x1B, */
+	{0x00},	/* 0x1C, */
+	{0x00},	/* 0x1D, */
+	{0x00},	/* 0x1E, */
+	{0x05},	/* 0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G */
+	/*  0x20 ~ 0x7F , New Define ===== */
+	{0x00},	/* 0x20, RT_CHANNEL_DOMAIN_WORLD_NULL */
+	{0x01},	/* 0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL */
+	{0x02},	/* 0x22, RT_CHANNEL_DOMAIN_FCC1_NULL */
+	{0x03},	/* 0x23, RT_CHANNEL_DOMAIN_MKK1_NULL */
+	{0x04},	/* 0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL */
+	{0x02},	/* 0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1 */
+	{0x00},	/* 0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1 */
+	{0x03},	/* 0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1 */
+	{0x00},	/* 0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1 */
+	{0x00},	/* 0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2 */
+	{0x00},	/* 0x2A, */
+	{0x00},	/* 0x2B, */
+	{0x00},	/* 0x2C, */
+	{0x00},	/* 0x2D, */
+	{0x00},	/* 0x2E, */
+	{0x00},	/* 0x2F, */
+	{0x00},	/* 0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3 */
+	{0x00},	/* 0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4 */
+	{0x00},	/* 0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5 */
+	{0x00},	/* 0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6 */
+	{0x02},	/* 0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7 */
+	{0x00},	/* 0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2 */
+	{0x00},	/* 0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3 */
+	{0x03},	/* 0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2 */
+	{0x03},	/* 0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3 */
+	{0x02},	/* 0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1 */
+	{0x00},	/* 0x3A, */
+	{0x00},	/* 0x3B, */
+	{0x00},	/* 0x3C, */
+	{0x00},	/* 0x3D, */
+	{0x00},	/* 0x3E, */
+	{0x00},	/* 0x3F, */
+	{0x02},	/* 0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2 */
+	{0x03},	/* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G */
+};
+
+static struct rt_channel_plan_map RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03}; /* 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(struct 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(struct adapter *padapter)
+{
+	struct	mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	return _SUCCESS;
+}
+
+static void init_mlme_ext_priv_value(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	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->oper_channel = pmlmeext->cur_channel ;
+	pmlmeext->oper_bwmode = pmlmeext->cur_bwmode;
+	pmlmeext->oper_ch_offset = pmlmeext->cur_ch_offset;
+	pmlmeext->retry = 0;
+
+	pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
+
+	memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
+	memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
+
+	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(struct 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(struct adapter *padapter, struct 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 },
+		{ -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(struct adapter *padapter, u8 ChannelPlan, struct rt_channel_info *channel_set)
+{
+	u8 index, chanset_size = 0;
+	u8 b2_4GBand = false;
+	u8 Index2G = 0;
+
+	_rtw_memset(channel_set, 0, sizeof(struct rt_channel_info) * MAX_CHANNEL_NUM);
+
+	if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
+		DBG_88E("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 (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_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++;
+		}
+	}
+	return chanset_size;
+}
+
+int	init_mlme_ext_priv(struct 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);
+
+	pmlmeext->padapter = padapter;
+
+	init_mlme_ext_priv_value(padapter);
+	pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
+
+	init_mlme_ext_timer(padapter);
+
+#ifdef CONFIG_88EU_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;
+
+
+	pmlmeext->active_keep_alive_check = true;
+
+	return res;
+}
+
+void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
+{
+	struct adapter *padapter = pmlmeext->padapter;
+
+	if (!padapter)
+		return;
+
+	if (padapter->bDriverStopped) {
+		_cancel_timer_ex(&pmlmeext->survey_timer);
+		_cancel_timer_ex(&pmlmeext->link_timer);
+		/* _cancel_timer_ex(&pmlmeext->ADDBA_timer); */
+	}
+}
+
+static void _mgt_dispatcher(struct 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(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	int index;
+	struct mlme_handler *ptable;
+#ifdef CONFIG_88EU_AP_MODE
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+#endif /* CONFIG_88EU_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 (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;
+
+	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 (psta != NULL) {
+		if (GetRetry(pframe)) {
+			if (precv_frame->u.hdr.attrib.seq_num == psta->RxMgmtFrameSeqNum) {
+				/* drop the duplicate management frame */
+				DBG_88E("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;
+	}
+
+#ifdef CONFIG_88EU_AP_MODE
+	switch (GetFrameSubType(pframe)) {
+	case WIFI_AUTH:
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+			ptable->func = &OnAuth;
+		else
+			ptable->func = &OnAuthClient;
+		/* fall through */
+	case WIFI_ASSOCREQ:
+	case WIFI_REASSOCREQ:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	case WIFI_PROBEREQ:
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+			_mgt_dispatcher(padapter, ptable, precv_frame);
+		else
+			_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	case WIFI_BEACON:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	case WIFI_ACTION:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	default:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+			rtw_hostapd_mlme_rx(padapter, precv_frame);
+		break;
+	}
+#else
+	_mgt_dispatcher(padapter, ptable, precv_frame);
+#endif
+}
+
+#ifdef CONFIG_88EU_P2P
+static u32 p2p_listen_state_process(struct adapter *padapter, unsigned char *da)
+{
+	bool response = true;
+
+	/*	do nothing if the device name is empty */
+	if (!padapter->wdinfo.device_name_len)
+		response = false;
+
+	if (response)
+		issue_probersp_p2p(padapter, da);
+
+	return _SUCCESS;
+}
+#endif /* CONFIG_88EU_P2P */
+
+
+/****************************************************************************
+
+Following are the callback functions for each subtype of the management frames
+
+*****************************************************************************/
+
+unsigned int OnProbeReq(struct 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);
+	struct 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_88EU_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	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)) {
+		/*	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 */
+		if (wifi_test_chk_rate == 1) {
+			is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len);
+			if (is_valid_p2p_probereq) {
+				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
+					/*  FIXME */
+					report_survey_event(padapter, precv_frame);
+					p2p_listen_state_process(padapter,  get_sa(pframe));
+
+					return _SUCCESS;
+				}
+
+				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+					goto _continue;
+			}
+		}
+	}
+
+_continue:
+#endif /* CONFIG_88EU_P2P */
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		return _SUCCESS;
+
+	if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    !check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE))
+		return _SUCCESS;
+
+	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)
+			goto _issue_probersp;
+
+		if ((ielen != 0 && !_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) &&
+		    pmlmepriv->cur_network.join_res)
+			issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
+	}
+	return _SUCCESS;
+}
+
+unsigned int OnProbeRsp(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+#ifdef CONFIG_88EU_P2P
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+#endif
+
+#ifdef CONFIG_88EU_P2P
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
+		if (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 (pwdinfo->nego_req_info.benable) {
+			DBG_88E("[%s] P2P State is GONEGO ING!\n", __func__);
+			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 (pwdinfo->invitereq_info.benable) {
+			DBG_88E("[%s] P2P_STATE_TX_INVITE_REQ!\n", __func__);
+			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);
+		return _SUCCESS;
+	}
+
+	return _SUCCESS;
+}
+
+unsigned int OnBeacon(struct 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;
+	struct wlan_bssid_ex *pbss;
+	int ret = _SUCCESS;
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		report_survey_event(padapter, precv_frame);
+		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 = (struct wlan_bssid_ex *)rtw_malloc(sizeof(struct 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));
+				}
+				kfree(pbss);
+			}
+
+			/* 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)) {
+			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+			if (psta != NULL) {
+				ret = rtw_check_bcn_info(padapter, pframe, len);
+				if (!ret) {
+						DBG_88E_LEVEL(_drv_info_, "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)
+					update_beacon_info(padapter, pframe, len, psta);
+				process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
+			}
+		} else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+			if (psta != 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)
+					update_beacon_info(padapter, pframe, len, psta);
+			} else {
+				/* allocate a new CAM entry for IBSS station */
+				cam_idx = allocate_fw_sta_entry(padapter);
+				if (cam_idx == 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(struct adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_88EU_AP_MODE
+	unsigned long irqL;
+	unsigned int	auth_mode, ie_len;
+	u16 seq;
+	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;
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	DBG_88E("+OnAuth\n");
+
+	sa = GetAddr2Ptr(pframe);
+
+	auth_mode = psecuritypriv->dot11AuthAlgrthm;
+	seq = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + 2));
+	algorithm = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN));
+
+	DBG_88E("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_88E("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 (!rtw_access_ctrl(padapter, sa)) {
+		status = _STATS_UNABLE_HANDLE_STA_;
+		goto auth_fail;
+	}
+
+	pstat = rtw_get_stainfo(pstapriv, sa);
+	if (pstat == NULL) {
+		/*  allocate a new one */
+		DBG_88E("going to alloc stainfo for sa=%pM\n", sa);
+		pstat = rtw_alloc_stainfo(pstapriv, sa);
+		if (pstat == NULL) {
+			DBG_88E(" 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;
+	} else {
+		_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+		if (!rtw_is_list_empty(&pstat->asoc_list)) {
+			rtw_list_delete(&pstat->asoc_list);
+			pstapriv->asoc_list_cnt--;
+		}
+		_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_88E("(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_88E("(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... */
+
+			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_88E("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_88E("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_88E("auth rejected because challenge failure!\n");
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+		} else {
+			DBG_88E("(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_88EU_AP_MODE
+	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;
+	memcpy(pstat->hwaddr, sa, 6);
+
+#ifdef CONFIG_88EU_AP_MODE
+	issue_auth(padapter, pstat, (unsigned short)status);
+#endif
+
+#endif
+	return _FAIL;
+}
+
+unsigned int OnAuthClient(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int	seq, len, status, 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_88E("%s\n", __func__);
+
+	/* 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;
+
+	seq	= le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 2));
+	status	= le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 4));
+
+	if (status != 0) {
+		DBG_88E("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;
+		}
+
+		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)
+				goto authclnt_fail;
+
+			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 */
+		goto authclnt_fail;
+	}
+
+	if (go2asoc) {
+		DBG_88E_LEVEL(_drv_info_, "auth success, start assoc\n");
+		start_clnt_assoc(padapter);
+		return _SUCCESS;
+	}
+authclnt_fail:
+	return _FAIL;
+}
+
+unsigned int OnAssocReq(struct adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_88EU_AP_MODE
+	unsigned long irqL;
+	u16 capab_info;
+	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);
+	struct 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_88EU_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8 p2p_status_code = P2P_STATUS_SUCCESS;
+	u8 *p2pie;
+	u32 p2pielen = 0;
+#endif /* CONFIG_88EU_P2P */
+
+	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_88E("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);
+
+	left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
+	pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
+
+
+	DBG_88E("%s\n", __func__);
+
+	/*  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;
+	}
+	pstat->capability = capab_info;
+	/* now parse all ieee802_11 ie to point to elems */
+	if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
+	    !elems.ssid) {
+		DBG_88E("STA %pM sent invalid association request\n",
+			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_88E("Rx a sta assoc-req which supported rate is empty!\n");
+		/*  use our own rate set as statoin used */
+		/* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
+		/* supportRateNum = AP_BSSRATE_LEN; */
+
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	} else {
+		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)) {
+				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;
+	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 (wpa_ie == NULL) {
+		if (elems.wps_ie) {
+			DBG_88E("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_88E("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_88E("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_88E("STA %pM: WPA/RSN IE in association "
+			"request, but AP don't support WPA/RSN\n", pstat->hwaddr);
+
+			status = WLAN_STATUS_INVALID_IE;
+
+			goto OnAssocReqFail;
+		}
+
+		if (elems.wps_ie) {
+			DBG_88E("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)
+			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;
+		}
+	}
+
+	/* 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;
+
+		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) && (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_88E("HT: %pM tried to "
+			"use TKIP with HT association\n", pstat->hwaddr);
+
+		/* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
+		/* goto OnAssocReqFail; */
+	}
+
+	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_88EU_P2P
+	pstat->is_p2p_device = false;
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , NULL, &p2pielen);
+		if (p2pie) {
+			pstat->is_p2p_device = true;
+			p2p_status_code = (u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat);
+			if (p2p_status_code > 0) {
+				pstat->p2p_status_code = p2p_status_code;
+				status = _STATS_CAP_FAIL_;
+				goto OnAssocReqFail;
+			}
+		}
+	}
+	pstat->p2p_status_code = p2p_status_code;
+#endif /* CONFIG_88EU_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_88E("  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_88E("  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_88E("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_88EU_AP_MODE
+		/* 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_88E("indicate_sta_join_event to upper layer - hostapd\n");
+		rtw_indicate_sta_assoc_event(padapter, pstat);
+
+		/* 3-(1) report sta add event */
+		report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
+#endif
+	}
+
+	return _SUCCESS;
+
+asoc_class2_error:
+
+#ifdef CONFIG_88EU_AP_MODE
+	issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
+#endif
+
+	return _FAIL;
+
+OnAssocReqFail:
+
+
+#ifdef CONFIG_88EU_AP_MODE
+	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_88EU_AP_MODE */
+
+	return _FAIL;
+}
+
+unsigned int OnAssocRsp(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	uint i;
+	int res;
+	unsigned short	status;
+	struct ndis_802_11_var_ie *pIE;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	/* struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); */
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+
+	DBG_88E("%s\n", __func__);
+
+	/* 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 */
+	status = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 2));
+	if (status > 0) {
+		DBG_88E("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(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	/* set slot time */
+	pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10)) ? 9 : 20;
+
+	/* AID */
+	pmlmeinfo->aid = (int)(le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
+	res = pmlmeinfo->aid;
+
+	/* 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 = (struct ndis_802_11_var_ie *)(pframe + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			if (_rtw_memcmp(pIE->data, WMM_PARA_OUI, 6))	/* WMM */
+				WMM_param_handler(padapter, pIE);
+			break;
+		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);
+	}
+
+	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(struct 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_88EU_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+	/* check A3 */
+	if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+#ifdef CONFIG_88EU_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_88EU_P2P */
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	DBG_88E("%s Reason code(%d)\n", __func__, reason);
+
+#ifdef CONFIG_88EU_AP_MODE
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		unsigned long irqL;
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		DBG_88E_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 = 0;
+
+			_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+			if (!rtw_is_list_empty(&psta->asoc_list)) {
+				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_88E_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(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	u16 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_88EU_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+	/* check A3 */
+	if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+#ifdef CONFIG_88EU_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_88EU_P2P */
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	DBG_88E("%s Reason code(%d)\n", __func__, reason);
+
+#ifdef CONFIG_88EU_AP_MODE
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		unsigned long 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_88E_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 = 0;
+
+			_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+			if (!rtw_is_list_empty(&psta->asoc_list)) {
+				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_88E_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(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	DBG_88E("%s\n", __func__);
+	return _SUCCESS;
+}
+
+unsigned int on_action_spct(struct 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;
+	u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+	u8 category;
+	u8 action;
+
+	DBG_88E(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:
+		break;
+	default:
+		break;
+	}
+
+exit:
+	return ret;
+}
+
+unsigned int OnAction_qos(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int OnAction_dls(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int OnAction_back(struct 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;
+	/* check RA matches or not */
+	if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))/* for if1, sta/ap mode */
+		return _SUCCESS;
+
+	DBG_88E("%s\n", __func__);
+
+	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 */
+		if (!pmlmeinfo->HT_enable)
+			return _SUCCESS;
+		action = frame_body[1];
+		DBG_88E("%s, action=%d\n", __func__, action);
+		switch (action) {
+		case RTW_WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
+			memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
+			process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), addr);
+
+			if (pmlmeinfo->bAcceptAddbaReq)
+				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 = RTW_GET_LE16(&frame_body[3]);
+			tid = ((frame_body[5] >> 2) & 0x7);
+			if (status == 0) {	/* successful */
+				DBG_88E("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);
+			}
+			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 = 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;
+			}
+			DBG_88E("%s(): DELBA: %x(%x)\n", __func__, pmlmeinfo->agg_enable_bitmap, reason_code);
+			/* todo: how to notify the host while receiving DELETE BA */
+			break;
+		default:
+			break;
+		}
+	}
+	return _SUCCESS;
+}
+
+#ifdef CONFIG_88EU_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;
+}
+
+void issue_p2p_GO_request(struct adapter *padapter, u8 *raddr)
+{
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	__be32 p2poui = cpu_to_be32(P2POUI);
+	u8 oui_subtype = P2P_GO_NEGO_REQ;
+	u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+	u8 wpsielen = 0, p2pielen = 0;
+	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 wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	DBG_88E("[%s] In\n", __func__);
+	/* 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;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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 */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	/*	Device Password ID */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+
+	if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
+	else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
+	else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC)
+		*(__be16 *)(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: */
+	*(__le16 *)(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: */
+	*(__le16 *)(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: */
+	*(__le16 *)(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: */
+	*(__le16 *)(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: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Availability Period */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+	p2pielen += 2;
+
+	/*	Availability Interval */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+	p2pielen += 2;
+
+
+	/*	Intended P2P Interface Address */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+	p2pielen += 2;
+
+	/*	Value: */
+	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);
+
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+	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 */
+
+	{
+		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];
+			}
+		}
+	}
+
+	/*	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) */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Config Method */
+	/*	This field should be big endian. Noted by P2P specification. */
+
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+
+	p2pielen += 2;
+
+	/*	Primary Device Type */
+	/*	Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+	p2pielen += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(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: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	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: */
+	*(__le16 *)(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;
+
+	/*	Channel Number */
+	p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+static void issue_p2p_GO_response(struct adapter *padapter, u8 *raddr, u8 *frame_body, uint len, u8 result)
+{
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8 oui_subtype = P2P_GO_NEGO_RESP;
+	u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+	u8 p2pielen = 0;
+	uint			wpsielen = 0;
+	u16 wps_devicepassword_id = 0x0000;
+	__be16			be_tmp;
+	uint			wps_devicepassword_id_len = 0;
+	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 wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	DBG_88E("[%s] In, result=%d\n", __func__,  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;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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 *)&be_tmp, &wps_devicepassword_id_len);
+	wps_devicepassword_id = be16_to_cpu(be_tmp);
+
+	_rtw_memset(wpsie, 0x00, 255);
+	wpsielen = 0;
+
+	/*	WPS Section */
+	wpsielen = 0;
+	/*	WPS OUI */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	/*	Device Password ID */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
+	else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
+	else
+		*(__be16 *)(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)
+			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
+		else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
+			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
+		else
+			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: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = result;
+
+	/*	P2P Capability */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	*(__le16 *)(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: */
+	*(__le16 *)(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: */
+	*(__le16 *)(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: */
+	*(__le16 *)(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;
+
+	/*	Channel Number */
+	p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
+
+	/*	Intended P2P Interface Address */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+	p2pielen += 2;
+
+	/*	Value: */
+	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);
+
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+
+	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 */
+
+	{
+		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];
+			}
+		}
+	}
+
+	/*	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) */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Config Method */
+	/*	This field should be big endian. Noted by P2P specification. */
+
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+
+	p2pielen += 2;
+
+	/*	Primary Device Type */
+	/*	Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+	p2pielen += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(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: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	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: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	p2P Device Address */
+		memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN);
+		p2pielen += ETH_ALEN;
+
+		/*	SSID */
+		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);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+	return;
+}
+
+static void issue_p2p_GO_confirm(struct adapter *padapter, u8 *raddr, u8 result)
+{
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8 oui_subtype = P2P_GO_NEGO_CONF;
+	u8 p2pie[255] = { 0x00 };
+	u8 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 wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	DBG_88E("[%s] In\n", __func__);
+	/* 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;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = result;
+
+	/*	P2P Capability */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	*(__le16 *)(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: */
+	*(__le16 *)(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)) {
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x51;
+		p2pie[p2pielen++] = pwdinfo->peer_operating_ch;
+	} else {
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x51;
+
+		/*	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: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(pwdinfo->channel_list_attr_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	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: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	p2P Device Address */
+		memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN);
+		p2pielen += ETH_ALEN;
+
+		/*	SSID */
+		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);
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	dump_mgntframe(padapter, pmgntframe);
+	return;
+}
+
+void issue_p2p_invitation_request(struct adapter *padapter, u8 *raddr)
+{
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8 oui_subtype = P2P_INVIT_REQ;
+	u8 p2pie[255] = { 0x00 };
+	u8 p2pielen = 0;
+	u8 dialogToken = 3;
+	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 wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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: */
+	*(__le16 *)(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: */
+	*(__le16 *)(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: */
+	*(__le16 *)(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;
+
+	/*	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: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	P2P Device Address for GO */
+		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);
+
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+
+	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 */
+	{
+		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];
+			}
+		}
+	}
+
+
+	/*	P2P Group ID */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(6 + pwdinfo->invitereq_info.ssidlen);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address for GO */
+	memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	SSID */
+	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) */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Config Method */
+	/*	This field should be big endian. Noted by P2P specification. */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
+	p2pielen += 2;
+
+	/*	Primary Device Type */
+	/*	Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+	p2pielen  += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(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: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	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);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+void issue_p2p_invitation_response(struct adapter *padapter, u8 *raddr, u8 dialogToken, u8 status_code)
+{
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8 oui_subtype = P2P_INVIT_RESP;
+	u8 p2pie[255] = { 0x00 };
+	u8 p2pielen = 0;
+	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 wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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: */
+	*(__le16  *)(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: */
+	*(__le16 *)(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: */
+			*(__le16 *)(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: */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+			p2pielen += 2;
+
+			/*	Value: */
+			/*	P2P Device Address for GO */
+			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);
+
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+		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 */
+		{
+			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];
+				}
+			}
+		}
+	}
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+void issue_p2p_provision_request(struct 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;
+	u8 oui_subtype = P2P_PROVISION_DISC_REQ;
+	u8 wpsie[100] = { 0x00 };
+	u8 wpsielen = 0;
+	__be32 p2poui = cpu_to_be32(P2POUI);
+	u32			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 wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	DBG_88E("[%s] In\n", __func__);
+	/* 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;
+
+	memcpy(pwlanhdr->addr1, pdev_raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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 */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	/*	Config Method */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	*(__be16 *)(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);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+static u8 is_matched_in_profilelist(u8 *peermacaddr, struct profile_info *profileinfo)
+{
+	u8 i, match_result = 0;
+
+	DBG_88E("[%s] peermac=%.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
+		peermacaddr[0], peermacaddr[1], peermacaddr[2], peermacaddr[3], peermacaddr[4], peermacaddr[5]);
+
+	for (i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++) {
+	       DBG_88E("[%s] profileinfo_mac=%.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
+			    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_88E("[%s] Match!\n", __func__);
+			break;
+		}
+	}
+	return match_result;
+}
+
+void issue_probersp_p2p(struct 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);
+	u16 beacon_interval = 100;
+	u16 capInfo = 0;
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8 wpsie[255] = { 0x00 };
+	u32					wpsielen = 0, p2pielen = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	/*	Use the device address for BSSID field. */
+	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 */
+	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;
+
+	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 */
+	pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&pwdinfo->listen_channel, &pattrib->pktlen);
+
+	/*	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 */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	/*	WiFi Simple Config State */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SIMPLE_CONF_STATE);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG;	/*	Not Configured. */
+
+	/*	Response Type */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_RESP_TYPE);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
+
+	/*	UUID-E */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010);
+	wpsielen += 2;
+
+	/*	Value: */
+	memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), ETH_ALEN);
+	wpsielen += 0x10;
+
+	/*	Manufacturer */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MANUFACTURER);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0007);
+	wpsielen += 2;
+
+	/*	Value: */
+	memcpy(wpsie + wpsielen, "Realtek", 7);
+	wpsielen += 7;
+
+	/*	Model Name */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NAME);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0006);
+	wpsielen += 2;
+
+	/*	Value: */
+	memcpy(wpsie + wpsielen, "8188EU", 6);
+	wpsielen += 6;
+
+	/*	Model Number */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NUMBER);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = 0x31;		/*	character 1 */
+
+	/*	Serial Number */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SERIAL_NUMBER);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(ETH_ALEN);
+	wpsielen += 2;
+
+	/*	Value: */
+	memcpy(wpsie + wpsielen, "123456" , ETH_ALEN);
+	wpsielen += ETH_ALEN;
+
+	/*	Primary Device Type */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008);
+	wpsielen += 2;
+
+	/*	Value: */
+	/*	Category ID */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+	wpsielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+	wpsielen += 2;
+
+	/*	Device Name */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len);
+	wpsielen += 2;
+
+	/*	Value: */
+	if (pwdinfo->device_name_len) {
+		memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len);
+		wpsielen += pwdinfo->device_name_len;
+	}
+
+	/*	Config Method */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	*(__be16 *)(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;
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+static int _issue_probereq_p2p(struct 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;
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	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;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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) {
+		memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+		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. */
+			memcpy(pwlanhdr->addr1, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
+			memcpy(pwlanhdr->addr3, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
+		} else {
+			/*	broadcast probe request frame */
+			memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+			memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
+		}
+	}
+	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);
+
+
+	/*	WPS IE */
+	/*	Noted by Albert 20110221 */
+	/*	According to the WPS specification, all the WPS attribute is presented by Big Endian. */
+
+	wpsielen = 0;
+	/*	WPS OUI */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(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: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010);
+		wpsielen += 2;
+
+		/*	Value: */
+		memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), ETH_ALEN);
+		wpsielen += 0x10;
+
+		/*	Config Method */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+		wpsielen += 2;
+
+		/*	Value: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+		wpsielen += 2;
+	}
+
+	/*	Device Name */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len);
+	wpsielen += 2;
+
+	/*	Value: */
+	memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len);
+	wpsielen += pwdinfo->device_name_len;
+
+	/*	Primary Device Type */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008);
+	wpsielen += 2;
+
+	/*	Value: */
+	/*	Category ID */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_RTK_WIDI);
+	wpsielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_RTK_DMP);
+	wpsielen += 2;
+
+	/*	Device Password ID */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	*(__be16 *)(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: */
+	*(__le16 *)(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: */
+	*(__le16 *)(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: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Availability Period */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+	p2pielen += 2;
+
+	/*	Availability Interval */
+	*(__le16 *)(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: */
+		*(__le16 *)(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 */
+		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;
+	}
+
+	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(struct adapter *adapter, u8 *da)
+{
+	_issue_probereq_p2p(adapter, da, false);
+}
+
+int issue_probereq_p2p_ex(struct 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;
+		goto exit;
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(adapter), da, rtw_get_oper_ch(adapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			DBG_88E(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_88EU_P2P */
+
+static s32 rtw_action_public_decache(union recv_frame *recv_frame, s32 token)
+{
+	struct 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_88E(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_88E(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;
+}
+
+static unsigned int on_action_public_p2p(union recv_frame *precv_frame)
+{
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u8 *frame_body;
+	u8 dialogToken = 0;
+#ifdef CONFIG_88EU_P2P
+	struct adapter *padapter = precv_frame->u.hdr.adapter;
+	uint len = precv_frame->u.hdr.len;
+	u8 *p2p_ie;
+	u32	p2p_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_88EU_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_88EU_P2P
+	_cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+	/*	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_88E("[%s] Got GO Nego Req Frame\n", __func__);
+		_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_88E("[%s] Restore the previous p2p state to %d\n", __func__, rtw_p2p_state(pwdinfo));
+		}
+
+		/*	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))
+			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);
+
+		/*	Commented by Albert 20110718 */
+		/*	No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer. */
+		_set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
+		break;
+	case P2P_GO_NEGO_RESP:
+		DBG_88E("[%s] Got GO Nego Resp Frame\n", __func__);
+
+		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_88E("[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __func__);
+		}
+		break;
+	case P2P_GO_NEGO_CONF:
+		DBG_88E("[%s] Got GO Nego Confirm Frame\n", __func__);
+		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_88E("[%s] Got invite request frame!\n", __func__);
+		p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+		if (p2p_ie) {
+			/*	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_88E("[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
+						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);
+								status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
+							}
+						}
+					} else {
+						DBG_88E("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
+						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 */
+							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_88E("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
+						status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+					}
+				}
+			} else {
+				DBG_88E("[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __func__);
+				status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+			}
+
+			DBG_88E("[%s] status_code = %d\n", __func__, status_code);
+
+			pwdinfo->inviteresp_info.token = frame_body[7];
+			issue_p2p_invitation_response(padapter, GetAddr2Ptr(pframe), pwdinfo->inviteresp_info.token, status_code);
+		}
+		break;
+	case P2P_INVIT_RESP: {
+		u8	attr_content = 0x00;
+		u32	attr_contentlen = 0;
+
+		DBG_88E("[%s] Got invite response frame!\n", __func__);
+		_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+		p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+		if (p2p_ie) {
+			rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+
+			if (attr_contentlen == 1) {
+				DBG_88E("[%s] Status = %d\n", __func__, 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_88E("[%s] Got Provisioning Discovery Request Frame\n", __func__);
+		process_p2p_provdisc_req(pwdinfo, pframe, len);
+		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);
+		break;
+	case P2P_PROVISION_DISC_RESP:
+		/*	Commented by Albert 20110707 */
+		/*	Should we check the pwdinfo->tx_prov_disc_info.bsent flag here?? */
+		DBG_88E("[%s] Got Provisioning Discovery Response Frame\n", __func__);
+		/*	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_88EU_P2P */
+
+	return _SUCCESS;
+}
+
+static unsigned int on_action_public_vendor(union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	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;
+}
+
+static 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;
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 token;
+
+	token = frame_body[2];
+
+	if (rtw_action_public_decache(precv_frame, token) == _FAIL)
+		goto exit;
+
+	ret = _SUCCESS;
+
+exit:
+	return ret;
+}
+
+unsigned int on_action_public(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	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(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int OnAction_wmm(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int OnAction_p2p(struct adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_88EU_P2P
+	u8 *frame_body;
+	u8 category, OUI_Subtype;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	struct	wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+
+	DBG_88E("%s\n", __func__);
+
+	/* 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 (be32_to_cpu(*((__be32 *)(frame_body + 1))) != P2POUI)
+		return _SUCCESS;
+
+	len -= sizeof(struct rtw_ieee80211_hdr_3addr);
+	OUI_Subtype = frame_body[5];
+
+	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_88EU_P2P */
+	return _SUCCESS;
+}
+
+unsigned int OnAction(struct 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(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_frame			*pmgntframe;
+	struct xmit_buf				*pxmitbuf;
+
+	pmgntframe = rtw_alloc_xmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		DBG_88E("%s, alloc xmitframe fail\n", __func__);
+		return NULL;
+	}
+
+	pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv);
+	if (pxmitbuf == NULL) {
+		DBG_88E("%s, alloc xmitbuf fail\n", __func__);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		return NULL;
+	}
+	pmgntframe->frame_tag = MGNT_FRAMETAG;
+	pmgntframe->pxmitbuf = pxmitbuf;
+	pmgntframe->buf_addr = pxmitbuf->pbuf;
+	pxmitbuf->priv_data = pmgntframe;
+	return pmgntframe;
+}
+
+/****************************************************************************
+
+Following are some TX fuctions for WiFi MLME
+
+*****************************************************************************/
+
+void update_mgnt_tx_rate(struct adapter *padapter, u8 rate)
+{
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+
+	pmlmeext->tx_rate = rate;
+	DBG_88E("%s(): rate = %x\n", __func__, rate);
+}
+
+void update_mgntframe_attrib(struct 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(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+		return;
+
+	rtw_hal_mgnt_xmit(padapter, pmgntframe);
+}
+
+s32 dump_mgntframe_and_wait(struct adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
+{
+	s32 ret = _FAIL;
+	struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
+	struct submit_ctx sctx;
+
+	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+		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);
+
+	 return ret;
+}
+
+s32 dump_mgntframe_and_wait_ack(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	s32 ret = _FAIL;
+	u32 timeout_ms = 500;/*   500ms */
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+
+	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+		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;
+}
+
+static int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
+{
+	u8 *ssid_ie;
+	int ssid_len_ori;
+	int len_diff = 0;
+
+	ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
+
+	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;
+			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(struct 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_88EU_AP_MODE)
+	unsigned long irqL;
+#endif /* if defined (CONFIG_88EU_AP_MODE) */
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*cur_network = &(pmlmeinfo->network);
+	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+#ifdef CONFIG_88EU_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		DBG_88E("%s, alloc mgnt frame fail\n", __func__);
+		return;
+	}
+#if defined (CONFIG_88EU_AP_MODE)
+	_enter_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
+#endif /* if defined (CONFIG_88EU_AP_MODE) */
+
+	/* 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;
+
+	memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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) {
+#ifdef CONFIG_88EU_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;
+			pframe_wscie = pframe + wps_offset;
+			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: */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+			insert_len += 2;
+
+			/*	Length: */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(0x0008);
+			insert_len += 2;
+
+			/*	Value: */
+			/*	Category ID */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+			insert_len += 2;
+
+			/*	OUI */
+			*(__be32 *)(pframe + insert_len) = cpu_to_be32(WPSOUI);
+			insert_len += 4;
+
+			/*	Sub Category ID */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+			insert_len += 2;
+
+			/*	Device Name */
+			/*	Type: */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+			insert_len += 2;
+
+			/*	Length: */
+			*(__be16 *)(pframe + insert_len) = cpu_to_be16(pwdinfo->device_name_len);
+			insert_len += 2;
+
+			/*	Value: */
+			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 */
+			memcpy(pframe, premainder_ie, remainder_ielen);
+			pframe += remainder_ielen;
+			pattrib->pktlen += remainder_ielen;
+		} else
+#endif /* CONFIG_88EU_P2P */
+		{
+			int len_diff;
+			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_88EU_P2P
+		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+			u32 len;
+			len = build_beacon_p2p_ie(pwdinfo, pframe);
+
+			pframe += len;
+			pattrib->pktlen += len;
+		}
+#endif /* CONFIG_88EU_P2P */
+
+		goto _issue_bcn;
+	}
+
+	/* below for ad-hoc mode */
+
+	/* timestamp will be inserted by hardware */
+	pframe += 8;
+	pattrib->pktlen += 8;
+
+	/*  beacon interval: 2 bytes */
+
+	memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/*  capability info: 2 bytes */
+
+	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);
+
+	{
+		u8 erpinfo = 0;
+		u32 ATIMWindow;
+		/*  IBSS Parameter Set... */
+		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_88EU_AP_MODE)
+	pmlmepriv->update_bcn = false;
+
+	_exit_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
+#endif /* if defined (CONFIG_88EU_AP_MODE) */
+
+	if ((pattrib->pktlen + TXDESC_SIZE) > 512) {
+		DBG_88E("beacon frame too large\n");
+		return;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	/* DBG_88E("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(struct 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_88EU_AP_MODE)
+	u8 *pwps_ie;
+	uint wps_ielen;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+#endif /* if defined (CONFIG_88EU_AP_MODE) */
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*cur_network = &(pmlmeinfo->network);
+	unsigned int	rate_len;
+#ifdef CONFIG_88EU_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		DBG_88E("%s, alloc mgnt frame fail\n", __func__);
+		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;
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+	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_88EU_AP_MODE)
+	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;
+
+			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) {
+				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) {
+				memcpy(pframe, premainder_ie, remainder_ielen);
+				pframe += remainder_ielen;
+				pattrib->pktlen += remainder_ielen;
+			}
+		} else {
+			memcpy(pframe, cur_network->IEs, cur_network->IELength);
+			pframe += cur_network->IELength;
+			pattrib->pktlen += cur_network->IELength;
+		}
+	} else
+#endif
+	{
+		/* timestamp will be inserted by hardware */
+		pframe += 8;
+		pattrib->pktlen += 8;
+
+		/*  beacon interval: 2 bytes */
+
+		memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
+
+		pframe += 2;
+		pattrib->pktlen += 2;
+
+		/*  capability info: 2 bytes */
+
+		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_88EU_P2P
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && is_valid_p2p_probereq) {
+		u32 len;
+		len = build_probe_resp_p2p_ie(pwdinfo, pframe);
+
+		pframe += len;
+		pattrib->pktlen += len;
+	}
+#endif /* CONFIG_88EU_P2P */
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+static int _issue_probereq(struct adapter *padapter, struct 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);
+	int	bssrate_len = 0;
+	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+issue_probereq\n"));
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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 */
+		memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, da, ETH_ALEN);
+	} else {
+		/*	broadcast probe request frame */
+		memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
+	}
+
+	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));
+	}
+
+	/* add wps_ie for wps2.0 */
+	if (pmlmepriv->wps_probe_req_ie_len > 0 && pmlmepriv->wps_probe_req_ie) {
+		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;
+	}
+
+	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(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da)
+{
+	_issue_probereq(padapter, pssid, da, false);
+}
+
+int issue_probereq_ex(struct adapter *padapter, struct 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;
+		goto exit;
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			DBG_88E(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(struct 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;
+	u16 val16;
+#ifdef CONFIG_88EU_AP_MODE
+	__le16 le_val16;
+#endif
+	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);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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_88EU_AP_MODE
+
+		memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+
+		/*  setting auth algo number */
+		val16 = (u16)psta->authalg;
+
+		if (status != _STATS_SUCCESSFUL_)
+			val16 = 0;
+
+		if (val16) {
+			le_val16 = cpu_to_le16(val16);
+			use_shared_key = 1;
+		} else {
+			le_val16 = 0;
+		}
+
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_val16, &(pattrib->pktlen));
+
+		/*  setting auth seq number */
+		val16 = (u16)psta->auth_seq;
+		le_val16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_val16, &(pattrib->pktlen));
+
+		/*  setting status code... */
+		val16 = status;
+		le_val16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_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 {
+		__le32 le_tmp32;
+		__le16 le_tmp16;
+		memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+		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)
+			use_shared_key = 1;
+
+		/* setting IV for auth seq #3 */
+		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
+			val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
+			le_tmp32 = cpu_to_le32(val32);
+			pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&le_tmp32, &(pattrib->pktlen));
+
+			pattrib->iv_len = 4;
+		}
+
+		le_tmp16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_tmp16, &(pattrib->pktlen));
+
+		/*  setting auth seq number */
+		val16 = pmlmeinfo->auth_seq;
+		le_tmp16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_tmp16, &(pattrib->pktlen));
+
+
+		/*  setting status code... */
+		le_tmp16 = cpu_to_le16(status);
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_tmp16, &(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_88E("%s\n", __func__);
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+
+void issue_asocrsp(struct adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
+{
+#ifdef CONFIG_88EU_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);
+	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+	u8 *ie = pnetwork->IEs;
+	__le16 lestatus, leval;
+#ifdef CONFIG_88EU_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+	DBG_88E("%s\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+
+	memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
+	memcpy((void *)GetAddr2Ptr(pwlanhdr), myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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));
+
+	lestatus = cpu_to_le16(status);
+	pframe = rtw_set_fixed_ie(pframe , _STATUS_CODE_ , (unsigned char *)&lestatus, &(pattrib->pktlen));
+
+	leval = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
+	pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_ , (unsigned char *)&leval, &(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));
+	}
+
+	if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) {
+		uint ie_len = 0;
+
+		/* FILL HT CAP INFO IE */
+		pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+		if (pbuf && ie_len > 0) {
+			memcpy(pframe, pbuf, ie_len+2);
+			pframe += (ie_len+2);
+			pattrib->pktlen += (ie_len+2);
+		}
+
+		/* FILL HT ADD INFO IE */
+		pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+		if (pbuf && ie_len > 0) {
+			memcpy(pframe, pbuf, ie_len+2);
+			pframe += (ie_len+2);
+			pattrib->pktlen += (ie_len+2);
+		}
+	}
+
+	/* 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)) {
+				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) {
+		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_88EU_P2P
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && (pstat->is_p2p_device)) {
+		u32 len;
+
+		len = build_assoc_resp_p2p_ie(pwdinfo, pframe, pstat->p2p_status_code);
+
+		pframe += len;
+		pattrib->pktlen += len;
+	}
+#endif /* CONFIG_88EU_P2P */
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	dump_mgntframe(padapter, pmgntframe);
+#endif
+}
+
+void issue_assocreq(struct 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;
+	__le16		le_tmp;
+	unsigned int	i, j, ie_len, index = 0;
+	unsigned char	rf_type, bssrate[NumRates], sta_bssrate[NumRates];
+	struct ndis_802_11_var_ie *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_88EU_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8 p2pie[255] = { 0x00 };
+	u16 p2pielen = 0;
+#endif /* CONFIG_88EU_P2P */
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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 */
+
+	memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/* listen interval */
+	/* todo: listen interval for power saving */
+	le_tmp = cpu_to_le16(3);
+	memcpy(pframe , (unsigned char *)&le_tmp, 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 */
+
+	/*  Check if the AP's supported rates are also supported by STA. */
+	get_rate_set(padapter, sta_bssrate, &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 < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		if (pmlmeinfo->network.SupportedRates[i] == 0)
+			break;
+		DBG_88E("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))
+				break;
+		}
+
+		if (j == sta_bssrate_len) {
+			/*  the rate is not supported by STA */
+			DBG_88E("%s(): the rate[%d]=%02X is not supported by STA!\n", __func__, i, pmlmeinfo->network.SupportedRates[i]);
+		} else {
+			/*  the rate is supported by STA */
+			bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
+		}
+	}
+
+	bssrate_len = index;
+	DBG_88E("bssrate_len=%d\n", bssrate_len);
+
+	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(struct ndis_802_11_fixed_ie)), _RSN_IE_2_, &ie_len, (pmlmeinfo->network.IELength - sizeof(struct ndis_802_11_fixed_ie)));
+	if (p != NULL)
+		pframe = rtw_set_ie(pframe, _RSN_IE_2_, ie_len, (p + 2), &(pattrib->pktlen));
+
+	/* HT caps */
+	if (padapter->mlmepriv.htpriv.ht_option) {
+		p = rtw_get_ie((pmlmeinfo->network.IEs + sizeof(struct ndis_802_11_fixed_ie)), _HT_CAPABILITY_IE_, &ie_len, (pmlmeinfo->network.IELength - sizeof(struct ndis_802_11_fixed_ie)));
+		if ((p != NULL) && (!(is_ap_in_tkip(padapter)))) {
+			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 &= cpu_to_le16(~(BIT(6) | BIT(1)));
+			else
+				pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(BIT(1));
+
+			/* todo: disable SM power save mode */
+			pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x000c);
+
+			rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+			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 */
+				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 */
+				    (pregpriv->wifi_spec == 1)) {
+					DBG_88E("declare supporting RX STBC\n");
+					pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0200);/* RX STBC two spatial stream */
+				}
+				memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_2R, 16);
+				break;
+			}
+			pframe = rtw_set_ie(pframe, _HT_CAPABILITY_IE_, ie_len , (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
+		}
+	}
+
+	/* vendor specific IE, such as WPA, WMM, WPS */
+	for (i = sizeof(struct ndis_802_11_fixed_ie); i < pmlmeinfo->network.IELength;) {
+		pIE = (struct ndis_802_11_var_ie *)(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_88EU_P2P
+
+	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: */
+		*(__le16 *)(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: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	Availability Period */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+		p2pielen += 2;
+
+		/*	Availability Interval */
+		*(__le16 *)(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) */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	P2P Device Address */
+		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))
+			*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
+		else
+			*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC);
+
+		p2pielen += 2;
+
+		/*	Primary Device Type */
+		/*	Category ID */
+		*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+		p2pielen += 2;
+
+		/*	OUI */
+		*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+		p2pielen += 4;
+
+		/*	Sub Category ID */
+		*(__be16 *)(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: */
+		*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+		p2pielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+		p2pielen += 2;
+
+		/*	Value: */
+		memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+		p2pielen += pwdinfo->device_name_len;
+
+		/*	P2P Interface */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_INTERFACE;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x000D);
+		p2pielen += 2;
+
+		/*	Value: */
+		memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);	/*	P2P Device Address */
+		p2pielen += ETH_ALEN;
+
+		p2pie[p2pielen++] = 1;	/*	P2P Interface Address Count */
+
+		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);
+	}
+
+#endif /* CONFIG_88EU_P2P */
+
+	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(struct 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;
+
+	if (!padapter)
+		goto exit;
+
+	pxmitpriv = &(padapter->xmitpriv);
+	pmlmeext = &(padapter->mlmeextpriv);
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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);
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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(struct 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;
+		goto exit;
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			DBG_88E(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(struct 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_88E("%s\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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);
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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(struct 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;
+		goto exit;
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			DBG_88E(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(struct 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;
+	__le16 le_tmp;
+#ifdef CONFIG_88EU_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_88EU_P2P */
+
+#ifdef CONFIG_88EU_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_88EU_P2P */
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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);
+
+	le_tmp = cpu_to_le16(reason);
+	pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_ , (unsigned char *)&le_tmp, &(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(struct adapter *padapter, unsigned char *da, unsigned short reason)
+{
+	DBG_88E("%s to %pM\n", __func__, da);
+	return _issue_deauth(padapter, da, reason, false);
+}
+
+int issue_deauth_ex(struct 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;
+		goto exit;
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			DBG_88E(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 (struct adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset)
+{
+	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);
+
+
+	DBG_88E(FUNC_NDEV_FMT" ra =%pM, ch:%u, offset:%u\n",
+		FUNC_NDEV_ARG(padapter->pnetdev), ra, new_ch, ch_offset);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+
+	memcpy(pwlanhdr->addr1, ra, ETH_ALEN); /* RA */
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); /* TA */
+	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(struct 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;
+	__le16	le_tmp;
+	u16 BA_starting_seqctrl = 0;
+	enum 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;
+
+	DBG_88E("%s, category=%d, action=%d, status=%d\n", __func__, category, action, status);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+
+	/* memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); */
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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 (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));
+
+			BA_para_set = (0x1002 | ((status & 0xf) << 2)); /* immediate ack & 64 buffer size */
+			le_tmp = cpu_to_le16(BA_para_set);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+			BA_timeout_value = 5000;/*  5ms */
+			le_tmp = cpu_to_le16(BA_timeout_value);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+			psta = rtw_get_stainfo(pstapriv, raddr);
+			if (psta != NULL) {
+				start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07]&0xfff) + 1;
+
+				DBG_88E("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;
+			}
+			le_tmp = cpu_to_le16(BA_starting_seqctrl);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(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));
+			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 = (((pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
+			else if (MAX_AMPDU_FACTOR_32K == max_rx_ampdu_factor)
+				BA_para_set = (((pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0800); /* 32 buffer size */
+			else if (MAX_AMPDU_FACTOR_16K == max_rx_ampdu_factor)
+				BA_para_set = (((pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0400); /* 16 buffer size */
+			else if (MAX_AMPDU_FACTOR_8K == max_rx_ampdu_factor)
+				BA_para_set = (((pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0200); /* 8 buffer size */
+			else
+				BA_para_set = (((pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
+
+			if (pregpriv->ampdu_amsdu == 0)/* disabled */
+				BA_para_set = BA_para_set & ~BIT(0);
+			else if (pregpriv->ampdu_amsdu == 1)/* enabled */
+				BA_para_set = BA_para_set | BIT(0);
+			le_tmp = cpu_to_le16(BA_para_set);
+
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(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;
+			le_tmp = cpu_to_le16(BA_para_set);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+			reason_code = 37;/* Requested from peer STA as it does not want to use the mechanism */
+			le_tmp = cpu_to_le16(reason_code);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+			break;
+		default:
+			break;
+		}
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_action_BSSCoexistPacket(struct adapter *padapter)
+{
+	unsigned long	irqL;
+	struct list_head *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);
+	struct __queue *queue	= &(pmlmepriv->scanned_queue);
+	u8 InfoContent[16] = {0};
+	u8 ICS[8][15];
+	if ((pmlmepriv->num_FortyMHzIntolerant == 0) || (pmlmepriv->num_sta_no_ht == 0))
+		return;
+
+	if (pmlmeinfo->bwmode_updated)
+		return;
+
+
+	DBG_88E("%s\n", __func__);
+
+
+	category = RTW_WLAN_CATEGORY_PUBLIC;
+	action = ACT_PUBLIC_BSSCOEXIST;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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;
+			struct wlan_bssid_ex *pbss_network;
+
+			if (rtw_end_of_queue_search(phead, plist))
+				break;
+
+			pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+			plist = get_next(plist);
+
+			pbss_network = (struct 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);
+}
+
+unsigned int send_delba(struct 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;
+
+	if (initiator == 0) { /*  recipient */
+		for (tid = 0; tid < MAXTID; tid++) {
+			if (psta->recvreorder_ctrl[tid].enable) {
+				DBG_88E("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;
+			}
+		}
+	} else if (initiator == 1) { /*  originator */
+		for (tid = 0; tid < MAXTID; tid++) {
+			if (psta->htpriv.agg_enable_bitmap & BIT(tid)) {
+				DBG_88E("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);
+			}
+		}
+	}
+
+	return _SUCCESS;
+}
+
+unsigned int send_beacon(struct adapter *padapter)
+{
+	u8 bxmitok = false;
+	int	issue = 0;
+	int poll = 0;
+
+	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 && !bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+	} while (!bxmitok && issue < 100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
+		return _FAIL;
+	if (!bxmitok) {
+		DBG_88E("%s fail! %u ms\n", __func__, 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_88E("%s success, issue:%d, poll:%d, %u ms\n", __func__, issue, poll, rtw_get_passing_time_ms(start));
+		return _SUCCESS;
+	}
+}
+
+/****************************************************************************
+
+Following are some utitity fuctions for WiFi MLME
+
+*****************************************************************************/
+
+void site_survey(struct adapter *padapter)
+{
+	unsigned char		survey_channel = 0, val8;
+	enum 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_88EU_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+	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_88EU_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 (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)); */
+		if (pmlmeext->sitesurvey_res.channel_idx == 0)
+			set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
+		else
+			SelectChannel(padapter, survey_channel);
+
+		if (ScanType == SCAN_ACTIVE) { /* obey the channel plan setting... */
+			#ifdef CONFIG_88EU_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_88EU_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);
+				}
+			}
+		}
+
+		set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
+	} else {
+		/*	channel number is 0 or this channel is not valid. */
+
+#ifdef CONFIG_88EU_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);
+			}
+		}
+
+		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_88EU_P2P */
+		{
+			/*  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;
+			}
+#ifdef CONFIG_88EU_P2P
+			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
+				rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+			rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
+#endif /* CONFIG_88EU_P2P */
+
+			pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
+
+			/* switch back to the original channel */
+
+#ifdef CONFIG_88EU_P2P
+			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
+				set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+#endif /* CONFIG_88EU_P2P */
+
+			/* 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))
+				issue_nulldata(padapter, NULL, 0, 3, 500);
+
+			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);
+		}
+	}
+	return;
+}
+
+/* collect bss info from Beacon and Probe request/response frames. */
+u8 collect_bss_info(struct adapter *padapter, union recv_frame *precv_frame, struct 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);
+	__le32 le32_tmp;
+
+	len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ)
+		return _FAIL;
+
+	_rtw_memset(bssid, 0, sizeof(struct 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(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+
+	/* below is to copy the information element */
+	bssid->IELength = len;
+	memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength);
+
+	/* get the signal strength */
+	bssid->Rssi = precv_frame->u.hdr.attrib.phy_info.recvpower; /*  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 */
+	rtw_hal_get_def_var(padapter, HAL_DEF_CURRENT_ANTENNA,  &bssid->PhyInfo.Optimum_antenna);
+
+	/*  checking SSID */
+	p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset);
+	if (p == NULL) {
+		DBG_88E("marc: cannot find SSID for survey event\n");
+		return _FAIL;
+	}
+
+	if (*(p + 1)) {
+		if (len > NDIS_802_11_LENGTH_SSID) {
+			DBG_88E("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+			return _FAIL;
+		}
+		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_88E("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+			return _FAIL;
+		}
+		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_88E("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+			return _FAIL;
+		}
+		memcpy(bssid->SupportedRates + i, (p + 2), len);
+	}
+
+	/* todo: */
+	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;
+		memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
+		bssid->Privacy = 1;
+		return _SUCCESS;
+	}
+
+	memcpy(&le32_tmp, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
+	bssid->Configuration.BeaconPeriod = le32_to_cpu(le32_tmp);
+
+	val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
+
+	if (val16 & BIT(0)) {
+		bssid->InfrastructureMode = Ndis802_11Infrastructure;
+		memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
+	} else {
+		bssid->InfrastructureMode = Ndis802_11IBSS;
+		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) && (!pmlmeinfo->bwmode_updated)) {
+		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+		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 (le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info)&BIT(14))
+				pmlmepriv->num_FortyMHzIntolerant++;
+		} else {
+			pmlmepriv->num_sta_no_ht++;
+		}
+	}
+
+	/*  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(struct 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);
+	struct wlan_bssid_ex *pnetwork = (struct 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((struct wlan_bssid_ex *)pnetwork);
+	update_capinfo(padapter, caps);
+	if (caps&cap_IBSS) {/* adhoc master */
+		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_88E("start_create_ibss, invalid cap:%x\n", caps);
+		return;
+	}
+}
+
+void start_clnt_join(struct adapter *padapter)
+{
+	unsigned short	caps;
+	u8 val8;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	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((struct wlan_bssid_ex *)pnetwork);
+	update_capinfo(padapter, caps);
+	if (caps&cap_ESS) {
+		Set_MSR(padapter, WIFI_FW_STATION_STATE);
+
+		val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ? 0xcc : 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);
+
+		/* 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 */
+		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 {
+		return;
+	}
+}
+
+void start_clnt_auth(struct 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 */
+	/*	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);
+
+	DBG_88E_LEVEL(_drv_info_, "start auth\n");
+	issue_auth(padapter, NULL, 0);
+
+	set_link_timer(pmlmeext, REAUTH_TO);
+}
+
+
+void start_clnt_assoc(struct 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(struct 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_88E("%s\n", __func__);
+
+	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;
+}
+
+static void process_80211d(struct adapter *padapter, struct wlan_bssid_ex *bssid)
+{
+	struct registry_priv *pregistrypriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct 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;
+		struct rt_channel_plan chplan_ap;
+		struct 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);
+		memcpy(country, p, 3);
+		p += 3;
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+			 ("%s: 802.11d country =%s\n", __func__, 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;
+
+		memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
+
+		_rtw_memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
+		chplan_new = pmlmeext->channel_set;
+
+		i = 0;
+		j = 0;
+		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 = 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 = 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++;
+		}
+
+		/*  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;
+	}
+
+	/*  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) {
+				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",
+					 __func__, channel));
+			}
+			break;
+		}
+		i++;
+	}
+}
+
+/****************************************************************************
+
+Following are the functions to report events
+
+*****************************************************************************/
+
+void report_survey_event(struct 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;
+
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree(pcmd_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, (struct wlan_bssid_ex *)&psurvey_evt->bss) == _FAIL) {
+		kfree(pcmd_obj);
+		kfree(pevtcmd);
+		return;
+	}
+
+	process_80211d(padapter, &psurvey_evt->bss);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	pmlmeext->sitesurvey_res.bss_cnt++;
+
+	return;
+}
+
+void report_surveydone_event(struct 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;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree(pcmd_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_88E("survey done event(%x)\n", psurveydone_evt->bss_cnt);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+}
+
+void report_join_res(struct 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;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree(pcmd_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));
+	memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(struct wlan_bssid_ex));
+	pjoinbss_evt->network.join_res	= res;
+	pjoinbss_evt->network.aid = res;
+
+	DBG_88E("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(struct 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;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree(pcmd_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));
+	memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+	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_88E("report_del_sta_event: delete STA, mac_id =%d\n", mac_id);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+}
+
+void report_add_sta_event(struct 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;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree(pcmd_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));
+	memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+	padd_sta_evt->cam_id = cam_idx;
+
+	DBG_88E("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(struct 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);
+
+	/* 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 {
+		psta->htpriv.ht_option = false;
+
+		psta->htpriv.ampdu_enable = false;
+
+		psta->htpriv.sgi = false;
+		psta->qos_option = false;
+	}
+	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 */
+
+	/* QoS */
+	if (pmlmepriv->qospriv.qos_option)
+		psta->qos_option = true;
+
+
+	psta->state = _FW_LINKED;
+}
+
+void mlmeext_joinbss_event_callback(struct 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);
+	struct 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->mac_id);
+		}
+	}
+
+
+	/* 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);
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
+	if (psta) { /* only for infra. mode */
+		pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+		psta->wireless_mode = pmlmeext->cur_wireless_mode;
+
+		/* set per sta rate after updating HT cap. */
+		set_sta_rate(padapter, psta);
+		rtw_hal_set_hwreg(padapter, HW_VAR_TX_RPT_MAX_MACID, (u8 *)&psta->mac_id);
+		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);
+	}
+	rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
+
+exit_mlmeext_joinbss_event_callback:
+
+	DBG_88E("=>%s\n", __func__);
+}
+
+void mlmeext_sta_add_event_callback(struct 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_88E("%s\n", __func__);
+
+	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 */
+			/*  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->mac_id);
+
+	/* update adhoc sta_info */
+	update_sta_info(padapter, psta);
+}
+
+void mlmeext_sta_del_event_callback(struct 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)) {
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+		rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
+
+		/* restore to initial setting. */
+		update_tx_basic_rate(padapter, padapter->registrypriv.wireless_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);
+
+
+		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(struct adapter *padapter);
+void _linked_rx_signal_strehgth_display(struct adapter *padapter)
+{
+	struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
+      struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 mac_id;
+	int UndecoratedSmoothedPWDB;
+	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);
+
+	rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
+	DBG_88E("UndecoratedSmoothedPWDB:%d\n", UndecoratedSmoothedPWDB);
+}
+
+static u8 chk_ap_is_alive(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 ret = false;
+
+	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(struct 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);
+
+	rtw_hal_sreset_linked_status_check(padapter);
+
+	if (is_client_associated_to_ap(padapter)) {
+		/* linked infrastructure client mode */
+
+		int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
+		int rx_chk_limit;
+
+		rx_chk_limit = 4;
+		psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
+		if (psta != NULL) {
+			bool is_p2p_enable = false;
+			#ifdef CONFIG_88EU_P2P
+			is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
+			#endif
+
+			if (!chk_ap_is_alive(padapter, psta))
+				rx_chk = _FAIL;
+
+			if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
+				tx_chk = _FAIL;
+
+			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 {
+				if (rx_chk != _SUCCESS) {
+					if (pmlmeext->retry == 0) {
+						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) {
+					tx_chk = issue_nulldata(padapter, NULL, 0, 1, 0);
+				}
+			}
+
+			if (rx_chk == _FAIL) {
+				pmlmeext->retry++;
+				if (pmlmeext->retry > rx_chk_limit) {
+					DBG_88E_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);
+				}
+			}
+		}
+	}
+}
+
+void survey_timer_hdl(struct 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_88EU_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif
+
+	/* issue rtw_sitesurvey_cmd */
+	if (pmlmeext->sitesurvey_res.state > SCAN_START) {
+		if (pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
+			pmlmeext->sitesurvey_res.channel_idx++;
+
+		if (pmlmeext->scan_abort) {
+			#ifdef CONFIG_88EU_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_88E("%s idx:%d, cnt:%u\n", __func__
+					, pmlmeext->sitesurvey_res.channel_idx
+					, pwdinfo->find_phase_state_exchange_cnt
+			);
+			} else
+			#endif
+			{
+				pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
+				DBG_88E("%s idx:%d\n", __func__
+					, pmlmeext->sitesurvey_res.channel_idx
+			);
+			}
+
+			pmlmeext->scan_abort = false;/* reset */
+		}
+
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL)
+			goto exit_survey_timer_hdl;
+
+		psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm));
+		if (psurveyPara == NULL) {
+			kfree(ph2c);
+			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:
+	return;
+}
+
+void link_timer_hdl(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+		DBG_88E("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) {
+			pmlmeinfo->state = 0;
+			report_join_res(padapter, -1);
+			return;
+		}
+
+		DBG_88E("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_88E("link_timer_hdl: assoc timeout and try again\n");
+		issue_assocreq(padapter);
+		set_link_timer(pmlmeext, REASSOC_TO);
+	}
+	return;
+}
+
+void addba_timer_hdl(struct sta_info *psta)
+{
+	struct ht_priv	*phtpriv;
+
+	if (!psta)
+		return;
+
+	phtpriv = &psta->htpriv;
+
+	if ((phtpriv->ht_option) && (phtpriv->ampdu_enable)) {
+		if (phtpriv->candidate_tid_bitmap)
+			phtpriv->candidate_tid_bitmap = 0x0;
+	}
+}
+
+u8 NULL_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	return H2C_SUCCESS;
+}
+
+u8 setopmode_hdl(struct 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_;
+	} 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(struct adapter *padapter, u8 *pbuf)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
+	/* u32	initialgain; */
+
+
+	if (pparm->network.InfrastructureMode == Ndis802_11APMode) {
+#ifdef CONFIG_88EU_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);
+
+		memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, IELength));
+		pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength;
+
+		if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
+			return H2C_PARAMETERS_ERROR;
+
+		memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength);
+
+		start_create_ibss(padapter);
+	}
+
+	return H2C_SUCCESS;
+}
+
+u8 join_cmd_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	u8 join_type;
+	struct ndis_802_11_var_ie *pIE;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	struct joinbss_parm	*pparm = (struct joinbss_parm *)pbuf;
+	u32 i;
+
+	/* 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_STATION_);
+
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+	}
+
+	rtw_antenna_select_cmd(padapter, pparm->network.PhyInfo.Optimum_antenna, false);
+
+	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;
+
+	memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, IELength));
+	pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength;
+
+	if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
+		return H2C_PARAMETERS_ERROR;
+
+	memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength);
+
+	/* Check AP vendor to move rtw_joinbss_cmd() */
+
+	for (i = sizeof(struct ndis_802_11_fixed_ie); i < pnetwork->IELength;) {
+		pIE = (struct ndis_802_11_var_ie *)(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. */
+			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 */
+			{
+				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_88E("set ch/bw before connected\n");
+				}
+			}
+			break;
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+	/* disable dynamic functions, such as high power, DIG */
+
+	/* config the initial gain under linking, need to write the BB registers */
+
+	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(struct 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);
+	struct wlan_bssid_ex *pnetwork = (struct 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);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+	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;
+
+	/* 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);
+
+	flush_all_cam_entry(padapter);
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	rtw_free_uc_swdec_pending_queue(padapter);
+
+	return	H2C_SUCCESS;
+}
+
+static int rtw_scan_ch_decision(struct adapter *padapter, struct rtw_ieee80211_channel *out,
+	u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
+{
+	int i, j;
+	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++) {
+		set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value);
+		if (in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED) &&
+		    set_idx >= 0) {
+			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++;
+		}
+	}
+
+	return j;
+}
+
+u8 sitesurvey_cmd_hdl(struct 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_88EU_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, NULL);
+
+		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) {
+				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;
+
+		/* issue null data if associating to the AP */
+		if (is_client_associated_to_ap(padapter)) {
+			pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
+
+			issue_nulldata(padapter, NULL, 1, 3, 500);
+
+			bdelayscan = true;
+		}
+		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)) {
+		/* 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_88EU_P2P
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+			initialgain = 0x1E;
+		else
+			initialgain = 0x28;
+#else	/*  CONFIG_88EU_P2P */
+		initialgain = 0x1E;
+#endif /*  CONFIG_88EU_P2P */
+
+		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(struct 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(struct 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_88E_LEVEL(_drv_info_, "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);
+
+	return H2C_SUCCESS;
+}
+
+u8 set_stakey_hdl(struct 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;
+
+	/* 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 */
+
+	cam_id = 4;
+
+	DBG_88E_LEVEL(_drv_info_, "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_88E("r871x_set_stakey_hdl(): enc_algorithm=%d\n", pparm->algorithm);
+
+			if ((psta->mac_id < 1) || (psta->mac_id > (NUM_STA-4))) {
+				DBG_88E("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_88E("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_88E("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);
+	write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+	pmlmeinfo->enc_algo = pparm->algorithm;
+	return H2C_SUCCESS;
+}
+
+u8 add_ba_hdl(struct 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;
+
+	if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
+	    ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)) {
+		issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
+		_set_timer(&psta->addba_retry_timer, ADDBA_TO);
+	} else {
+		psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
+	}
+	return	H2C_SUCCESS;
+}
+
+u8 set_tx_beacon_cmd(struct 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_;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param));
+	if (ptxBeacon_parm == NULL) {
+		kfree(ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(struct 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(struct adapter *padapter, unsigned char *pbuf)
+{
+	u8 evt_code;
+	u16 evt_sz;
+	uint	*peventbuf;
+	void (*event_callback)(struct adapter *dev, u8 *pbuf);
+	struct evt_priv *pevt_priv = &(padapter->evtpriv);
+
+	peventbuf = (uint *)pbuf;
+	evt_sz = (u16)(*peventbuf&0xffff);
+	evt_code = (u8)((*peventbuf>>16)&0xff);
+
+	/*  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(struct adapter *padapter, unsigned char *pbuf)
+{
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	return H2C_SUCCESS;
+}
+
+u8 tx_beacon_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	if (send_beacon(padapter) == _FAIL) {
+		DBG_88E("issue_beacon, fail!\n");
+		return H2C_PARAMETERS_ERROR;
+	}
+#ifdef CONFIG_88EU_AP_MODE
+	else { /* tx bc/mc frames after update TIM */
+		unsigned long irqL;
+		struct sta_info *psta_bmc;
+		struct list_head *xmitframe_plist, *xmitframe_phead;
+		struct xmit_frame *pxmitframe = NULL;
+		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)) {
+			rtw_msleep_os(10);/*  10ms, ATIM(HIQ) Windows */
+			_enter_critical_bh(&psta_bmc->sleep_q.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)) {
+				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 */
+
+				_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
+				if (rtw_hal_xmit(padapter, pxmitframe))
+					rtw_os_xmit_complete(padapter, pxmitframe);
+				_enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
+			}
+			_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
+		}
+	}
+#endif
+	return H2C_SUCCESS;
+}
+
+u8 set_ch_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct set_ch_parm *set_ch_parm;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	set_ch_parm = (struct set_ch_parm *)pbuf;
+
+	DBG_88E(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(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct SetChannelPlan_param *setChannelPlan_param;
+	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(struct adapter *padapter, unsigned char *pbuf)
+{
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+	return	H2C_SUCCESS;
+}
+
+u8 set_csa_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	return	H2C_REJECTED;
+}
+
+/*  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(struct adapter *padapter, unsigned char *pbuf)
+{
+	return H2C_REJECTED;
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_mp.c b/drivers/staging/rtl8188eu/core/rtw_mp.c
new file mode 100644
index 0000000..c7ff2e4
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_mp.c
@@ -0,0 +1,997 @@
+/******************************************************************************
+ *
+ * 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>
+
+#include "odm_precomp.h"
+#include "rtl8188e_hal.h"
+
+u32 read_macreg(struct 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(struct 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(struct adapter *padapter, u32 addr, u32 bitmask)
+{
+	return rtw_hal_read_bbreg(padapter, addr, bitmask);
+}
+
+void write_bbreg(struct adapter *padapter, u32 addr, u32 bitmask, u32 val)
+{
+	rtw_hal_write_bbreg(padapter, addr, bitmask, val);
+}
+
+u32 _read_rfreg(struct adapter *padapter, u8 rfpath, u32 addr, u32 bitmask)
+{
+	return rtw_hal_read_rfreg(padapter, (enum rf_radio_path)rfpath, addr, bitmask);
+}
+
+void _write_rfreg(struct adapter *padapter, u8 rfpath, u32 addr, u32 bitmask, u32 val)
+{
+	rtw_hal_write_rfreg(padapter, (enum rf_radio_path)rfpath, addr, bitmask, val);
+}
+
+u32 read_rfreg(struct adapter *padapter, u8 rfpath, u32 addr)
+{
+	return _read_rfreg(padapter, (enum rf_radio_path)rfpath, addr, bRFRegOffsetMask);
+}
+
+void write_rfreg(struct adapter *padapter, u8 rfpath, u32 addr, u32 val)
+{
+	_write_rfreg(padapter, (enum rf_radio_path)rfpath, addr, bRFRegOffsetMask, val);
+}
+
+static void _init_mp_priv_(struct mp_priv *pmp_priv)
+{
+	struct 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;
+	memcpy(pnetwork->MacAddress, pmp_priv->network_macaddr, ETH_ALEN);
+
+	pnetwork->Ssid.SsidLength = 8;
+	memcpy(pnetwork->Ssid.Ssid, "mp_871x", pnetwork->Ssid.SsidLength);
+}
+
+static void mp_init_xmit_attrib(struct mp_tx *pmptx, struct adapter *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_memset(pattrib->dst, 0xFF, ETH_ALEN);
+	pattrib->ack_policy = 0;
+	pattrib->hdrlen = WLAN_HDR_A3_LEN;
+	pattrib->subtype = WIFI_DATA;
+	pattrib->priority = 0;
+	pattrib->qsel = pattrib->priority;
+	pattrib->nr_frags = 1;
+	pattrib->encrypt = 0;
+	pattrib->bswenc = false;
+	pattrib->qos_en = false;
+}
+
+s32 init_mp_priv(struct adapter *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)
+{
+	kfree(pmp_priv->pallocated_mp_xmitframe_buf);
+	pmp_priv->pallocated_mp_xmitframe_buf = NULL;
+	pmp_priv->pmp_xmtframe_buf = NULL;
+}
+
+#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)
+
+s32 MPT_InitializeAdapter(struct adapter *pAdapter, u8 Channel)
+{
+	struct hal_data_8188e	*pHalData = GET_HAL_DATA(pAdapter);
+	s32		rtStatus = _SUCCESS;
+	struct mpt_context *pMptCtx = &pAdapter->mppriv.MptCtx;
+	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. */
+	/* init for BT MP */
+
+	pMptCtx->bMptWorkItemInProgress = false;
+	pMptCtx->CurrMptAct = NULL;
+	/*  */
+
+	/*  Don't accept any packets */
+	rtw_write32(pAdapter, REG_RCR, 0);
+
+	PHY_IQCalibrate(pAdapter, false);
+	dm_CheckTXPowerTracking(&pHalData->odmpriv);	/* trigger thermal meter */
+	PHY_LCCalibrate(pAdapter);
+
+	pMptCtx->backup0xc50 = (u8)PHY_QueryBBReg(pAdapter, rOFDM0_XAAGCCore1, bMaskByte0);
+	pMptCtx->backup0xc58 = (u8)PHY_QueryBBReg(pAdapter, rOFDM0_XBAGCCore1, bMaskByte0);
+	pMptCtx->backup0xc30 = (u8)PHY_QueryBBReg(pAdapter, rOFDM0_RxDetector1, bMaskByte0);
+	pMptCtx->backup0x52_RF_A = (u8)PHY_QueryRFReg(pAdapter, RF_PATH_A, RF_0x52, 0x000F0);
+	pMptCtx->backup0x52_RF_B = (u8)PHY_QueryRFReg(pAdapter, RF_PATH_A, RF_0x52, 0x000F0);
+
+	/* 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:		struct adapter *	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(struct adapter *pAdapter)
+{
+	struct mpt_context *pMptCtx = &pAdapter->mppriv.MptCtx;
+
+	pMptCtx->bMptDrvUnload = true;
+}
+
+static u8 mpt_ProStartTest(struct adapter *padapter)
+{
+	struct mpt_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(struct adapter *padapter, u8 enable)
+{
+	Hal_SetPowerTracking(padapter, enable);
+	return 0;
+}
+
+void GetPowerTracking(struct adapter *padapter, u8 *enable)
+{
+	Hal_GetPowerTracking(padapter, enable);
+}
+
+static void disable_dm(struct adapter *padapter)
+{
+	u8 v8;
+
+	/* 3 1. disable firmware dynamic mechanism */
+	/*  disable Power Training, Rate Adaptive */
+	v8 = rtw_read8(padapter, REG_BCN_CTRL);
+	v8 &= ~EN_BCN_FUNCTION;
+	rtw_write8(padapter, REG_BCN_CTRL, v8);
+
+	/* 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 */
+	Switch_DM_Func(padapter, DYNAMIC_RF_CALIBRATION, true);
+}
+
+/* This function initializes the DUT to the MP test mode */
+s32 mp_start_test(struct adapter *padapter)
+{
+	struct wlan_bssid_ex bssid;
+	struct sta_info *psta;
+	u32 length;
+	u8 val8;
+
+	unsigned long 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 (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 struct wlan_bssid_ex */
+/*	_rtw_memset(&bssid, 0, sizeof(struct wlan_bssid_ex)); */
+	memcpy(bssid.MacAddress, pmppriv->network_macaddr, ETH_ALEN);
+	bssid.Ssid.SsidLength = strlen("mp_pseudo_adhoc");
+	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;
+	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 = 1;
+	psta->aid = 1;
+	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 */
+		val8 = rtw_read8(padapter, MSR) & 0xFC; /*  0x0102 */
+		val8 |= WIFI_FW_ADHOC_STATE;
+		rtw_write8(padapter, MSR, val8); /*  Link in ad hoc network */
+	}
+	return res;
+}
+/*  */
+/* This function change the DUT from the MP test mode into normal mode */
+void mp_stop_test(struct adapter *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;
+
+	unsigned long 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. */
+		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_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(struct adapter *pAdapter)
+{
+	Hal_SetChannel(pAdapter);
+}
+
+/*
+ * Notice
+ *	Switch bandwitdth may change center frequency(channel)
+ */
+void SetBandwidth(struct adapter *pAdapter)
+{
+	Hal_SetBandwidth(pAdapter);
+}
+
+void SetAntenna(struct adapter *pAdapter)
+{
+	Hal_SetAntenna(pAdapter);
+}
+
+void	SetAntennaPathPower(struct adapter *pAdapter)
+{
+	Hal_SetAntennaPathPower(pAdapter);
+}
+
+void SetTxPower(struct adapter *pAdapter)
+{
+	Hal_SetTxPower(pAdapter);
+	}
+
+void SetDataRate(struct adapter *pAdapter)
+{
+	Hal_SetDataRate(pAdapter);
+}
+
+void MP_PHY_SetRFPathSwitch(struct adapter *pAdapter , bool bMain)
+{
+	PHY_SetRFPathSwitch(pAdapter, bMain);
+}
+
+s32 SetThermalMeter(struct adapter *pAdapter, u8 target_ther)
+{
+	return Hal_SetThermalMeter(pAdapter, target_ther);
+}
+
+void GetThermalMeter(struct adapter *pAdapter, u8 *value)
+{
+	Hal_GetThermalMeter(pAdapter, value);
+}
+
+void SetSingleCarrierTx(struct adapter *pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	Hal_SetSingleCarrierTx(pAdapter, bStart);
+}
+
+void SetSingleToneTx(struct adapter *pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	Hal_SetSingleToneTx(pAdapter, bStart);
+}
+
+void SetCarrierSuppressionTx(struct adapter *pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	Hal_SetCarrierSuppressionTx(pAdapter, bStart);
+}
+
+void SetContinuousTx(struct adapter *pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	Hal_SetContinuousTx(pAdapter, bStart);
+}
+
+
+void PhySetTxPowerLevel(struct adapter *pAdapter)
+{
+	struct mp_priv *pmp_priv = &pAdapter->mppriv;
+
+	if (pmp_priv->bSetTxPower == 0) /*  for NO manually set power index */
+		PHY_SetTxPowerLevel8188E(pAdapter, pmp_priv->channel);
+}
+
+/*  */
+static void dump_mpframe(struct adapter *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;
+
+	pmpframe = rtw_alloc_xmitframe(pxmitpriv);
+	if (pmpframe == NULL)
+		return NULL;
+
+	pxmitbuf = rtw_alloc_xmitbuf(pxmitpriv);
+	if (pxmitbuf == 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 int mp_xmit_packet_thread(void *context)
+{
+	struct xmit_frame	*pxmitframe;
+	struct mp_tx		*pmptx;
+	struct mp_priv	*pmp_priv;
+	struct xmit_priv	*pxmitpriv;
+	struct adapter *padapter;
+
+	pmp_priv = (struct mp_priv *)context;
+	pmptx = &pmp_priv->tx;
+	padapter = pmp_priv->papdater;
+	pxmitpriv = &(padapter->xmitpriv);
+
+	thread_enter("RTW_MP_THREAD");
+
+	/* DBG_88E("%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;
+			}
+		}
+
+		memcpy((u8 *)(pxmitframe->buf_addr+TXDESC_OFFSET), pmptx->buf, pmptx->write_size);
+		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:
+	kfree(pmptx->pallocated_buf);
+	pmptx->pallocated_buf = NULL;
+	pmptx->stop = 1;
+
+	thread_exit();
+}
+
+void fill_txdesc_for_mp(struct adapter *padapter, struct tx_desc *ptxdesc)
+{
+	struct mp_priv *pmp_priv = &padapter->mppriv;
+	memcpy(ptxdesc, &(pmp_priv->tx.desc), TXDESC_SIZE);
+}
+
+void SetPacketTx(struct adapter *padapter)
+{
+	u8 *ptr, *pkt_start, *pkt_end;
+	u32 pkt_size;
+	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;
+	memcpy(pattrib->src, padapter->eeprompriv.mac_addr, ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+	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;
+
+	kfree(pmp_priv->tx.pallocated_buf);
+	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_88E("%s: malloc(%d) fail!!\n", __func__, pmp_priv->tx.buf_size);
+		return;
+	}
+	pmp_priv->tx.buf = (u8 *)N_BYTE_ALIGMENT((size_t)(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 */
+	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);
+	/*  offset 4 */
+		desc->txdw1 |= cpu_to_le32((pattrib->mac_id) & 0x3F); /* CAM_ID(MAC_ID) */
+	desc->txdw1 |= cpu_to_le32((pattrib->qsel << QSEL_SHT) & 0x00001F00); /*  Queue Select, TID */
+
+	desc->txdw1 |= cpu_to_le32((pattrib->raid << RATE_ID_SHT) & 0x000F0000); /*  Rate Adaptive ID */
+	/*  offset 8 */
+	/*  offset 12 */
+
+	desc->txdw3 |= cpu_to_le32((pattrib->seqnum<<16)&0x0fff0000);
+
+	/*  offset 16 */
+	desc->txdw4 |= cpu_to_le32(HW_SSN);
+	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 */
+	}
+	desc->txdw5 |= cpu_to_le32(RTY_LMT_EN); /*  retry limit enable */
+	desc->txdw5 |= cpu_to_le32(0x00180000); /*  DATA/RTS Rate Fallback Limit */
+
+	/* 3 4. make wlan header, make_wlanhdr() */
+	hdr = (struct rtw_ieee80211_hdr *)pkt_start;
+	SetFrameSubType(&hdr->frame_ctl, pattrib->subtype);
+	memcpy(hdr->addr1, pattrib->dst, ETH_ALEN); /*  DA */
+	memcpy(hdr->addr2, pattrib->src, ETH_ALEN); /*  SA */
+	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 */
+	pmp_priv->tx.PktTxThread = kthread_run(mp_xmit_packet_thread, pmp_priv, "RTW_MP_THREAD");
+	if (IS_ERR(pmp_priv->tx.PktTxThread))
+		DBG_88E("Create PktTx Thread Fail !!!!!\n");
+}
+
+void SetPacketRx(struct adapter *pAdapter, u8 bStartRx)
+{
+	struct hal_data_8188e	*pHalData = GET_HAL_DATA(pAdapter);
+
+	if (bStartRx) {
+		/*  Accept CRC error and destination address */
+		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, 0);
+	}
+}
+
+void ResetPhyRxPktCount(struct adapter *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(struct adapter *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(struct adapter *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(struct adapter *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(struct adapter *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(struct adapter *pAdapter, u8 *data)
+{
+	u32 i, psd_pts = 0, psd_start = 0, psd_stop = 0;
+	u32 psd_data = 0;
+
+
+	if (!netif_running(pAdapter->pnetdev)) {
+		RT_TRACE(_module_mp_, _drv_warning_, ("mp_query_psd: Fail! interface not opened!\n"));
+		return 0;
+	}
+
+	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++;
+	}
+
+	rtw_msleep_os(100);
+	return strlen(data)+1;
+}
+
+void _rtw_mp_xmit_priv(struct xmit_priv *pxmitpriv)
+{
+	   int i, res;
+	  struct adapter *padapter = pxmitpriv->adapter;
+	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 = 20000;
+		num_xmit_extbuf = 1;
+	}
+
+	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 = 20000;
+		num_xmit_extbuf = 1;
+	} 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_t)(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;
+
+		res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, max_xmit_extbuf_size + XMITBUF_ALIGN_SZ);
+		if (res == _FAIL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		rtw_list_insert_tail(&pxmitbuf->list, &(pxmitpriv->free_xmit_extbuf_queue.queue));
+		pxmitbuf++;
+	}
+
+	pxmitpriv->free_xmit_extbuf_cnt = num_xmit_extbuf;
+
+exit:
+	;
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_mp_ioctl.c b/drivers/staging/rtl8188eu/core/rtw_mp_ioctl.c
new file mode 100644
index 0000000..12fecde
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_mp_ioctl.c
@@ -0,0 +1,1508 @@
+/******************************************************************************
+ *
+ * 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 <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 **************** */
+int oid_rt_wireless_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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 **************** */
+int oid_rt_pro_write_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct bb_reg_param *pbbreg;
+	u16 offset;
+	u32 value;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_pro_read_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct bb_reg_param *pbbreg;
+	u16 offset;
+	u32 value;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_pro_write_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct rf_reg_param *pbbreg;
+	u8 path;
+	u8 offset;
+	u32 value;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_pro_read_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct rf_reg_param *pbbreg;
+	u8 path;
+	u8 offset;
+	u32 value;
+	struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+	int 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 **************** */
+/*  */
+int oid_rt_pro_set_data_rate_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u32		ratevalue;/* 4 */
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_pro_start_test_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u32		mode;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_pro_stop_test_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_pro_set_channel_direct_call_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u32		Channel;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_set_bandwidth_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u16		bandwidth;
+	u16		channel_offset;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *padapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_pro_set_antenna_bb_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u32		antenna;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+
+int oid_rt_pro_set_tx_power_control_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u32		tx_pwr_idx;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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 **************** */
+/*  */
+int oid_rt_pro_query_tx_packet_sent_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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(u32)) {
+		*(u32 *)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;
+}
+/*  */
+int oid_rt_pro_query_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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(u32)) {
+		*(u32 *)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;
+}
+/*  */
+int oid_rt_pro_query_rx_packet_crc32_error_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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(u32)) {
+		*(u32 *)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;
+}
+/*  */
+
+int oid_rt_pro_reset_tx_packet_sent_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_pro_reset_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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(u32)) {
+		Adapter->mppriv.rx_pktcount = 0;
+		Adapter->mppriv.rx_crcerrpktcount = 0;
+	} else {
+		status = NDIS_STATUS_INVALID_LENGTH;
+	}
+
+_func_exit_;
+
+	return status;
+}
+/*  */
+int oid_rt_reset_phy_rx_packet_count_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_get_phy_rx_packet_received_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	*(u32 *)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", *(u32 *)poid_par_priv->information_buf));
+
+_func_exit_;
+
+	return status;
+}
+/*  */
+int oid_rt_get_phy_rx_packet_crc32_error_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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(u32))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	*(u32 *)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",
+		 *(u32 *)poid_par_priv->information_buf));
+
+_func_exit_;
+
+	return status;
+}
+/*   oid_rtl_seg_81_80_20   section end **************** */
+int oid_rt_pro_set_continuous_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u32		bStartTest;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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_88E("%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;
+}
+
+int oid_rt_pro_set_single_carrier_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u32		bStartTest;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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_88E("%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;
+}
+
+int oid_rt_pro_set_carrier_suppression_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u32		bStartTest;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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_88E("%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;
+}
+
+int oid_rt_pro_set_single_tone_tx_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u32		bStartTest;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+
+int oid_rt_pro_set_modulation_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+
+int oid_rt_pro_trigger_gpio_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+	int 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, NULL);
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+/*   oid_rtl_seg_81_80_00   section end **************** */
+/*  */
+int oid_rt_pro8711_join_bss_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro_read_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct mp_rw_reg *RegRWStruct;
+	u32		offset, width;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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 = (struct mp_rw_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;
+}
+/*  */
+int oid_rt_pro_write_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct mp_rw_reg *RegRWStruct;
+	u32		offset, width, value;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *padapter = (struct adapter *)(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 = (struct mp_rw_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;
+}
+/*  */
+int oid_rt_pro_burst_read_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro_burst_write_register_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro_write_txcmd_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+
+/*  */
+int oid_rt_pro_read16_eeprom_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+
+/*  */
+int oid_rt_pro_write16_eeprom_hdl (struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro8711_wi_poll_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro8711_pkt_loss_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_rd_attrib_mem_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_wr_attrib_mem_hdl (struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int  oid_rt_pro_set_rf_intfs_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_poll_rx_status_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro_cfg_debug_message_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro_set_data_rate_ex_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+	int 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;
+}
+/*  */
+int oid_rt_get_thermal_meter_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	u8 thermal = 0;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_pro_read_tssi_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro_set_power_tracking_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+
+_func_enter_;
+
+	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;
+}
+/*  */
+int oid_rt_pro_set_basic_rate_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro_qry_pwrstate_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro_set_pwrstate_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro_h2c_set_rate_table_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+/*  */
+int oid_rt_pro_h2c_get_rate_table_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+
+/*   oid_rtl_seg_87_12_00   section start **************** */
+int oid_rt_pro_encryption_ctrl_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+
+int oid_rt_pro_add_sta_info_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+
+int oid_rt_pro_dele_sta_info_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+
+int oid_rt_pro_query_dr_variable_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+
+int oid_rt_pro_rx_packet_type_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return NDIS_STATUS_SUCCESS;
+}
+/*  */
+int oid_rt_pro_read_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct efuse_access_struct *pefuse;
+	u8 *data;
+	u16 addr = 0, cnts = 0, max_available_size = 0;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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(struct efuse_access_struct))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	pefuse = (struct efuse_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, (void *)&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;
+}
+/*  */
+int oid_rt_pro_write_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct efuse_access_struct *pefuse;
+	u8 *data;
+	u16 addr = 0, cnts = 0, max_available_size = 0;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+
+_func_enter_;
+
+	if (poid_par_priv->type_of_oid != SET_OID)
+		return NDIS_STATUS_NOT_ACCEPTED;
+
+	pefuse = (struct efuse_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, (void *)&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;
+}
+/*  */
+int oid_rt_pro_rw_efuse_pgpkt_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct pgpkt *ppgpkt;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(poid_par_priv->adapter_context);
+
+_func_enter_;
+
+	*poid_par_priv->bytes_rw = 0;
+
+	if (poid_par_priv->information_buf_len < sizeof(struct pgpkt *))
+		return NDIS_STATUS_INVALID_LENGTH;
+
+	ppgpkt = (struct pgpkt *)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;
+}
+/*  */
+int oid_rt_get_efuse_current_size_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u16 size;
+	u8 ret;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_get_efuse_max_size_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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;
+}
+/*  */
+int oid_rt_pro_efuse_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int 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;
+}
+/*  */
+int oid_rt_pro_efuse_map_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u8		*data;
+	int status = NDIS_STATUS_SUCCESS;
+	struct adapter *Adapter = (struct adapter *)(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, (void *)&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;
+}
+
+int oid_rt_set_crystal_cap_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+	return status;
+}
+
+int oid_rt_set_rx_packet_type_hdl(struct oid_par_priv *poid_par_priv)
+{
+	u8		rx_pkt_type;
+	int status = NDIS_STATUS_SUCCESS;
+
+_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));
+_func_exit_;
+
+	return status;
+}
+
+int oid_rt_pro_set_tx_agc_offset_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+
+int oid_rt_pro_set_pkt_test_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
+
+int mp_ioctl_xmit_packet_hdl(struct oid_par_priv *poid_par_priv)
+{
+	struct mp_xmit_parm *pparm;
+	struct adapter *padapter;
+	struct mp_priv *pmp_priv;
+	struct pkt_attrib *pattrib;
+
+	RT_TRACE(_module_mp_, _drv_notice_, ("+%s\n", __func__));
+
+	pparm = (struct mp_xmit_parm *)poid_par_priv->information_buf;
+	padapter = (struct adapter *)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;
+			memcpy(pattrib->dst, pparm->da, ETH_ALEN);
+			SetPacketTx(padapter);
+		} else {
+			return NDIS_STATUS_FAILURE;
+		}
+	}
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*  */
+int oid_rt_set_power_down_hdl(struct oid_par_priv *poid_par_priv)
+{
+	int status = NDIS_STATUS_SUCCESS;
+
+_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);
+
+	/* CALL  the power_down function */
+	_irqlevel_changed_(&oldirql, RAISE);
+
+_func_exit_;
+
+	return status;
+}
+/*  */
+int oid_rt_get_power_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	return 0;
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_p2p.c b/drivers/staging/rtl8188eu/core/rtw_p2p.c
new file mode 100644
index 0000000..8cf915f
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_p2p.c
@@ -0,0 +1,2064 @@
+/******************************************************************************
+ *
+ * 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_88EU_P2P
+
+static 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;
+}
+
+static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	unsigned long irqL;
+	struct list_head *phead, *plist;
+	u32 len = 0;
+	u16 attr_len = 0;
+	u8 tmplen, *pdata_attr, *pstart, *pcur;
+	struct sta_info *psta = NULL;
+	struct adapter *padapter = pwdinfo->padapter;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	DBG_88E("%s\n", __func__);
+
+	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 */
+			memcpy(pcur, psta->dev_addr, ETH_ALEN);
+			pcur += ETH_ALEN;
+
+			/* P2P interface address */
+			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;
+
+			memcpy(pcur, psta->primary_dev_type, 8);
+			pcur += 8;
+
+			*pcur = psta->num_of_secdev_type;
+			pcur++;
+
+			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;
+
+				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);
+
+	kfree(pdata_attr);
+	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;
+	struct 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 */
+	__be32	p2poui = cpu_to_be32(P2POUI);
+	u8	oui_subtype = P2P_GO_DISC_REQUEST;
+	u8	dialogToken = 0;
+
+	DBG_88E("[%s]\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
+	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;
+	struct 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;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_DEVDISC_RESP;
+	u8 p2pie[8] = { 0x00 };
+	u32 p2pielen = 0;
+
+	DBG_88E("[%s]\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, pwdinfo->device_addr, ETH_ALEN);
+	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)
+{
+	struct 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. */
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_PROVISION_DISC_RESP;
+	u8			wpsie[100] = { 0x00 };
+	u8			wpsielen = 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);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	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 */
+	RTW_PUT_BE32(wpsie, WPSOUI);
+	wpsielen += 4;
+
+	/*	Config Method */
+	/*	Type: */
+	RTW_PUT_BE16(wpsie + wpsielen, WPS_ATTR_CONF_METHOD);
+	wpsielen += 2;
+
+	/*	Length: */
+	RTW_PUT_BE16(wpsie + wpsielen, 0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	RTW_PUT_BE16(wpsie + wpsielen, config_method);
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);
+
+	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;
+	struct 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 */
+	__be32	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_88E("[%s]\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == 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;
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
+	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;
+	__le16 le_tmp;
+
+	/*	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);
+
+	le_tmp = cpu_to_le16(capability);
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8 *)&le_tmp);
+
+	/*  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: */
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &len);
+	return len;
+}
+
+u32 build_probe_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	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 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: */
+
+	/*	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 */
+	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 */
+	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: */
+	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 */
+	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: */
+	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: */
+		memcpy(p2pie + p2pielen, pdev_raddr, ETH_ALEN);
+		p2pielen += ETH_ALEN;
+
+		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)) {
+		p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_ , len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_ , NULL, &p2pielen);
+		if (p2pie) {
+			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;
+	__be16 be_tmp;
+	__le16 le_tmp;
+
+	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_88E("[%s] P2P IE not Found!!\n", __func__);
+		status_code =  P2P_STATUS_FAIL_INVALID_PARAM;
+	} else {
+		DBG_88E("[%s] P2P IE Found!!\n", __func__);
+	}
+
+	while (p2p_ie) {
+		/* Check P2P Capability ATTR */
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8 *)&le_tmp, (uint *)&attr_contentlen)) {
+			DBG_88E("[%s] Got P2P Capability Attr!!\n", __func__);
+			cap_attr = le16_to_cpu(le_tmp);
+			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_88E("[%s] Got P2P DEVICE INFO Attr!!\n", __func__);
+			pattr_content = rtw_zmalloc(attr_contentlen);
+			pbuf = pattr_content;
+			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);
+
+				memcpy(psta->dev_addr,	pattr_content, ETH_ALEN);/* P2P Device Address */
+
+				pattr_content += ETH_ALEN;
+
+				memcpy(&be_tmp, pattr_content, 2);/* Config Methods */
+				psta->config_methods = be16_to_cpu(be_tmp);
+
+				pattr_content += 2;
+
+				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);
+
+					memcpy(psta->secdev_types_list, pattr_content, len);
+
+					pattr_content += (num_of_secdev_type*8);
+				}
+
+
+				psta->dev_name_len = 0;
+				if (WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(__be16 *)pattr_content)) {
+					dev_name_len = be16_to_cpu(*(__be16 *)(pattr_content+2));
+
+					psta->dev_name_len = (sizeof(psta->dev_name) < dev_name_len) ? sizeof(psta->dev_name) : dev_name_len;
+
+					memcpy(psta->dev_name, pattr_content+4, psta->dev_name_len);
+				}
+				kfree(pbuf);
+			}
+		}
+
+		/* 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;
+	struct 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;
+
+	p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+	if (p2p_ie) {
+		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)) {
+					unsigned long irqL;
+					struct list_head *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)) {
+							/* issue GO Discoverability Request */
+							issue_group_disc_req(pwdinfo, psta->hwaddr);
+							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;
+	__be16 be_tmp;
+
+	frame_body = (pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	wpsie = rtw_get_wps_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen);
+	if (wpsie) {
+		if (rtw_get_wps_attr_content(wpsie, wps_ielen, WPS_ATTR_CONF_METHOD, (u8 *)&be_tmp, &attr_contentlen)) {
+			uconfig_method = be16_to_cpu(be_tmp);
+			switch (uconfig_method) {
+			case WPS_CM_DISPLYA:
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
+				break;
+			case WPS_CM_LABEL:
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "lab", 3);
+				break;
+			case WPS_CM_PUSH_BUTTON:
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
+				break;
+			case WPS_CM_KEYPAD:
+				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_88E("[%s] config method = %s\n", __func__, 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;
+}
+
+static 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;
+}
+
+static 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)
+{
+	struct 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;
+	__be16 be_tmp;
+
+	wpsie = rtw_get_wps_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen);
+	if (wpsie) {
+		/*	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 *)&be_tmp, &wps_devicepassword_id_len);
+			wps_devicepassword_id = be16_to_cpu(be_tmp);
+
+			if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
+			else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
+			else
+				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
+		}
+	} else {
+		DBG_88E("[%s] WPS IE not Found!!\n", __func__);
+		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_88E("[%s] P2P IE not Found!!\n", __func__);
+		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;
+
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING);
+
+			if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen)) {
+			DBG_88E("[%s] GO Intent = %d, tie = %d\n", __func__, 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. */
+				memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
+				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_88E("[%s] No common channel in channel list!\n", __func__);
+				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)) {
+					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_88E("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
+					} else {
+						/*  Take first channel of ch_list_inclusioned as operating channel */
+						pwdinfo->operating_channel = ch_list_inclusioned[0];
+						DBG_88E("[%s] Change op ch to %02x\n", __func__, 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);
+	}
+	return result;
+}
+
+u8 process_p2p_group_negotation_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	struct adapter *padapter = pwdinfo->padapter;
+	u8	result = P2P_STATUS_SUCCESS;
+	u32	p2p_ielen, wps_ielen;
+	u8 *ies;
+	u32 ies_len;
+	u8 *p2p_ie;
+
+	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_88E("[%s] WPS IE not Found!!\n", __func__);
+		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 };
+		u8	groupid[38];
+		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. */
+			rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+			if (attr_contentlen == 1) {
+				DBG_88E("[%s] Status = %d\n", __func__, 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_88E("[%s] GO Intent = %d, tie = %d\n", __func__, 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. */
+					memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
+					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_88E("[%s] Peer's operating channel = %d\n", __func__, 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_88E("[%s] channel list attribute found, len = %d\n", __func__,  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_88E("[%s] No common channel in channel list!\n", __func__);
+					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)) {
+						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_88E("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
+						} else {
+							/*  Take first channel of ch_list_inclusioned as operating channel */
+							pwdinfo->operating_channel = ch_list_inclusioned[0];
+							DBG_88E("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
+						}
+					}
+				}
+			} else {
+				DBG_88E("[%s] channel list attribute not found!\n", __func__);
+			}
+
+			/*	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)) {
+				memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
+				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);
+		}
+	}
+	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_88E("[%s] Status = %d\n", __func__, 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);
+				}
+			} 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_88E("[%s] Ssid = %s, ssidlen = %zu\n", __func__, &groupid[ETH_ALEN], strlen(&groupid[ETH_ALEN]));
+			memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
+			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_88E("[%s] Peer's operating channel = %d\n", __func__, 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;
+}
+
+static void find_phase_handler(struct adapter *padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct ndis_802_11_ssid	ssid;
+	unsigned long				irqL;
+
+_func_enter_;
+
+	_rtw_memset((unsigned char *)&ssid, 0, sizeof(struct ndis_802_11_ssid));
+	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);
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+
+_func_exit_;
+}
+
+void p2p_concurrent_handler(struct adapter *padapter);
+
+static void restore_p2p_state_handler(struct adapter *padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+
+_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);
+	rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
+		/*	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);
+	}
+_func_exit_;
+}
+
+static void pre_tx_invitereq_handler(struct 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_;
+}
+
+static void pre_tx_provdisc_handler(struct 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_;
+}
+
+static void pre_tx_negoreq_handler(struct 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_;
+}
+
+void p2p_protocol_wk_hdl(struct adapter *padapter, int intCmdType)
+{
+_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:
+		pre_tx_provdisc_handler(padapter);
+		break;
+	case P2P_PRE_TX_INVITEREQ_PROCESS_WK:
+		pre_tx_invitereq_handler(padapter);
+		break;
+	case P2P_PRE_TX_NEGOREQ_PROCESS_WK:
+		pre_tx_negoreq_handler(padapter);
+		break;
+	}
+
+_func_exit_;
+}
+
+void process_p2p_ps_ie(struct adapter *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;
+	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) {
+						/* memcpy(&wifidirect_info->noa_count[noa_num], &noa_attr[noa_offset], 1); */
+						pwdinfo->noa_count[noa_num] = noa_attr[noa_offset];
+						noa_offset += 1;
+
+						memcpy(&pwdinfo->noa_duration[noa_num], &noa_attr[noa_offset], 4);
+						noa_offset += 4;
+
+						memcpy(&pwdinfo->noa_interval[noa_num], &noa_attr[noa_offset], 4);
+						noa_offset += 4;
+
+						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)
+						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) {
+		if ((pwdinfo->p2p_ps_mode > P2P_PS_NONE) && !find_p2p_ps)
+			p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+	}
+
+_func_exit_;
+}
+
+void p2p_ps_wk_hdl(struct 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) {
+			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_88E("%s(): Enter CTW, change SmartPS\n", __func__);
+					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(struct 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))
+		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) {
+			kfree(ph2c);
+			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;
+}
+
+static void reset_ch_sitesurvey_timer_process (void *FunctionContext)
+{
+	struct adapter *adapter = (struct adapter *)FunctionContext;
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	DBG_88E("[%s] In\n", __func__);
+	/*	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)
+{
+	struct adapter *adapter = (struct adapter *)FunctionContext;
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	DBG_88E("[%s] In\n", __func__);
+	/*	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)
+{
+	struct adapter *adapter = (struct 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)
+{
+	struct adapter *adapter = (struct adapter *)FunctionContext;
+	struct	wifidirect_info *pwdinfo = &adapter->wdinfo;
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	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 (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 (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 (pwdinfo->invitereq_info.benable)
+			p2p_protocol_wk_cmd(adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK);
+	} else {
+		DBG_88E("[%s] p2p_state is %d, ignore!!\n", __func__, rtw_p2p_state(pwdinfo));
+	}
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+}
+
+static void find_phase_timer_process(void *FunctionContext)
+{
+	struct adapter *adapter = (struct 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);
+}
+
+void reset_global_wifidirect_info(struct 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;
+}
+
+void rtw_init_wifidirect_timers(struct 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);
+}
+
+void rtw_init_wifidirect_addrs(struct adapter *padapter, u8 *dev_addr, u8 *iface_addr)
+{
+#ifdef CONFIG_88EU_P2P
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	/*init device&interface address */
+	if (dev_addr)
+		memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN);
+	if (iface_addr)
+		memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN);
+#endif
+}
+
+void init_wifidirect_info(struct adapter *padapter, enum P2P_ROLE role)
+{
+	struct wifidirect_info	*pwdinfo;
+
+	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. */
+
+	/*	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);
+		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 */
+
+	memcpy(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);
+
+	_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;
+	pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD;
+	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));
+	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;
+}
+
+int rtw_p2p_enable(struct adapter *padapter, enum P2P_ROLE role)
+{
+	int ret = _SUCCESS;
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+	if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT || role == P2P_ROLE_GO) {
+		/* 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);
+	} 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);
+			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);
+
+		/* Restore to initial setting. */
+		update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+	}
+
+exit:
+	return ret;
+}
+
+#else
+u8 p2p_ps_wk_cmd(struct adapter *padapter, u8 p2p_ps_state, u8 enqueue)
+{
+	return _FAIL;
+}
+
+void process_p2p_ps_ie(struct adapter *padapter, u8 *IEs, u32 IELength)
+{
+}
+
+#endif /* CONFIG_88EU_P2P */