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>

drivers/staging/rtl8188eu/core/rtw_led.c

+/******************************************************************************
+ *
+ * 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));
+}

drivers/staging/rtl8188eu/core/rtw_mlme.c

+/******************************************************************************
+ *
+ * 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;
+				}
+			}
+		}
+	}
+}

drivers/staging/rtl8188eu/core/rtw_mp.c

+/******************************************************************************
+ *
+ * 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:
+	;
+}

drivers/staging/rtl8188eu/core/rtw_mp_ioctl.c

+/******************************************************************************
+ *
+ * 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;
+}

drivers/staging/rtl8188eu/core/rtw_p2p.c

+/******************************************************************************
+ *
+ * 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 */