[PATCH 2/7] staging: rtl8188eu: introduce new core dir for RTL8188eu driver

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

 



This patchset is split in order to keep the file sizes down. This core
directory is part of the newer/better driver from GitHub modified by
Larry Finger. Import this as the basis for all future work going
forward.

Suggested-by: Larry Finger <Larry.Finger@xxxxxxxxxxxx>
Signed-off-by: Phillip Potter <phil@xxxxxxxxxxxxxxxx>
---
 drivers/staging/rtl8188eu/core/rtw_ap.c       |  3778 ++++
 .../staging/rtl8188eu/core/rtw_beamforming.c  |  3071 ++++
 drivers/staging/rtl8188eu/core/rtw_br_ext.c   |  1529 ++
 drivers/staging/rtl8188eu/core/rtw_btcoex.c   |  1632 ++
 .../rtl8188eu/core/rtw_btcoex_wifionly.c      |    26 +
 drivers/staging/rtl8188eu/core/rtw_cmd.c      |  4433 +++++
 drivers/staging/rtl8188eu/core/rtw_debug.c    |  4800 +++++
 drivers/staging/rtl8188eu/core/rtw_efuse.c    |  1523 ++
 .../staging/rtl8188eu/core/rtw_ieee80211.c    |  2673 +++
 drivers/staging/rtl8188eu/core/rtw_io.c       |   473 +
 .../staging/rtl8188eu/core/rtw_ioctl_query.c  |     6 +
 .../staging/rtl8188eu/core/rtw_ioctl_set.c    |  1035 ++
 drivers/staging/rtl8188eu/core/rtw_iol.c      |   355 +
 drivers/staging/rtl8188eu/core/rtw_led.c      |  1705 ++
 drivers/staging/rtl8188eu/core/rtw_mi.c       |  1198 ++
 drivers/staging/rtl8188eu/core/rtw_mlme.c     |  4401 +++++
 drivers/staging/rtl8188eu/core/rtw_mlme_ext.c | 15277 ++++++++++++++++
 drivers/staging/rtl8188eu/core/rtw_mp.c       |  2620 +++
 drivers/staging/rtl8188eu/core/rtw_odm.c      |   430 +
 drivers/staging/rtl8188eu/core/rtw_p2p.c      |  5351 ++++++
 drivers/staging/rtl8188eu/core/rtw_pwrctrl.c  |  2480 +++
 drivers/staging/rtl8188eu/core/rtw_recv.c     |  4191 +++++
 drivers/staging/rtl8188eu/core/rtw_rf.c       |  1077 ++
 drivers/staging/rtl8188eu/core/rtw_security.c |  3127 ++++
 drivers/staging/rtl8188eu/core/rtw_sreset.c   |   291 +
 drivers/staging/rtl8188eu/core/rtw_sta_mgt.c  |  1096 ++
 drivers/staging/rtl8188eu/core/rtw_tdls.c     |  3092 ++++
 .../staging/rtl8188eu/core/rtw_wlan_util.c    |  4140 +++++
 drivers/staging/rtl8188eu/core/rtw_xmit.c     |  5070 +++++
 29 files changed, 80880 insertions(+)
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_ap.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_beamforming.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_br_ext.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_btcoex.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_btcoex_wifionly.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_cmd.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_debug.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_efuse.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_ieee80211.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_io.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_ioctl_query.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_iol.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_led.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_mi.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_mlme.c
 create mode 100755 drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_mp.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_odm.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_p2p.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
 create mode 100755 drivers/staging/rtl8188eu/core/rtw_recv.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_rf.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_security.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_sreset.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_tdls.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_wlan_util.c
 create mode 100644 drivers/staging/rtl8188eu/core/rtw_xmit.c

diff --git a/drivers/staging/rtl8188eu/core/rtw_ap.c b/drivers/staging/rtl8188eu/core/rtw_ap.c
new file mode 100644
index 000000000000..f75dcd0e6d16
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_ap.c
@@ -0,0 +1,3778 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_AP_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#ifdef CONFIG_AP_MODE
+
+extern unsigned char	RTW_WPA_OUI[];
+extern unsigned char	WMM_OUI[];
+extern unsigned char	WPS_OUI[];
+extern unsigned char	P2P_OUI[];
+extern unsigned char	WFD_OUI[];
+
+void init_mlme_ap_info(_adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	spin_lock_init(&pmlmepriv->bcn_update_lock);
+
+	/* pmlmeext->bstart_bss = false; */
+
+}
+
+void free_mlme_ap_info(_adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	stop_ap_mode(padapter);
+}
+
+static void update_BCNTIM(_adapter *padapter)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX *pnetwork_mlmeext = &(pmlmeinfo->network);
+	unsigned char *pie = pnetwork_mlmeext->IEs;
+	u8 *p, *dst_ie, *premainder_ie = NULL, *pbackup_remainder_ie = NULL;
+	__le16 tim_bitmap_le;
+	uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen;
+
+	tim_bitmap_le = cpu_to_le16(pstapriv->tim_bitmap);
+
+	p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, _TIM_IE_, &tim_ielen, pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_);
+	if (p != NULL && tim_ielen > 0) {
+		tim_ielen += 2;
+
+		premainder_ie = p + tim_ielen;
+
+		tim_ie_offset = (sint)(p - pie);
+
+		remainder_ielen = pnetwork_mlmeext->IELength - tim_ie_offset - tim_ielen;
+
+		/*append TIM IE from dst_ie offset*/
+		dst_ie = p;
+	} else {
+		tim_ielen = 0;
+
+		/*calculate head_len*/
+		offset = _FIXED_IE_LENGTH_;
+
+		/* get ssid_ie len */
+		p = rtw_get_ie(pie + _BEACON_IE_OFFSET_, _SSID_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_));
+		if (p != NULL)
+			offset += tmp_len + 2;
+
+		/*get supported rates len*/
+		p = rtw_get_ie(pie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_));
+		if (p !=  NULL)
+			offset += tmp_len + 2;
+
+		/*DS Parameter Set IE, len=3*/
+		offset += 3;
+
+		premainder_ie = pie + offset;
+
+		remainder_ielen = pnetwork_mlmeext->IELength - offset - tim_ielen;
+
+		/*append TIM IE from offset*/
+		dst_ie = pie + offset;
+
+	}
+
+	if (remainder_ielen > 0) {
+		pbackup_remainder_ie = rtw_malloc(remainder_ielen);
+		if (pbackup_remainder_ie && premainder_ie)
+			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+	}
+
+	*dst_ie++ = _TIM_IE_;
+
+	if ((pstapriv->tim_bitmap & 0xff00) && (pstapriv->tim_bitmap & 0x00fe))
+		tim_ielen = 5;
+	else
+		tim_ielen = 4;
+
+	*dst_ie++ = tim_ielen;
+
+	*dst_ie++ = 0;/*DTIM count*/
+	*dst_ie++ = 1;/*DTIM period*/
+
+	if (pstapriv->tim_bitmap & BIT(0))/*for bc/mc frames*/
+		*dst_ie++ = BIT(0);/*bitmap ctrl */
+	else
+		*dst_ie++ = 0;
+	if (tim_ielen == 4) {
+		u8 pvb = 0;
+
+		if (pstapriv->tim_bitmap & 0xff00)
+			pvb = le16_to_cpu(tim_bitmap_le) >> 8;
+		else
+			pvb = le16_to_cpu(tim_bitmap_le);
+		*dst_ie++ = pvb;
+	} else if (tim_ielen == 5) {
+		memcpy(dst_ie, &tim_bitmap_le, 2);
+		dst_ie += 2;
+	}
+
+	/*copy remainder IE*/
+	if (pbackup_remainder_ie) {
+		memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen);
+
+		rtw_mfree(pbackup_remainder_ie, remainder_ielen);
+	}
+
+	offset = (uint)(dst_ie - pie);
+	pnetwork_mlmeext->IELength = offset + remainder_ielen;
+}
+
+void rtw_add_bcn_ie(_adapter *padapter, WLAN_BSSID_EX *pnetwork, u8 index, u8 *data, u8 len)
+{
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8	bmatch = false;
+	u8	*pie = pnetwork->IEs;
+	u8	*p = NULL, *dst_ie = NULL, *premainder_ie = NULL, *pbackup_remainder_ie = NULL;
+	u32	i, offset, ielen, ie_offset, remainder_ielen = 0;
+
+	for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pnetwork->IELength;) {
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pnetwork->IEs + i);
+
+		if (pIE->ElementID > index)
+			break;
+		else if (pIE->ElementID == index) { /* already exist the same IE */
+			p = (u8 *)pIE;
+			ielen = pIE->Length;
+			bmatch = true;
+			break;
+		}
+
+		p = (u8 *)pIE;
+		ielen = pIE->Length;
+		i += (pIE->Length + 2);
+	}
+
+	if (p != NULL && ielen > 0) {
+		ielen += 2;
+
+		premainder_ie = p + ielen;
+
+		ie_offset = (sint)(p - pie);
+
+		remainder_ielen = pnetwork->IELength - ie_offset - ielen;
+
+		if (bmatch)
+			dst_ie = p;
+		else
+			dst_ie = (p + ielen);
+	}
+
+	if (dst_ie == NULL)
+		return;
+
+	if (remainder_ielen > 0) {
+		pbackup_remainder_ie = rtw_malloc(remainder_ielen);
+		if (pbackup_remainder_ie && premainder_ie)
+			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+	}
+
+	*dst_ie++ = index;
+	*dst_ie++ = len;
+
+	memcpy(dst_ie, data, len);
+	dst_ie += len;
+
+	/* copy remainder IE */
+	if (pbackup_remainder_ie) {
+		memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen);
+
+		rtw_mfree(pbackup_remainder_ie, remainder_ielen);
+	}
+
+	offset = (uint)(dst_ie - pie);
+	pnetwork->IELength = offset + remainder_ielen;
+}
+
+void rtw_remove_bcn_ie(_adapter *padapter, WLAN_BSSID_EX *pnetwork, u8 index)
+{
+	u8 *p, *dst_ie = NULL, *premainder_ie = NULL, *pbackup_remainder_ie = NULL;
+	uint offset, ielen, ie_offset, remainder_ielen = 0;
+	u8	*pie = pnetwork->IEs;
+
+	p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, index, &ielen, pnetwork->IELength - _FIXED_IE_LENGTH_);
+	if (p != NULL && ielen > 0) {
+		ielen += 2;
+
+		premainder_ie = p + ielen;
+
+		ie_offset = (sint)(p - pie);
+
+		remainder_ielen = pnetwork->IELength - ie_offset - ielen;
+
+		dst_ie = p;
+	} else
+		return;
+
+	if (remainder_ielen > 0) {
+		pbackup_remainder_ie = rtw_malloc(remainder_ielen);
+		if (pbackup_remainder_ie && premainder_ie)
+			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+	}
+
+	/* copy remainder IE */
+	if (pbackup_remainder_ie) {
+		memcpy(dst_ie, pbackup_remainder_ie, remainder_ielen);
+
+		rtw_mfree(pbackup_remainder_ie, remainder_ielen);
+	}
+
+	offset = (uint)(dst_ie - pie);
+	pnetwork->IELength = offset + remainder_ielen;
+}
+
+
+u8 chk_sta_is_alive(struct sta_info *psta);
+u8 chk_sta_is_alive(struct sta_info *psta)
+{
+	u8 ret = false;
+#ifdef DBG_EXPIRATION_CHK
+	RTW_INFO("sta:"MAC_FMT", rssi:%d, rx:"STA_PKTS_FMT", expire_to:%u, %s%ssq_len:%u\n"
+		 , MAC_ARG(psta->hwaddr)
+		 , psta->rssi_stat.undecorated_smoothed_pwdb
+		 /* , STA_RX_PKTS_ARG(psta) */
+		 , STA_RX_PKTS_DIFF_ARG(psta)
+		 , psta->expire_to
+		 , psta->state & WIFI_SLEEP_STATE ? "PS, " : ""
+		 , psta->state & WIFI_STA_ALIVE_CHK_STATE ? "SAC, " : ""
+		 , psta->sleepq_len
+		);
+#endif
+
+	/* if(sta_last_rx_pkts(psta) == sta_rx_pkts(psta)) */
+	if ((psta->sta_stats.last_rx_data_pkts + psta->sta_stats.last_rx_ctrl_pkts) == (psta->sta_stats.rx_data_pkts + psta->sta_stats.rx_ctrl_pkts)) {
+	} else
+		ret = true;
+
+	sta_update_last_rx_pkts(psta);
+
+	return ret;
+}
+
+void	expire_timeout_chk(_adapter *padapter)
+{
+	unsigned long irqL;
+	_list	*phead, *plist;
+	u8 updated = false;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 chk_alive_num = 0;
+	char chk_alive_list[NUM_STA];
+	int i;
+
+
+#ifdef CONFIG_MCC_MODE
+	/*	then driver may check fail due to not recv client's frame under sitesurvey,
+	 *	don't expire timeout chk under MCC under sitesurvey */
+
+	if (!rtw_hal_mcc_link_status_chk(padapter, __func__))
+		return;
+#endif
+
+	_enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
+
+	phead = &pstapriv->auth_list;
+	plist = get_next(phead);
+
+	/* check auth_queue */
+#ifdef DBG_EXPIRATION_CHK
+	if (!rtw_end_of_queue_search(phead, plist)) {
+		RTW_INFO(FUNC_NDEV_FMT" auth_list, cnt:%u\n"
+			, FUNC_NDEV_ARG(padapter->pnetdev), pstapriv->auth_list_cnt);
+	}
+#endif
+	while ((!rtw_end_of_queue_search(phead, plist))) {
+		psta = LIST_CONTAINOR(plist, struct sta_info, auth_list);
+
+		plist = get_next(plist);
+
+
+#ifdef CONFIG_ATMEL_RC_PATCH
+		if (!memcmp((void *)(pstapriv->atmel_rc_pattern), (void *)(psta->hwaddr), ETH_ALEN))
+			continue;
+		if (psta->flag_atmel_rc)
+			continue;
+#endif
+		if (psta->expire_to > 0) {
+			psta->expire_to--;
+			if (psta->expire_to == 0) {
+				list_del_init(&psta->auth_list);
+				pstapriv->auth_list_cnt--;
+
+				RTW_INFO("auth expire %02X%02X%02X%02X%02X%02X\n",
+					psta->hwaddr[0], psta->hwaddr[1], psta->hwaddr[2], psta->hwaddr[3], psta->hwaddr[4], psta->hwaddr[5]);
+
+				_exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
+
+				/* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);	 */
+				rtw_free_stainfo(padapter, psta);
+				/* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);	 */
+
+				_enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
+			}
+		}
+
+	}
+
+	_exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
+	psta = NULL;
+
+
+	_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+	phead = &pstapriv->asoc_list;
+	plist = get_next(phead);
+
+	/* check asoc_queue */
+#ifdef DBG_EXPIRATION_CHK
+	if (!rtw_end_of_queue_search(phead, plist)) {
+		RTW_INFO(FUNC_NDEV_FMT" asoc_list, cnt:%u\n"
+			, FUNC_NDEV_ARG(padapter->pnetdev), pstapriv->asoc_list_cnt);
+	}
+#endif
+	while ((!rtw_end_of_queue_search(phead, plist))) {
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+		plist = get_next(plist);
+#ifdef CONFIG_ATMEL_RC_PATCH
+		RTW_INFO("%s:%d  psta=%p, %02x,%02x||%02x,%02x  \n\n", __func__,  __LINE__,
+			psta, pstapriv->atmel_rc_pattern[0], pstapriv->atmel_rc_pattern[5], psta->hwaddr[0], psta->hwaddr[5]);
+		if (!memcmp((void *)pstapriv->atmel_rc_pattern, (void *)(psta->hwaddr), ETH_ALEN))
+			continue;
+		if (psta->flag_atmel_rc)
+			continue;
+		RTW_INFO("%s: debug line:%d\n", __func__, __LINE__);
+#endif
+#ifdef CONFIG_AUTO_AP_MODE
+		if (psta->isrc)
+			continue;
+#endif
+		if (chk_sta_is_alive(psta) || !psta->expire_to) {
+			psta->expire_to = pstapriv->expire_to;
+			psta->keep_alive_trycnt = 0;
+#ifdef CONFIG_TX_MCAST2UNI
+			psta->under_exist_checking = 0;
+#endif	/* CONFIG_TX_MCAST2UNI */
+		} else
+			psta->expire_to--;
+
+#ifndef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+#ifdef CONFIG_TX_MCAST2UNI
+		if ((psta->flags & WLAN_STA_HT) && (psta->htpriv.agg_enable_bitmap || psta->under_exist_checking)) {
+			/* check sta by delba(addba) for 11n STA */
+			/* ToDo: use CCX report to check for all STAs */
+			/* RTW_INFO("asoc check by DELBA/ADDBA! (pstapriv->expire_to=%d s)(psta->expire_to=%d s), [%02x, %d]\n", pstapriv->expire_to*2, psta->expire_to*2, psta->htpriv.agg_enable_bitmap, psta->under_exist_checking); */
+
+			if (psta->expire_to <= (pstapriv->expire_to - 50)) {
+				RTW_INFO("asoc expire by DELBA/ADDBA! (%d s)\n", (pstapriv->expire_to - psta->expire_to) * 2);
+				psta->under_exist_checking = 0;
+				psta->expire_to = 0;
+			} else if (psta->expire_to <= (pstapriv->expire_to - 3) && (psta->under_exist_checking == 0)) {
+				RTW_INFO("asoc check by DELBA/ADDBA! (%d s)\n", (pstapriv->expire_to - psta->expire_to) * 2);
+				psta->under_exist_checking = 1;
+				/* tear down TX AMPDU */
+				send_delba(padapter, 1, psta->hwaddr);/*  */ /* originator */
+				psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+				psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+			}
+		}
+#endif /* CONFIG_TX_MCAST2UNI */
+#endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */
+
+		if (psta->expire_to <= 0) {
+			struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+			if (padapter->registrypriv.wifi_spec == 1) {
+				psta->expire_to = pstapriv->expire_to;
+				continue;
+			}
+
+#ifndef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+
+#define KEEP_ALIVE_TRYCNT (3)
+
+			if (psta->keep_alive_trycnt > 0 && psta->keep_alive_trycnt <= KEEP_ALIVE_TRYCNT) {
+				if (psta->state & WIFI_STA_ALIVE_CHK_STATE)
+					psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+				else
+					psta->keep_alive_trycnt = 0;
+
+			} else if ((psta->keep_alive_trycnt > KEEP_ALIVE_TRYCNT) && !(psta->state & WIFI_STA_ALIVE_CHK_STATE))
+				psta->keep_alive_trycnt = 0;
+			if ((psta->htpriv.ht_option ) && (psta->htpriv.ampdu_enable == true)) {
+				uint priority = 1; /* test using BK */
+				u8 issued = 0;
+
+				/* issued = (psta->htpriv.agg_enable_bitmap>>priority)&0x1; */
+				issued |= (psta->htpriv.candidate_tid_bitmap >> priority) & 0x1;
+
+				if (0 == issued) {
+					if (!(psta->state & WIFI_STA_ALIVE_CHK_STATE)) {
+						psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority);
+
+						if (psta->state & WIFI_SLEEP_STATE)
+							psta->expire_to = 2; /* 2x2=4 sec */
+						else
+							psta->expire_to = 1; /* 2 sec */
+
+						psta->state |= WIFI_STA_ALIVE_CHK_STATE;
+
+						/* add_ba_hdl(padapter, (u8*)paddbareq_parm); */
+
+						RTW_INFO("issue addba_req to check if sta alive, keep_alive_trycnt=%d\n", psta->keep_alive_trycnt);
+
+						issue_addba_req(padapter, psta->hwaddr, (u8)priority);
+
+						_set_timer(&psta->addba_retry_timer, ADDBA_TO);
+
+						psta->keep_alive_trycnt++;
+
+						continue;
+					}
+				}
+			}
+			if (psta->keep_alive_trycnt > 0 && psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+				psta->keep_alive_trycnt = 0;
+				psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+				RTW_INFO("change to another methods to check alive if staion is at ps mode\n");
+			}
+
+#endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK	 */
+			if (psta->state & WIFI_SLEEP_STATE) {
+				if (!(psta->state & WIFI_STA_ALIVE_CHK_STATE)) {
+					/* to check if alive by another methods if staion is at ps mode.					 */
+					psta->expire_to = pstapriv->expire_to;
+					psta->state |= WIFI_STA_ALIVE_CHK_STATE;
+
+					/* RTW_INFO("alive chk, sta:" MAC_FMT " is at ps mode!\n", MAC_ARG(psta->hwaddr)); */
+
+					/* to update bcn with tim_bitmap for this station */
+					pstapriv->tim_bitmap |= BIT(psta->aid);
+					update_beacon(padapter, _TIM_IE_, NULL, true);
+
+					if (!pmlmeext->active_keep_alive_check)
+						continue;
+				}
+			}
+#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+			if (pmlmeext->active_keep_alive_check) {
+				int stainfo_offset;
+
+				stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
+				if (stainfo_offset_valid(stainfo_offset))
+					chk_alive_list[chk_alive_num++] = stainfo_offset;
+
+				continue;
+			}
+#endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */
+			list_del_init(&psta->asoc_list);
+			pstapriv->asoc_list_cnt--;
+			RTW_INFO("asoc expire "MAC_FMT", state=0x%x\n", MAC_ARG(psta->hwaddr), psta->state);
+			updated = ap_free_sta(padapter, psta, false, WLAN_REASON_DEAUTH_LEAVING, true);
+		} else {
+			/* TODO: Aging mechanism to digest frames in sleep_q to avoid running out of xmitframe */
+			if (psta->sleepq_len > (NR_XMITFRAME / pstapriv->asoc_list_cnt)
+			    && padapter->xmitpriv.free_xmitframe_cnt < ((NR_XMITFRAME / pstapriv->asoc_list_cnt) / 2)
+			   ) {
+				RTW_INFO("%s sta:"MAC_FMT", sleepq_len:%u, free_xmitframe_cnt:%u, asoc_list_cnt:%u, clear sleep_q\n", __func__
+					 , MAC_ARG(psta->hwaddr)
+					, psta->sleepq_len, padapter->xmitpriv.free_xmitframe_cnt, pstapriv->asoc_list_cnt);
+				wakeup_sta_to_xmit(padapter, psta);
+			}
+		}
+	}
+
+	_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+	if (chk_alive_num) {
+
+		u8 backup_ch = 0, backup_bw, backup_offset;
+		u8 union_ch = 0, union_bw, union_offset;
+		u8 switch_channel = true;
+		struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+		if (!rtw_mi_get_ch_setting_union(padapter, &union_ch, &union_bw, &union_offset)
+			|| pmlmeext->cur_channel != union_ch)
+			goto bypass_active_keep_alive;
+
+#ifdef CONFIG_MCC_MODE
+		if (MCC_EN(padapter)) {
+			/* driver doesn't switch channel under MCC */
+			if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_DOING_MCC))
+				switch_channel = false;
+		}
+#endif
+		/* switch to correct channel of current network  before issue keep-alive frames */
+		if (switch_channel  && rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
+			backup_ch = rtw_get_oper_ch(padapter);
+			backup_bw = rtw_get_oper_bw(padapter);
+			backup_offset = rtw_get_oper_choffset(padapter);
+			set_channel_bwmode(padapter, union_ch, union_offset, union_bw);
+		}
+
+		/* issue null data to check sta alive*/
+		for (i = 0; i < chk_alive_num; i++) {
+			int ret = _FAIL;
+
+			psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);
+#ifdef CONFIG_ATMEL_RC_PATCH
+			if (!memcmp(pstapriv->atmel_rc_pattern, psta->hwaddr, ETH_ALEN))
+				continue;
+			if (psta->flag_atmel_rc)
+				continue;
+#endif
+			if (!(psta->state & _FW_LINKED))
+				continue;
+
+			if (psta->state & WIFI_SLEEP_STATE)
+				ret = issue_nulldata(padapter, psta->hwaddr, 0, 1, 50);
+			else
+				ret = issue_nulldata(padapter, psta->hwaddr, 0, 3, 50);
+
+			psta->keep_alive_trycnt++;
+			if (ret == _SUCCESS) {
+				RTW_INFO("asoc check, sta(" MAC_FMT ") is alive\n", MAC_ARG(psta->hwaddr));
+				psta->expire_to = pstapriv->expire_to;
+				psta->keep_alive_trycnt = 0;
+				continue;
+			} else if (psta->keep_alive_trycnt <= 3) {
+				RTW_INFO("ack check for asoc expire, keep_alive_trycnt=%d\n", psta->keep_alive_trycnt);
+				psta->expire_to = 1;
+				continue;
+			}
+
+			psta->keep_alive_trycnt = 0;
+			RTW_INFO("asoc expire "MAC_FMT", state=0x%x\n", MAC_ARG(psta->hwaddr), psta->state);
+			_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+			if (!list_empty(&psta->asoc_list)) {
+				list_del_init(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, false, WLAN_REASON_DEAUTH_LEAVING, true);
+			}
+			_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+		}
+
+		/* back to the original operation channel */
+		if (switch_channel && backup_ch > 0)
+			set_channel_bwmode(padapter, backup_ch, backup_offset, backup_bw);
+
+bypass_active_keep_alive:
+		;
+	}
+#endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */
+
+	associated_clients_update(padapter, updated, STA_INFO_UPDATE_ALL);
+}
+
+void add_RATid(_adapter *padapter, struct sta_info *psta, u8 rssi_level, u8 is_update_bw)
+{
+	int i;
+	u8 rf_type;
+	unsigned char sta_band = 0;
+	u64 tx_ra_bitmap = 0;
+	struct ht_priv	*psta_ht = NULL;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	WLAN_BSSID_EX *pcur_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;
+
+	if (psta)
+		psta_ht = &psta->htpriv;
+	else
+		return;
+
+	if (!(psta->state & _FW_LINKED))
+		return;
+
+	rtw_hal_update_sta_rate_mask(padapter, psta);
+	tx_ra_bitmap = psta->ra_mask;
+
+	if (pcur_network->Configuration.DSConfig > 14) {
+		if (tx_ra_bitmap & 0xffff000)
+			sta_band |= WIRELESS_11_5N ;
+
+		if (tx_ra_bitmap & 0xff0)
+			sta_band |= WIRELESS_11A;
+	} else {
+		/* 5G band */
+		if (tx_ra_bitmap & 0xffff000)
+			sta_band |= WIRELESS_11_24N;
+
+		if (tx_ra_bitmap & 0xff0)
+			sta_band |= WIRELESS_11G;
+
+		if (tx_ra_bitmap & 0x0f)
+			sta_band |= WIRELESS_11B;
+	}
+
+	psta->wireless_mode = sta_band;
+	psta->raid = rtw_hal_networktype_to_raid(padapter, psta);
+
+	if (psta->aid < NUM_STA) {
+		RTW_INFO("%s=> mac_id:%d , raid:%d, tx_ra_bitmap:0x%016llx, networkType:0x%02x\n",
+			__func__, psta->mac_id, psta->raid, tx_ra_bitmap, psta->wireless_mode);
+
+		rtw_update_ramask(padapter, psta, psta->mac_id, rssi_level, is_update_bw);
+	} else
+		RTW_INFO("station aid %d exceed the max number\n", psta->aid);
+
+}
+
+void update_bmc_sta(_adapter *padapter)
+{
+	unsigned long	irqL;
+	unsigned char	network_type;
+	int supportRateNum = 0;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	WLAN_BSSID_EX *pcur_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;
+	struct sta_info *psta = rtw_get_bcmc_stainfo(padapter);
+
+	if (psta) {
+		psta->aid = 0;/* default set to 0 */
+		psta->qos_option = 0;
+		psta->htpriv.ht_option = false;
+
+		psta->ieee8021x_blocked = 0;
+
+		memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+
+		/* psta->dot118021XPrivacy = _NO_PRIVACY_; */ /* !!! remove it, because it has been set before this. */
+
+		/* prepare for add_RATid		 */
+		supportRateNum = rtw_get_rateset_len((u8 *)&pcur_network->SupportedRates);
+		network_type = rtw_check_network_type((u8 *)&pcur_network->SupportedRates, supportRateNum, pcur_network->Configuration.DSConfig);
+		if (IsSupportedTxCCK(network_type))
+			network_type = WIRELESS_11B;
+		else if (network_type == WIRELESS_INVALID) { /* error handling */
+			if (pcur_network->Configuration.DSConfig > 14)
+				network_type = WIRELESS_11A;
+			else
+				network_type = WIRELESS_11B;
+		}
+		update_sta_basic_rate(psta, network_type);
+		psta->wireless_mode = network_type;
+
+		rtw_hal_update_sta_rate_mask(padapter, psta);
+
+		psta->raid = rtw_hal_networktype_to_raid(padapter, psta);
+
+		_enter_critical_bh(&psta->lock, &irqL);
+		psta->state = _FW_LINKED;
+		_exit_critical_bh(&psta->lock, &irqL);
+
+		rtw_sta_media_status_rpt(padapter, psta, 1);
+		rtw_hal_update_ra_mask(psta, psta->rssi_level, true);
+	} else
+		RTW_INFO("add_RATid_bmc_sta error!\n");
+
+}
+
+/* notes:
+ * AID: 1~MAX for sta and 0 for bc/mc in ap/adhoc mode  */
+void update_sta_info_apmode(_adapter *padapter, struct sta_info *psta)
+{
+	unsigned long	irqL;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct ht_priv	*phtpriv_ap = &pmlmepriv->htpriv;
+	struct ht_priv	*phtpriv_sta = &psta->htpriv;
+	u8	cur_ldpc_cap = 0, cur_stbc_cap = 0, cur_beamform_cap = 0;
+	/* set intf_tag to if1 */
+	/* psta->intf_tag = 0; */
+
+	RTW_INFO("%s\n", __func__);
+
+	/*alloc macid when call rtw_alloc_stainfo(),release macid when call rtw_free_stainfo()*/
+
+	/* ap mode */
+	rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
+
+	if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
+		psta->ieee8021x_blocked = true;
+	else
+		psta->ieee8021x_blocked = false;
+
+
+	/* update sta's cap */
+
+	/* ERP */
+	VCS_update(padapter, psta);
+	/* HT related cap */
+	if (phtpriv_sta->ht_option) {
+		/* check if sta supports rx ampdu */
+		phtpriv_sta->ampdu_enable = phtpriv_ap->ampdu_enable;
+
+		phtpriv_sta->rx_ampdu_min_spacing = (phtpriv_sta->ht_cap.ampdu_params_info & IEEE80211_HT_CAP_AMPDU_DENSITY) >> 2;
+
+		/* bwmode */
+		if ((phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH))
+			psta->bw_mode = CHANNEL_WIDTH_40;
+		else
+			psta->bw_mode = CHANNEL_WIDTH_20;
+
+		if (psta->ht_40mhz_intolerant)
+			psta->bw_mode = CHANNEL_WIDTH_20;
+
+		if (pmlmeext->cur_bwmode < psta->bw_mode)
+			psta->bw_mode = pmlmeext->cur_bwmode;
+
+		phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;
+
+
+		/* check if sta support s Short GI 20M */
+		if ((phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_20))
+			phtpriv_sta->sgi_20m = true;
+
+		/* check if sta support s Short GI 40M */
+		if ((phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_40)) {
+			if (psta->bw_mode == CHANNEL_WIDTH_40) /* according to psta->bw_mode */
+				phtpriv_sta->sgi_40m = true;
+			else
+				phtpriv_sta->sgi_40m = false;
+		}
+
+		psta->qos_option = true;
+
+		/* B0 Config LDPC Coding Capability */
+		if (TEST_FLAG(phtpriv_ap->ldpc_cap, LDPC_HT_ENABLE_TX) &&
+		    GET_HT_CAP_ELE_LDPC_CAP((u8 *)(&phtpriv_sta->ht_cap))) {
+			SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX));
+			RTW_INFO("Enable HT Tx LDPC for STA(%d)\n", psta->aid);
+		}
+
+		/* B7 B8 B9 Config STBC setting */
+		if (TEST_FLAG(phtpriv_ap->stbc_cap, STBC_HT_ENABLE_TX) &&
+		    GET_HT_CAP_ELE_RX_STBC((u8 *)(&phtpriv_sta->ht_cap))) {
+			SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX));
+			RTW_INFO("Enable HT Tx STBC for STA(%d)\n", psta->aid);
+		}
+
+#ifdef CONFIG_BEAMFORMING
+		/*Config Tx beamforming setting*/
+		if (TEST_FLAG(phtpriv_ap->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE) &&
+		    GET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP((u8 *)(&phtpriv_sta->ht_cap))) {
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE);
+			/*Shift to BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP*/
+			SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS((u8 *)(&phtpriv_sta->ht_cap)) << 6);
+		}
+
+		if (TEST_FLAG(phtpriv_ap->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE) &&
+		    GET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP((u8 *)(&phtpriv_sta->ht_cap))) {
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE);
+			/*Shift to BEAMFORMING_HT_BEAMFORMER_STEER_NUM*/
+			SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS((u8 *)(&phtpriv_sta->ht_cap)) << 4);
+		}
+		if (cur_beamform_cap)
+			RTW_INFO("Client STA(%d) HT Beamforming Cap = 0x%02X\n", psta->aid, cur_beamform_cap);
+#endif /*CONFIG_BEAMFORMING*/
+	} else {
+		phtpriv_sta->ampdu_enable = false;
+
+		phtpriv_sta->sgi_20m = false;
+		phtpriv_sta->sgi_40m = false;
+		psta->bw_mode = CHANNEL_WIDTH_20;
+		phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	}
+
+	phtpriv_sta->ldpc_cap = cur_ldpc_cap;
+	phtpriv_sta->stbc_cap = cur_stbc_cap;
+	phtpriv_sta->beamform_cap = cur_beamform_cap;
+
+	/* Rx AMPDU */
+	send_delba(padapter, 0, psta->hwaddr);/* recipient */
+
+	/* TX AMPDU */
+	send_delba(padapter, 1, psta->hwaddr);/*  */ /* originator */
+	phtpriv_sta->agg_enable_bitmap = 0x0;/* reset */
+	phtpriv_sta->candidate_tid_bitmap = 0x0;/* reset */
+
+	update_ldpc_stbc_cap(psta);
+
+	/* todo: init other variables */
+
+	memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+
+	_enter_critical_bh(&psta->lock, &irqL);
+	psta->state |= _FW_LINKED;
+	_exit_critical_bh(&psta->lock, &irqL);
+
+
+}
+
+static void update_ap_info(_adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct ht_priv	*phtpriv_ap = &pmlmepriv->htpriv;
+
+	psta->wireless_mode = pmlmeext->cur_wireless_mode;
+
+	psta->bssratelen = rtw_get_rateset_len(pnetwork->SupportedRates);
+	memcpy(psta->bssrateset, pnetwork->SupportedRates, psta->bssratelen);
+
+	/* HT related cap */
+	if (phtpriv_ap->ht_option) {
+		/* check if sta supports rx ampdu */
+		/* phtpriv_ap->ampdu_enable = phtpriv_ap->ampdu_enable; */
+
+		/* check if sta support s Short GI 20M */
+		if ((phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_20))
+			phtpriv_ap->sgi_20m = true;
+		/* check if sta support s Short GI 40M */
+		if ((phtpriv_ap->ht_cap.cap_info) & cpu_to_le16(IEEE80211_HT_CAP_SGI_40))
+			phtpriv_ap->sgi_40m = true;
+
+		psta->qos_option = true;
+	} else {
+		phtpriv_ap->ampdu_enable = false;
+
+		phtpriv_ap->sgi_20m = false;
+		phtpriv_ap->sgi_40m = false;
+	}
+
+	psta->bw_mode = pmlmeext->cur_bwmode;
+	phtpriv_ap->ch_offset = pmlmeext->cur_ch_offset;
+
+	phtpriv_ap->agg_enable_bitmap = 0x0;/* reset */
+	phtpriv_ap->candidate_tid_bitmap = 0x0;/* reset */
+
+	memcpy(&psta->htpriv, &pmlmepriv->htpriv, sizeof(struct ht_priv));
+
+	psta->state |= WIFI_AP_STATE; /* Aries, add,fix bug of flush_cam_entry at STOP AP mode , 0724 */
+}
+
+static void rtw_set_hw_wmm_param(_adapter *padapter)
+{
+	u8	ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime;
+	u8	acm_mask;
+	u16	TXOP;
+	u32	acParm, i;
+	u32	edca[4], inx[4];
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+
+	acm_mask = 0;
+
+	if (is_supported_5g(pmlmeext->cur_wireless_mode) ||
+	    (pmlmeext->cur_wireless_mode & WIRELESS_11_24N))
+		aSifsTime = 16;
+	else
+		aSifsTime = 10;
+
+	if (pmlmeinfo->WMM_enable == 0) {
+		padapter->mlmepriv.acm_mask = 0;
+
+		AIFS = aSifsTime + (2 * pmlmeinfo->slotTime);
+
+		if (pmlmeext->cur_wireless_mode & (WIRELESS_11G | WIRELESS_11A)) {
+			ECWMin = 4;
+			ECWMax = 10;
+		} else if (pmlmeext->cur_wireless_mode & WIRELESS_11B) {
+			ECWMin = 5;
+			ECWMax = 10;
+		} else {
+			ECWMin = 4;
+			ECWMax = 10;
+		}
+
+		TXOP = 0;
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm));
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm));
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm));
+
+		ECWMin = 2;
+		ECWMax = 3;
+		TXOP = 0x2f;
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm));
+
+	} else {
+		edca[0] = edca[1] = edca[2] = edca[3] = 0;
+
+		/*TODO:*/
+		acm_mask = 0;
+		padapter->mlmepriv.acm_mask = acm_mask;
+		AIFS = (7 * pmlmeinfo->slotTime) + aSifsTime;
+		ECWMin = 4;
+		ECWMax = 10;
+		TXOP = 0;
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm));
+		edca[XMIT_BK_QUEUE] = acParm;
+		RTW_INFO("WMM(BK): %x\n", acParm);
+
+		/* BE */
+		AIFS = (3 * pmlmeinfo->slotTime) + aSifsTime;
+		ECWMin = 4;
+		ECWMax = 6;
+		TXOP = 0;
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm));
+		edca[XMIT_BE_QUEUE] = acParm;
+		RTW_INFO("WMM(BE): %x\n", acParm);
+
+		/* VI */
+		AIFS = (1 * pmlmeinfo->slotTime) + aSifsTime;
+		ECWMin = 3;
+		ECWMax = 4;
+		TXOP = 94;
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm));
+		edca[XMIT_VI_QUEUE] = acParm;
+		RTW_INFO("WMM(VI): %x\n", acParm);
+
+		/* VO */
+		AIFS = (1 * pmlmeinfo->slotTime) + aSifsTime;
+		ECWMin = 2;
+		ECWMax = 3;
+		TXOP = 47;
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm));
+		edca[XMIT_VO_QUEUE] = acParm;
+		RTW_INFO("WMM(VO): %x\n", acParm);
+
+
+		if (padapter->registrypriv.acm_method == 1)
+			rtw_hal_set_hwreg(padapter, HW_VAR_ACM_CTRL, (u8 *)(&acm_mask));
+		else
+			padapter->mlmepriv.acm_mask = acm_mask;
+
+		inx[0] = 0;
+		inx[1] = 1;
+		inx[2] = 2;
+		inx[3] = 3;
+
+		if (pregpriv->wifi_spec == 1) {
+			u32	j, tmp, change_inx = false;
+
+			/* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */
+			for (i = 0 ; i < 4 ; i++) {
+				for (j = i + 1 ; j < 4 ; j++) {
+					/* compare CW and AIFS */
+					if ((edca[j] & 0xFFFF) < (edca[i] & 0xFFFF))
+						change_inx = true;
+					else if ((edca[j] & 0xFFFF) == (edca[i] & 0xFFFF)) {
+						/* compare TXOP */
+						if ((edca[j] >> 16) > (edca[i] >> 16))
+							change_inx = true;
+					}
+
+					if (change_inx) {
+						tmp = edca[i];
+						edca[i] = edca[j];
+						edca[j] = tmp;
+
+						tmp = inx[i];
+						inx[i] = inx[j];
+						inx[j] = tmp;
+
+						change_inx = false;
+					}
+				}
+			}
+		}
+
+		for (i = 0 ; i < 4 ; i++) {
+			pxmitpriv->wmm_para_seq[i] = inx[i];
+			RTW_INFO("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]);
+		}
+
+	}
+
+}
+
+static void update_hw_ht_param(_adapter *padapter)
+{
+	unsigned char		max_AMPDU_len;
+	unsigned char		min_MPDU_spacing;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	RTW_INFO("%s\n", __func__);
+
+
+	/* handle A-MPDU parameter field */
+	/*
+		AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+		AMPDU_para [4:2]:Min MPDU Start Spacing
+	*/
+	max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
+
+	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));
+
+	/* Config SM Power Save setting */
+	pmlmeinfo->SM_PS = (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)
+		RTW_INFO("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__);
+}
+
+static void rtw_ap_check_scan(_adapter *padapter)
+{
+	unsigned long	irqL;
+	_list		*plist, *phead;
+	u32	delta_time, lifetime;
+	struct	wlan_network	*pnetwork = NULL;
+	WLAN_BSSID_EX *pbss = NULL;
+	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	_queue	*queue	= &(pmlmepriv->scanned_queue);
+	u8 do_scan = false;
+	u8 reason = RTW_AUTO_SCAN_REASON_UNSPECIFIED;
+
+	lifetime = SCANQUEUE_LIFETIME; /* 20 sec */
+
+	_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+	phead = get_list_head(queue);
+	if (rtw_end_of_queue_search(phead, get_next(phead)) )
+		if (padapter->registrypriv.wifi_spec) {
+			do_scan = true;
+			reason |= RTW_AUTO_SCAN_REASON_2040_BSS;
+		}
+	_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+#ifdef CONFIG_AUTO_CHNL_SEL_NHM
+	if (padapter->registrypriv.acs_auto_scan) {
+		do_scan = true;
+		reason |= RTW_AUTO_SCAN_REASON_ACS;
+		rtw_acs_start(padapter, true);
+	}
+#endif
+
+	if (do_scan) {
+		RTW_INFO("%s : drv scans by itself and wait_completed\n", __func__);
+		rtw_drv_scan_by_self(padapter, reason);
+		rtw_scan_wait_completed(padapter);
+	}
+
+#ifdef CONFIG_AUTO_CHNL_SEL_NHM
+	if (padapter->registrypriv.acs_auto_scan)
+		rtw_acs_start(padapter, false);
+#endif
+	_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+	phead = get_list_head(queue);
+	plist = get_next(phead);
+
+	while (1) {
+
+		if (rtw_end_of_queue_search(phead, plist) )
+			break;
+
+		pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		if (rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, pnetwork->network.Configuration.DSConfig) >= 0
+		    && rtw_mlme_band_check(padapter, pnetwork->network.Configuration.DSConfig) 
+		    && rtw_validate_ssid(&(pnetwork->network.Ssid))) {
+			delta_time = (u32) rtw_get_passing_time_ms(pnetwork->last_scanned);
+
+			if (delta_time < lifetime) {
+
+				uint ie_len = 0;
+				u8 *pbuf = NULL;
+				u8 *ie = NULL;
+
+				pbss = &pnetwork->network;
+				ie = pbss->IEs;
+
+				/*check if HT CAP INFO IE exists or not*/
+				pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pbss->IELength - _BEACON_IE_OFFSET_));
+				if (pbuf == NULL) {
+					/* HT CAP INFO IE don't exist, it is b/g mode bss.*/
+
+					if (false == ATOMIC_READ(&pmlmepriv->olbc))
+						ATOMIC_SET(&pmlmepriv->olbc, true);
+
+					if (false == ATOMIC_READ(&pmlmepriv->olbc_ht))
+						ATOMIC_SET(&pmlmepriv->olbc_ht, true);
+					
+					if (padapter->registrypriv.wifi_spec)
+						RTW_INFO("%s: %s is a/b/g ap\n", __func__, pnetwork->network.Ssid.Ssid);
+				}
+			}
+		}
+
+		plist = get_next(plist);
+
+	}
+
+	_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+	pmlmepriv->num_sta_no_ht = 0; /* reset to 0 after ap do scanning*/
+
+}
+
+void rtw_start_bss_hdl_after_chbw_decided(_adapter *adapter)
+{
+	WLAN_BSSID_EX *pnetwork = &(adapter->mlmepriv.cur_network.network);
+	struct sta_info *sta = NULL;
+
+	/* update cur_wireless_mode */
+	update_wireless_mode(adapter);
+
+	/* update RRSR and RTS_INIT_RATE register after set channel and bandwidth */
+	UpdateBrateTbl(adapter, pnetwork->SupportedRates);
+	rtw_hal_set_hwreg(adapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
+
+	/* update capability after cur_wireless_mode updated */
+	update_capinfo(adapter, rtw_get_capability(pnetwork));
+
+	/* update bc/mc sta_info */
+	update_bmc_sta(adapter);
+
+	/* update AP's sta info */
+	sta = rtw_get_stainfo(&adapter->stapriv, pnetwork->MacAddress);
+	if (!sta) {
+		RTW_INFO(FUNC_ADPT_FMT" !sta for macaddr="MAC_FMT"\n", FUNC_ADPT_ARG(adapter), MAC_ARG(pnetwork->MacAddress));
+		rtw_warn_on(1);
+		return;
+	}
+
+	update_ap_info(adapter, sta);
+}
+
+void start_bss_network(_adapter *padapter, struct createbss_parm *parm)
+{
+#define DUMP_ADAPTERS_STATUS 0
+
+	u8 val8;
+	u16 bcn_interval;
+	u32	acparm;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network; /* used as input */
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX *pnetwork_mlmeext = &(pmlmeinfo->network);
+	struct dvobj_priv *pdvobj = padapter->dvobj;
+	s16 req_ch = -1, req_bw = -1, req_offset = -1;
+	bool ch_setting_changed = false;
+	u8 ch_to_set = 0, bw_to_set, offset_to_set;
+	u8 doiqk = false;
+	/* use for check ch bw offset can be allowed or not */
+	u8 chbw_allow = true;
+
+	if (parm->req_ch != 0) {
+		/* bypass other setting, go checking ch, bw, offset */
+		req_ch = parm->req_ch;
+		req_bw = parm->req_bw;
+		req_offset = parm->req_offset;
+		goto chbw_decision;
+	} else {
+		/* inform this request comes from upper layer */
+		req_ch = 0;
+	}
+
+	bcn_interval = (u16)pnetwork->Configuration.BeaconPeriod;
+
+	/* check if there is wps ie, */
+	/* if there is wpsie in beacon, the hostapd will update beacon twice when stating hostapd, */
+	/* and at first time the security ie ( RSN/WPA IE) will not include in beacon. */
+	if (NULL == rtw_get_wps_ie(pnetwork->IEs + _FIXED_IE_LENGTH_, pnetwork->IELength - _FIXED_IE_LENGTH_, NULL, NULL))
+		pmlmeext->bstart_bss = true;
+
+	/* todo: update wmm, ht cap */
+	/* pmlmeinfo->WMM_enable; */
+	/* pmlmeinfo->HT_enable; */
+	if (pmlmepriv->qospriv.qos_option)
+		pmlmeinfo->WMM_enable = true;
+	if (pmlmepriv->htpriv.ht_option) {
+		pmlmeinfo->WMM_enable = true;
+		pmlmeinfo->HT_enable = true;
+		/* pmlmeinfo->HT_info_enable = true; */
+		/* pmlmeinfo->HT_caps_enable = true; */
+
+		update_hw_ht_param(padapter);
+	}
+	if (pmlmepriv->cur_network.join_res != true) { /* setting only at  first time */
+		/* WEP Key will be set before this function, do not clear CAM. */
+		if ((psecuritypriv->dot11PrivacyAlgrthm != _WEP40_) && (psecuritypriv->dot11PrivacyAlgrthm != _WEP104_))
+			flush_all_cam_entry(padapter);	/* clear CAM */
+	}
+
+	/* set MSR to AP_Mode		 */
+	Set_MSR(padapter, _HW_STATE_AP_);
+
+	/* Set BSSID REG */
+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pnetwork->MacAddress);
+
+	/* Set EDCA param reg */
+#ifdef CONFIG_CONCURRENT_MODE
+	acparm = 0x005ea42b;
+#else
+	acparm = 0x002F3217; /* VO */
+#endif
+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
+	acparm = 0x005E4317; /* VI */
+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
+	/* acparm = 0x00105320; */ /* BE */
+	acparm = 0x005ea42b;
+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
+	acparm = 0x0000A444; /* BK */
+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
+
+	/* Set Security */
+	val8 = (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf;
+	rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+	/* Beacon Control related register */
+	rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&bcn_interval));
+
+chbw_decision:
+	ch_setting_changed = rtw_ap_chbw_decision(padapter, req_ch, req_bw, req_offset
+		     , &ch_to_set, &bw_to_set, &offset_to_set, &chbw_allow);
+
+	/* let pnetwork_mlmeext == pnetwork_mlme. */
+	memcpy(pnetwork_mlmeext, pnetwork, pnetwork->Length);
+
+	rtw_start_bss_hdl_after_chbw_decided(padapter);
+
+#if defined(CONFIG_DFS_MASTER)
+	rtw_dfs_master_status_apply(padapter, MLME_AP_STARTED);
+#endif
+
+#ifdef CONFIG_MCC_MODE
+	if (MCC_EN(padapter)) {
+		/* 
+		* due to check under rtw_ap_chbw_decision
+		* if under MCC mode, means req channel setting is the same as current channel setting
+		* if not under MCC mode, mean req channel setting is not the same as current channel setting
+		*/
+		if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_DOING_MCC)) {
+				RTW_INFO(FUNC_ADPT_FMT": req channel setting is the same as current channel setting, go to update BCN\n"
+				, FUNC_ADPT_ARG(padapter));
+
+				goto update_beacon;
+
+		}
+	}
+
+	/* issue null data to AP for all interface connecting to AP before switch channel setting for softap */
+	rtw_hal_mcc_issue_null_data(padapter, chbw_allow, 1);
+#endif /* CONFIG_MCC_MODE */
+
+	doiqk = true;
+	rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk);
+
+	if (ch_to_set != 0) {
+		set_channel_bwmode(padapter, ch_to_set, offset_to_set, bw_to_set);
+		rtw_mi_update_union_chan_inf(padapter, ch_to_set, offset_to_set, bw_to_set);
+	}
+
+	doiqk = false;
+	rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk);
+
+#ifdef CONFIG_MCC_MODE
+	/* after set_channel_bwmode for backup IQK */
+	rtw_hal_set_mcc_setting_start_bss_network(padapter, chbw_allow);
+#endif
+
+	if (DUMP_ADAPTERS_STATUS) {
+		RTW_INFO(FUNC_ADPT_FMT" done\n", FUNC_ADPT_ARG(padapter));
+		dump_adapters_status(RTW_DBGDUMP , adapter_to_dvobj(padapter));
+	}
+
+update_beacon:
+	/* update beacon content only if bstart_bss is true */
+	if (pmlmeext->bstart_bss) {
+
+		unsigned long irqL;
+
+		if ((ATOMIC_READ(&pmlmepriv->olbc) ) || (ATOMIC_READ(&pmlmepriv->olbc_ht) == true)) {
+			/* AP is not starting a 40 MHz BSS in presence of an 802.11g BSS. */
+
+			pmlmepriv->ht_op_mode &= (~HT_INFO_OPERATION_MODE_OP_MODE_MASK);
+			pmlmepriv->ht_op_mode |= OP_MODE_MAY_BE_LEGACY_STAS;
+			update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, false);
+		}
+
+		update_beacon(padapter, _TIM_IE_, NULL, false);
+
+#ifdef CONFIG_SWTIMER_BASED_TXBCN
+		_enter_critical_bh(&pdvobj->ap_if_q.lock, &irqL);
+		if (list_empty(&padapter->list)) {
+			list_add_tail(&padapter->list, get_list_head(&pdvobj->ap_if_q));
+			pdvobj->nr_ap_if++;
+			pdvobj->inter_bcn_space = DEFAULT_BCN_INTERVAL / pdvobj->nr_ap_if;
+		}
+		_exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL);
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pdvobj->inter_bcn_space));
+
+#endif /*CONFIG_SWTIMER_BASED_TXBCN*/
+
+	}
+
+	rtw_scan_wait_completed(padapter);
+
+	/* send beacon */
+	if ((0 == rtw_mi_check_fwstate(padapter, _FW_UNDER_SURVEY))
+		&& (0 == rtw_mi_check_fwstate(padapter, WIFI_OP_CH_SWITCHING))
+	) {
+
+		/*update_beacon(padapter, _TIM_IE_, NULL, true);*/
+
+#if !defined(CONFIG_INTERRUPT_BASED_TXBCN)
+#ifdef CONFIG_SWTIMER_BASED_TXBCN
+		if (pdvobj->nr_ap_if == 1) {
+			RTW_INFO("start SW BCN TIMER!\n");
+			_set_timer(&pdvobj->txbcn_timer, bcn_interval);
+		}
+#else
+		/* other case will  tx beacon when bcn interrupt coming in. */
+		if (send_beacon(padapter) == _FAIL)
+			RTW_INFO("issue_beacon, fail!\n");
+#endif
+#endif /* !defined(CONFIG_INTERRUPT_BASED_TXBCN) */
+	}
+
+	/*Set EDCA param reg after update cur_wireless_mode & update_capinfo*/
+	if (pregpriv->wifi_spec == 1)
+		rtw_set_hw_wmm_param(padapter);
+
+	/*pmlmeext->bstart_bss = true;*/
+}
+
+int rtw_check_beacon_data(_adapter *padapter, u8 *pbuf,  int len)
+{
+	int ret = _SUCCESS;
+	u8 *p;
+	u8 *pHT_caps_ie = NULL;
+	u8 *pHT_info_ie = NULL;
+	u16 cap, ht_cap = false;
+	uint ie_len = 0;
+	int group_cipher, pairwise_cipher;
+	u8	channel, network_type, supportRate[NDIS_802_11_LENGTH_RATES_EX];
+	int supportRateNum = 0;
+	u8 OUI1[] = {0x00, 0x50, 0xf2, 0x01};
+	u8 wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+	u8 WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	WLAN_BSSID_EX *pbss_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *ie = pbss_network->IEs;
+	u8 vht_cap = false;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 rf_num = 0;
+
+	/* SSID */
+	/* Supported rates */
+	/* DS Params */
+	/* WLAN_EID_COUNTRY */
+	/* ERP Information element */
+	/* Extended supported rates */
+	/* WPA/WPA2 */
+	/* Wi-Fi Wireless Multimedia Extensions */
+	/* ht_capab, ht_oper */
+	/* WPS IE */
+
+	RTW_INFO("%s, len=%d\n", __func__, len);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return _FAIL;
+
+
+	if (len > MAX_IE_SZ)
+		return _FAIL;
+
+	pbss_network->IELength = len;
+
+	memset(ie, 0, MAX_IE_SZ);
+
+	memcpy(ie, pbuf, pbss_network->IELength);
+
+
+	if (pbss_network->InfrastructureMode != Ndis802_11APMode)
+		return _FAIL;
+
+
+	rtw_ap_check_scan(padapter);
+
+
+	pbss_network->Rssi = 0;
+
+	memcpy(pbss_network->MacAddress, adapter_mac_addr(padapter), ETH_ALEN);
+
+	/* beacon interval */
+	p = rtw_get_beacon_interval_from_ie(ie);/* ie + 8;	 */ /* 8: TimeStamp, 2: Beacon Interval 2:Capability */
+	/* pbss_network->Configuration.BeaconPeriod = le16_to_cpu(*(unsigned short*)p); */
+	pbss_network->Configuration.BeaconPeriod = RTW_GET_LE16(p);
+
+	/* capability */
+	/* cap = *(unsigned short *)rtw_get_capability_from_ie(ie); */
+	/* cap = le16_to_cpu(cap); */
+	cap = RTW_GET_LE16(ie);
+
+	/* SSID */
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _SSID_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+	if (p && ie_len > 0) {
+		memset(&pbss_network->Ssid, 0, sizeof(NDIS_802_11_SSID));
+		memcpy(pbss_network->Ssid.Ssid, (p + 2), ie_len);
+		pbss_network->Ssid.SsidLength = ie_len;
+#ifdef CONFIG_P2P
+		memcpy(padapter->wdinfo.p2p_group_ssid, pbss_network->Ssid.Ssid, pbss_network->Ssid.SsidLength);
+		padapter->wdinfo.p2p_group_ssid_len = pbss_network->Ssid.SsidLength;
+#endif
+	}
+
+	/* chnnel */
+	channel = 0;
+	pbss_network->Configuration.Length = 0;
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _DSSET_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+	if (p && ie_len > 0)
+		channel = *(p + 2);
+
+	pbss_network->Configuration.DSConfig = channel;
+
+
+	memset(supportRate, 0, NDIS_802_11_LENGTH_RATES_EX);
+	/* get supported rates */
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+	if (p !=  NULL) {
+		memcpy(supportRate, p + 2, ie_len);
+		supportRateNum = ie_len;
+	}
+
+	/* get ext_supported rates */
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ie_len, pbss_network->IELength - _BEACON_IE_OFFSET_);
+	if (p !=  NULL) {
+		memcpy(supportRate + supportRateNum, p + 2, ie_len);
+		supportRateNum += ie_len;
+
+	}
+
+	network_type = rtw_check_network_type(supportRate, supportRateNum, channel);
+
+	rtw_set_supported_rate(pbss_network->SupportedRates, network_type);
+
+
+	/* parsing ERP_IE */
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+	if (p && ie_len > 0)
+		ERP_IE_handler(padapter, (PNDIS_802_11_VARIABLE_IEs)p);
+
+	/* update privacy/security */
+	if (cap & BIT(4))
+		pbss_network->Privacy = 1;
+	else
+		pbss_network->Privacy = 0;
+
+	psecuritypriv->wpa_psk = 0;
+
+	/* wpa2 */
+	group_cipher = 0;
+	pairwise_cipher = 0;
+	psecuritypriv->wpa2_group_cipher = _NO_PRIVACY_;
+	psecuritypriv->wpa2_pairwise_cipher = _NO_PRIVACY_;
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _RSN_IE_2_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+	if (p && ie_len > 0) {
+		if (rtw_parse_wpa2_ie(p, ie_len + 2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+			psecuritypriv->dot8021xalg = 1;/* psk,  todo:802.1x */
+			psecuritypriv->wpa_psk |= BIT(1);
+
+			psecuritypriv->wpa2_group_cipher = group_cipher;
+			psecuritypriv->wpa2_pairwise_cipher = pairwise_cipher;
+		}
+	}
+
+	/* wpa */
+	ie_len = 0;
+	group_cipher = 0;
+	pairwise_cipher = 0;
+	psecuritypriv->wpa_group_cipher = _NO_PRIVACY_;
+	psecuritypriv->wpa_pairwise_cipher = _NO_PRIVACY_;
+	for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) {
+		p = rtw_get_ie(p, _SSN_IE_1_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
+		if ((p) && (!memcmp(p + 2, OUI1, 4))) {
+			if (rtw_parse_wpa_ie(p, ie_len + 2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+				psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+				psecuritypriv->dot8021xalg = 1;/* psk,  todo:802.1x */
+
+				psecuritypriv->wpa_psk |= BIT(0);
+
+				psecuritypriv->wpa_group_cipher = group_cipher;
+				psecuritypriv->wpa_pairwise_cipher = pairwise_cipher;
+			}
+			break;
+		}
+
+		if ((p == NULL) || (ie_len == 0))
+			break;
+	}
+
+	/* wmm */
+	ie_len = 0;
+	pmlmepriv->qospriv.qos_option = 0;
+	if (pregistrypriv->wmm_enable) {
+		for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) {
+			p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
+			if ((p) && !memcmp(p + 2, WMM_PARA_IE, 6)) {
+				pmlmepriv->qospriv.qos_option = 1;
+
+				*(p + 8) |= BIT(7); /* QoS Info, support U-APSD */
+
+				/* disable all ACM bits since the WMM admission control is not supported */
+				*(p + 10) &= ~BIT(4); /* BE */
+				*(p + 14) &= ~BIT(4); /* BK */
+				*(p + 18) &= ~BIT(4); /* VI */
+				*(p + 22) &= ~BIT(4); /* VO */
+
+				break;
+			}
+
+			if ((p == NULL) || (ie_len == 0))
+				break;
+		}
+	}
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+	if (p && ie_len > 0) {
+		u8 rf_type = 0;
+		HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor = MAX_AMPDU_FACTOR_64K;
+		struct rtw_ieee80211_ht_cap *pht_cap = (struct rtw_ieee80211_ht_cap *)(p + 2);
+
+		if (0) {
+			RTW_INFO(FUNC_ADPT_FMT" HT_CAP_IE from upper layer:\n", FUNC_ADPT_ARG(padapter));
+			dump_ht_cap_ie_content(RTW_DBGDUMP, p + 2, ie_len);
+		}
+
+		pHT_caps_ie = p;
+
+		ht_cap = true;
+		network_type |= WIRELESS_11_24N;
+
+		rtw_ht_use_default_setting(padapter);
+
+		/* Update HT Capabilities Info field */
+		if (!pmlmepriv->htpriv.sgi_20m)
+			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_SGI_20));
+
+		if (!pmlmepriv->htpriv.sgi_40m)
+			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_SGI_40));
+
+		if (!TEST_FLAG(pmlmepriv->htpriv.ldpc_cap, LDPC_HT_ENABLE_RX))
+			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_LDPC_CODING));
+
+		if (!TEST_FLAG(pmlmepriv->htpriv.stbc_cap, STBC_HT_ENABLE_TX))
+			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_TX_STBC));
+
+		if (!TEST_FLAG(pmlmepriv->htpriv.stbc_cap, STBC_HT_ENABLE_RX))
+			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_RX_STBC_3R));
+
+		/* Update A-MPDU Parameters field */
+		pht_cap->ampdu_params_info &= ~(IEEE80211_HT_CAP_AMPDU_FACTOR | IEEE80211_HT_CAP_AMPDU_DENSITY);
+
+		if ((psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_CCMP) ||
+		    (psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_CCMP))
+			pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY & (0x07 << 2));
+		else
+			pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY & 0x00);
+
+		rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
+		pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_FACTOR & max_rx_ampdu_factor); /* set  Max Rx AMPDU size  to 64K */
+
+		memcpy(&(pmlmeinfo->HT_caps), pht_cap, sizeof(struct HT_caps_element));
+
+		/* Update Supported MCS Set field */
+		{
+			struct hal_spec_t *hal_spec = GET_HAL_SPEC(padapter);
+			u8 rx_nss = 0;
+			int i;
+
+			rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+			rx_nss = rtw_min(rf_type_to_rf_rx_cnt(rf_type), hal_spec->rx_nss_num);
+
+			/* RX MCS Bitmask */
+			switch (rx_nss) {
+			case 1:
+				set_mcs_rate_by_mask(HT_CAP_ELE_RX_MCS_MAP(pht_cap), MCS_RATE_1R);
+				break;
+			case 2:
+				set_mcs_rate_by_mask(HT_CAP_ELE_RX_MCS_MAP(pht_cap), MCS_RATE_2R);
+				break;
+			case 3:
+				set_mcs_rate_by_mask(HT_CAP_ELE_RX_MCS_MAP(pht_cap), MCS_RATE_3R);
+				break;
+			case 4:
+				set_mcs_rate_by_mask(HT_CAP_ELE_RX_MCS_MAP(pht_cap), MCS_RATE_4R);
+				break;
+			default:
+				RTW_WARN("rf_type:%d or rx_nss:%u is not expected\n", rf_type, hal_spec->rx_nss_num);
+			}
+			for (i = 0; i < 10; i++)
+				*(HT_CAP_ELE_RX_MCS_MAP(pht_cap) + i) &= padapter->mlmeextpriv.default_supported_mcs_set[i];
+		}
+
+#ifdef CONFIG_BEAMFORMING
+		/* Use registry value to enable HT Beamforming. */
+		/* ToDo: use configure file to set these capability. */
+		pht_cap->tx_BF_cap_info = 0;
+
+		/* HT Beamformer */
+		if (TEST_FLAG(pmlmepriv->htpriv.beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE)) {
+			/* Transmit NDP Capable */
+			SET_HT_CAP_TXBF_TRANSMIT_NDP_CAP(pht_cap, 1);
+			/* Explicit Compressed Steering Capable */
+			SET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(pht_cap, 1);
+			/* Compressed Steering Number Antennas */
+			SET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(pht_cap, 1);
+			rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMER_CAP, (u8 *)&rf_num);
+			SET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS(pht_cap, rf_num);
+		}
+
+		/* HT Beamformee */
+		if (TEST_FLAG(pmlmepriv->htpriv.beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE)) {
+			/* Receive NDP Capable */
+			SET_HT_CAP_TXBF_RECEIVE_NDP_CAP(pht_cap, 1);
+			/* Explicit Compressed Beamforming Feedback Capable */
+			SET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(pht_cap, 2);
+			rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMEE_CAP, (u8 *)&rf_num);
+			SET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(pht_cap, rf_num);
+		}
+#endif /* CONFIG_BEAMFORMING */
+
+		memcpy(&pmlmepriv->htpriv.ht_cap, p + 2, ie_len);
+
+		if (0) {
+			RTW_INFO(FUNC_ADPT_FMT" HT_CAP_IE driver masked:\n", FUNC_ADPT_ARG(padapter));
+			dump_ht_cap_ie_content(RTW_DBGDUMP, p + 2, ie_len);
+		}
+	}
+
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pbss_network->IELength - _BEACON_IE_OFFSET_));
+	if (p && ie_len > 0)
+		pHT_info_ie = p;
+	switch (network_type) {
+	case WIRELESS_11B:
+		pbss_network->NetworkTypeInUse = Ndis802_11DS;
+		break;
+	case WIRELESS_11G:
+	case WIRELESS_11BG:
+	case WIRELESS_11G_24N:
+	case WIRELESS_11BG_24N:
+		pbss_network->NetworkTypeInUse = Ndis802_11OFDM24;
+		break;
+	case WIRELESS_11A:
+		pbss_network->NetworkTypeInUse = Ndis802_11OFDM5;
+		break;
+	default:
+		pbss_network->NetworkTypeInUse = Ndis802_11OFDM24;
+		break;
+	}
+
+	pmlmepriv->cur_network.network_type = network_type;
+
+	pmlmepriv->htpriv.ht_option = false;
+
+	if ((psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_TKIP) ||
+	    (psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_TKIP)) {
+		/* todo: */
+		/* ht_cap = false; */
+	}
+
+	/* ht_cap	 */
+	if (pregistrypriv->ht_enable && ht_cap ) {
+		pmlmepriv->htpriv.ht_option = true;
+		pmlmepriv->qospriv.qos_option = 1;
+
+		pmlmepriv->htpriv.ampdu_enable = pregistrypriv->ampdu_enable ? true : false;
+
+		HT_caps_handler(padapter, (PNDIS_802_11_VARIABLE_IEs)pHT_caps_ie);
+
+		HT_info_handler(padapter, (PNDIS_802_11_VARIABLE_IEs)pHT_info_ie);
+	}
+	if(pbss_network->Configuration.DSConfig <= 14 && padapter->registrypriv.wifi_spec == 1) {
+		uint len = 0;
+
+		SET_EXT_CAPABILITY_ELE_BSS_COEXIST(pmlmepriv->ext_capab_ie_data, 1);
+		pmlmepriv->ext_capab_ie_len = 10;
+		rtw_set_ie(pbss_network->IEs + pbss_network->IELength, EID_EXTCapability, 8, pmlmepriv->ext_capab_ie_data, &len);
+		pbss_network->IELength += pmlmepriv->ext_capab_ie_len;
+	}
+
+	pbss_network->Length = get_WLAN_BSSID_EX_sz((WLAN_BSSID_EX *)pbss_network);
+
+	rtw_ies_get_chbw(pbss_network->IEs + _BEACON_IE_OFFSET_, pbss_network->IELength - _BEACON_IE_OFFSET_
+		, &pmlmepriv->ori_ch, &pmlmepriv->ori_bw, &pmlmepriv->ori_offset);
+	rtw_warn_on(pmlmepriv->ori_ch == 0);
+
+	{
+		/* alloc sta_info for ap itself */
+
+		struct sta_info *sta;
+
+		sta = rtw_get_stainfo(&padapter->stapriv, pbss_network->MacAddress);
+		if (!sta) {
+			sta = rtw_alloc_stainfo(&padapter->stapriv, pbss_network->MacAddress);
+			if (sta == NULL)
+				return _FAIL;
+		}
+	}
+
+	rtw_startbss_cmd(padapter, RTW_CMDF_WAIT_ACK);
+	{
+		int sk_band = RTW_GET_SCAN_BAND_SKIP(padapter);
+
+		if (sk_band)
+			RTW_CLR_SCAN_BAND_SKIP(padapter, sk_band);
+	}
+
+	rtw_indicate_connect(padapter);
+
+	pmlmepriv->cur_network.join_res = true;/* for check if already set beacon */
+
+	/* update bc/mc sta_info */
+	/* update_bmc_sta(padapter); */
+
+	return ret;
+
+}
+
+#if CONFIG_RTW_MACADDR_ACL
+static void rtw_macaddr_acl_init(_adapter *adapter)
+{
+	struct sta_priv *stapriv = &adapter->stapriv;
+	struct wlan_acl_pool *acl = &stapriv->acl_list;
+	_queue *acl_node_q = &acl->acl_node_q;
+	int i;
+	unsigned long irqL;
+
+	_enter_critical_bh(&(acl_node_q->lock), &irqL);
+	INIT_LIST_HEAD(&(acl_node_q->queue));
+	acl->num = 0;
+	acl->mode = RTW_ACL_MODE_DISABLED;
+	for (i = 0; i < NUM_ACL; i++) {
+		INIT_LIST_HEAD(&acl->aclnode[i].list);
+		acl->aclnode[i].valid = false;
+	}
+	_exit_critical_bh(&(acl_node_q->lock), &irqL);
+}
+
+static void rtw_macaddr_acl_deinit(_adapter *adapter)
+{
+	struct sta_priv *stapriv = &adapter->stapriv;
+	struct wlan_acl_pool *acl = &stapriv->acl_list;
+	_queue *acl_node_q = &acl->acl_node_q;
+	unsigned long irqL;
+	_list *head, *list;
+	struct rtw_wlan_acl_node *acl_node;
+
+	_enter_critical_bh(&(acl_node_q->lock), &irqL);
+	head = get_list_head(acl_node_q);
+	list = get_next(head);
+	while (!rtw_end_of_queue_search(head, list)) {
+		acl_node = LIST_CONTAINOR(list, struct rtw_wlan_acl_node, list);
+		list = get_next(list);
+
+		if (acl_node->valid ) {
+			acl_node->valid = false;
+			list_del_init(&acl_node->list);
+			acl->num--;
+		}
+	}
+	_exit_critical_bh(&(acl_node_q->lock), &irqL);
+
+	rtw_warn_on(acl->num);
+	acl->mode = RTW_ACL_MODE_DISABLED;
+}
+
+void rtw_set_macaddr_acl(_adapter *adapter, int mode)
+{
+	struct sta_priv *stapriv = &adapter->stapriv;
+	struct wlan_acl_pool *acl = &stapriv->acl_list;
+
+	RTW_INFO(FUNC_ADPT_FMT" mode=%d\n", FUNC_ADPT_ARG(adapter), mode);
+
+	acl->mode = mode;
+
+	if (mode == RTW_ACL_MODE_DISABLED)
+		rtw_macaddr_acl_deinit(adapter);
+}
+
+int rtw_acl_add_sta(_adapter *adapter, const u8 *addr)
+{
+	unsigned long irqL;
+	_list *list, *head;
+	u8 existed = 0;
+	int i = -1, ret = 0;
+	struct rtw_wlan_acl_node *acl_node;
+	struct sta_priv *stapriv = &adapter->stapriv;
+	struct wlan_acl_pool *acl = &stapriv->acl_list;
+	_queue *acl_node_q = &acl->acl_node_q;
+
+	_enter_critical_bh(&(acl_node_q->lock), &irqL);
+
+	head = get_list_head(acl_node_q);
+	list = get_next(head);
+
+	/* search for existed entry */
+	while (rtw_end_of_queue_search(head, list) == false) {
+		acl_node = LIST_CONTAINOR(list, struct rtw_wlan_acl_node, list);
+		list = get_next(list);
+
+		if (!memcmp(acl_node->addr, addr, ETH_ALEN)) {
+			if (acl_node->valid ) {
+				existed = 1;
+				break;
+			}
+		}
+	}
+	if (existed)
+		goto release_lock;
+
+	if (acl->num >= NUM_ACL)
+		goto release_lock;
+
+	/* find empty one and use */
+	for (i = 0; i < NUM_ACL; i++) {
+
+		acl_node = &acl->aclnode[i];
+		if (acl_node->valid == false) {
+
+			INIT_LIST_HEAD(&acl_node->list);
+			memcpy(acl_node->addr, addr, ETH_ALEN);
+			acl_node->valid = true;
+
+			list_add_tail(&acl_node->list, get_list_head(acl_node_q));
+			acl->num++;
+			break;
+		}
+	}
+
+release_lock:
+	_exit_critical_bh(&(acl_node_q->lock), &irqL);
+
+	if (!existed && (i < 0 || i >= NUM_ACL))
+		ret = -1;
+
+	RTW_INFO(FUNC_ADPT_FMT" "MAC_FMT" %s (acl_num=%d)\n"
+		 , FUNC_ADPT_ARG(adapter), MAC_ARG(addr)
+		, (existed ? "existed" : ((i < 0 || i >= NUM_ACL) ? "no room" : "added"))
+		 , acl->num);
+
+	return ret;
+}
+
+int rtw_acl_remove_sta(_adapter *adapter, const u8 *addr)
+{
+	unsigned long irqL;
+	_list *list, *head;
+	int ret = 0;
+	struct rtw_wlan_acl_node *acl_node;
+	struct sta_priv *stapriv = &adapter->stapriv;
+	struct wlan_acl_pool *acl = &stapriv->acl_list;
+	_queue	*acl_node_q = &acl->acl_node_q;
+	u8 is_baddr = is_broadcast_mac_addr(addr);
+	u8 match = 0;
+
+	_enter_critical_bh(&(acl_node_q->lock), &irqL);
+
+	head = get_list_head(acl_node_q);
+	list = get_next(head);
+
+	while (rtw_end_of_queue_search(head, list) == false) {
+		acl_node = LIST_CONTAINOR(list, struct rtw_wlan_acl_node, list);
+		list = get_next(list);
+
+		if (is_baddr || !memcmp(acl_node->addr, addr, ETH_ALEN)) {
+			if (acl_node->valid ) {
+				acl_node->valid = false;
+				list_del_init(&acl_node->list);
+				acl->num--;
+				match = 1;
+			}
+		}
+	}
+
+	_exit_critical_bh(&(acl_node_q->lock), &irqL);
+
+	RTW_INFO(FUNC_ADPT_FMT" "MAC_FMT" %s (acl_num=%d)\n"
+		 , FUNC_ADPT_ARG(adapter), MAC_ARG(addr)
+		 , is_baddr ? "clear all" : (match ? "match" : "no found")
+		 , acl->num);
+
+	return ret;
+}
+#endif /* CONFIG_RTW_MACADDR_ACL */
+
+u8 rtw_ap_set_pairwise_key(_adapter *padapter, struct sta_info *psta)
+{
+	struct cmd_obj			*ph2c;
+	struct set_stakey_parm	*psetstakey_para;
+	struct cmd_priv			*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm));
+	if (psetstakey_para == NULL) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+
+
+	psetstakey_para->algorithm = (u8)psta->dot118021XPrivacy;
+
+	memcpy(psetstakey_para->addr, psta->hwaddr, ETH_ALEN);
+
+	memcpy(psetstakey_para->key, &psta->dot118021x_UncstKey, 16);
+
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+
+}
+
+static int rtw_ap_set_key(_adapter *padapter, u8 *key, u8 alg, int keyid, u8 set_tx)
+{
+	u8 keylen;
+	struct cmd_obj *pcmd;
+	struct setkey_parm *psetkeyparm;
+	struct cmd_priv	*pcmdpriv = &(padapter->cmdpriv);
+	int res = _SUCCESS;
+
+	/* RTW_INFO("%s\n", __func__); */
+
+	pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	psetkeyparm = (struct setkey_parm *)rtw_zmalloc(sizeof(struct setkey_parm));
+	if (psetkeyparm == NULL) {
+		rtw_mfree((unsigned char *)pcmd, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	memset(psetkeyparm, 0, sizeof(struct setkey_parm));
+
+	psetkeyparm->keyid = (u8)keyid;
+	if (is_wep_enc(alg))
+		padapter->securitypriv.key_mask |= BIT(psetkeyparm->keyid);
+
+	psetkeyparm->algorithm = alg;
+
+	psetkeyparm->set_tx = set_tx;
+
+	switch (alg) {
+	case _WEP40_:
+		keylen = 5;
+		break;
+	case _WEP104_:
+		keylen = 13;
+		break;
+	case _TKIP_:
+	case _TKIP_WTMIC_:
+	case _AES_:
+	default:
+		keylen = 16;
+	}
+
+	memcpy(&(psetkeyparm->key[0]), key, keylen);
+
+	pcmd->cmdcode = _SetKey_CMD_;
+	pcmd->parmbuf = (u8 *)psetkeyparm;
+	pcmd->cmdsz = (sizeof(struct setkey_parm));
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+
+
+	INIT_LIST_HEAD(&pcmd->list);
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+
+	return res;
+}
+
+int rtw_ap_set_group_key(_adapter *padapter, u8 *key, u8 alg, int keyid)
+{
+	RTW_INFO("%s\n", __func__);
+
+	return rtw_ap_set_key(padapter, key, alg, keyid, 1);
+}
+
+int rtw_ap_set_wep_key(_adapter *padapter, u8 *key, u8 keylen, int keyid, u8 set_tx)
+{
+	u8 alg;
+
+	switch (keylen) {
+	case 5:
+		alg = _WEP40_;
+		break;
+	case 13:
+		alg = _WEP104_;
+		break;
+	default:
+		alg = _NO_PRIVACY_;
+	}
+
+	RTW_INFO("%s\n", __func__);
+
+	return rtw_ap_set_key(padapter, key, alg, keyid, set_tx);
+}
+
+static u8 rtw_ap_bmc_frames_hdl(_adapter *padapter)
+{
+#define HIQ_XMIT_COUNTS (6)
+	unsigned long irqL;
+	struct sta_info *psta_bmc;
+	_list	*xmitframe_plist, *xmitframe_phead;
+	struct xmit_frame *pxmitframe = NULL;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct sta_priv  *pstapriv = &padapter->stapriv;
+	bool update_tim = false;
+
+
+	if (padapter->registrypriv.wifi_spec != 1)
+		return H2C_SUCCESS;
+
+
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+	if (!psta_bmc)
+		return H2C_SUCCESS;
+
+
+	_enter_critical_bh(&pxmitpriv->lock, &irqL);
+
+	if ((pstapriv->tim_bitmap & BIT(0)) && (psta_bmc->sleepq_len > 0)) {
+		int tx_counts = 0;
+
+		_update_beacon(padapter, _TIM_IE_, NULL, false, "update TIM with TIB=1");
+
+		RTW_INFO("sleepq_len of bmc_sta = %d\n", psta_bmc->sleepq_len);
+
+		xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+		xmitframe_plist = get_next(xmitframe_phead);
+
+		while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == false) {
+			pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+			xmitframe_plist = get_next(xmitframe_plist);
+
+			list_del_init(&pxmitframe->list);
+
+			psta_bmc->sleepq_len--;
+			tx_counts++;
+
+			if (psta_bmc->sleepq_len > 0)
+				pxmitframe->attrib.mdata = 1;
+			else
+				pxmitframe->attrib.mdata = 0;
+
+			if (tx_counts == HIQ_XMIT_COUNTS)
+				pxmitframe->attrib.mdata = 0;
+
+			pxmitframe->attrib.triggered = 1;
+
+			if (xmitframe_hiq_filter(pxmitframe) )
+				pxmitframe->attrib.qsel = QSLT_HIGH;/*HIQ*/
+
+			rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+			if (tx_counts == HIQ_XMIT_COUNTS)
+				break;
+
+		}
+
+	} else {
+		if (psta_bmc->sleepq_len == 0) {
+
+			/*RTW_INFO("sleepq_len of bmc_sta = %d\n", psta_bmc->sleepq_len);*/
+
+			if (pstapriv->tim_bitmap & BIT(0))
+				update_tim = true;
+
+			pstapriv->tim_bitmap &= ~BIT(0);
+			pstapriv->sta_dz_bitmap &= ~BIT(0);
+
+			if (update_tim ) {
+				RTW_INFO("clear TIB\n");
+				_update_beacon(padapter, _TIM_IE_, NULL, true, "bmc sleepq and HIQ empty");
+			}
+		}
+	}
+
+	_exit_critical_bh(&pxmitpriv->lock, &irqL);
+
+	return H2C_SUCCESS;
+}
+
+#ifdef CONFIG_NATIVEAP_MLME
+
+static void associated_stainfo_update(_adapter *padapter, struct sta_info *psta, u32 sta_info_type)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	RTW_INFO("%s: "MAC_FMT", updated_type=0x%x\n", __func__, MAC_ARG(psta->hwaddr), sta_info_type);
+
+	if (sta_info_type & STA_INFO_UPDATE_BW) {
+
+		if ((psta->flags & WLAN_STA_HT) && !psta->ht_20mhz_set) {
+			if (pmlmepriv->sw_to_20mhz) {
+				psta->bw_mode = CHANNEL_WIDTH_20;
+				/*psta->htpriv.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;*/
+				psta->htpriv.sgi_40m = false;
+			} else {
+				/*TODO: Switch back to 40MHZ?80MHZ*/
+			}
+		}
+	}
+
+	/*
+		if (sta_info_type & STA_INFO_UPDATE_RATE) {
+
+		}
+	*/
+
+	if (sta_info_type & STA_INFO_UPDATE_PROTECTION_MODE)
+		VCS_update(padapter, psta);
+
+	/*
+		if (sta_info_type & STA_INFO_UPDATE_CAP) {
+
+		}
+
+		if (sta_info_type & STA_INFO_UPDATE_HT_CAP) {
+
+		}
+
+		if (sta_info_type & STA_INFO_UPDATE_VHT_CAP) {
+
+		}
+	*/
+
+}
+
+static void update_bcn_ext_capab_ie(_adapter *padapter)
+{
+	sint ie_len = 0;
+	unsigned char	*pbuf;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);
+	u8 *ie = pnetwork->IEs;
+	u8 null_extcap_data[8] = {0};
+
+	pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _EXT_CAP_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+	if (pbuf && ie_len > 0)
+		rtw_remove_bcn_ie(padapter, pnetwork, _EXT_CAP_IE_);
+
+	if ((pmlmepriv->ext_capab_ie_len > 0) &&
+	    (!memcmp(pmlmepriv->ext_capab_ie_data, null_extcap_data, sizeof(null_extcap_data)) == false))
+		rtw_add_bcn_ie(padapter, pnetwork, _EXT_CAP_IE_, pmlmepriv->ext_capab_ie_data, pmlmepriv->ext_capab_ie_len);
+
+}
+
+static void update_bcn_fixed_ie(_adapter *padapter)
+{
+	RTW_INFO("%s\n", __func__);
+
+}
+
+static void update_bcn_erpinfo_ie(_adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);
+	unsigned char *p, *ie = pnetwork->IEs;
+	u32 len = 0;
+
+	RTW_INFO("%s, ERP_enable=%d\n", __func__, pmlmeinfo->ERP_enable);
+
+	if (!pmlmeinfo->ERP_enable)
+		return;
+
+	/* parsing ERP_IE */
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+	if (p && len > 0) {
+		PNDIS_802_11_VARIABLE_IEs pIE = (PNDIS_802_11_VARIABLE_IEs)p;
+
+		if (pmlmepriv->num_sta_non_erp == 1)
+			pIE->data[0] |= RTW_ERP_INFO_NON_ERP_PRESENT | RTW_ERP_INFO_USE_PROTECTION;
+		else
+			pIE->data[0] &= ~(RTW_ERP_INFO_NON_ERP_PRESENT | RTW_ERP_INFO_USE_PROTECTION);
+
+		if (pmlmepriv->num_sta_no_short_preamble > 0)
+			pIE->data[0] |= RTW_ERP_INFO_BARKER_PREAMBLE_MODE;
+		else
+			pIE->data[0] &= ~(RTW_ERP_INFO_BARKER_PREAMBLE_MODE);
+
+		ERP_IE_handler(padapter, pIE);
+	}
+
+}
+
+static void update_bcn_htcap_ie(_adapter *padapter)
+{
+	RTW_INFO("%s\n", __func__);
+
+}
+
+static void update_bcn_htinfo_ie(_adapter *padapter)
+{
+	/*
+	u8 beacon_updated = false;
+	u32 sta_info_update_type = STA_INFO_UPDATE_NONE;
+	*/
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);
+	unsigned char *p, *ie = pnetwork->IEs;
+	u32 len = 0;
+
+	if (pmlmepriv->htpriv.ht_option == false)
+		return;
+
+	if (pmlmeinfo->HT_info_enable != 1)
+		return;
+
+
+	RTW_INFO("%s current operation mode=0x%X\n",
+		 __func__, pmlmepriv->ht_op_mode);
+
+	RTW_INFO("num_sta_40mhz_intolerant(%d), 20mhz_width_req(%d), intolerant_ch_rpt(%d), olbc(%d)\n",
+		pmlmepriv->num_sta_40mhz_intolerant, pmlmepriv->ht_20mhz_width_req, pmlmepriv->ht_intolerant_ch_reported, ATOMIC_READ(&pmlmepriv->olbc));
+
+	/*parsing HT_INFO_IE, currently only update ht_op_mode - pht_info->infos[1] & pht_info->infos[2] for wifi logo test*/
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+	if (p && len > 0) {
+		struct HT_info_element *pht_info = NULL;
+
+		pht_info = (struct HT_info_element *)(p + 2);
+
+		/* for STA Channel Width/Secondary Channel Offset*/
+		if ((pmlmepriv->sw_to_20mhz == 0) && (pmlmeext->cur_channel <= 14)) {
+			if ((pmlmepriv->num_sta_40mhz_intolerant > 0) || (pmlmepriv->ht_20mhz_width_req )
+			    || (pmlmepriv->ht_intolerant_ch_reported ) || (ATOMIC_READ(&pmlmepriv->olbc) == true)) {
+				SET_HT_OP_ELE_2ND_CHL_OFFSET(pht_info, 0);
+				SET_HT_OP_ELE_STA_CHL_WIDTH(pht_info, 0);
+
+				pmlmepriv->sw_to_20mhz = 1;
+				/*
+				sta_info_update_type |= STA_INFO_UPDATE_BW;
+				beacon_updated = true;
+				*/
+
+				RTW_INFO("%s:switching to 20Mhz\n", __func__);
+
+				/*TODO : cur_bwmode/cur_ch_offset switches to 20Mhz*/
+			}
+		} else {
+
+			if ((pmlmepriv->num_sta_40mhz_intolerant == 0) && (pmlmepriv->ht_20mhz_width_req == false)
+			    && (pmlmepriv->ht_intolerant_ch_reported == false) && (ATOMIC_READ(&pmlmepriv->olbc) == false)) {
+
+				if (pmlmeext->cur_bwmode >= CHANNEL_WIDTH_40) {
+
+					SET_HT_OP_ELE_STA_CHL_WIDTH(pht_info, 1);
+
+					SET_HT_OP_ELE_2ND_CHL_OFFSET(pht_info,
+						(pmlmeext->cur_ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER) ?
+						HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE : HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW);
+
+					pmlmepriv->sw_to_20mhz = 0;
+					/*
+					sta_info_update_type |= STA_INFO_UPDATE_BW;
+					beacon_updated = true;
+					*/
+
+					RTW_INFO("%s:switching back to 40Mhz\n", __func__);
+				}
+			}
+		}
+
+		/* to update  ht_op_mode*/
+		*(__le16 *)(pht_info->infos + 1) = cpu_to_le16(pmlmepriv->ht_op_mode);
+
+	}
+
+	/*associated_clients_update(padapter, beacon_updated, sta_info_update_type);*/
+
+}
+
+static void update_bcn_rsn_ie(_adapter *padapter)
+{
+	RTW_INFO("%s\n", __func__);
+
+}
+
+static void update_bcn_wpa_ie(_adapter *padapter)
+{
+	RTW_INFO("%s\n", __func__);
+
+}
+
+static void update_bcn_wmm_ie(_adapter *padapter)
+{
+	RTW_INFO("%s\n", __func__);
+
+}
+
+static void update_bcn_wps_ie(_adapter *padapter)
+{
+	u8 *pwps_ie = NULL, *pwps_ie_src, *premainder_ie, *pbackup_remainder_ie = NULL;
+	uint wps_ielen = 0, wps_offset, remainder_ielen;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);
+	unsigned char *ie = pnetwork->IEs;
+	u32 ielen = pnetwork->IELength;
+
+
+	RTW_INFO("%s\n", __func__);
+
+	pwps_ie = rtw_get_wps_ie(ie + _FIXED_IE_LENGTH_, ielen - _FIXED_IE_LENGTH_, NULL, &wps_ielen);
+
+	if (pwps_ie == NULL || wps_ielen == 0)
+		return;
+
+	pwps_ie_src = pmlmepriv->wps_beacon_ie;
+	if (pwps_ie_src == NULL)
+		return;
+
+	wps_offset = (uint)(pwps_ie - ie);
+
+	premainder_ie = pwps_ie + wps_ielen;
+
+	remainder_ielen = ielen - wps_offset - wps_ielen;
+
+	if (remainder_ielen > 0) {
+		pbackup_remainder_ie = rtw_malloc(remainder_ielen);
+		if (pbackup_remainder_ie)
+			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+	}
+
+	wps_ielen = (uint)pwps_ie_src[1];/* to get ie data len */
+	if ((wps_offset + wps_ielen + 2 + remainder_ielen) <= MAX_IE_SZ) {
+		memcpy(pwps_ie, pwps_ie_src, wps_ielen + 2);
+		pwps_ie += (wps_ielen + 2);
+
+		if (pbackup_remainder_ie)
+			memcpy(pwps_ie, pbackup_remainder_ie, remainder_ielen);
+
+		/* update IELength */
+		pnetwork->IELength = wps_offset + (wps_ielen + 2) + remainder_ielen;
+	}
+
+	if (pbackup_remainder_ie)
+		rtw_mfree(pbackup_remainder_ie, remainder_ielen);
+
+	/* deal with the case without set_tx_beacon_cmd() in update_beacon() */
+#if defined(CONFIG_INTERRUPT_BASED_TXBCN)
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
+		u8 sr = 0;
+		rtw_get_wps_attr_content(pwps_ie_src,  wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8 *)(&sr), NULL);
+
+		if (sr) {
+			set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
+			RTW_INFO("%s, set WIFI_UNDER_WPS\n", __func__);
+		} else {
+			clr_fwstate(pmlmepriv, WIFI_UNDER_WPS);
+			RTW_INFO("%s, clr WIFI_UNDER_WPS\n", __func__);
+		}
+	}
+#endif
+}
+
+static void update_bcn_p2p_ie(_adapter *padapter)
+{
+
+}
+
+static void update_bcn_vendor_spec_ie(_adapter *padapter, u8 *oui)
+{
+	RTW_INFO("%s\n", __func__);
+
+	if (!memcmp(RTW_WPA_OUI, oui, 4))
+		update_bcn_wpa_ie(padapter);
+	else if (!memcmp(WMM_OUI, oui, 4))
+		update_bcn_wmm_ie(padapter);
+	else if (!memcmp(WPS_OUI, oui, 4))
+		update_bcn_wps_ie(padapter);
+	else if (!memcmp(P2P_OUI, oui, 4))
+		update_bcn_p2p_ie(padapter);
+	else
+		RTW_INFO("unknown OUI type!\n");
+
+
+}
+
+void _update_beacon(_adapter *padapter, u8 ie_id, u8 *oui, u8 tx, const char *tag)
+{
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv;
+	struct mlme_ext_priv	*pmlmeext;
+	/* struct mlme_ext_info	*pmlmeinfo; */
+
+	/* RTW_INFO("%s\n", __func__); */
+
+	if (!padapter)
+		return;
+
+	pmlmepriv = &(padapter->mlmepriv);
+	pmlmeext = &(padapter->mlmeextpriv);
+	/* pmlmeinfo = &(pmlmeext->mlmext_info); */
+
+	if (false == pmlmeext->bstart_bss)
+		return;
+
+	_enter_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
+
+	switch (ie_id) {
+	case 0xFF:
+
+		update_bcn_fixed_ie(padapter);/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
+
+		break;
+
+	case _TIM_IE_:
+
+		update_BCNTIM(padapter);
+
+		break;
+
+	case _ERPINFO_IE_:
+
+		update_bcn_erpinfo_ie(padapter);
+
+		break;
+
+	case _HT_CAPABILITY_IE_:
+
+		update_bcn_htcap_ie(padapter);
+
+		break;
+
+	case _RSN_IE_2_:
+
+		update_bcn_rsn_ie(padapter);
+
+		break;
+
+	case _HT_ADD_INFO_IE_:
+
+		update_bcn_htinfo_ie(padapter);
+
+		break;
+
+	case _EXT_CAP_IE_:
+
+		update_bcn_ext_capab_ie(padapter);
+
+		break;
+
+	case _VENDOR_SPECIFIC_IE_:
+
+		update_bcn_vendor_spec_ie(padapter, oui);
+
+		break;
+
+	default:
+		break;
+	}
+
+	pmlmepriv->update_bcn = true;
+
+	_exit_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
+
+#ifndef CONFIG_INTERRUPT_BASED_TXBCN
+	if (tx) {
+		/* send_beacon(padapter); */ /* send_beacon must execute on TSR level */
+		if (0)
+			RTW_INFO(FUNC_ADPT_FMT" ie_id:%u - %s\n", FUNC_ADPT_ARG(padapter), ie_id, tag);
+		set_tx_beacon_cmd(padapter);
+	}
+#endif /* !CONFIG_INTERRUPT_BASED_TXBCN */
+}
+
+void rtw_process_public_act_bsscoex(_adapter *padapter, u8 *pframe, uint frame_len)
+{
+	struct sta_info *psta;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 beacon_updated = false;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	uint frame_body_len = frame_len - sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 category, action;
+
+	psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe));
+	if (psta == NULL)
+		return;
+
+
+	category = frame_body[0];
+	action = frame_body[1];
+
+	if (frame_body_len > 0) {
+		if ((frame_body[2] == EID_BSSCoexistence) && (frame_body[3] > 0)) {
+			u8 ie_data = frame_body[4];
+
+			if (ie_data & RTW_WLAN_20_40_BSS_COEX_40MHZ_INTOL) {
+				if (psta->ht_40mhz_intolerant == 0) {
+					psta->ht_40mhz_intolerant = 1;
+					pmlmepriv->num_sta_40mhz_intolerant++;
+					beacon_updated = true;
+				}
+			} else if (ie_data & RTW_WLAN_20_40_BSS_COEX_20MHZ_WIDTH_REQ)	{
+				if (pmlmepriv->ht_20mhz_width_req == false) {
+					pmlmepriv->ht_20mhz_width_req = true;
+					beacon_updated = true;
+				}
+			} else
+				beacon_updated = false;
+		}
+	}
+
+	if (frame_body_len > 8) {
+		/* if EID_BSSIntolerantChlReport ie exists */
+		if ((frame_body[5] == EID_BSSIntolerantChlReport) && (frame_body[6] > 0)) {
+			/*todo:*/
+			if (pmlmepriv->ht_intolerant_ch_reported == false) {
+				pmlmepriv->ht_intolerant_ch_reported = true;
+				beacon_updated = true;
+			}
+		}
+	}
+
+	if (beacon_updated) {
+
+		update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, true);
+
+		associated_stainfo_update(padapter, psta, STA_INFO_UPDATE_BW);
+	}
+
+
+
+}
+
+void rtw_process_ht_action_smps(_adapter *padapter, u8 *ta, u8 ctrl_field)
+{
+	u8 e_field, m_field;
+	struct sta_info *psta;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	psta = rtw_get_stainfo(pstapriv, ta);
+	if (psta == NULL)
+		return;
+
+	e_field = (ctrl_field & BIT(0)) ? 1 : 0;
+	m_field = (ctrl_field & BIT(1)) ? 1 : 0;
+
+	if (e_field) {
+
+		/* enable */
+		/* 0:static SMPS, 1:dynamic SMPS, 3:SMPS disabled, 2:reserved*/
+
+		if (m_field) /*mode*/
+			psta->htpriv.smps_cap = 1;
+		else
+			psta->htpriv.smps_cap = 0;
+	} else {
+		/*disable*/
+		psta->htpriv.smps_cap = 3;
+	}
+
+	rtw_dm_ra_mask_wk_cmd(padapter, (u8 *)psta);
+
+}
+
+/*
+op_mode
+Set to 0 (HT pure) under the followign conditions
+	- all STAs in the BSS are 20/40 MHz HT in 20/40 MHz BSS or
+	- all STAs in the BSS are 20 MHz HT in 20 MHz BSS
+Set to 1 (HT non-member protection) if there may be non-HT STAs
+	in both the primary and the secondary channel
+Set to 2 if only HT STAs are associated in BSS,
+	however and at least one 20 MHz HT STA is associated
+Set to 3 (HT mixed mode) when one or more non-HT STAs are associated
+	(currently non-GF HT station is considered as non-HT STA also)
+*/
+int rtw_ht_operation_update(_adapter *padapter)
+{
+	u16 cur_op_mode, new_op_mode;
+	int op_mode_changes = 0;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct ht_priv	*phtpriv_ap = &pmlmepriv->htpriv;
+
+	if (pmlmepriv->htpriv.ht_option == false)
+		return 0;
+
+	/*if (!iface->conf->ieee80211n || iface->conf->ht_op_mode_fixed)
+		return 0;*/
+
+	RTW_INFO("%s current operation mode=0x%X\n",
+		 __func__, pmlmepriv->ht_op_mode);
+
+	if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT)
+	    && pmlmepriv->num_sta_ht_no_gf) {
+		pmlmepriv->ht_op_mode |=
+			HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;
+		op_mode_changes++;
+	} else if ((pmlmepriv->ht_op_mode &
+		    HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) &&
+		   pmlmepriv->num_sta_ht_no_gf == 0) {
+		pmlmepriv->ht_op_mode &=
+			~HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;
+		op_mode_changes++;
+	}
+
+	if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&
+	    (pmlmepriv->num_sta_no_ht || ATOMIC_READ(&pmlmepriv->olbc_ht))) {
+		pmlmepriv->ht_op_mode |= HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+		op_mode_changes++;
+	} else if ((pmlmepriv->ht_op_mode &
+		    HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&
+		   (pmlmepriv->num_sta_no_ht == 0 && !ATOMIC_READ(&pmlmepriv->olbc_ht))) {
+		pmlmepriv->ht_op_mode &=
+			~HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+		op_mode_changes++;
+	}
+
+	/* Note: currently we switch to the MIXED op mode if HT non-greenfield
+	 * station is associated. Probably it's a theoretical case, since
+	 * it looks like all known HT STAs support greenfield.
+	 */
+	new_op_mode = 0;
+	if (pmlmepriv->num_sta_no_ht /*||
+	    (pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT)*/)
+		new_op_mode = OP_MODE_MIXED;
+	else if ((phtpriv_ap->ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH))
+		 && pmlmepriv->num_sta_ht_20mhz)
+		new_op_mode = OP_MODE_20MHZ_HT_STA_ASSOCED;
+	else if (ATOMIC_READ(&pmlmepriv->olbc_ht))
+		new_op_mode = OP_MODE_MAY_BE_LEGACY_STAS;
+	else
+		new_op_mode = OP_MODE_PURE;
+
+	cur_op_mode = pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+	if (cur_op_mode != new_op_mode) {
+		pmlmepriv->ht_op_mode &= ~HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+		pmlmepriv->ht_op_mode |= new_op_mode;
+		op_mode_changes++;
+	}
+
+	RTW_INFO("%s new operation mode=0x%X changes=%d\n",
+		 __func__, pmlmepriv->ht_op_mode, op_mode_changes);
+
+	return op_mode_changes;
+
+}
+
+void associated_clients_update(_adapter *padapter, u8 updated, u32 sta_info_type)
+{
+	/* update associcated stations cap. */
+	if (updated ) {
+		unsigned long irqL;
+		_list	*phead, *plist;
+		struct sta_info *psta = NULL;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+		phead = &pstapriv->asoc_list;
+		plist = get_next(phead);
+
+		/* check asoc_queue */
+		while ((rtw_end_of_queue_search(phead, plist)) == false) {
+			psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+
+			plist = get_next(plist);
+
+			associated_stainfo_update(padapter, psta, sta_info_type);
+		}
+
+		_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+	}
+
+}
+
+/* called > TSR LEVEL for USB or SDIO Interface*/
+void bss_cap_update_on_sta_join(_adapter *padapter, struct sta_info *psta)
+{
+	u8 beacon_updated = false;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+	if (!(psta->flags & WLAN_STA_SHORT_PREAMBLE)) {
+		if (!psta->no_short_preamble_set) {
+			psta->no_short_preamble_set = 1;
+
+			pmlmepriv->num_sta_no_short_preamble++;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_preamble == 1)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+
+		}
+	} else {
+		if (psta->no_short_preamble_set) {
+			psta->no_short_preamble_set = 0;
+
+			pmlmepriv->num_sta_no_short_preamble--;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_preamble == 0)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+
+		}
+	}
+
+	if (psta->flags & WLAN_STA_NONERP) {
+		if (!psta->nonerp_set) {
+			psta->nonerp_set = 1;
+
+			pmlmepriv->num_sta_non_erp++;
+
+			if (pmlmepriv->num_sta_non_erp == 1) {
+				beacon_updated = true;
+				update_beacon(padapter, _ERPINFO_IE_, NULL, true);
+			}
+		}
+
+	} else {
+		if (psta->nonerp_set) {
+			psta->nonerp_set = 0;
+
+			pmlmepriv->num_sta_non_erp--;
+
+			if (pmlmepriv->num_sta_non_erp == 0) {
+				beacon_updated = true;
+				update_beacon(padapter, _ERPINFO_IE_, NULL, true);
+			}
+		}
+
+	}
+	if (!(psta->capability & WLAN_CAPABILITY_SHORT_SLOT)) {
+		if (!psta->no_short_slot_time_set) {
+			psta->no_short_slot_time_set = 1;
+
+			pmlmepriv->num_sta_no_short_slot_time++;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_slot_time == 1)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+
+		}
+	} else {
+		if (psta->no_short_slot_time_set) {
+			psta->no_short_slot_time_set = 0;
+
+			pmlmepriv->num_sta_no_short_slot_time--;
+
+			if ((pmlmeext->cur_wireless_mode > WIRELESS_11B) &&
+			    (pmlmepriv->num_sta_no_short_slot_time == 0)) {
+				beacon_updated = true;
+				update_beacon(padapter, 0xFF, NULL, true);
+			}
+		}
+	}
+
+	if (psta->flags & WLAN_STA_HT) {
+		u16 ht_capab = le16_to_cpu(psta->htpriv.ht_cap.cap_info);
+
+		RTW_INFO("HT: STA " MAC_FMT " HT Capabilities "
+			 "Info: 0x%04x\n", MAC_ARG(psta->hwaddr), ht_capab);
+
+		if (psta->no_ht_set) {
+			psta->no_ht_set = 0;
+			pmlmepriv->num_sta_no_ht--;
+		}
+
+		if ((ht_capab & IEEE80211_HT_CAP_GRN_FLD) == 0) {
+			if (!psta->no_ht_gf_set) {
+				psta->no_ht_gf_set = 1;
+				pmlmepriv->num_sta_ht_no_gf++;
+			}
+			RTW_INFO("%s STA " MAC_FMT " - no "
+				 "greenfield, num of non-gf stations %d\n",
+				 __func__, MAC_ARG(psta->hwaddr),
+				 pmlmepriv->num_sta_ht_no_gf);
+		}
+
+		if ((ht_capab & IEEE80211_HT_CAP_SUP_WIDTH) == 0) {
+			if (!psta->ht_20mhz_set) {
+				psta->ht_20mhz_set = 1;
+				pmlmepriv->num_sta_ht_20mhz++;
+			}
+			RTW_INFO("%s STA " MAC_FMT " - 20 MHz HT, "
+				 "num of 20MHz HT STAs %d\n",
+				 __func__, MAC_ARG(psta->hwaddr),
+				 pmlmepriv->num_sta_ht_20mhz);
+		}
+
+	} else {
+		if (!psta->no_ht_set) {
+			psta->no_ht_set = 1;
+			pmlmepriv->num_sta_no_ht++;
+		}
+		if (pmlmepriv->htpriv.ht_option ) {
+			RTW_INFO("%s STA " MAC_FMT
+				 " - no HT, num of non-HT stations %d\n",
+				 __func__, MAC_ARG(psta->hwaddr),
+				 pmlmepriv->num_sta_no_ht);
+		}
+	}
+
+	if (rtw_ht_operation_update(padapter) > 0) {
+		update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, false);
+		update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, true);
+		/*beacon_updated = true;*/
+	}
+
+	/* update associcated stations cap. */
+	associated_clients_update(padapter,  beacon_updated, STA_INFO_UPDATE_ALL);
+
+	RTW_INFO("%s, updated=%d\n", __func__, beacon_updated);
+
+}
+
+u8 bss_cap_update_on_sta_leave(_adapter *padapter, struct sta_info *psta)
+{
+	u8 beacon_updated = false;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+	if (!psta)
+		return beacon_updated;
+
+	if (psta->no_short_preamble_set) {
+		psta->no_short_preamble_set = 0;
+		pmlmepriv->num_sta_no_short_preamble--;
+		if (pmlmeext->cur_wireless_mode > WIRELESS_11B
+		    && pmlmepriv->num_sta_no_short_preamble == 0) {
+			beacon_updated = true;
+			update_beacon(padapter, 0xFF, NULL, true);
+		}
+	}
+
+	if (psta->nonerp_set) {
+		psta->nonerp_set = 0;
+		pmlmepriv->num_sta_non_erp--;
+		if (pmlmepriv->num_sta_non_erp == 0) {
+			beacon_updated = true;
+			update_beacon(padapter, _ERPINFO_IE_, NULL, true);
+		}
+	}
+
+	if (psta->no_short_slot_time_set) {
+		psta->no_short_slot_time_set = 0;
+		pmlmepriv->num_sta_no_short_slot_time--;
+		if (pmlmeext->cur_wireless_mode > WIRELESS_11B
+		    && pmlmepriv->num_sta_no_short_slot_time == 0) {
+			beacon_updated = true;
+			update_beacon(padapter, 0xFF, NULL, true);
+		}
+	}
+
+	if (psta->no_ht_gf_set) {
+		psta->no_ht_gf_set = 0;
+		pmlmepriv->num_sta_ht_no_gf--;
+	}
+
+	if (psta->no_ht_set) {
+		psta->no_ht_set = 0;
+		pmlmepriv->num_sta_no_ht--;
+	}
+
+	if (psta->ht_20mhz_set) {
+		psta->ht_20mhz_set = 0;
+		pmlmepriv->num_sta_ht_20mhz--;
+	}
+
+	if (rtw_ht_operation_update(padapter) > 0) {
+		update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, false);
+		update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, true);
+	}
+
+	RTW_INFO("%s, updated=%d\n", __func__, beacon_updated);
+
+	return beacon_updated;
+}
+
+u8 ap_free_sta(_adapter *padapter, struct sta_info *psta, bool active, u16 reason, bool enqueue)
+{
+	unsigned long irqL;
+	u8 beacon_updated = false;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	if (!psta)
+		return beacon_updated;
+
+	if (active ) {
+		/* tear down Rx AMPDU */
+		send_delba(padapter, 0, psta->hwaddr);/* recipient */
+
+		/* tear down TX AMPDU */
+		send_delba(padapter, 1, psta->hwaddr);/*  */ /* originator */
+
+		issue_deauth(padapter, psta->hwaddr, reason);
+	}
+
+#ifdef CONFIG_BEAMFORMING
+	beamforming_wk_cmd(padapter, BEAMFORMING_CTRL_LEAVE, psta->hwaddr, ETH_ALEN, 1);
+#endif
+
+	psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+	psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+
+	/* clear cam entry / key */
+	rtw_clearstakey_cmd(padapter, psta, enqueue);
+
+
+	_enter_critical_bh(&psta->lock, &irqL);
+	psta->state &= ~_FW_LINKED;
+	_exit_critical_bh(&psta->lock, &irqL);
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (1) {
+#ifdef COMPAT_KERNEL_RELEASE
+		rtw_cfg80211_indicate_sta_disassoc(padapter, psta->hwaddr, reason);
+#elif (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)
+		rtw_cfg80211_indicate_sta_disassoc(padapter, psta->hwaddr, reason);
+#else /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER) */
+		/* will call rtw_cfg80211_indicate_sta_disassoc() in cmd_thread for old API context */
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER) */
+	} else
+#endif /* CONFIG_IOCTL_CFG80211 */
+	{
+		rtw_indicate_sta_disassoc_event(padapter, psta);
+	}
+
+	report_del_sta_event(padapter, psta->hwaddr, reason, enqueue, false);
+
+	beacon_updated = bss_cap_update_on_sta_leave(padapter, psta);
+
+	/* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);					 */
+	rtw_free_stainfo(padapter, psta);
+	/* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */
+
+
+	return beacon_updated;
+
+}
+
+int rtw_ap_inform_ch_switch(_adapter *padapter, u8 new_ch, u8 ch_offset)
+{
+	unsigned long irqL;
+	_list	*phead, *plist;
+	int ret = 0;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		return ret;
+
+	RTW_INFO(FUNC_NDEV_FMT" with ch:%u, offset:%u\n",
+		 FUNC_NDEV_ARG(padapter->pnetdev), new_ch, ch_offset);
+
+	_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+	phead = &pstapriv->asoc_list;
+	plist = get_next(phead);
+
+	/* for each sta in asoc_queue */
+	while ((rtw_end_of_queue_search(phead, plist)) == false) {
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+		plist = get_next(plist);
+
+		issue_action_spct_ch_switch(padapter, psta->hwaddr, new_ch, ch_offset);
+		psta->expire_to = ((pstapriv->expire_to * 2) > 5) ? 5 : (pstapriv->expire_to * 2);
+	}
+	_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+	issue_action_spct_ch_switch(padapter, bc_addr, new_ch, ch_offset);
+
+	return ret;
+}
+
+int rtw_sta_flush(_adapter *padapter, bool enqueue)
+{
+	unsigned long irqL;
+	_list	*phead, *plist;
+	int ret = 0;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 flush_num = 0;
+	char flush_list[NUM_STA];
+	int i;
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		return ret;
+
+	RTW_INFO(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+
+	/* pick sta from sta asoc_queue */
+	_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+	phead = &pstapriv->asoc_list;
+	plist = get_next(phead);
+	while ((rtw_end_of_queue_search(phead, plist)) == false) {
+		int stainfo_offset;
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+		plist = get_next(plist);
+
+		list_del_init(&psta->asoc_list);
+		pstapriv->asoc_list_cnt--;
+
+		stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
+		if (stainfo_offset_valid(stainfo_offset))
+			flush_list[flush_num++] = stainfo_offset;
+		else
+			rtw_warn_on(1);
+	}
+	_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+	/* call ap_free_sta() for each sta picked */
+	for (i = 0; i < flush_num; i++) {
+		psta = rtw_get_stainfo_by_offset(pstapriv, flush_list[i]);
+		ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING, enqueue);
+	}
+
+	issue_deauth(padapter, bc_addr, WLAN_REASON_DEAUTH_LEAVING);
+
+	associated_clients_update(padapter, true, STA_INFO_UPDATE_ALL);
+
+	return ret;
+}
+
+/* called > TSR LEVEL for USB or SDIO Interface*/
+void sta_info_update(_adapter *padapter, struct sta_info *psta)
+{
+	int flags = psta->flags;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+
+	/* update wmm cap. */
+	if (WLAN_STA_WME & flags)
+		psta->qos_option = 1;
+	else
+		psta->qos_option = 0;
+
+	if (pmlmepriv->qospriv.qos_option == 0)
+		psta->qos_option = 0;
+
+	/* update 802.11n ht cap. */
+	if (WLAN_STA_HT & flags) {
+		psta->htpriv.ht_option = true;
+		psta->qos_option = 1;
+
+		psta->htpriv.smps_cap = (le16_to_cpu(psta->htpriv.ht_cap.cap_info) &
+					 IEEE80211_HT_CAP_SM_PS) >> 2;
+	} else
+		psta->htpriv.ht_option = false;
+
+	if (pmlmepriv->htpriv.ht_option == false)
+		psta->htpriv.ht_option = false;
+	update_sta_info_apmode(padapter, psta);
+}
+
+/* called >= TSR LEVEL for USB or SDIO Interface*/
+void ap_sta_info_defer_update(_adapter *padapter, struct sta_info *psta)
+{
+	if (psta->state & _FW_LINKED)
+		rtw_hal_update_ra_mask(psta, psta->rssi_level, true); /* DM_RATR_STA_INIT */
+}
+/* restore hw setting from sw data structures */
+void rtw_ap_restore_network(_adapter *padapter)
+{
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta;
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	unsigned long irqL;
+	_list	*phead, *plist;
+	u8 chk_alive_num = 0;
+	char chk_alive_list[NUM_STA];
+	int i;
+
+	rtw_setopmode_cmd(padapter, Ndis802_11APMode, false);
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	rtw_startbss_cmd(padapter, RTW_CMDF_DIRECTLY);
+
+	if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
+	    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+		/* restore group key, WEP keys is restored in ips_leave() */
+		rtw_set_key(padapter, psecuritypriv, psecuritypriv->dot118021XGrpKeyid, 0, false);
+	}
+
+	_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+	phead = &pstapriv->asoc_list;
+	plist = get_next(phead);
+
+	while ((rtw_end_of_queue_search(phead, plist)) == false) {
+		int stainfo_offset;
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+		plist = get_next(plist);
+
+		stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
+		if (stainfo_offset_valid(stainfo_offset))
+			chk_alive_list[chk_alive_num++] = stainfo_offset;
+	}
+
+	_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+	for (i = 0; i < chk_alive_num; i++) {
+		psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);
+
+		if (psta == NULL)
+			RTW_INFO(FUNC_ADPT_FMT" sta_info is null\n", FUNC_ADPT_ARG(padapter));
+		else if (psta->state & _FW_LINKED) {
+			rtw_sta_media_status_rpt(padapter, psta, 1);
+			Update_RA_Entry(padapter, psta);
+			/* pairwise key */
+			/* per sta pairwise key and settings */
+			if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
+			    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_))
+				rtw_setstakey_cmd(padapter, psta, UNICAST_KEY, false);
+		}
+	}
+
+}
+
+void start_ap_mode(_adapter *padapter)
+{
+	int i;
+	struct sta_info *psta = NULL;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	pmlmepriv->update_bcn = false;
+
+	/*init_mlme_ap_info(padapter);*/
+
+	pmlmeext->bstart_bss = false;
+
+	pmlmepriv->num_sta_non_erp = 0;
+
+	pmlmepriv->num_sta_no_short_slot_time = 0;
+
+	pmlmepriv->num_sta_no_short_preamble = 0;
+
+	pmlmepriv->num_sta_ht_no_gf = 0;
+	pmlmepriv->num_sta_no_ht = 0;
+	pmlmeinfo->HT_info_enable = 0;
+	pmlmeinfo->HT_caps_enable = 0;
+	pmlmeinfo->HT_enable = 0;
+
+	pmlmepriv->num_sta_ht_20mhz = 0;
+	pmlmepriv->num_sta_40mhz_intolerant = 0;
+	ATOMIC_SET(&pmlmepriv->olbc, false);
+	ATOMIC_SET(&pmlmepriv->olbc_ht, false);
+
+	pmlmepriv->ht_20mhz_width_req = false;
+	pmlmepriv->ht_intolerant_ch_reported = false;
+	pmlmepriv->ht_op_mode = 0;
+	pmlmepriv->sw_to_20mhz = 0;
+
+	memset(pmlmepriv->ext_capab_ie_data, 0, sizeof(pmlmepriv->ext_capab_ie_data));
+	pmlmepriv->ext_capab_ie_len = 0;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	psecuritypriv->dot118021x_bmc_cam_id = INVALID_SEC_MAC_CAM_ID;
+#endif
+
+	for (i = 0 ;  i < NUM_STA ; i++)
+		pstapriv->sta_aid[i] = NULL;
+
+#if CONFIG_RTW_MACADDR_ACL
+	rtw_macaddr_acl_init(padapter);
+#endif
+
+	psta = rtw_get_bcmc_stainfo(padapter);
+	/*_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);*/
+	if (psta)
+		rtw_free_stainfo(padapter, psta);
+	/*_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);*/
+
+	rtw_init_bcmc_stainfo(padapter);
+
+	if (rtw_mi_get_ap_num(padapter))
+		RTW_SET_SCAN_BAND_SKIP(padapter, BAND_5G);
+
+}
+
+void stop_ap_mode(_adapter *padapter)
+{
+	unsigned long irqL;
+	struct sta_info *psta = NULL;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct dvobj_priv *pdvobj = padapter->dvobj;
+
+	RTW_INFO("%s -"ADPT_FMT"\n", __func__, ADPT_ARG(padapter));
+
+	pmlmepriv->update_bcn = false;
+	padapter->netif_up = false;
+
+	/* reset and init security priv , this can refine with rtw_reset_securitypriv */
+	memset((unsigned char *)&padapter->securitypriv, 0, sizeof(struct security_priv));
+	padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
+	padapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled;
+
+#ifdef CONFIG_DFS_MASTER
+	rtw_dfs_master_status_apply(padapter, MLME_AP_STOPPED);
+#endif
+
+	/* free scan queue */
+	rtw_free_network_queue(padapter, true);
+
+#if CONFIG_RTW_MACADDR_ACL
+	rtw_macaddr_acl_deinit(padapter);
+#endif
+
+	rtw_sta_flush(padapter, true);
+
+	/* free_assoc_sta_resources	 */
+	rtw_free_all_stainfo(padapter);
+
+	psta = rtw_get_bcmc_stainfo(padapter);
+	/* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		 */
+	rtw_free_stainfo(padapter, psta);
+	/*_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);*/
+
+	rtw_free_mlme_priv_ie_data(pmlmepriv);
+
+#ifdef CONFIG_SWTIMER_BASED_TXBCN
+	if (pmlmeext->bstart_bss ) {
+		_enter_critical_bh(&pdvobj->ap_if_q.lock, &irqL);
+		pdvobj->nr_ap_if--;
+		if (pdvobj->nr_ap_if > 0)
+			pdvobj->inter_bcn_space = DEFAULT_BCN_INTERVAL / pdvobj->nr_ap_if;
+		else
+			pdvobj->inter_bcn_space = DEFAULT_BCN_INTERVAL;
+
+		list_del_init(&padapter->list);
+		_exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL);
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pdvobj->inter_bcn_space));
+
+		if (pdvobj->nr_ap_if == 0)
+			_cancel_timer_ex(&pdvobj->txbcn_timer);
+	}
+#endif
+
+	pmlmeext->bstart_bss = false;
+
+#ifdef CONFIG_BT_COEXIST
+	rtw_btcoex_MediaStatusNotify(padapter, 0); /* disconnect */
+#endif
+
+}
+
+#endif /* CONFIG_NATIVEAP_MLME */
+
+void rtw_ap_update_bss_chbw(_adapter *adapter, WLAN_BSSID_EX *bss, u8 ch, u8 bw, u8 offset)
+{
+#define UPDATE_VHT_CAP 1
+#define UPDATE_HT_CAP 1
+
+	struct ht_priv	*htpriv = &adapter->mlmepriv.htpriv;
+	u8 *ht_cap_ie, *ht_op_ie;
+	int ht_cap_ielen, ht_op_ielen;
+
+	ht_cap_ie = rtw_get_ie((bss->IEs + sizeof(NDIS_802_11_FIXED_IEs)), EID_HTCapability, &ht_cap_ielen, (bss->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+	ht_op_ie = rtw_get_ie((bss->IEs + sizeof(NDIS_802_11_FIXED_IEs)), EID_HTInfo, &ht_op_ielen, (bss->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+
+	/* update ht cap ie */
+	if (ht_cap_ie && ht_cap_ielen) {
+		#if UPDATE_HT_CAP
+		if (bw >= CHANNEL_WIDTH_40)
+			SET_HT_CAP_ELE_CHL_WIDTH(ht_cap_ie + 2, 1);
+		else
+			SET_HT_CAP_ELE_CHL_WIDTH(ht_cap_ie + 2, 0);
+
+		if (bw >= CHANNEL_WIDTH_40 && htpriv->sgi_40m)
+			SET_HT_CAP_ELE_SHORT_GI40M(ht_cap_ie + 2, 1);
+		else
+			SET_HT_CAP_ELE_SHORT_GI40M(ht_cap_ie + 2, 0);
+
+		if (htpriv->sgi_20m)
+			SET_HT_CAP_ELE_SHORT_GI20M(ht_cap_ie + 2, 1);
+		else
+			SET_HT_CAP_ELE_SHORT_GI20M(ht_cap_ie + 2, 0);
+		#endif
+	}
+
+	/* update ht op ie */
+	if (ht_op_ie && ht_op_ielen) {
+		SET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2, ch);
+		switch (offset) {
+		case HAL_PRIME_CHNL_OFFSET_LOWER:
+			SET_HT_OP_ELE_2ND_CHL_OFFSET(ht_op_ie + 2, SCA);
+			break;
+		case HAL_PRIME_CHNL_OFFSET_UPPER:
+			SET_HT_OP_ELE_2ND_CHL_OFFSET(ht_op_ie + 2, SCB);
+			break;
+		case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
+		default:
+			SET_HT_OP_ELE_2ND_CHL_OFFSET(ht_op_ie + 2, SCN);
+			break;
+		}
+
+		if (bw >= CHANNEL_WIDTH_40)
+			SET_HT_OP_ELE_STA_CHL_WIDTH(ht_op_ie + 2, 1);
+		else
+			SET_HT_OP_ELE_STA_CHL_WIDTH(ht_op_ie + 2, 0);
+	}
+
+	{
+		u8 *p;
+		int ie_len;
+		u8 old_ch = bss->Configuration.DSConfig;
+		bool change_band = false;
+
+		if ((ch <= 14 && old_ch >= 36) || (ch >= 36 && old_ch <= 14))
+			change_band = true;
+
+		/* update channel in IE */
+		p = rtw_get_ie((bss->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (bss->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
+		if (p && ie_len > 0)
+			*(p + 2) = ch;
+
+		bss->Configuration.DSConfig = ch;
+
+		/* band is changed, update ERP, support rate, ext support rate IE */
+		if (change_band )
+			change_band_update_ie(adapter, bss, ch);
+	}
+
+}
+
+bool rtw_ap_chbw_decision(_adapter *adapter, s16 req_ch, s8 req_bw, s8 req_offset
+			  , u8 *ch, u8 *bw, u8 *offset, u8 *chbw_allow)
+{
+	u8 cur_ie_ch, cur_ie_bw, cur_ie_offset;
+	u8 dec_ch, dec_bw, dec_offset;
+	u8 u_ch = 0, u_offset, u_bw;
+	bool changed = false;
+	struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
+	WLAN_BSSID_EX *network = &(adapter->mlmepriv.cur_network.network);
+	struct mi_state mstate;
+	bool set_u_ch = false, set_dec_ch = false;
+
+	rtw_ies_get_chbw(network->IEs + sizeof(NDIS_802_11_FIXED_IEs)
+			 , network->IELength - sizeof(NDIS_802_11_FIXED_IEs)
+			 , &cur_ie_ch, &cur_ie_bw, &cur_ie_offset);
+
+#ifdef CONFIG_MCC_MODE
+	if (MCC_EN(adapter)) {
+		if (rtw_hal_check_mcc_status(adapter, MCC_STATUS_DOING_MCC)) {
+			/* check channel settings are the same */
+			if (cur_ie_ch == mlmeext->cur_channel
+				&& cur_ie_bw == mlmeext->cur_bwmode
+					&& cur_ie_offset == mlmeext->cur_ch_offset) {
+
+
+					RTW_INFO(FUNC_ADPT_FMT"req ch settings are the same as current ch setting, go to exit\n"
+						, FUNC_ADPT_ARG(adapter));
+
+					*chbw_allow = false;
+					goto exit;
+			} else {
+					RTW_INFO(FUNC_ADPT_FMT"request channel settings are not the same as current channel setting(%d,%d,%d,%d,%d,%d), restart MCC\n"
+						, FUNC_ADPT_ARG(adapter)
+						, cur_ie_ch, cur_ie_bw, cur_ie_bw
+						, mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset);
+
+				rtw_hal_set_mcc_setting_disconnect(adapter);
+			}
+		}	
+	}
+#endif /* CONFIG_MCC_MODE */
+
+	/* use chbw of cur_ie updated with specifying req as temporary decision */
+	dec_ch = (req_ch <= 0) ? cur_ie_ch : req_ch;
+	dec_bw = (req_bw < 0) ? cur_ie_bw : req_bw;
+	dec_offset = (req_offset < 0) ? cur_ie_offset : req_offset;
+
+	rtw_mi_status_no_self(adapter, &mstate);
+	RTW_INFO(FUNC_ADPT_FMT" ld_sta_num:%u, lg_sta_num%u, ap_num:%u\n"
+		, FUNC_ADPT_ARG(adapter), MSTATE_STA_LD_NUM(&mstate), MSTATE_STA_LG_NUM(&mstate), MSTATE_AP_NUM(&mstate));
+
+	if (MSTATE_STA_LD_NUM(&mstate) || MSTATE_AP_NUM(&mstate)) {
+		/* has linked STA or AP mode, follow */
+
+		rtw_warn_on(!rtw_mi_get_ch_setting_union_no_self(adapter, &u_ch, &u_bw, &u_offset));
+
+		RTW_INFO(FUNC_ADPT_FMT" union no self: %u,%u,%u\n", FUNC_ADPT_ARG(adapter), u_ch, u_bw, u_offset);
+		RTW_INFO(FUNC_ADPT_FMT" req: %d,%d,%d\n", FUNC_ADPT_ARG(adapter), req_ch, req_bw, req_offset);
+
+		rtw_adjust_chbw(adapter, u_ch, &dec_bw, &dec_offset);
+#ifdef CONFIG_MCC_MODE
+		if (MCC_EN(adapter)) {
+			if (!rtw_is_chbw_grouped(u_ch, u_bw, u_offset, dec_ch, dec_bw, dec_offset)) {
+				mlmeext->cur_channel = *ch = dec_ch;
+				mlmeext->cur_bwmode = *bw = dec_bw;
+				mlmeext->cur_ch_offset = *offset = dec_offset;
+				/* channel bw offset can not be allowed, need MCC */
+				*chbw_allow = false;
+				RTW_INFO(FUNC_ADPT_FMT" enable mcc: %u,%u,%u\n", FUNC_ADPT_ARG(adapter)
+					 , *ch, *bw, *offset);
+				goto exit;
+			} else
+				/* channel bw offset can be allowed, not need MCC */
+				*chbw_allow = true;
+		}
+#endif /* CONFIG_MCC_MODE */
+		rtw_sync_chbw(&dec_ch, &dec_bw, &dec_offset
+			      , &u_ch, &u_bw, &u_offset);
+
+		rtw_ap_update_bss_chbw(adapter, &(adapter->mlmepriv.cur_network.network)
+				       , dec_ch, dec_bw, dec_offset);
+
+		set_u_ch = true;
+	} else if (MSTATE_STA_LG_NUM(&mstate)) {
+		/* has linking STA */
+
+		rtw_warn_on(!rtw_mi_get_ch_setting_union_no_self(adapter, &u_ch, &u_bw, &u_offset));
+
+		RTW_INFO(FUNC_ADPT_FMT" union no self: %u,%u,%u\n", FUNC_ADPT_ARG(adapter), u_ch, u_bw, u_offset);
+		RTW_INFO(FUNC_ADPT_FMT" req: %d,%d,%d\n", FUNC_ADPT_ARG(adapter), req_ch, req_bw, req_offset);
+
+		rtw_adjust_chbw(adapter, dec_ch, &dec_bw, &dec_offset);
+
+		if (rtw_is_chbw_grouped(u_ch, u_bw, u_offset, dec_ch, dec_bw, dec_offset)) {
+
+			rtw_sync_chbw(&dec_ch, &dec_bw, &dec_offset
+				      , &u_ch, &u_bw, &u_offset);
+
+			rtw_ap_update_bss_chbw(adapter, &(adapter->mlmepriv.cur_network.network)
+					       , dec_ch, dec_bw, dec_offset);
+
+			set_u_ch = true;
+
+			/* channel bw offset can be allowed, not need MCC */
+			*chbw_allow = true;
+		} else {
+#ifdef CONFIG_MCC_MODE
+			if (MCC_EN(adapter)) {
+				mlmeext->cur_channel = *ch = dec_ch;
+				mlmeext->cur_bwmode = *bw = dec_bw;
+				mlmeext->cur_ch_offset = *offset = dec_offset;
+
+				/* channel bw offset can not be allowed, need MCC */
+				*chbw_allow = false;
+				RTW_INFO(FUNC_ADPT_FMT" enable mcc: %u,%u,%u\n", FUNC_ADPT_ARG(adapter)
+					 , *ch, *bw, *offset);
+				goto exit;
+			}
+#endif /* CONFIG_MCC_MODE */
+			/* set this for possible ch change when join down*/
+			set_fwstate(&adapter->mlmepriv, WIFI_OP_CH_SWITCHING);
+		}
+	} else {
+		/* single AP mode */
+
+		RTW_INFO(FUNC_ADPT_FMT" req: %d,%d,%d\n", FUNC_ADPT_ARG(adapter), req_ch, req_bw, req_offset);
+
+		/* check temporary decision first */
+		rtw_adjust_chbw(adapter, dec_ch, &dec_bw, &dec_offset);
+		if (!rtw_get_offset_by_chbw(dec_ch, dec_bw, &dec_offset)) {
+			if (req_ch == -1 || req_bw == -1)
+				goto choose_chbw;
+			RTW_WARN(FUNC_ADPT_FMT" req: %u,%u has no valid offset\n", FUNC_ADPT_ARG(adapter), dec_ch, dec_bw);
+			*chbw_allow = false;
+			goto exit;
+		}
+
+		if (!rtw_chset_is_chbw_valid(mlmeext->channel_set, dec_ch, dec_bw, dec_offset)) {
+			if (req_ch == -1 || req_bw == -1)
+				goto choose_chbw;
+			RTW_WARN(FUNC_ADPT_FMT" req: %u,%u,%u doesn't fit in chplan\n", FUNC_ADPT_ARG(adapter), dec_ch, dec_bw, dec_offset);
+			*chbw_allow = false;
+			goto exit;
+		}
+
+		if (rtw_odm_dfs_domain_unknown(adapter) && rtw_is_dfs_ch(dec_ch, dec_bw, dec_offset)) {
+			if (req_ch >= 0)
+				RTW_WARN(FUNC_ADPT_FMT" DFS channel %u,%u,%u can't be used\n", FUNC_ADPT_ARG(adapter), dec_ch, dec_bw, dec_offset);
+			if (req_ch > 0) {
+				/* specific channel and not from IE => don't change channel setting */
+				*chbw_allow = false;
+				goto exit;
+			}
+			goto choose_chbw;
+		}
+
+		if (rtw_chset_is_ch_non_ocp(mlmeext->channel_set, dec_ch, dec_bw, dec_offset) == false)
+			goto update_bss_chbw;
+
+choose_chbw:
+		if (req_bw < 0)
+			req_bw = cur_ie_bw;
+
+#if defined(CONFIG_DFS_MASTER)
+		if (!rtw_odm_dfs_domain_unknown(adapter)) {
+			/* choose 5G DFS channel for debug */
+			if (adapter_to_rfctl(adapter)->dbg_dfs_master_choose_dfs_ch_first
+				&& rtw_choose_shortest_waiting_ch(adapter, req_bw, &dec_ch, &dec_bw, &dec_offset, RTW_CHF_2G | RTW_CHF_NON_DFS) )
+				RTW_INFO(FUNC_ADPT_FMT" choose 5G DFS channel for debug\n", FUNC_ADPT_ARG(adapter));
+			else if (adapter_to_rfctl(adapter)->dfs_ch_sel_d_flags
+				&& rtw_choose_shortest_waiting_ch(adapter, req_bw, &dec_ch, &dec_bw, &dec_offset, adapter_to_rfctl(adapter)->dfs_ch_sel_d_flags) )
+				RTW_INFO(FUNC_ADPT_FMT" choose with dfs_ch_sel_d_flags:0x%02x for debug\n", FUNC_ADPT_ARG(adapter), adapter_to_rfctl(adapter)->dfs_ch_sel_d_flags);
+			else if (rtw_choose_shortest_waiting_ch(adapter, req_bw, &dec_ch, &dec_bw, &dec_offset, 0) == false) {
+				RTW_WARN(FUNC_ADPT_FMT" no available channel\n", FUNC_ADPT_ARG(adapter));
+				*chbw_allow = false;
+				goto exit;
+			}
+		} else
+#endif /* defined(CONFIG_DFS_MASTER) */
+		if (rtw_choose_shortest_waiting_ch(adapter, req_bw, &dec_ch, &dec_bw, &dec_offset, RTW_CHF_DFS) == false) {
+			RTW_WARN(FUNC_ADPT_FMT" no available channel\n", FUNC_ADPT_ARG(adapter));
+			*chbw_allow = false;
+			goto exit;
+		}
+
+update_bss_chbw:
+		rtw_ap_update_bss_chbw(adapter, &(adapter->mlmepriv.cur_network.network)
+				       , dec_ch, dec_bw, dec_offset);
+
+		/* channel bw offset can be allowed for single AP, not need MCC */
+		*chbw_allow = true;
+		set_dec_ch = true;
+	}
+
+	if (rtw_mi_check_fwstate(adapter, _FW_UNDER_SURVEY)) {
+		/* scanning, leave ch setting to scan state machine */
+		set_u_ch = set_dec_ch = false;
+	}
+
+	if (mlmeext->cur_channel != dec_ch
+	    || mlmeext->cur_bwmode != dec_bw
+	    || mlmeext->cur_ch_offset != dec_offset)
+		changed = true;
+
+	if (changed  && rtw_linked_check(adapter) == true) {
+#ifdef CONFIG_SPCT_CH_SWITCH
+		if (1)
+			rtw_ap_inform_ch_switch(adapter, dec_ch, dec_offset);
+		else
+#endif
+			rtw_sta_flush(adapter, false);
+	}
+
+	mlmeext->cur_channel = dec_ch;
+	mlmeext->cur_bwmode = dec_bw;
+	mlmeext->cur_ch_offset = dec_offset;
+
+	if (u_ch != 0)
+		RTW_INFO(FUNC_ADPT_FMT" union: %u,%u,%u\n", FUNC_ADPT_ARG(adapter), u_ch, u_bw, u_offset);
+
+	RTW_INFO(FUNC_ADPT_FMT" dec: %u,%u,%u\n", FUNC_ADPT_ARG(adapter), dec_ch, dec_bw, dec_offset);
+
+	if (set_u_ch ) {
+		*ch = u_ch;
+		*bw = u_bw;
+		*offset = u_offset;
+	} else if (set_dec_ch ) {
+		*ch = dec_ch;
+		*bw = dec_bw;
+		*offset = dec_offset;
+	}
+exit:
+	return changed;
+}
+
+/*#define DBG_SWTIMER_BASED_TXBCN*/
+
+#ifdef CONFIG_SWTIMER_BASED_TXBCN
+void tx_beacon_handler(struct dvobj_priv *pdvobj)
+{
+#define BEACON_EARLY_TIME		20	/* unit:TU*/
+	unsigned long irqL;
+	_list	*plist, *phead;
+	u32 timestamp[2];
+	u32 bcn_interval_us; /* unit : usec */
+	u64 time;
+	u32 cur_tick, time_offset; /* unit : usec */
+	u32 inter_bcn_space_us; /* unit : usec */
+	int nr_vap, idx, bcn_idx;
+	int i;
+	u8 val8, late = 0;
+	_adapter *padapter = NULL;
+
+	i = 0;
+
+	/* get first ap mode interface */
+	_enter_critical_bh(&pdvobj->ap_if_q.lock, &irqL);
+	if (list_empty(&pdvobj->ap_if_q.queue) || (pdvobj->nr_ap_if == 0)) {
+		RTW_INFO("[%s] ERROR: ap_if_q is empty!or nr_ap = %d\n", __func__, pdvobj->nr_ap_if);
+		_exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL);
+		return;
+	} else
+		padapter = LIST_CONTAINOR(get_next(&(pdvobj->ap_if_q.queue)), struct _ADAPTER, list);
+	_exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL);
+
+	if (NULL == padapter) {
+		RTW_INFO("[%s] ERROR: no any ap interface!\n", __func__);
+		return;
+	}
+
+
+	bcn_interval_us = DEFAULT_BCN_INTERVAL * NET80211_TU_TO_US;
+	if (0 == bcn_interval_us) {
+		RTW_INFO("[%s] ERROR: beacon interval = 0\n", __func__);
+		return;
+	}
+
+	/* read TSF */
+	timestamp[1] = rtw_read32(padapter, 0x560 + 4);
+	timestamp[0] = rtw_read32(padapter, 0x560);
+	while (timestamp[1]) {
+		time = (0xFFFFFFFF % bcn_interval_us + 1) * timestamp[1] + timestamp[0];
+		timestamp[0] = (u32)time;
+		timestamp[1] = (u32)(time >> 32);
+	}
+	cur_tick = timestamp[0] % bcn_interval_us;
+
+
+	_enter_critical_bh(&pdvobj->ap_if_q.lock, &irqL);
+
+	nr_vap = (pdvobj->nr_ap_if - 1);
+	if (nr_vap > 0) {
+		inter_bcn_space_us = pdvobj->inter_bcn_space * NET80211_TU_TO_US; /* beacon_interval / (nr_vap+1); */
+		idx = cur_tick / inter_bcn_space_us;
+		if (idx < nr_vap)	/* if (idx < (nr_vap+1))*/
+			bcn_idx = idx + 1;	/* bcn_idx = (idx + 1) % (nr_vap+1);*/
+		else
+			bcn_idx = 0;
+
+		/* to get padapter based on bcn_idx */
+		padapter = NULL;
+		phead = get_list_head(&pdvobj->ap_if_q);
+		plist = get_next(phead);
+		while ((!rtw_end_of_queue_search(phead, plist))) {
+			padapter = LIST_CONTAINOR(plist, struct _ADAPTER, list);
+
+			plist = get_next(plist);
+
+			if (i == bcn_idx)
+				break;
+
+			i++;
+		}
+		if ((NULL == padapter) || (i > pdvobj->nr_ap_if)) {
+			RTW_INFO("[%s] ERROR: nr_ap_if = %d, padapter=%p, bcn_idx=%d, index=%d\n",
+				__func__, pdvobj->nr_ap_if, padapter, bcn_idx, i);
+			_exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL);
+			return;
+		}
+#ifdef DBG_SWTIMER_BASED_TXBCN
+		RTW_INFO("BCN_IDX=%d, cur_tick=%d, padapter=%p\n", bcn_idx, cur_tick, padapter);
+#endif
+		if (((idx + 2 == nr_vap + 1) && (idx < nr_vap + 1)) || (0 == bcn_idx)) {
+			time_offset = bcn_interval_us - cur_tick - BEACON_EARLY_TIME * NET80211_TU_TO_US;
+			if ((s32)time_offset < 0)
+				time_offset += inter_bcn_space_us;
+
+		} else {
+			time_offset = (idx + 2) * inter_bcn_space_us - cur_tick - BEACON_EARLY_TIME * NET80211_TU_TO_US;
+			if (time_offset > (inter_bcn_space_us + (inter_bcn_space_us >> 1))) {
+				time_offset -= inter_bcn_space_us;
+				late = 1;
+			}
+		}
+	} else
+		/*#endif*/ { /* MBSSID */
+		time_offset = 2 * bcn_interval_us - cur_tick - BEACON_EARLY_TIME * NET80211_TU_TO_US;
+		if (time_offset > (bcn_interval_us + (bcn_interval_us >> 1))) {
+			time_offset -= bcn_interval_us;
+			late = 1;
+		}
+	}
+	_exit_critical_bh(&pdvobj->ap_if_q.lock, &irqL);
+
+#ifdef DBG_SWTIMER_BASED_TXBCN
+	RTW_INFO("set sw bcn timer %d us\n", time_offset);
+#endif
+	_set_timer(&pdvobj->txbcn_timer, time_offset / NET80211_TU_TO_US);
+
+	if (padapter) {
+#ifdef DBG_SWTIMER_BASED_TXBCN
+		RTW_INFO("padapter=%p, PORT=%d\n", padapter, padapter->hw_port);
+#endif
+		/*update_beacon(padapter, _TIM_IE_, NULL, false);*/
+		issue_beacon(padapter, 0);
+	}
+}
+
+void tx_beacon_timer_handler(struct dvobj_priv *pdvobj)
+{
+	_adapter *padapter = pdvobj->padapters[0];
+
+	if (padapter)
+		set_tx_beacon_cmd(padapter);
+}
+#endif
+
+#endif /* CONFIG_AP_MODE */
diff --git a/drivers/staging/rtl8188eu/core/rtw_beamforming.c b/drivers/staging/rtl8188eu/core/rtw_beamforming.c
new file mode 100644
index 000000000000..2ec9ba408fc4
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_beamforming.c
@@ -0,0 +1,3071 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_BEAMFORMING_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#ifdef CONFIG_BEAMFORMING
+
+#ifdef RTW_BEAMFORMING_VERSION_2
+
+struct ndpa_sta_info {
+	u16 aid:12;
+	u16 feedback_type:1;
+	u16 nc_index:3;
+};
+
+static void _get_txvector_parameter(PADAPTER adapter, struct sta_info *sta, u8 *g_id, u16 *p_aid)
+{
+	struct mlme_priv *mlme;
+	u16 aid;
+	u8 *bssid;
+	u16 val16;
+	u8 i;
+
+
+	mlme = &adapter->mlmepriv;
+
+	if (check_fwstate(mlme, WIFI_AP_STATE)) {
+		/*
+		 * Sent by an AP and addressed to a STA associated with that AP
+		 * or sent by a DLS or TDLS STA in a direct path to
+		 * a DLS or TDLS peer STA
+		 */
+
+		aid = sta->aid;
+		bssid = adapter_mac_addr(adapter);
+		RTW_INFO("%s: AID=0x%x BSSID=" MAC_FMT "\n",
+			 __func__, sta->aid, MAC_ARG(bssid));
+
+		/* AID[0:8] */
+		aid &= 0x1FF;
+		/* BSSID[44:47] xor BSSID[40:43] */
+		val16 = ((bssid[5] & 0xF0) >> 4) ^ (bssid[5] & 0xF);
+		/* (dec(AID[0:8]) + dec(BSSID)*2^5) mod 2^9 */
+		*p_aid = (aid + (val16 << 5)) & 0x1FF;
+		*g_id = 63;
+	} else if ((check_fwstate(mlme, WIFI_ADHOC_STATE) == true)
+		   || (check_fwstate(mlme, WIFI_ADHOC_MASTER_STATE) == true)) {
+		/*
+		 * Otherwise, includes
+		 * 1. Sent to an IBSS STA
+		 * 2. Sent by an AP to a non associated STA
+		 * 3. Sent to a STA for which it is not known
+		 *    which condition is applicable
+		 */
+		*p_aid = 0;
+		*g_id = 63;
+	} else {
+		/* Addressed to AP */
+		bssid = sta->hwaddr;
+		RTW_INFO("%s: BSSID=" MAC_FMT "\n", __func__, MAC_ARG(bssid));
+
+		/* BSSID[39:47] */
+		*p_aid = (bssid[5] << 1) | (bssid[4] >> 7);
+		*g_id = 0;
+	}
+
+	RTW_INFO("%s: GROUP_ID=0x%02x PARTIAL_AID=0x%04x\n",
+		 __func__, *g_id, *p_aid);
+}
+
+/*
+ * Parameters
+ *	adapter		struct _adapter*
+ *	sta		struct sta_info*
+ *	sta_bf_cap	beamforming capabe of sta
+ *	sounding_dim	Number of Sounding Dimensions
+ *	comp_steering	Compressed Steering Number of Beamformer Antennas Supported
+ */
+static void _get_sta_beamform_cap(PADAPTER adapter, struct sta_info *sta,
+	u8 *sta_bf_cap, u8 *sounding_dim, u8 *comp_steering)
+{
+	struct beamforming_info *info;
+	struct ht_priv *ht;
+	u16 bf_cap;
+
+
+	*sta_bf_cap = 0;
+	*sounding_dim = 0;
+	*comp_steering = 0;
+
+	info = GET_BEAMFORM_INFO(adapter);
+	ht = &adapter->mlmepriv.htpriv;
+
+	if (is_supported_ht(sta->wireless_mode) == true) {
+		/* HT */
+		bf_cap = ht->beamform_cap;
+
+		if (TEST_FLAG(bf_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE)) {
+			info->beamforming_cap |= BEAMFORMEE_CAP_HT_EXPLICIT;
+			*sta_bf_cap |= BEAMFORMER_CAP_HT_EXPLICIT;
+			*sounding_dim = (bf_cap & BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP) >> 6;
+		}
+		if (TEST_FLAG(bf_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE)) {
+			info->beamforming_cap |= BEAMFORMER_CAP_HT_EXPLICIT;
+			*sta_bf_cap |= BEAMFORMEE_CAP_HT_EXPLICIT;
+			*comp_steering = (bf_cap & BEAMFORMING_HT_BEAMFORMER_STEER_NUM) >> 4;
+		}
+	}
+}
+
+static u8 _send_ht_ndpa_packet(PADAPTER adapter, u8 *ra, CHANNEL_WIDTH bw)
+{
+	/* General */
+	struct xmit_priv		*pxmitpriv;
+	struct mlme_ext_priv		*pmlmeext;
+	struct mlme_ext_info		*pmlmeinfo;
+	struct xmit_frame		*pmgntframe;
+	/* Beamforming */
+	struct beamforming_info		*info;
+	struct beamformee_entry		*bfee;
+	struct ndpa_sta_info		sta_info;
+	u8 ActionHdr[4] = {ACT_CAT_VENDOR, 0x00, 0xE0, 0x4C};
+	/* MISC */
+	struct pkt_attrib		*attrib;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	enum MGN_RATE txrate;
+	u8 *pframe;
+	u16 duration = 0;
+	u8 aSifsTime = 0;
+
+
+	RTW_INFO("+%s: Send to " MAC_FMT "\n", __func__, MAC_ARG(ra));
+
+	pxmitpriv = &adapter->xmitpriv;
+	pmlmeext = &adapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+	bfee = rtw_bf_bfee_get_entry_by_addr(adapter, ra);
+	if (!bfee) {
+		RTW_ERR("%s: Cann't find beamformee entry!\n", __func__);
+		return false;
+	}
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe) {
+		RTW_ERR("%s: alloc mgnt frame fail!\n", __func__);
+		return false;
+	}
+
+	txrate = beamforming_get_htndp_tx_rate(GET_PDM_ODM(adapter), bfee->comp_steering_num_of_bfer);
+
+	/* update attribute */
+	attrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(adapter, attrib);
+	/*attrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */
+	attrib->subtype = WIFI_ACTION_NOACK;
+	attrib->bwmode = bw;
+	/*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */
+	attrib->order = 1;
+	attrib->rate = (u8)txrate;
+	attrib->bf_pkt_type = 0;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	/* Frame control */
+	pwlanhdr->frame_ctl = 0;
+	set_frame_sub_type(pframe, attrib->subtype);
+	set_order_bit(pframe);
+
+	/* Duration */
+	if (pmlmeext->cur_wireless_mode == WIRELESS_11B)
+		aSifsTime = 10;
+	else
+		aSifsTime = 16;
+	duration = 2 * aSifsTime + 40;
+	if (bw == CHANNEL_WIDTH_40)
+		duration += 87;
+	else
+		duration += 180;
+	set_duration(pframe, duration);
+
+	/* DA */
+	memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
+	/* SA */
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN);
+	/* BSSID */
+	memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+	/* HT control field */
+	SET_HT_CTRL_CSI_STEERING(pframe + 24, 3);
+	SET_HT_CTRL_NDP_ANNOUNCEMENT(pframe + 24, 1);
+
+	/*
+	 * Frame Body
+	 * Category field: vender-specific value, 0x7F
+	 * OUI: 0x00E04C
+	 */
+	memcpy(pframe + 28, ActionHdr, 4);
+
+	attrib->pktlen = 32;
+	attrib->last_txcmdsz = attrib->pktlen;
+
+	dump_mgntframe(adapter, pmgntframe);
+
+	return true;
+}
+
+static u8 _send_vht_ndpa_packet(PADAPTER adapter, u8 *ra, u16 aid, CHANNEL_WIDTH bw)
+{
+	/* General */
+	struct xmit_priv		*pxmitpriv;
+	struct mlme_ext_priv		*pmlmeext;
+	struct xmit_frame		*pmgntframe;
+	/* Beamforming */
+	struct beamforming_info		*info;
+	struct beamformee_entry		*bfee;
+	struct ndpa_sta_info		sta_info;
+	/* MISC */
+	struct pkt_attrib		*attrib;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	u8 *pframe;
+	enum MGN_RATE txrate;
+	u16 duration = 0;
+	u8 sequence = 0, aSifsTime = 0;
+
+
+	RTW_INFO("+%s: Send to " MAC_FMT "\n", __func__, MAC_ARG(ra));
+
+	pxmitpriv = &adapter->xmitpriv;
+	pmlmeext = &adapter->mlmeextpriv;
+	info = GET_BEAMFORM_INFO(adapter);
+	bfee = rtw_bf_bfee_get_entry_by_addr(adapter, ra);
+	if (!bfee) {
+		RTW_ERR("%s: Cann't find beamformee entry!\n", __func__);
+		return false;
+	}
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe) {
+		RTW_ERR("%s: alloc mgnt frame fail!\n", __func__);
+		return false;
+	}
+
+	txrate = beamforming_get_vht_ndp_tx_rate(GET_PDM_ODM(adapter), bfee->comp_steering_num_of_bfer);
+
+	/* update attribute */
+	attrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(adapter, attrib);
+	/*pattrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */
+	attrib->subtype = WIFI_NDPA;
+	attrib->bwmode = bw;
+	/*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */
+	attrib->rate = (u8)txrate;
+	attrib->bf_pkt_type = 0;
+
+	memset(pmgntframe->buf_addr, 0, TXDESC_OFFSET + WLANHDR_OFFSET);
+	pframe = pmgntframe->buf_addr + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	/* Frame control */
+	pwlanhdr->frame_ctl = 0;
+	set_frame_sub_type(pframe, attrib->subtype);
+
+	/* Duration */
+	if (is_supported_5g(pmlmeext->cur_wireless_mode) || is_supported_ht(pmlmeext->cur_wireless_mode))
+		aSifsTime = 16;
+	else
+		aSifsTime = 10;
+	duration = 2 * aSifsTime + 44;
+	if (bw == CHANNEL_WIDTH_80)
+		duration += 40;
+	else if (bw == CHANNEL_WIDTH_40)
+		duration += 87;
+	else
+		duration += 180;
+	set_duration(pframe, duration);
+
+	/* RA */
+	memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
+
+	/* TA */
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN);
+
+	/* Sounding Sequence, bit0~1 is reserved */
+	sequence = info->sounding_sequence << 2;
+	if (info->sounding_sequence >= 0x3f)
+		info->sounding_sequence = 0;
+	else
+		info->sounding_sequence++;
+	memcpy(pframe + 16, &sequence, 1);
+
+	/* STA Info */
+	/*
+	 * "AID12" Equal to 0 if the STA is an AP, mesh STA or
+	 * STA that is a member of an IBSS
+	 */
+	if (check_fwstate(&adapter->mlmepriv, WIFI_AP_STATE) == false)
+		aid = 0;
+	sta_info.aid = aid;
+	/* "Feedback Type" set to 0 for SU */
+	sta_info.feedback_type = 0;
+	/* "Nc Index" reserved if the Feedback Type field indicates SU */
+	sta_info.nc_index = 0;
+	memcpy(pframe + 17, (u8 *)&sta_info, 2);
+
+	attrib->pktlen = 19;
+	attrib->last_txcmdsz = attrib->pktlen;
+
+	dump_mgntframe(adapter, pmgntframe);
+
+	return true;
+}
+
+static u8 _send_vht_mu_ndpa_packet(PADAPTER adapter, CHANNEL_WIDTH bw)
+{
+	/* General */
+	struct xmit_priv		*pxmitpriv;
+	struct mlme_ext_priv		*pmlmeext;
+	struct xmit_frame		*pmgntframe;
+	/* Beamforming */
+	struct beamforming_info		*info;
+	struct sounding_info		*sounding;
+	struct beamformee_entry		*bfee;
+	struct ndpa_sta_info		sta_info;
+	/* MISC */
+	struct pkt_attrib		*attrib;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	enum MGN_RATE txrate;
+	u8 *pframe;
+	u8 *ra = NULL;
+	u16 duration = 0;
+	u8 sequence = 0, aSifsTime = 0;
+	u8 i;
+
+
+	RTW_INFO("+%s\n", __func__);
+
+	pxmitpriv = &adapter->xmitpriv;
+	pmlmeext = &adapter->mlmeextpriv;
+	info = GET_BEAMFORM_INFO(adapter);
+	sounding = &info->sounding_info;
+
+	txrate = MGN_VHT2SS_MCS0;
+
+	/*
+	 * Fill the first MU BFee entry (STA1) MAC addr to destination address then
+	 * HW will change A1 to broadcast addr.
+	 * 2015.05.28. Suggested by SD1 Chunchu.
+	 */
+	bfee = &info->bfee_entry[sounding->mu_sounding_list[0]];
+	ra = bfee->mac_addr;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe) {
+		RTW_ERR("%s: alloc mgnt frame fail!\n", __func__);
+		return false;
+	}
+
+	/* update attribute */
+	attrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(adapter, attrib);
+	/*attrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */
+	attrib->subtype = WIFI_NDPA;
+	attrib->bwmode = bw;
+	/*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */
+	attrib->rate = (u8)txrate;
+	/* Set TxBFPktType of Tx desc to unicast type if there is only one MU STA for HW design */
+	if (info->sounding_info.candidate_mu_bfee_cnt > 1)
+		attrib->bf_pkt_type = 1;
+	else
+		attrib->bf_pkt_type = 0;
+
+	memset(pmgntframe->buf_addr, 0, TXDESC_OFFSET + WLANHDR_OFFSET);
+	pframe = pmgntframe->buf_addr + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	/* Frame control */
+	pwlanhdr->frame_ctl = 0;
+	set_frame_sub_type(pframe, attrib->subtype);
+
+	/* Duration */
+	if (is_supported_5g(pmlmeext->cur_wireless_mode) || is_supported_ht(pmlmeext->cur_wireless_mode))
+		aSifsTime = 16;
+	else
+		aSifsTime = 10;
+	duration = 2 * aSifsTime + 44;
+	if (bw == CHANNEL_WIDTH_80)
+		duration += 40;
+	else if (bw == CHANNEL_WIDTH_40)
+		duration += 87;
+	else
+		duration += 180;
+	set_duration(pframe, duration);
+
+	/* RA */
+	memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
+
+	/* TA */
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN);
+
+	/* Sounding Sequence, bit0~1 is reserved */
+	sequence = info->sounding_sequence << 2;
+	if (info->sounding_sequence >= 0x3f)
+		info->sounding_sequence = 0;
+	else
+		info->sounding_sequence++;
+	memcpy(pframe + 16, &sequence, 1);
+
+	attrib->pktlen = 17;
+
+	/*
+	 * Construct STA info. for multiple STAs
+	 * STA Info1, ..., STA Info n
+	 */
+	for (i = 0; i < sounding->candidate_mu_bfee_cnt; i++) {
+		bfee = &info->bfee_entry[sounding->mu_sounding_list[i]];
+		sta_info.aid = bfee->aid;
+		sta_info.feedback_type = 1; /* 1'b1: MU */
+		sta_info.nc_index = 0;
+		memcpy(pframe + attrib->pktlen, (u8 *)&sta_info, 2);
+		attrib->pktlen += 2;
+	}
+
+	attrib->last_txcmdsz = attrib->pktlen;
+
+	dump_mgntframe(adapter, pmgntframe);
+
+	return true;
+}
+
+static u8 _send_bf_report_poll(PADAPTER adapter, u8 *ra, u8 bFinalPoll)
+{
+	/* General */
+	struct xmit_priv *pxmitpriv;
+	struct xmit_frame *pmgntframe;
+	/* MISC */
+	struct pkt_attrib *attrib;
+	struct rtw_ieee80211_hdr *pwlanhdr;
+	u8 *pframe;
+
+
+	RTW_INFO("+%s: Send to " MAC_FMT "\n", __func__, MAC_ARG(ra));
+
+	pxmitpriv = &adapter->xmitpriv;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (!pmgntframe) {
+		RTW_ERR("%s: alloc mgnt frame fail!\n", __func__);
+		return false;
+	}
+
+	/* update attribute */
+	attrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(adapter, attrib);
+	/*attrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */
+	attrib->subtype = WIFI_BF_REPORT_POLL;
+	attrib->bwmode = CHANNEL_WIDTH_20;
+	/*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */
+	attrib->rate = MGN_6M;
+	if (bFinalPoll)
+		attrib->bf_pkt_type = 3;
+	else
+		attrib->bf_pkt_type = 2;
+
+	memset(pmgntframe->buf_addr, 0, TXDESC_OFFSET + WLANHDR_OFFSET);
+	pframe = pmgntframe->buf_addr + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	/* Frame control */
+	pwlanhdr->frame_ctl = 0;
+	set_frame_sub_type(pframe, attrib->subtype);
+
+	/* Duration */
+	set_duration(pframe, 100);
+
+	/* RA */
+	memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
+
+	/* TA */
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN);
+
+	/* Feedback Segment Retransmission Bitmap */
+	pframe[16] = 0xFF;
+
+	attrib->pktlen = 17;
+	attrib->last_txcmdsz = attrib->pktlen;
+
+	dump_mgntframe(adapter, pmgntframe);
+
+	return true;
+}
+
+static void _sounding_update_min_period(PADAPTER adapter, u16 period, u8 leave)
+{
+	struct beamforming_info *info;
+	struct beamformee_entry *bfee;
+	u8 i = 0;
+	u16 min_val = 0xFFFF;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+
+	if (leave) {
+		/*
+		 * When a BFee left,
+		 * we need to find the latest min sounding period
+		 * from the remaining BFees
+		 */
+		for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
+			bfee = &info->bfee_entry[i];
+			if ((bfee->used == true)
+			    && (bfee->sound_period < min_val))
+				min_val = bfee->sound_period;
+		}
+
+		if (min_val == 0xFFFF)
+			info->sounding_info.min_sounding_period = 0;
+		else
+			info->sounding_info.min_sounding_period = min_val;
+	} else {
+		if ((info->sounding_info.min_sounding_period == 0)
+		    || (period < info->sounding_info.min_sounding_period))
+			info->sounding_info.min_sounding_period = period;
+	}
+}
+
+static void _sounding_init(struct sounding_info *sounding)
+{
+	memset(sounding->su_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_SU);
+	memset(sounding->mu_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_MU);
+	sounding->state = SOUNDING_STATE_NONE;
+	sounding->su_bfee_curidx = 0xFF;
+	sounding->candidate_mu_bfee_cnt = 0;
+	sounding->min_sounding_period = 0;
+	sounding->sound_remain_cnt_per_period = 0;
+}
+
+static void _sounding_reset_vars(PADAPTER adapter)
+{
+	struct beamforming_info	*info;
+	struct sounding_info *sounding;
+	u8 idx;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+	sounding = &info->sounding_info;
+
+	memset(sounding->su_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_SU);
+	memset(sounding->mu_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_MU);
+	sounding->su_bfee_curidx = 0xFF;
+	sounding->candidate_mu_bfee_cnt = 0;
+
+	/* Clear bSound flag for the new period */
+	for (idx = 0; idx < MAX_BEAMFORMEE_ENTRY_NUM; idx++) {
+		if ((info->bfee_entry[idx].used == true)
+		    && (info->bfee_entry[idx].sounding == true)) {
+			info->bfee_entry[idx].sounding = false;
+			info->bfee_entry[idx].bCandidateSoundingPeer = false;
+		}
+	}
+}
+
+/*
+ * Return
+ *	0	Prepare sounding list OK
+ *	-1	Fail to prepare sounding list, because no beamformee need to souding
+ *	-2	Fail to prepare sounding list, because beamformee state not ready
+ *
+ */
+static int _sounding_get_list(PADAPTER adapter)
+{
+	struct beamforming_info	*info;
+	struct sounding_info *sounding;
+	struct beamformee_entry *bfee;
+	u8 i, mu_idx = 0, su_idx = 0, not_ready = 0;
+	int ret = 0;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+	sounding = &info->sounding_info;
+
+	/* Add MU BFee list first because MU priority is higher than SU */
+	for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
+		bfee = &info->bfee_entry[i];
+		if (bfee->used == false)
+			continue;
+
+		if (bfee->state != BEAMFORM_ENTRY_HW_STATE_ADDED) {
+			RTW_ERR("%s: Invalid BFee idx(%d) Hw state=%d\n", __func__, i, bfee->state);
+			not_ready++;
+			continue;
+		}
+
+		/*
+		 * Decrease BFee's SoundCnt per period
+		 * If the remain count is 0,
+		 * then it can be sounded at this time
+		 */
+		if (bfee->SoundCnt) {
+			bfee->SoundCnt--;
+			if (bfee->SoundCnt)
+				continue;
+		}
+
+		/*
+		 * <tynli_Note>
+		 *	If the STA supports MU BFee capability then we add it to MUSoundingList directly
+		 *	because we can only sound one STA by unicast NDPA with MU cap enabled to get correct channel info.
+		 *	Suggested by BB team Luke Lee. 2015.11.25.
+		 */
+		if (bfee->cap & BEAMFORMEE_CAP_VHT_MU) {
+			/* MU BFee */
+			if (mu_idx >= MAX_NUM_BEAMFORMEE_MU) {
+				RTW_ERR("%s: Too much MU bfee entry(Limit:%d)\n", __func__, MAX_NUM_BEAMFORMEE_MU);
+				continue;
+			}
+
+			if (bfee->bApplySounding == true) {
+				bfee->bCandidateSoundingPeer = true;
+				bfee->SoundCnt = GetInitSoundCnt(bfee->sound_period, sounding->min_sounding_period);
+				sounding->mu_sounding_list[mu_idx] = i;
+				mu_idx++;
+			}
+		} else if (bfee->cap & (BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT)) {
+			/* SU BFee (HT/VHT) */
+			if (su_idx >= MAX_NUM_BEAMFORMEE_SU) {
+				RTW_ERR("%s: Too much SU bfee entry(Limit:%d)\n", __func__, MAX_NUM_BEAMFORMEE_SU);
+				continue;
+			}
+
+			if (bfee->bDeleteSounding == true) {
+				sounding->su_sounding_list[su_idx] = i;
+				su_idx++;
+			} else if ((bfee->bApplySounding == true)
+			    && (bfee->bSuspendSUCap == false)) {
+				bfee->bCandidateSoundingPeer = true;
+				bfee->SoundCnt = GetInitSoundCnt(bfee->sound_period, sounding->min_sounding_period);
+				sounding->su_sounding_list[su_idx] = i;
+				su_idx++;
+			}
+		}
+	}
+
+	sounding->candidate_mu_bfee_cnt = mu_idx;
+
+	if (su_idx + mu_idx == 0) {
+		ret = -1;
+		if (not_ready)
+			ret = -2;
+	}
+
+	RTW_INFO("-%s: There are %d SU and %d MU BFees in this sounding period\n", __func__, su_idx, mu_idx);
+
+	return ret;
+}
+
+static void _sounding_handler(PADAPTER adapter)
+{
+	struct beamforming_info	*info;
+	struct sounding_info *sounding;
+	struct beamformee_entry *bfee;
+	u8 su_idx, i;
+	u32 timeout_period = 0;
+	u8 set_timer = false;
+	int ret = 0;
+	static u16 wait_cnt = 0;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+	sounding = &info->sounding_info;
+
+	RTW_DBG("+%s: state=%d\n", __func__, sounding->state);
+	if ((sounding->state != SOUNDING_STATE_INIT)
+	    && (sounding->state != SOUNDING_STATE_SU_SOUNDDOWN)
+	    && (sounding->state != SOUNDING_STATE_MU_SOUNDDOWN)
+	    && (sounding->state != SOUNDING_STATE_SOUNDING_TIMEOUT)) {
+		RTW_WARN("%s: Invalid State(%d) and return!\n", __func__, sounding->state);
+		return;
+	}
+
+	if (sounding->state == SOUNDING_STATE_INIT) {
+		RTW_INFO("%s: Sounding start\n", __func__);
+
+		/* Init Var */
+		_sounding_reset_vars(adapter);
+
+		/* Get the sounding list of this sounding period */
+		ret = _sounding_get_list(adapter);
+		if (ret == -1) {
+			wait_cnt = 0;
+			sounding->state = SOUNDING_STATE_NONE;
+			RTW_ERR("%s: No BFees found, set to SOUNDING_STATE_NONE\n", __func__);
+			info->sounding_running--;
+			return;
+		}
+		if (ret == -2) {
+			RTW_WARN("%s: Temporarily cann't find BFee to sounding\n", __func__);
+			if (wait_cnt < 5) {
+				wait_cnt++;
+			} else {
+				wait_cnt = 0;
+				sounding->state = SOUNDING_STATE_NONE;
+				RTW_ERR("%s: Wait changing state timeout!! Set to SOUNDING_STATE_NONE\n", __func__);
+			}
+			info->sounding_running--;
+			return;
+		}
+		if (ret != 0) {
+			wait_cnt = 0;
+			RTW_ERR("%s: Unkown state(%d)!\n", __func__, ret);
+			info->sounding_running--;
+			return;
+
+		}
+
+		wait_cnt = 0;
+
+		if (check_fwstate(&adapter->mlmepriv, WIFI_SITE_MONITOR) == true) {
+			RTW_INFO("%s: Sounding abort! scanning APs...\n", __func__);
+			info->sounding_running--;
+			return;
+		}
+
+		rtw_ps_deny(adapter, PS_DENY_BEAMFORMING);
+		LeaveAllPowerSaveModeDirect(adapter);
+	}
+
+	/* Get non-sound SU BFee index */
+	for (i = 0; i < MAX_NUM_BEAMFORMEE_SU; i++) {
+		su_idx = sounding->su_sounding_list[i];
+		if (su_idx >= MAX_BEAMFORMEE_ENTRY_NUM)
+			continue;
+		bfee = &info->bfee_entry[su_idx];
+		if (false == bfee->sounding)
+			break;
+	}
+	if (i < MAX_NUM_BEAMFORMEE_SU) {
+		sounding->su_bfee_curidx = su_idx;
+		/* Set to sounding start state */
+		sounding->state = SOUNDING_STATE_SU_START;
+		RTW_DBG("%s: Set to SOUNDING_STATE_SU_START\n", __func__);
+
+		bfee->sounding = true;
+		/* Reset sounding timeout flag for the new sounding */
+		bfee->bSoundingTimeout = false;
+
+		if (bfee->bDeleteSounding) {
+			u8 res = false;
+			rtw_bf_cmd(adapter, BEAMFORMING_CTRL_END_PERIOD, &res, 1, 0);
+			return;
+		}
+
+		/* Start SU sounding */
+		if (bfee->cap & BEAMFORMEE_CAP_VHT_SU)
+			_send_vht_ndpa_packet(adapter, bfee->mac_addr, bfee->aid, bfee->sound_bw);
+		else if (bfee->cap & BEAMFORMEE_CAP_HT_EXPLICIT)
+			_send_ht_ndpa_packet(adapter, bfee->mac_addr, bfee->sound_bw);
+
+		/* Set sounding timeout timer */
+		_set_timer(&info->sounding_timeout_timer, SU_SOUNDING_TIMEOUT);
+		return;
+	}
+
+	if (sounding->candidate_mu_bfee_cnt > 0) {
+		/*
+		 * If there is no SU BFee then find MU BFee and perform MU sounding
+		 *
+		 * <tynli_note> Need to check the MU starting condition. 2015.12.15.
+		 */
+		sounding->state = SOUNDING_STATE_MU_START;
+		RTW_DBG("%s: Set to SOUNDING_STATE_MU_START\n", __func__);
+
+		/* Update MU BFee info */
+		for (i = 0; i < sounding->candidate_mu_bfee_cnt; i++) {
+			bfee = &info->bfee_entry[sounding->mu_sounding_list[i]];
+			bfee->sounding = true;
+		}
+
+		/* Send MU NDPA */
+		bfee = &info->bfee_entry[sounding->mu_sounding_list[0]];
+		_send_vht_mu_ndpa_packet(adapter, bfee->sound_bw);
+
+		/* Send BF report poll if more than 1 MU STA */
+		for (i = 1; i < sounding->candidate_mu_bfee_cnt; i++) {
+			bfee = &info->bfee_entry[sounding->mu_sounding_list[i]];
+
+			if (i == (sounding->candidate_mu_bfee_cnt - 1))/* The last STA*/
+				_send_bf_report_poll(adapter, bfee->mac_addr, true);
+			else
+				_send_bf_report_poll(adapter, bfee->mac_addr, false);
+		}
+
+		sounding->candidate_mu_bfee_cnt = 0;
+
+		/* Set sounding timeout timer */
+		_set_timer(&info->sounding_timeout_timer, MU_SOUNDING_TIMEOUT);
+		return;
+	}
+
+	info->sounding_running--;
+	sounding->state = SOUNDING_STATE_INIT;
+	RTW_INFO("%s: Sounding finished!\n", __func__);
+	rtw_ps_deny_cancel(adapter, PS_DENY_BEAMFORMING);
+}
+
+static void _sounding_force_stop(PADAPTER adapter)
+{
+	struct beamforming_info	*info;
+	struct sounding_info *sounding;
+	u8 cancelled;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+	sounding = &info->sounding_info;
+
+	if ((sounding->state == SOUNDING_STATE_SU_START)
+	    || (sounding->state == SOUNDING_STATE_MU_START)) {
+		u8 res = false;
+		_cancel_timer(&info->sounding_timeout_timer, &cancelled);
+		rtw_bf_cmd(adapter, BEAMFORMING_CTRL_END_PERIOD, &res, 1, 1);
+		return;
+	}
+
+	info->sounding_running--;
+	sounding->state = SOUNDING_STATE_INIT;
+	RTW_INFO("%s: Sounding finished!\n", __func__);
+	rtw_ps_deny_cancel(adapter, PS_DENY_BEAMFORMING);
+}
+
+static void _sounding_timer_handler(void *FunctionContext)
+{
+	PADAPTER adapter;
+	struct beamforming_info	*info;
+	struct sounding_info *sounding;
+	static u8 delay = 0;
+
+
+	RTW_DBG("+%s\n", __func__);
+
+	adapter = (PADAPTER)FunctionContext;
+	info = GET_BEAMFORM_INFO(adapter);
+	sounding = &info->sounding_info;
+
+	if (SOUNDING_STATE_NONE == sounding->state) {
+		RTW_INFO("%s: Stop!\n", __func__);
+		if (info->sounding_running)
+			RTW_WARN("%s: souding_running=%d when thread stop!\n",
+				 __func__, info->sounding_running);
+		return;
+	}
+
+	_set_timer(&info->sounding_timer, sounding->min_sounding_period);
+
+	if (!info->sounding_running) {
+		if (SOUNDING_STATE_INIT != sounding->state) {
+			RTW_WARN("%s: state(%d) != SOUNDING_STATE_INIT!!\n", __func__, sounding->state);
+			sounding->state = SOUNDING_STATE_INIT;
+		}
+		delay = 0;
+		info->sounding_running++;
+		rtw_bf_cmd(adapter, BEAMFORMING_CTRL_START_PERIOD, NULL, 0, 1);
+	} else {
+		if (delay != 0xFF)
+			delay++;
+		RTW_WARN("%s: souding is still processing...(state:%d, running:%d, delay:%d)\n",
+			 __func__, sounding->state, info->sounding_running, delay);
+		if (delay > 3) {
+			RTW_WARN("%s: Stop sounding!!\n", __func__);
+			_sounding_force_stop(adapter);
+		}
+	}
+}
+
+static void _sounding_timeout_timer_handler(void *FunctionContext)
+{
+	PADAPTER adapter;
+	struct beamforming_info	*info;
+	struct sounding_info *sounding;
+	struct beamformee_entry *bfee;
+
+
+	RTW_WARN("+%s\n", __func__);
+
+	adapter = (PADAPTER)FunctionContext;
+	info = GET_BEAMFORM_INFO(adapter);
+	sounding = &info->sounding_info;
+
+	if (SOUNDING_STATE_SU_START == sounding->state) {
+		sounding->state = SOUNDING_STATE_SOUNDING_TIMEOUT;
+		RTW_ERR("%s: Set to SU SOUNDING_STATE_SOUNDING_TIMEOUT\n", __func__);
+		/* SU BFee */
+		bfee = &info->bfee_entry[sounding->su_bfee_curidx];
+		bfee->bSoundingTimeout = true;
+		RTW_WARN("%s: The BFee entry[%d] is Sounding Timeout!\n", __func__, sounding->su_bfee_curidx);
+	} else if (SOUNDING_STATE_MU_START == sounding->state) {
+		sounding->state = SOUNDING_STATE_SOUNDING_TIMEOUT;
+		RTW_ERR("%s: Set to MU SOUNDING_STATE_SOUNDING_TIMEOUT\n", __func__);
+	} else {
+		RTW_WARN("%s: unexpected sounding state:0x%02x\n", __func__, sounding->state);
+		return;
+	}
+
+	rtw_bf_cmd(adapter, BEAMFORMING_CTRL_START_PERIOD, NULL, 0, 1);
+}
+
+static struct beamformer_entry *_bfer_get_free_entry(PADAPTER adapter)
+{
+	u8 i = 0;
+	struct beamforming_info *info;
+	struct beamformer_entry *bfer;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+
+	for (i = 0; i < MAX_BEAMFORMER_ENTRY_NUM; i++) {
+		bfer = &info->bfer_entry[i];
+		if (bfer->used == false)
+			return bfer;
+	}
+
+	return NULL;
+}
+
+static struct beamformer_entry *_bfer_get_entry_by_addr(PADAPTER adapter, u8 *ra)
+{
+	u8 i = 0;
+	struct beamforming_info *info;
+	struct beamformer_entry *bfer;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+
+	for (i = 0; i < MAX_BEAMFORMER_ENTRY_NUM; i++) {
+		bfer = &info->bfer_entry[i];
+		if (bfer->used == false)
+			continue;
+		if (!memcmp(ra, bfer->mac_addr, ETH_ALEN) == true)
+			return bfer;
+	}
+
+	return NULL;
+}
+
+static struct beamformer_entry *_bfer_add_entry(PADAPTER adapter,
+	struct sta_info *sta, u8 bf_cap, u8 sounding_dim, u8 comp_steering)
+{
+	struct mlme_priv *mlme;
+	struct beamforming_info *info;
+	struct beamformer_entry *bfer;
+	u8 *bssid;
+	u16 val16;
+	u8 i;
+
+
+	mlme = &adapter->mlmepriv;
+	info = GET_BEAMFORM_INFO(adapter);
+
+	bfer = _bfer_get_entry_by_addr(adapter, sta->hwaddr);
+	if (!bfer) {
+		bfer = _bfer_get_free_entry(adapter);
+		if (!bfer)
+			return NULL;
+	}
+
+	bfer->used = true;
+	_get_txvector_parameter(adapter, sta, &bfer->g_id, &bfer->p_aid);
+	memcpy(bfer->mac_addr, sta->hwaddr, ETH_ALEN);
+	bfer->cap = bf_cap;
+	bfer->state = BEAMFORM_ENTRY_HW_STATE_ADD_INIT;
+	bfer->NumofSoundingDim = sounding_dim;
+
+	if (TEST_FLAG(bf_cap, BEAMFORMER_CAP_VHT_MU)) {
+		info->beamformer_mu_cnt += 1;
+		bfer->aid = sta->aid;
+	} else if (TEST_FLAG(bf_cap, BEAMFORMER_CAP_VHT_SU|BEAMFORMER_CAP_HT_EXPLICIT)) {
+		info->beamformer_su_cnt += 1;
+
+		/* Record HW idx info */
+		for (i = 0; i < MAX_NUM_BEAMFORMER_SU; i++) {
+			if ((info->beamformer_su_reg_maping & BIT(i)) == 0) {
+				info->beamformer_su_reg_maping |= BIT(i);
+				bfer->su_reg_index = i;
+				break;
+			}
+		}
+		RTW_INFO("%s: Add BFer entry beamformer_su_reg_maping=%#x, su_reg_index=%d\n",
+			 __func__, info->beamformer_su_reg_maping, bfer->su_reg_index);
+	}
+
+	return bfer;
+}
+
+static void _bfer_remove_entry(PADAPTER adapter, struct beamformer_entry *entry)
+{
+	struct beamforming_info *info;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+
+	entry->state = BEAMFORM_ENTRY_HW_STATE_DELETE_INIT;
+
+	if (TEST_FLAG(entry->cap, BEAMFORMER_CAP_VHT_MU)) {
+		info->beamformer_mu_cnt -= 1;
+		memset(entry->gid_valid, 0, 8);
+		memset(entry->user_position, 0, 16);
+	} else if (TEST_FLAG(entry->cap, BEAMFORMER_CAP_VHT_SU|BEAMFORMER_CAP_HT_EXPLICIT)) {
+		info->beamformer_su_cnt -= 1;
+	}
+
+	if (info->beamformer_mu_cnt == 0)
+		info->beamforming_cap &= ~BEAMFORMEE_CAP_VHT_MU;
+	if (info->beamformer_su_cnt == 0)
+		info->beamforming_cap &= ~(BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT);
+}
+
+static u8 _bfer_set_entry_gid(PADAPTER adapter, u8 *addr, u8 *gid, u8 *position)
+{
+	struct beamformer_entry *bfer = NULL;
+
+
+	bfer = _bfer_get_entry_by_addr(adapter, addr);
+	if (!bfer) {
+		RTW_INFO("%s: Cannot find BFer entry!!\n", __func__);
+		return _FAIL;
+	}
+
+	/* Parsing Membership Status Array */
+	memcpy(bfer->gid_valid, gid, 8);
+	/* Parsing User Position Array */
+	memcpy(bfer->user_position, position, 16);
+
+	/* Config HW GID table */
+	rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_GID_TABLE, (u8*)&bfer, sizeof(struct beamformer_entry *), 1);
+
+	return _SUCCESS;
+}
+
+static struct beamformee_entry *_bfee_get_free_entry(PADAPTER adapter)
+{
+	u8 i = 0;
+	struct beamforming_info *info;
+	struct beamformee_entry *bfee;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+
+	for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
+		bfee = &info->bfee_entry[i];
+		if (bfee->used == false)
+			return bfee;
+	}
+
+	return NULL;
+}
+
+static struct beamformee_entry *_bfee_get_entry_by_addr(PADAPTER adapter, u8 *ra)
+{
+	u8 i = 0;
+	struct beamforming_info *info;
+	struct beamformee_entry *bfee;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+
+	for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
+		bfee = &info->bfee_entry[i];
+		if (bfee->used == false)
+			continue;
+		if (!memcmp(ra, bfee->mac_addr, ETH_ALEN) == true)
+			return bfee;
+	}
+
+	return NULL;
+}
+
+static u8 _bfee_get_first_su_entry_idx(PADAPTER adapter, struct beamformee_entry *ignore)
+{
+	struct beamforming_info *info;
+	struct beamformee_entry *bfee;
+	u8 i;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+
+	for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
+		bfee = &info->bfee_entry[i];
+		if (ignore && (bfee == ignore))
+			continue;
+		if (bfee->used == false)
+			continue;
+		if ((!TEST_FLAG(bfee->cap, BEAMFORMEE_CAP_VHT_MU))
+		    && TEST_FLAG(bfee->cap, BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT))
+			return i;
+	}
+
+	return 0xFF;
+}
+
+/*
+ * Description:
+ *	Get the first entry index of MU Beamformee.
+ *
+ * Return Value:
+ *	Index of the first MU sta, or 0xFF for invalid index.
+ *
+ * 2015.05.25. Created by tynli.
+ *
+ */
+static u8 _bfee_get_first_mu_entry_idx(PADAPTER adapter, struct beamformee_entry *ignore)
+{
+	struct beamforming_info *info;
+	struct beamformee_entry *bfee;
+	u8 i;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+
+	for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
+		bfee = &info->bfee_entry[i];
+		if (ignore && (bfee == ignore))
+			continue;
+		if (bfee->used == false)
+			continue;
+		if (TEST_FLAG(bfee->cap, BEAMFORMEE_CAP_VHT_MU))
+			return i;
+	}
+
+	return 0xFF;
+}
+
+static struct beamformee_entry *_bfee_add_entry(PADAPTER adapter,
+	struct sta_info *sta, u8 bf_cap, u8 sounding_dim, u8 comp_steering)
+{
+	struct mlme_priv *mlme;
+	struct beamforming_info *info;
+	struct beamformee_entry *bfee;
+	u8 *bssid;
+	u16 val16;
+	u8 i;
+
+
+	mlme = &adapter->mlmepriv;
+	info = GET_BEAMFORM_INFO(adapter);
+
+	bfee = _bfee_get_entry_by_addr(adapter, sta->hwaddr);
+	if (!bfee) {
+		bfee = _bfee_get_free_entry(adapter);
+		if (!bfee)
+			return NULL;
+	}
+
+	bfee->used = true;
+	bfee->aid = sta->aid;
+	bfee->mac_id = sta->mac_id;
+	bfee->sound_bw = sta->bw_mode;
+
+	_get_txvector_parameter(adapter, sta, &bfee->g_id, &bfee->p_aid);
+	sta->txbf_gid = bfee->g_id;
+	sta->txbf_paid = bfee->p_aid;
+
+	memcpy(bfee->mac_addr, sta->hwaddr, ETH_ALEN);
+	bfee->txbf = false;
+	bfee->sounding = false;
+	bfee->sound_period = 40;
+	_sounding_update_min_period(adapter, bfee->sound_period, false);
+	bfee->SoundCnt = GetInitSoundCnt(bfee->sound_period, info->sounding_info.min_sounding_period);
+	bfee->cap = bf_cap;
+	bfee->state = BEAMFORM_ENTRY_HW_STATE_ADD_INIT;
+
+	bfee->bCandidateSoundingPeer = false;
+	bfee->bSoundingTimeout = false;
+	bfee->bDeleteSounding = false;
+	bfee->bApplySounding = true;
+
+	bfee->tx_timestamp = 0;
+	bfee->tx_bytes = 0;
+
+	bfee->LogStatusFailCnt = 0;
+	bfee->NumofSoundingDim = sounding_dim;
+	bfee->comp_steering_num_of_bfer = comp_steering;
+	bfee->bSuspendSUCap = false;
+
+	if (TEST_FLAG(bf_cap, BEAMFORMEE_CAP_VHT_MU)) {
+		info->beamformee_mu_cnt += 1;
+		info->first_mu_bfee_index = _bfee_get_first_mu_entry_idx(adapter, NULL);
+
+		if (info->bEnableSUTxBFWorkAround) {
+			/* When the first MU BFee added, discard SU BFee bfee's capability */
+			if ((info->beamformee_mu_cnt == 1) && (info->beamformee_su_cnt > 0)) {
+				if (info->TargetSUBFee) {
+					info->TargetSUBFee->bSuspendSUCap = true;
+					info->TargetSUBFee->bDeleteSounding = true;
+				} else {
+					RTW_ERR("%s: UNEXPECTED!! info->TargetSUBFee is NULL!", __func__);
+				}
+				info->TargetSUBFee = NULL;
+				memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO));
+				rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_CSI_REPORT, (u8*)&info->TargetCSIInfo, sizeof(struct _RT_CSI_INFO), 0);
+			}
+		}
+
+		/* Record HW idx info */
+		for (i = 0; i < MAX_NUM_BEAMFORMEE_MU; i++) {
+			if ((info->beamformee_mu_reg_maping & BIT(i)) == 0) {
+				info->beamformee_mu_reg_maping |= BIT(i);
+				bfee->mu_reg_index = i;
+				break;
+			}
+		}
+		RTW_INFO("%s: Add BFee entry beamformee_mu_reg_maping=%#x, mu_reg_index=%d\n",
+			 __func__, info->beamformee_mu_reg_maping, bfee->mu_reg_index);
+
+	} else if (TEST_FLAG(bf_cap, BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT)) {
+		info->beamformee_su_cnt += 1;
+
+		if (info->bEnableSUTxBFWorkAround) {
+			/* Record the first SU BFee index. We only allow the first SU BFee to be sound */
+			if ((info->beamformee_su_cnt == 1) && (info->beamformee_mu_cnt == 0)) {
+				info->TargetSUBFee = bfee;
+				memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO));
+				bfee->bSuspendSUCap = false;
+			} else {
+				bfee->bSuspendSUCap = true;
+			}
+		}
+
+		/* Record HW idx info */
+		for (i = 0; i < MAX_NUM_BEAMFORMEE_SU; i++) {
+			if ((info->beamformee_su_reg_maping & BIT(i)) == 0) {
+				info->beamformee_su_reg_maping |= BIT(i);
+				bfee->su_reg_index = i;
+				break;
+			}
+		}
+		RTW_INFO("%s: Add BFee entry beamformee_su_reg_maping=%#x, su_reg_index=%d\n",
+			 __func__, info->beamformee_su_reg_maping, bfee->su_reg_index);
+	}
+
+	return bfee;
+}
+
+static void _bfee_remove_entry(PADAPTER adapter, struct beamformee_entry *entry)
+{
+	struct beamforming_info *info;
+	u8 idx;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+
+	entry->state = BEAMFORM_ENTRY_HW_STATE_DELETE_INIT;
+
+	if (TEST_FLAG(entry->cap, BEAMFORMEE_CAP_VHT_MU)) {
+		info->beamformee_mu_cnt -= 1;
+		info->first_mu_bfee_index = _bfee_get_first_mu_entry_idx(adapter, entry);
+
+		if (info->bEnableSUTxBFWorkAround) {
+			if ((info->beamformee_mu_cnt == 0) && (info->beamformee_su_cnt > 0)) {
+				idx = _bfee_get_first_su_entry_idx(adapter, NULL);
+				info->TargetSUBFee = &info->bfee_entry[idx];
+				memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO));
+				info->TargetSUBFee->bSuspendSUCap = false;
+			}
+		}
+	} else if (TEST_FLAG(entry->cap, BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT)) {
+		info->beamformee_su_cnt -= 1;
+
+		/* When the target SU BFee leaves, disable workaround */
+		if ((info->bEnableSUTxBFWorkAround)
+		    && (entry == info->TargetSUBFee)) {
+			entry->bSuspendSUCap = true;
+			info->TargetSUBFee = NULL;
+			memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO));
+			rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_CSI_REPORT, (u8*)&info->TargetCSIInfo, sizeof(struct _RT_CSI_INFO), 0);
+		}
+	}
+
+	if (info->beamformee_mu_cnt == 0)
+		info->beamforming_cap &= ~BEAMFORMER_CAP_VHT_MU;
+	if (info->beamformee_su_cnt == 0)
+		info->beamforming_cap &= ~(BEAMFORMER_CAP_VHT_SU|BEAMFORMER_CAP_HT_EXPLICIT);
+
+	_sounding_update_min_period(adapter, 0, true);
+}
+
+static enum beamforming_cap _bfee_get_entry_cap_by_macid(PADAPTER adapter, u8 macid)
+{
+	struct beamforming_info *info;
+	struct beamformee_entry *bfee;
+	u8 i;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+
+	for (i = 0; i < MAX_BEAMFORMER_ENTRY_NUM; i++) {
+		bfee = &info->bfee_entry[i];
+		if (bfee->used == false)
+			continue;
+		if (bfee->mac_id == macid)
+			return bfee->cap;
+	}
+
+	return BEAMFORMING_CAP_NONE;
+}
+
+static void _beamforming_enter(PADAPTER adapter, void *p)
+{
+	struct mlme_priv *mlme;
+	struct ht_priv *htpriv;
+	struct mlme_ext_priv *mlme_ext;
+	struct sta_info *sta, *sta_copy;
+	struct beamforming_info *info;
+	struct beamformer_entry *bfer = NULL;
+	struct beamformee_entry *bfee = NULL;
+	u8 wireless_mode;
+	u8 sta_bf_cap;
+	u8 sounding_dim = 0; /* number of sounding dimensions */
+	u8 comp_steering_num = 0; /* compressed steering number */
+
+
+	mlme = &adapter->mlmepriv;
+	htpriv = &mlme->htpriv;
+	mlme_ext = &adapter->mlmeextpriv;
+	info = GET_BEAMFORM_INFO(adapter);
+
+	sta_copy = (struct sta_info *)p;
+	sta = rtw_get_stainfo(&adapter->stapriv, sta_copy->hwaddr);
+	if (!sta) {
+		RTW_ERR("%s: Cann't find STA info for " MAC_FMT "\n",
+		        __func__, MAC_ARG(sta_copy->hwaddr));
+		return;
+	}
+	if (sta != sta_copy) {
+		RTW_WARN("%s: Origin sta(fake)=%p realsta=%p for " MAC_FMT "\n",
+	        	 __func__, sta_copy, sta, MAC_ARG(sta_copy->hwaddr));
+	}
+
+	/* The current setting does not support Beaforming */
+	wireless_mode = sta->wireless_mode;
+	if ((is_supported_ht(wireless_mode) == false)
+	    && (is_supported_vht(wireless_mode) == false)) {
+		RTW_WARN("%s: Not support HT or VHT mode\n", __func__);
+		return;
+	}
+
+	if ((0 == htpriv->beamform_cap)) {
+		RTW_INFO("The configuration disabled Beamforming! Skip...\n");
+		return;
+	}
+
+	_get_sta_beamform_cap(adapter, sta,
+			      &sta_bf_cap, &sounding_dim, &comp_steering_num);
+	RTW_INFO("STA Beamforming Capability=0x%02X\n", sta_bf_cap);
+	if (sta_bf_cap == BEAMFORMING_CAP_NONE)
+		return;
+	if ((sta_bf_cap & BEAMFORMEE_CAP_HT_EXPLICIT)
+	    || (sta_bf_cap & BEAMFORMEE_CAP_VHT_SU)
+	    || (sta_bf_cap & BEAMFORMEE_CAP_VHT_MU))
+		sta_bf_cap |= BEAMFORMEE_CAP;
+	if ((sta_bf_cap & BEAMFORMER_CAP_HT_EXPLICIT)
+	    || (sta_bf_cap & BEAMFORMER_CAP_VHT_SU)
+	    || (sta_bf_cap & BEAMFORMER_CAP_VHT_MU))
+		sta_bf_cap |= BEAMFORMER_CAP;
+
+	if (sta_bf_cap & BEAMFORMER_CAP) {
+		/* The other side is beamformer */
+		bfer = _bfer_add_entry(adapter, sta, sta_bf_cap, sounding_dim, comp_steering_num);
+		if (!bfer)
+			RTW_ERR("%s: Fail to allocate bfer entry!\n", __func__);
+	}
+	if (sta_bf_cap & BEAMFORMEE_CAP) {
+		/* The other side is beamformee */
+		bfee = _bfee_add_entry(adapter, sta, sta_bf_cap, sounding_dim, comp_steering_num);
+		if (!bfee)
+			RTW_ERR("%s: Fail to allocate bfee entry!\n", __func__);
+	}
+	if (!bfer && !bfee)
+		return;
+
+	rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_ENTER, (u8*)sta);
+
+	/* Perform sounding if there is BFee */
+	if ((info->beamformee_su_cnt != 0)
+	    || (info->beamformee_mu_cnt != 0)) {
+		if (SOUNDING_STATE_NONE == info->sounding_info.state) {
+			info->sounding_info.state = SOUNDING_STATE_INIT;
+			/* Start sounding after 2 sec */
+			_set_timer(&info->sounding_timer, 2000);
+		}
+	}
+}
+
+static void _beamforming_reset(PADAPTER adapter)
+{
+	RTW_ERR("%s: Not ready!!\n", __func__);
+}
+
+static void _beamforming_leave(PADAPTER adapter, u8 *ra)
+{
+	struct beamforming_info *info;
+	struct beamformer_entry *bfer = NULL;
+	struct beamformee_entry *bfee = NULL;
+	u8 bHwStateAddInit = false;
+
+
+	RTW_INFO("+%s\n", __func__);
+
+	info = GET_BEAMFORM_INFO(adapter);
+	bfer = _bfer_get_entry_by_addr(adapter, ra);
+	bfee = _bfee_get_entry_by_addr(adapter, ra);
+
+	if (!bfer && !bfee) {
+		RTW_WARN("%s: " MAC_FMT " is neither beamforming ee or er!!\n",
+			__func__, MAC_ARG(ra));
+		return;
+	}
+
+	if (bfer)
+		_bfer_remove_entry(adapter, bfer);
+
+	if (bfee)
+		_bfee_remove_entry(adapter, bfee);
+
+	rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_LEAVE, ra);
+
+	/* Stop sounding if there is no any BFee */
+	if ((info->beamformee_su_cnt == 0)
+	    && (info->beamformee_mu_cnt == 0)) {
+		u8 cancelled;
+		_cancel_timer(&info->sounding_timer, &cancelled);
+		_sounding_init(&info->sounding_info);
+	}
+
+	RTW_INFO("-%s\n", __func__);
+}
+
+static void _beamforming_sounding_down(PADAPTER adapter, u8 status)
+{
+	struct beamforming_info	*info;
+	struct sounding_info *sounding;
+	struct beamformee_entry *bfee;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+	sounding = &info->sounding_info;
+
+	RTW_INFO("+%s: sounding=%d, status=0x%02x\n", __func__, sounding->state, status);
+
+	if (sounding->state == SOUNDING_STATE_MU_START) {
+		RTW_INFO("%s: MU sounding done\n", __func__);
+		sounding->state = SOUNDING_STATE_MU_SOUNDDOWN;
+		RTW_INFO("%s: Set to SOUNDING_STATE_MU_SOUNDDOWN\n", __func__);
+		info->SetHalSoundownOnDemandCnt++;
+		rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_STATUS, &status);
+	} else if (sounding->state == SOUNDING_STATE_SU_START) {
+		RTW_INFO("%s: SU entry[%d] sounding down\n", __func__, sounding->su_bfee_curidx);
+		bfee = &info->bfee_entry[sounding->su_bfee_curidx];
+		sounding->state = SOUNDING_STATE_SU_SOUNDDOWN;
+		RTW_INFO("%s: Set to SOUNDING_STATE_SU_SOUNDDOWN\n", __func__);
+
+		/*
+		 * <tynli_note>
+		 *	bfee->bSoundingTimeout this flag still cannot avoid
+		 *	old sound down event happens in the new sounding period.
+		 *	2015.12.10
+		 */
+		if (bfee->bSoundingTimeout) {
+			RTW_WARN("%s: The entry[%d] is bSoundingTimeout!\n", __func__, sounding->su_bfee_curidx);
+			bfee->bSoundingTimeout = false;
+			return;
+		}
+
+		if (status) {
+			/* success */
+			bfee->LogStatusFailCnt = 0;
+			info->SetHalSoundownOnDemandCnt++;
+			rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_STATUS, &status);
+		} else if (bfee->bDeleteSounding) {
+			RTW_WARN("%s: Delete entry[%d] sounding info!\n", __func__, sounding->su_bfee_curidx);
+			rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_STATUS, &status);
+			bfee->bDeleteSounding = false;
+		} else {
+			bfee->LogStatusFailCnt++;
+			RTW_WARN("%s: LogStatusFailCnt=%d\n", __func__, bfee->LogStatusFailCnt);
+			if (bfee->LogStatusFailCnt > 30) {
+				RTW_ERR("%s: LogStatusFailCnt > 30, Stop SOUNDING!!\n", __func__);
+				rtw_bf_cmd(adapter, BEAMFORMING_CTRL_LEAVE, bfee->mac_addr, ETH_ALEN, 1);
+			}
+		}
+	} else {
+		RTW_WARN("%s: unexpected sounding state:0x%02x\n", __func__, sounding->state);
+		return;
+	}
+
+	rtw_bf_cmd(adapter, BEAMFORMING_CTRL_START_PERIOD, NULL, 0, 0);
+}
+
+static void _c2h_snd_txbf(PADAPTER adapter, u8 *buf, u8 buf_len)
+{
+	struct beamforming_info	*info;
+	u8 cancelled;
+	u8 res;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+
+	_cancel_timer(&info->sounding_timeout_timer, &cancelled);
+
+	res = C2H_SND_TXBF_GET_SND_RESULT(buf) ? true : false;
+	RTW_INFO("+%s: %s\n", __func__, res==true?"Success":"Fail!");
+
+	rtw_bf_cmd(adapter, BEAMFORMING_CTRL_END_PERIOD, &res, 1, 1);
+}
+
+/*
+ * Description:
+ *	This function is for phydm only
+ */
+enum beamforming_cap rtw_bf_bfee_get_entry_cap_by_macid(void *mlme, u8 macid)
+{
+	PADAPTER adapter;
+	enum beamforming_cap cap = BEAMFORMING_CAP_NONE;
+
+
+	adapter = mlme_to_adapter((struct mlme_priv *)mlme);
+	cap = _bfee_get_entry_cap_by_macid(adapter, macid);
+
+	return cap;
+}
+
+struct beamformer_entry *rtw_bf_bfer_get_entry_by_addr(PADAPTER adapter, u8 *ra)
+{
+	return _bfer_get_entry_by_addr(adapter, ra);
+}
+
+struct beamformee_entry *rtw_bf_bfee_get_entry_by_addr(PADAPTER adapter, u8 *ra)
+{
+	return _bfee_get_entry_by_addr(adapter, ra);
+}
+
+void rtw_bf_get_ndpa_packet(PADAPTER adapter, union recv_frame *precv_frame)
+{
+	RTW_DBG("+%s\n", __func__);
+}
+
+u32 rtw_bf_get_report_packet(PADAPTER adapter, union recv_frame *precv_frame)
+{
+	u32 ret = _SUCCESS;
+	struct beamforming_info *info;
+	struct beamformee_entry *bfee = NULL;
+	u8 *pframe;
+	u32 frame_len;
+	u8 *ta;
+	u8 *frame_body;
+	u8 category, action;
+	u8 *pMIMOCtrlField, *pCSIMatrix;
+	u8 Nc = 0, Nr = 0, CH_W = 0, Ng = 0, CodeBook = 0;
+	u16 CSIMatrixLen = 0;
+
+
+	RTW_INFO("+%s\n", __func__);
+
+	info = GET_BEAMFORM_INFO(adapter);
+	pframe = precv_frame->u.hdr.rx_data;
+	frame_len = precv_frame->u.hdr.len;
+
+	/* Memory comparison to see if CSI report is the same with previous one */
+	ta = get_addr2_ptr(pframe);
+	bfee = _bfee_get_entry_by_addr(adapter, ta);
+	if (!bfee)
+		return _FAIL;
+
+	frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	category = frame_body[0];
+	action = frame_body[1];
+
+	if ((category == RTW_WLAN_CATEGORY_VHT)
+	    && (action == RTW_WLAN_ACTION_VHT_COMPRESSED_BEAMFORMING)) {
+		pMIMOCtrlField = pframe + 26;
+		Nc = (*pMIMOCtrlField) & 0x7;
+		Nr = ((*pMIMOCtrlField) & 0x38) >> 3;
+		CH_W =  (((*pMIMOCtrlField) & 0xC0) >> 6);
+		Ng = (*(pMIMOCtrlField+1)) & 0x3;
+		CodeBook = ((*(pMIMOCtrlField+1)) & 0x4) >> 2;
+		/*
+		 * 24+(1+1+3)+2
+		 * ==> MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2)
+		 */
+		pCSIMatrix = pMIMOCtrlField + 3 + Nc;
+		CSIMatrixLen = frame_len - 26 - 3 - Nc;
+		info->TargetCSIInfo.bVHT = true;
+	} else if ((category == RTW_WLAN_CATEGORY_HT)
+		   && (action == RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING)) {
+		pMIMOCtrlField = pframe + 26;
+		Nc = (*pMIMOCtrlField) & 0x3;
+		Nr = ((*pMIMOCtrlField) & 0xC) >> 2;
+		CH_W = ((*pMIMOCtrlField) & 0x10) >> 4;
+		Ng = ((*pMIMOCtrlField) & 0x60) >> 5;
+		CodeBook = ((*(pMIMOCtrlField+1)) & 0x6) >> 1;
+		/*
+		 * 24+(1+1+6)+2
+		 * ==> MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2)
+		 */
+		pCSIMatrix = pMIMOCtrlField + 6 + Nr;
+		CSIMatrixLen = frame_len  - 26 - 6 - Nr;
+		info->TargetCSIInfo.bVHT = false;
+	}
+
+	/* Update current CSI report info */
+	if ((info->bEnableSUTxBFWorkAround)
+	    && (info->TargetSUBFee == bfee)) {
+		if ((info->TargetCSIInfo.Nc != Nc) || (info->TargetCSIInfo.Nr != Nr) ||
+			(info->TargetCSIInfo.ChnlWidth != CH_W) || (info->TargetCSIInfo.Ng != Ng) ||
+			(info->TargetCSIInfo.CodeBook != CodeBook)) {
+			info->TargetCSIInfo.Nc = Nc;
+			info->TargetCSIInfo.Nr = Nr;
+			info->TargetCSIInfo.ChnlWidth = CH_W;
+			info->TargetCSIInfo.Ng = Ng;
+			info->TargetCSIInfo.CodeBook = CodeBook;
+
+			rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_CSI_REPORT, (u8*)&info->TargetCSIInfo, sizeof(struct _RT_CSI_INFO), 1);
+		}
+	}
+
+	RTW_INFO("%s: pkt type=%d-%d, Nc=%d, Nr=%d, CH_W=%d, Ng=%d, CodeBook=%d\n",
+		 __func__, category, action, Nc, Nr, CH_W, Ng, CodeBook);
+
+	return ret;
+}
+
+u8 rtw_bf_send_vht_gid_mgnt_packet(PADAPTER adapter, u8 *ra, u8 *gid, u8 *position)
+{
+	/* General */
+	struct xmit_priv *xmitpriv;
+	struct mlme_priv *mlmepriv;
+	struct xmit_frame *pmgntframe;
+	/* MISC */
+	struct pkt_attrib *attrib;
+	struct rtw_ieee80211_hdr *wlanhdr;
+	u8 *pframe, *ptr;
+
+
+	xmitpriv = &adapter->xmitpriv;
+	mlmepriv = &adapter->mlmepriv;
+
+	pmgntframe = alloc_mgtxmitframe(xmitpriv);
+	if (!pmgntframe)
+		return false;
+
+	/* update attribute */
+	attrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(adapter, attrib);
+	attrib->rate = MGN_6M;
+	attrib->bwmode = CHANNEL_WIDTH_20;
+	attrib->subtype = WIFI_ACTION;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
+	wlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	wlanhdr->frame_ctl = 0;
+	set_frame_sub_type(pframe, attrib->subtype);
+	set_duration(pframe, 0);
+	SetFragNum(pframe, 0);
+	SetSeqNum(pframe, 0);
+
+	memcpy(wlanhdr->addr1, ra, ETH_ALEN);
+	memcpy(wlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN);
+	memcpy(wlanhdr->addr3, get_bssid(mlmepriv), ETH_ALEN);
+
+	pframe[24] = RTW_WLAN_CATEGORY_VHT;
+	pframe[25] = RTW_WLAN_ACTION_VHT_GROUPID_MANAGEMENT;
+	/* Set Membership Status Array */
+	ptr = pframe + 26;
+	memcpy(ptr, gid, 8);
+	/* Set User Position Array */
+	ptr = pframe + 34;
+	memcpy(ptr, position, 16);
+
+	attrib->pktlen = 54;
+	attrib->last_txcmdsz = attrib->pktlen;
+
+	dump_mgntframe(adapter, pmgntframe);
+
+	return true;
+}
+
+/*
+ * Description:
+ *	On VHT GID management frame by an MU beamformee.
+ */
+void rtw_bf_get_vht_gid_mgnt_packet(PADAPTER adapter, union recv_frame *precv_frame)
+{
+	u8 *pframe;
+	u8 *ta, *gid, *position;
+
+
+	RTW_DBG("+%s\n", __func__);
+
+	pframe = precv_frame->u.hdr.rx_data;
+
+	/* Get address by Addr2 */
+	ta = get_addr2_ptr(pframe);
+	/* Remove signaling TA */
+	ta[0] &= 0xFE;
+
+	/* Membership Status Array */
+	gid = pframe + 26;
+	/* User Position Array */
+	position= pframe + 34;
+
+	_bfer_set_entry_gid(adapter, ta, gid, position);
+}
+
+void rtw_bf_init(PADAPTER adapter)
+{
+	struct beamforming_info	*info;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+	info->beamforming_cap = BEAMFORMING_CAP_NONE;
+	info->beamforming_state = BEAMFORMING_STATE_IDLE;
+/*
+	info->bfee_entry[MAX_BEAMFORMEE_ENTRY_NUM];
+	info->bfer_entry[MAX_BEAMFORMER_ENTRY_NUM];
+*/
+	info->sounding_sequence = 0;
+	info->beamformee_su_cnt = 0;
+	info->beamformer_su_cnt = 0;
+	info->beamformee_su_reg_maping = 0;
+	info->beamformer_su_reg_maping = 0;
+	info->beamformee_mu_cnt = 0;
+	info->beamformer_mu_cnt = 0;
+	info->beamformee_mu_reg_maping = 0;
+	info->first_mu_bfee_index = 0xFF;
+	info->mu_bfer_curidx = 0xFF;
+
+	_sounding_init(&info->sounding_info);
+	_init_timer(&info->sounding_timer, adapter->pnetdev, _sounding_timer_handler, adapter);
+	_init_timer(&info->sounding_timeout_timer, adapter->pnetdev, _sounding_timeout_timer_handler, adapter);
+
+	info->SetHalBFEnterOnDemandCnt = 0;
+	info->SetHalBFLeaveOnDemandCnt = 0;
+	info->SetHalSoundownOnDemandCnt = 0;
+
+	info->bEnableSUTxBFWorkAround = true;
+	info->TargetSUBFee = NULL;
+
+	info->sounding_running = 0;
+}
+
+void rtw_bf_cmd_hdl(PADAPTER adapter, u8 type, u8 *pbuf)
+{
+	switch (type) {
+	case BEAMFORMING_CTRL_ENTER:
+		_beamforming_enter(adapter, pbuf);
+		break;
+
+	case BEAMFORMING_CTRL_LEAVE:
+		if (pbuf == NULL)
+			_beamforming_reset(adapter);
+		else
+			_beamforming_leave(adapter, pbuf);
+		break;
+
+	case BEAMFORMING_CTRL_START_PERIOD:
+		_sounding_handler(adapter);
+		break;
+
+	case BEAMFORMING_CTRL_END_PERIOD:
+		_beamforming_sounding_down(adapter, *pbuf);
+		break;
+
+	case BEAMFORMING_CTRL_SET_GID_TABLE:
+		rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_SET_GID_TABLE, *(void**)pbuf);
+		break;
+
+	case BEAMFORMING_CTRL_SET_CSI_REPORT:
+		rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_CSI_REPORT, pbuf);
+		break;
+
+	default:
+		break;
+	}
+}
+
+u8 rtw_bf_cmd(PADAPTER adapter, s32 type, u8 *pbuf, s32 size, u8 enqueue)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &adapter->cmdpriv;
+	u8 *wk_buf;
+	u8 res = _SUCCESS;
+
+
+	if (!enqueue) {
+		rtw_bf_cmd_hdl(adapter, type, pbuf);
+		goto exit;
+	}
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	if (pbuf != NULL) {
+		wk_buf = rtw_zmalloc(size);
+		if (wk_buf == NULL) {
+			rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
+			rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm));
+			res = _FAIL;
+			goto exit;
+		}
+
+		memcpy(wk_buf, pbuf, size);
+	} else {
+		wk_buf = NULL;
+		size = 0;
+	}
+
+	pdrvextra_cmd_parm->ec_id = BEAMFORMING_WK_CID;
+	pdrvextra_cmd_parm->type = type;
+	pdrvextra_cmd_parm->size = size;
+	pdrvextra_cmd_parm->pbuf = wk_buf;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+void rtw_bf_update_attrib(PADAPTER adapter, struct pkt_attrib *attrib, struct sta_info *sta)
+{
+	if (sta) {
+		attrib->txbf_g_id = sta->txbf_gid;
+		attrib->txbf_p_aid = sta->txbf_paid;
+	}
+}
+
+void rtw_bf_c2h_handler(PADAPTER adapter, u8 id, u8 *buf, u8 buf_len)
+{
+	switch (id) {
+	case CMD_ID_C2H_SND_TXBF:
+		_c2h_snd_txbf(adapter, buf, buf_len);
+		break;
+	}
+}
+
+#define toMbps(bytes, secs)	(rtw_division64(bytes >> 17, secs))
+void rtw_bf_update_traffic(PADAPTER adapter)
+{
+	struct beamforming_info	*info;
+	struct sounding_info *sounding;
+	struct beamformee_entry *bfee;
+	struct sta_info *sta;
+	u8 bfee_cnt, sounding_idx, i;
+	u16 tp[MAX_BEAMFORMEE_ENTRY_NUM] = {0};
+	u8 tx_rate[MAX_BEAMFORMEE_ENTRY_NUM] = {0};
+	u64 tx_bytes, last_bytes;
+	u32 time, last_timestamp;
+	u8 set_timer = false;
+
+
+	info = GET_BEAMFORM_INFO(adapter);
+	sounding = &info->sounding_info;
+
+	/* Check any bfee exist? */
+	bfee_cnt = info->beamformee_su_cnt + info->beamformee_mu_cnt;
+	if (bfee_cnt == 0)
+		return;
+
+	for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
+		bfee = &info->bfee_entry[i];
+		if (false == bfee->used)
+			continue;
+
+		sta = rtw_get_stainfo(&adapter->stapriv, bfee->mac_addr);
+		if (!sta) {
+			RTW_ERR("%s: Cann't find sta_info for " MAC_FMT "!\n", __func__, MAC_ARG(bfee->mac_addr));
+			continue;
+		}
+
+		last_timestamp = bfee->tx_timestamp;
+		last_bytes = bfee->tx_bytes;
+		bfee->tx_timestamp = jiffies;
+		bfee->tx_bytes = sta->sta_stats.tx_bytes;
+		if (last_timestamp) {
+			if (bfee->tx_bytes >= last_bytes)
+				tx_bytes = bfee->tx_bytes - last_bytes;
+			else
+				tx_bytes = bfee->tx_bytes + (~last_bytes);
+			time = rtw_get_time_interval_ms(last_timestamp, bfee->tx_timestamp);
+			time = (time > 1000) ? time/1000 : 1;
+			tp[i] = toMbps(tx_bytes, time);
+			tx_rate[i] = rtw_get_current_tx_rate(adapter, bfee->mac_id);
+			RTW_INFO("%s: BFee idx(%d), MadId(%d), TxTP=%lld bytes (%d Mbps), txrate=%d\n",
+				 __func__, i, bfee->mac_id, tx_bytes, tp[i], tx_rate[i]);
+		}
+	}
+
+	sounding_idx = phydm_get_beamforming_sounding_info(GET_PDM_ODM(adapter), tp, MAX_BEAMFORMEE_ENTRY_NUM, tx_rate);
+
+	for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
+		bfee = &info->bfee_entry[i];
+		if (false == bfee->used) {
+			if (sounding_idx & BIT(i))
+				RTW_WARN("%s: bfee(%d) not in used but need sounding?!\n", __func__, i);
+			continue;
+		}
+
+		if (sounding_idx & BIT(i)) {
+			if (false == bfee->bApplySounding) {
+				bfee->bApplySounding = true;
+				bfee->SoundCnt = 0;
+				set_timer = true;
+			}
+		} else {
+			if (bfee->bApplySounding) {
+				bfee->bApplySounding = false;
+				bfee->bDeleteSounding = true;
+				bfee->SoundCnt = 0;
+				set_timer = true;
+			}
+		}
+	}
+
+	if (set_timer) {
+		if (SOUNDING_STATE_NONE == info->sounding_info.state) {
+			info->sounding_info.state = SOUNDING_STATE_INIT;
+			_set_timer(&info->sounding_timer, 0);
+		}
+	}
+}
+
+#else /* !RTW_BEAMFORMING_VERSION_2 */
+
+#if (BEAMFORMING_SUPPORT == 0) /*for diver defined beamforming*/
+struct beamforming_entry	*beamforming_get_entry_by_addr(struct mlme_priv *pmlmepriv, u8 *ra, u8 *idx)
+{
+	u8	i = 0;
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
+
+	for (i = 0; i < BEAMFORMING_ENTRY_NUM; i++) {
+		if (pBeamInfo->beamforming_entry[i].bUsed &&
+		    (!memcmp(ra, pBeamInfo->beamforming_entry[i].mac_addr, ETH_ALEN))) {
+			*idx = i;
+			return &(pBeamInfo->beamforming_entry[i]);
+		}
+	}
+
+	return NULL;
+}
+
+BEAMFORMING_CAP beamforming_get_entry_beam_cap_by_mac_id(void * pmlmepriv , u8 mac_id)
+{
+	u8	i = 0;
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO((struct mlme_priv *)pmlmepriv);
+	BEAMFORMING_CAP		BeamformEntryCap = BEAMFORMING_CAP_NONE;
+
+	for (i = 0; i < BEAMFORMING_ENTRY_NUM; i++) {
+		if (pBeamInfo->beamforming_entry[i].bUsed &&
+		    (mac_id == pBeamInfo->beamforming_entry[i].mac_id)) {
+			BeamformEntryCap =  pBeamInfo->beamforming_entry[i].beamforming_entry_cap;
+			i = BEAMFORMING_ENTRY_NUM;
+		}
+	}
+
+	return BeamformEntryCap;
+}
+
+struct beamforming_entry	*beamforming_get_free_entry(struct mlme_priv *pmlmepriv, u8 *idx)
+{
+	u8	i = 0;
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
+
+	for (i = 0; i < BEAMFORMING_ENTRY_NUM; i++) {
+		if (pBeamInfo->beamforming_entry[i].bUsed == false) {
+			*idx = i;
+			return &(pBeamInfo->beamforming_entry[i]);
+		}
+	}
+	return NULL;
+}
+
+
+struct beamforming_entry	*beamforming_add_entry(PADAPTER adapter, u8 *ra, u16 aid,
+	u16 mac_id, CHANNEL_WIDTH bw, BEAMFORMING_CAP beamfrom_cap, u8 *idx)
+{
+	struct mlme_priv			*pmlmepriv = &(adapter->mlmepriv);
+	struct beamforming_entry	*pEntry = beamforming_get_free_entry(pmlmepriv, idx);
+
+	if (pEntry != NULL) {
+		pEntry->bUsed = true;
+		pEntry->aid = aid;
+		pEntry->mac_id = mac_id;
+		pEntry->sound_bw = bw;
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+			u16	BSSID = ((*(adapter_mac_addr(adapter) + 5) & 0xf0) >> 4) ^
+				(*(adapter_mac_addr(adapter) + 5) & 0xf); /* BSSID[44:47] xor BSSID[40:43] */
+			pEntry->p_aid = (aid + BSSID * 32) & 0x1ff;		/* (dec(A) + dec(B)*32) mod 512 */
+			pEntry->g_id = 63;
+		} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+			pEntry->p_aid = 0;
+			pEntry->g_id = 63;
+		} else {
+			pEntry->p_aid =  ra[5];						/* BSSID[39:47] */
+			pEntry->p_aid = (pEntry->p_aid << 1) | (ra[4] >> 7);
+			pEntry->g_id = 0;
+		}
+		memcpy(pEntry->mac_addr, ra, ETH_ALEN);
+		pEntry->bSound = false;
+
+		/* 3 TODO SW/FW sound period */
+		pEntry->sound_period = 200;
+		pEntry->beamforming_entry_cap = beamfrom_cap;
+		pEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_UNINITIALIZE;
+
+
+		pEntry->PreLogSeq = 0;	/*Modified by Jeffery @2015-04-13*/
+		pEntry->LogSeq = 0;		/*Modified by Jeffery @2014-10-29*/
+		pEntry->LogRetryCnt = 0;	/*Modified by Jeffery @2014-10-29*/
+		pEntry->LogSuccess = 0;	/*LogSuccess is NOT needed to be accumulated, so  LogSuccessCnt->LogSuccess, 2015-04-13, Jeffery*/
+		pEntry->ClockResetTimes = 0;	/*Modified by Jeffery @2015-04-13*/
+		pEntry->LogStatusFailCnt = 0;
+
+		return pEntry;
+	} else
+		return NULL;
+}
+
+bool	beamforming_remove_entry(struct mlme_priv *pmlmepriv, u8 *ra, u8 *idx)
+{
+	struct beamforming_entry	*pEntry = beamforming_get_entry_by_addr(pmlmepriv, ra, idx);
+
+	if (pEntry != NULL) {
+		pEntry->bUsed = false;
+		pEntry->beamforming_entry_cap = BEAMFORMING_CAP_NONE;
+		pEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_UNINITIALIZE;
+		return true;
+	} else
+		return false;
+}
+
+/* Used for BeamformingStart_V1 */
+void	beamforming_dym_ndpa_rate(PADAPTER adapter)
+{
+	u16	NDPARate = MGN_6M;
+	PHAL_DATA_TYPE	pHalData = GET_HAL_DATA(adapter);
+
+	if (pHalData->min_undecorated_pwdb_for_dm > 30) /* link RSSI > 30% */
+		NDPARate = MGN_24M;
+	else
+		NDPARate = MGN_6M;
+
+	/* BW = CHANNEL_WIDTH_20; */
+	NDPARate = NDPARate << 8;
+	rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_RATE, (u8 *)&NDPARate);
+}
+
+void beamforming_dym_period(PADAPTER Adapter)
+{
+	u8	Idx;
+	bool	bChangePeriod = false;
+	u16	SoundPeriod_SW, SoundPeriod_FW;
+	PHAL_DATA_TYPE	pHalData = GET_HAL_DATA(Adapter);
+	struct dvobj_priv	*pdvobjpriv = adapter_to_dvobj(Adapter);
+	struct beamforming_entry	*pBeamformEntry;
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO((&Adapter->mlmepriv));
+	struct sounding_info		*pSoundInfo = &(pBeamInfo->sounding_info);
+
+	/* 3 TODO  per-client throughput caculation. */
+
+	if (pdvobjpriv->traffic_stat.cur_tx_tp + pdvobjpriv->traffic_stat.cur_rx_tp > 2) {
+		SoundPeriod_SW = 32 * 20;
+		SoundPeriod_FW = 2;
+	} else {
+		SoundPeriod_SW = 32 * 2000;
+		SoundPeriod_FW = 200;
+	}
+
+	for (Idx = 0; Idx < BEAMFORMING_ENTRY_NUM; Idx++) {
+		pBeamformEntry = pBeamInfo->beamforming_entry + Idx;
+		if (pBeamformEntry->bDefaultCSI) {
+			SoundPeriod_SW = 32 * 2000;
+			SoundPeriod_FW = 200;
+		}
+
+		if (pBeamformEntry->beamforming_entry_cap & (BEAMFORMER_CAP_HT_EXPLICIT | BEAMFORMER_CAP_VHT_SU)) {
+			if (pSoundInfo->sound_mode == SOUNDING_FW_VHT_TIMER || pSoundInfo->sound_mode == SOUNDING_FW_HT_TIMER) {
+				if (pBeamformEntry->sound_period != SoundPeriod_FW) {
+					pBeamformEntry->sound_period = SoundPeriod_FW;
+					bChangePeriod = true;	/* Only FW sounding need to send H2C packet to change sound period. */
+				}
+			} else if (pBeamformEntry->sound_period != SoundPeriod_SW)
+				pBeamformEntry->sound_period = SoundPeriod_SW;
+		}
+	}
+
+	if (bChangePeriod)
+		rtw_hal_set_hwreg(Adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&Idx);
+}
+
+bool	issue_ht_sw_ndpa_packet(PADAPTER Adapter, u8 *ra, CHANNEL_WIDTH bw, u8 qidx)
+{
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	struct xmit_priv		*pxmitpriv = &(Adapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8	ActionHdr[4] = {ACT_CAT_VENDOR, 0x00, 0xe0, 0x4c};
+	u8	*pframe;
+	u16	*fctrl;
+	u16	duration = 0;
+	u8	aSifsTime = 0;
+	u8	NDPTxRate = 0;
+
+	RTW_INFO("%s: issue_ht_sw_ndpa_packet!\n", __func__);
+
+	NDPTxRate = MGN_MCS8;
+	RTW_INFO("%s: NDPTxRate =%d\n", __func__, NDPTxRate);
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+
+	if (pmgntframe == NULL)
+		return false;
+
+	/*update attribute*/
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(Adapter, pattrib);
+	pattrib->qsel = QSLT_MGNT;
+	pattrib->rate = NDPTxRate;
+	pattrib->bwmode = bw;
+	pattrib->order = 1;
+	pattrib->subtype = WIFI_ACTION_NOACK;
+
+	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;
+
+	set_order_bit(pframe);
+	set_frame_sub_type(pframe, WIFI_ACTION_NOACK);
+
+	memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	if (pmlmeext->cur_wireless_mode == WIRELESS_11B)
+		aSifsTime = 10;
+	else
+		aSifsTime = 16;
+
+	duration = 2 * aSifsTime + 40;
+
+	if (bw == CHANNEL_WIDTH_40)
+		duration += 87;
+	else
+		duration += 180;
+
+	set_duration(pframe, duration);
+
+	/*HT control field*/
+	SET_HT_CTRL_CSI_STEERING(pframe + 24, 3);
+	SET_HT_CTRL_NDP_ANNOUNCEMENT(pframe + 24, 1);
+
+	memcpy(pframe + 28, ActionHdr, 4);
+
+	pattrib->pktlen = 32;
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(Adapter, pmgntframe);
+
+	return true;
+
+
+}
+bool	issue_ht_ndpa_packet(PADAPTER Adapter, u8 *ra, CHANNEL_WIDTH bw, u8 qidx)
+{
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	struct xmit_priv		*pxmitpriv = &(Adapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8	ActionHdr[4] = {ACT_CAT_VENDOR, 0x00, 0xe0, 0x4c};
+	u8	*pframe;
+	u16	*fctrl;
+	u16	duration = 0;
+	u8	aSifsTime = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+
+	if (pmgntframe == NULL)
+		return false;
+
+	/*update attribute*/
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(Adapter, pattrib);
+
+	if (qidx == BCN_QUEUE_INX)
+		pattrib->qsel = QSLT_BEACON;
+	pattrib->rate = MGN_MCS8;
+	pattrib->bwmode = bw;
+	pattrib->order = 1;
+	pattrib->subtype = WIFI_ACTION_NOACK;
+
+	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;
+
+	set_order_bit(pframe);
+	set_frame_sub_type(pframe, WIFI_ACTION_NOACK);
+
+	memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	if (pmlmeext->cur_wireless_mode == WIRELESS_11B)
+		aSifsTime = 10;
+	else
+		aSifsTime = 16;
+
+	duration = 2 * aSifsTime + 40;
+
+	if (bw == CHANNEL_WIDTH_40)
+		duration += 87;
+	else
+		duration += 180;
+
+	set_duration(pframe, duration);
+
+	/* HT control field */
+	SET_HT_CTRL_CSI_STEERING(pframe + 24, 3);
+	SET_HT_CTRL_NDP_ANNOUNCEMENT(pframe + 24, 1);
+
+	memcpy(pframe + 28, ActionHdr, 4);
+
+	pattrib->pktlen = 32;
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(Adapter, pmgntframe);
+
+	return true;
+}
+
+bool	beamforming_send_ht_ndpa_packet(PADAPTER Adapter, u8 *ra, CHANNEL_WIDTH bw, u8 qidx)
+{
+	return issue_ht_ndpa_packet(Adapter, ra, bw, qidx);
+}
+bool	issue_vht_sw_ndpa_packet(PADAPTER Adapter, u8 *ra, u16 aid, CHANNEL_WIDTH bw, u8 qidx)
+{
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	struct xmit_priv		*pxmitpriv = &(Adapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv		*pmlmepriv = &(Adapter->mlmepriv);
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
+	struct rtw_ndpa_sta_info	sta_info;
+	u8		 NDPTxRate = 0;
+
+	u8	*pframe;
+	u16	*fctrl;
+	u16	duration = 0;
+	u8	sequence = 0, aSifsTime = 0;
+
+	RTW_INFO("%s: issue_vht_sw_ndpa_packet!\n", __func__);
+
+
+	NDPTxRate = MGN_VHT2SS_MCS0;
+	RTW_INFO("%s: NDPTxRate =%d\n", __func__, NDPTxRate);
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+
+	if (pmgntframe == NULL) {
+		RTW_INFO("%s, alloc mgnt frame fail\n", __func__);
+		return false;
+	}
+
+	/*update attribute*/
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(Adapter, pattrib);
+	pattrib->qsel = QSLT_MGNT;
+	pattrib->rate = NDPTxRate;
+	pattrib->bwmode = bw;
+	pattrib->subtype = WIFI_NDPA;
+
+	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;
+
+	set_frame_sub_type(pframe, WIFI_NDPA);
+
+	memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);
+
+	if (is_supported_5g(pmlmeext->cur_wireless_mode) || is_supported_ht(pmlmeext->cur_wireless_mode))
+		aSifsTime = 16;
+	else
+		aSifsTime = 10;
+
+	duration = 2 * aSifsTime + 44;
+
+	if (bw == CHANNEL_WIDTH_80)
+		duration += 40;
+	else if (bw == CHANNEL_WIDTH_40)
+		duration += 87;
+	else
+		duration += 180;
+
+	set_duration(pframe, duration);
+
+	sequence = pBeamInfo->sounding_sequence << 2;
+	if (pBeamInfo->sounding_sequence >= 0x3f)
+		pBeamInfo->sounding_sequence = 0;
+	else
+		pBeamInfo->sounding_sequence++;
+
+	memcpy(pframe + 16, &sequence, 1);
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE))
+		aid = 0;
+
+	sta_info.aid = aid;
+	sta_info.feedback_type = 0;
+	sta_info.nc_index = 0;
+
+	memcpy(pframe + 17, (u8 *)&sta_info, 2);
+
+	pattrib->pktlen = 19;
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(Adapter, pmgntframe);
+
+
+	return true;
+
+}
+bool	issue_vht_ndpa_packet(PADAPTER Adapter, u8 *ra, u16 aid, CHANNEL_WIDTH bw, u8 qidx)
+{
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	struct xmit_priv		*pxmitpriv = &(Adapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv		*pmlmepriv = &(Adapter->mlmepriv);
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
+	struct rtw_ndpa_sta_info	sta_info;
+	u8	*pframe;
+	u16	*fctrl;
+	u16	duration = 0;
+	u8	sequence = 0, aSifsTime = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return false;
+
+	/*update attribute*/
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(Adapter, pattrib);
+
+	if (qidx == BCN_QUEUE_INX)
+		pattrib->qsel = QSLT_BEACON;
+	pattrib->rate = MGN_VHT2SS_MCS0;
+	pattrib->bwmode = bw;
+	pattrib->subtype = WIFI_NDPA;
+
+	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;
+
+	set_frame_sub_type(pframe, WIFI_NDPA);
+
+	memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);
+
+	if (is_supported_5g(pmlmeext->cur_wireless_mode) || is_supported_ht(pmlmeext->cur_wireless_mode))
+		aSifsTime = 16;
+	else
+		aSifsTime = 10;
+
+	duration = 2 * aSifsTime + 44;
+
+	if (bw == CHANNEL_WIDTH_80)
+		duration += 40;
+	else if (bw == CHANNEL_WIDTH_40)
+		duration += 87;
+	else
+		duration += 180;
+
+	set_duration(pframe, duration);
+
+	sequence = pBeamInfo->sounding_sequence << 2;
+	if (pBeamInfo->sounding_sequence >= 0x3f)
+		pBeamInfo->sounding_sequence = 0;
+	else
+		pBeamInfo->sounding_sequence++;
+
+	memcpy(pframe + 16, &sequence, 1);
+
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE))
+		aid = 0;
+
+	sta_info.aid = aid;
+	sta_info.feedback_type = 0;
+	sta_info.nc_index = 0;
+
+	memcpy(pframe + 17, (u8 *)&sta_info, 2);
+
+	pattrib->pktlen = 19;
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(Adapter, pmgntframe);
+
+	return true;
+}
+
+bool	beamforming_send_vht_ndpa_packet(PADAPTER Adapter, u8 *ra, u16 aid, CHANNEL_WIDTH bw, u8 qidx)
+{
+	return issue_vht_ndpa_packet(Adapter, ra, aid, bw, qidx);
+}
+
+bool	beamfomring_bSounding(struct beamforming_info *pBeamInfo)
+{
+	bool		bSounding = false;
+
+	if ((beamforming_get_beamform_cap(pBeamInfo) & BEAMFORMER_CAP) == 0)
+		bSounding = false;
+	else
+		bSounding = true;
+
+	return bSounding;
+}
+
+u8	beamforming_sounding_idx(struct beamforming_info *pBeamInfo)
+{
+	u8	idx = 0;
+	u8	i;
+
+	for (i = 0; i < BEAMFORMING_ENTRY_NUM; i++) {
+		if (pBeamInfo->beamforming_entry[i].bUsed &&
+		    (false == pBeamInfo->beamforming_entry[i].bSound)) {
+			idx = i;
+			break;
+		}
+	}
+
+	return idx;
+}
+
+SOUNDING_MODE	beamforming_sounding_mode(struct beamforming_info *pBeamInfo, u8 idx)
+{
+	struct beamforming_entry	BeamEntry = pBeamInfo->beamforming_entry[idx];
+	SOUNDING_MODE	mode;
+
+	if (BeamEntry.beamforming_entry_cap & BEAMFORMER_CAP_VHT_SU)
+		mode = SOUNDING_FW_VHT_TIMER;
+	else if (BeamEntry.beamforming_entry_cap & BEAMFORMER_CAP_HT_EXPLICIT)
+		mode = SOUNDING_FW_HT_TIMER;
+	else
+		mode = SOUNDING_STOP_All_TIMER;
+
+	return mode;
+}
+
+u16	beamforming_sounding_time(struct beamforming_info *pBeamInfo, SOUNDING_MODE mode, u8 idx)
+{
+	u16						sounding_time = 0xffff;
+	struct beamforming_entry	BeamEntry = pBeamInfo->beamforming_entry[idx];
+
+	sounding_time = BeamEntry.sound_period;
+
+	return sounding_time;
+}
+
+CHANNEL_WIDTH	beamforming_sounding_bw(struct beamforming_info *pBeamInfo, SOUNDING_MODE mode, u8 idx)
+{
+	CHANNEL_WIDTH				sounding_bw = CHANNEL_WIDTH_20;
+	struct beamforming_entry		BeamEntry = pBeamInfo->beamforming_entry[idx];
+
+	sounding_bw = BeamEntry.sound_bw;
+
+	return sounding_bw;
+}
+
+bool	beamforming_select_beam_entry(struct beamforming_info *pBeamInfo)
+{
+	struct sounding_info		*pSoundInfo = &(pBeamInfo->sounding_info);
+
+	pSoundInfo->sound_idx = beamforming_sounding_idx(pBeamInfo);
+
+	if (pSoundInfo->sound_idx < BEAMFORMING_ENTRY_NUM)
+		pSoundInfo->sound_mode = beamforming_sounding_mode(pBeamInfo, pSoundInfo->sound_idx);
+	else
+		pSoundInfo->sound_mode = SOUNDING_STOP_All_TIMER;
+
+	if (SOUNDING_STOP_All_TIMER == pSoundInfo->sound_mode)
+		return false;
+	else {
+		pSoundInfo->sound_bw = beamforming_sounding_bw(pBeamInfo, pSoundInfo->sound_mode, pSoundInfo->sound_idx);
+		pSoundInfo->sound_period = beamforming_sounding_time(pBeamInfo, pSoundInfo->sound_mode, pSoundInfo->sound_idx);
+		return true;
+	}
+}
+
+bool	beamforming_start_fw(PADAPTER adapter, u8 idx)
+{
+	u8						*RA = NULL;
+	struct beamforming_entry	*pEntry;
+	bool					ret = true;
+	struct mlme_priv			*pmlmepriv = &(adapter->mlmepriv);
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
+
+	pEntry = &(pBeamInfo->beamforming_entry[idx]);
+	if (pEntry->bUsed == false) {
+		RTW_INFO("Skip Beamforming, no entry for Idx =%d\n", idx);
+		return false;
+	}
+
+	pEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_PROGRESSING;
+	pEntry->bSound = true;
+	rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&idx);
+
+	return true;
+}
+
+void	beamforming_end_fw(PADAPTER adapter)
+{
+	u8	idx = 0;
+
+	rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&idx);
+
+	RTW_INFO("%s\n", __func__);
+}
+
+bool	beamforming_start_period(PADAPTER adapter)
+{
+	bool	ret = true;
+	struct mlme_priv			*pmlmepriv = &(adapter->mlmepriv);
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
+	struct sounding_info		*pSoundInfo = &(pBeamInfo->sounding_info);
+
+	beamforming_dym_ndpa_rate(adapter);
+
+	beamforming_select_beam_entry(pBeamInfo);
+
+	if (pSoundInfo->sound_mode == SOUNDING_FW_VHT_TIMER || pSoundInfo->sound_mode == SOUNDING_FW_HT_TIMER)
+		ret = beamforming_start_fw(adapter, pSoundInfo->sound_idx);
+	else
+		ret = false;
+
+	RTW_INFO("%s Idx %d Mode %d BW %d Period %d\n", __func__,
+		pSoundInfo->sound_idx, pSoundInfo->sound_mode, pSoundInfo->sound_bw, pSoundInfo->sound_period);
+
+	return ret;
+}
+
+void	beamforming_end_period(PADAPTER adapter)
+{
+	u8						idx = 0;
+	struct beamforming_entry	*pBeamformEntry;
+	struct mlme_priv			*pmlmepriv = &(adapter->mlmepriv);
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
+	struct sounding_info		*pSoundInfo = &(pBeamInfo->sounding_info);
+
+
+	if (pSoundInfo->sound_mode == SOUNDING_FW_VHT_TIMER || pSoundInfo->sound_mode == SOUNDING_FW_HT_TIMER)
+		beamforming_end_fw(adapter);
+}
+
+void	beamforming_notify(PADAPTER adapter)
+{
+	bool		bSounding = false;
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO(&(adapter->mlmepriv));
+
+	bSounding = beamfomring_bSounding(pBeamInfo);
+
+	if (pBeamInfo->beamforming_state == BEAMFORMING_STATE_IDLE) {
+		if (bSounding) {
+			if (beamforming_start_period(adapter) == true)
+				pBeamInfo->beamforming_state = BEAMFORMING_STATE_START;
+		}
+	} else if (pBeamInfo->beamforming_state == BEAMFORMING_STATE_START) {
+		if (bSounding) {
+			if (beamforming_start_period(adapter) == false)
+				pBeamInfo->beamforming_state = BEAMFORMING_STATE_END;
+		} else {
+			beamforming_end_period(adapter);
+			pBeamInfo->beamforming_state = BEAMFORMING_STATE_END;
+		}
+	} else if (pBeamInfo->beamforming_state == BEAMFORMING_STATE_END) {
+		if (bSounding) {
+			if (beamforming_start_period(adapter) == true)
+				pBeamInfo->beamforming_state = BEAMFORMING_STATE_START;
+		}
+	} else
+		RTW_INFO("%s BeamformState %d\n", __func__, pBeamInfo->beamforming_state);
+
+	RTW_INFO("%s BeamformState %d bSounding %d\n", __func__, pBeamInfo->beamforming_state, bSounding);
+}
+
+bool	beamforming_init_entry(PADAPTER	adapter, struct sta_info *psta, u8 *idx)
+{
+	struct mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+	struct ht_priv		*phtpriv = &(pmlmepriv->htpriv);
+	struct mlme_ext_priv	*pmlmeext = &(adapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct beamforming_entry	*pBeamformEntry = NULL;
+	u8	*ra;
+	u16	aid, mac_id;
+	u8	wireless_mode;
+	CHANNEL_WIDTH	bw = CHANNEL_WIDTH_20;
+	BEAMFORMING_CAP	beamform_cap = BEAMFORMING_CAP_NONE;
+
+	/* The current setting does not support Beaforming */
+	if (0 == phtpriv->beamform_cap) {
+		RTW_INFO("The configuration disabled Beamforming! Skip...\n");
+		return false;
+	}
+
+	aid = psta->aid;
+	ra = psta->hwaddr;
+	mac_id = psta->mac_id;
+	wireless_mode = psta->wireless_mode;
+	bw = psta->bw_mode;
+
+	if (is_supported_ht(wireless_mode) || is_supported_vht(wireless_mode)) {
+		/* 3 */ /* HT */
+		u8	cur_beamform;
+
+		cur_beamform = psta->htpriv.beamform_cap;
+
+		/* We are Beamformee because the STA is Beamformer */
+		if (TEST_FLAG(cur_beamform, BEAMFORMING_HT_BEAMFORMER_ENABLE))
+			beamform_cap = (BEAMFORMING_CAP)(beamform_cap | BEAMFORMEE_CAP_HT_EXPLICIT);
+
+		/* We are Beamformer because the STA is Beamformee */
+		if (TEST_FLAG(cur_beamform, BEAMFORMING_HT_BEAMFORMEE_ENABLE))
+			beamform_cap = (BEAMFORMING_CAP)(beamform_cap | BEAMFORMER_CAP_HT_EXPLICIT);
+
+		if (beamform_cap == BEAMFORMING_CAP_NONE)
+			return false;
+
+		RTW_INFO("Beamforming Config Capability = 0x%02X\n", beamform_cap);
+
+		pBeamformEntry = beamforming_get_entry_by_addr(pmlmepriv, ra, idx);
+		if (pBeamformEntry == NULL) {
+			pBeamformEntry = beamforming_add_entry(adapter, ra, aid, mac_id, bw, beamform_cap, idx);
+			if (pBeamformEntry == NULL)
+				return false;
+			else
+				pBeamformEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_INITIALIZEING;
+		} else {
+			/* Entry has been created. If entry is initialing or progressing then errors occur. */
+			if (pBeamformEntry->beamforming_entry_state != BEAMFORMING_ENTRY_STATE_INITIALIZED &&
+			    pBeamformEntry->beamforming_entry_state != BEAMFORMING_ENTRY_STATE_PROGRESSED) {
+				RTW_INFO("Error State of Beamforming");
+				return false;
+			} else
+				pBeamformEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_INITIALIZEING;
+		}
+
+		pBeamformEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_INITIALIZED;
+		psta->txbf_paid = pBeamformEntry->p_aid;
+		psta->txbf_gid = pBeamformEntry->g_id;
+
+		RTW_INFO("%s Idx %d\n", __func__, *idx);
+	} else
+		return false;
+
+	return _SUCCESS;
+}
+
+void	beamforming_deinit_entry(PADAPTER adapter, u8 *ra)
+{
+	u8	idx = 0;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+	if (beamforming_remove_entry(pmlmepriv, ra, &idx) == true)
+		rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_LEAVE, (u8 *)&idx);
+
+	RTW_INFO("%s Idx %d\n", __func__, idx);
+}
+
+void	beamforming_reset(PADAPTER adapter)
+{
+	u8	idx = 0;
+	struct mlme_priv			*pmlmepriv = &(adapter->mlmepriv);
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
+
+	for (idx = 0; idx < BEAMFORMING_ENTRY_NUM; idx++) {
+		if (pBeamInfo->beamforming_entry[idx].bUsed == true) {
+			pBeamInfo->beamforming_entry[idx].bUsed = false;
+			pBeamInfo->beamforming_entry[idx].beamforming_entry_cap = BEAMFORMING_CAP_NONE;
+			pBeamInfo->beamforming_entry[idx].beamforming_entry_state = BEAMFORMING_ENTRY_STATE_UNINITIALIZE;
+			rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_LEAVE, (u8 *)&idx);
+		}
+	}
+
+	RTW_INFO("%s\n", __func__);
+}
+
+void beamforming_sounding_fail(PADAPTER Adapter)
+{
+	struct mlme_priv			*pmlmepriv = &(Adapter->mlmepriv);
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
+	struct beamforming_entry	*pEntry = &(pBeamInfo->beamforming_entry[pBeamInfo->beamforming_cur_idx]);
+
+	pEntry->bSound = false;
+	rtw_hal_set_hwreg(Adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&pBeamInfo->beamforming_cur_idx);
+	beamforming_deinit_entry(Adapter, pEntry->mac_addr);
+}
+
+void	beamforming_check_sounding_success(PADAPTER Adapter, bool status)
+{
+	struct mlme_priv			*pmlmepriv = &(Adapter->mlmepriv);
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
+	struct beamforming_entry	*pEntry = &(pBeamInfo->beamforming_entry[pBeamInfo->beamforming_cur_idx]);
+
+	if (status == 1)
+		pEntry->LogStatusFailCnt = 0;
+	else {
+		pEntry->LogStatusFailCnt++;
+		RTW_INFO("%s LogStatusFailCnt %d\n", __func__, pEntry->LogStatusFailCnt);
+	}
+	if (pEntry->LogStatusFailCnt > 20) {
+		RTW_INFO("%s LogStatusFailCnt > 20, Stop SOUNDING\n", __func__);
+		/* pEntry->bSound = false; */
+		/* rtw_hal_set_hwreg(Adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&pBeamInfo->beamforming_cur_idx); */
+		/* beamforming_deinit_entry(Adapter, pEntry->mac_addr); */
+		beamforming_wk_cmd(Adapter, BEAMFORMING_CTRL_SOUNDING_FAIL, NULL, 0, 1);
+	}
+}
+
+void	beamforming_enter(PADAPTER adapter, void * psta)
+{
+	u8	idx = 0xff;
+
+	if (beamforming_init_entry(adapter, (struct sta_info *)psta, &idx))
+		rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_ENTER, (u8 *)&idx);
+
+	/* RTW_INFO("%s Idx %d\n", __func__, idx); */
+}
+
+void	beamforming_leave(PADAPTER adapter, u8 *ra)
+{
+	if (ra == NULL)
+		beamforming_reset(adapter);
+	else
+		beamforming_deinit_entry(adapter, ra);
+
+	beamforming_notify(adapter);
+}
+
+BEAMFORMING_CAP beamforming_get_beamform_cap(struct beamforming_info	*pBeamInfo)
+{
+	u8	i;
+	bool				bSelfBeamformer = false;
+	bool				bSelfBeamformee = false;
+	struct beamforming_entry	beamforming_entry;
+	BEAMFORMING_CAP		beamform_cap = BEAMFORMING_CAP_NONE;
+
+	for (i = 0; i < BEAMFORMING_ENTRY_NUM; i++) {
+		beamforming_entry = pBeamInfo->beamforming_entry[i];
+
+		if (beamforming_entry.bUsed) {
+			if ((beamforming_entry.beamforming_entry_cap & BEAMFORMEE_CAP_VHT_SU) ||
+			    (beamforming_entry.beamforming_entry_cap & BEAMFORMEE_CAP_HT_EXPLICIT))
+				bSelfBeamformee = true;
+			if ((beamforming_entry.beamforming_entry_cap & BEAMFORMER_CAP_VHT_SU) ||
+			    (beamforming_entry.beamforming_entry_cap & BEAMFORMER_CAP_HT_EXPLICIT))
+				bSelfBeamformer = true;
+		}
+
+		if (bSelfBeamformer && bSelfBeamformee)
+			i = BEAMFORMING_ENTRY_NUM;
+	}
+
+	if (bSelfBeamformer)
+		beamform_cap |= BEAMFORMER_CAP;
+	if (bSelfBeamformee)
+		beamform_cap |= BEAMFORMEE_CAP;
+
+	return beamform_cap;
+}
+
+void	beamforming_watchdog(PADAPTER Adapter)
+{
+	struct beamforming_info	*pBeamInfo = GET_BEAMFORM_INFO((&(Adapter->mlmepriv)));
+
+	if (pBeamInfo->beamforming_state != BEAMFORMING_STATE_START)
+		return;
+
+	beamforming_dym_period(Adapter);
+	beamforming_dym_ndpa_rate(Adapter);
+}
+#endif/* #if (BEAMFORMING_SUPPORT ==0) - for diver defined beamforming*/
+
+u32	rtw_beamforming_get_report_frame(PADAPTER	 Adapter, union recv_frame *precv_frame)
+{
+	u32	ret = _SUCCESS;
+#if (BEAMFORMING_SUPPORT == 1)
+	PHAL_DATA_TYPE	pHalData = GET_HAL_DATA(Adapter);
+	struct PHY_DM_STRUCT		*pDM_Odm = &(pHalData->odmpriv);
+
+	ret = beamforming_get_report_frame(pDM_Odm, precv_frame);
+
+#else /*(BEAMFORMING_SUPPORT == 0)- for drv beamfoming*/
+	struct beamforming_entry	*pBeamformEntry = NULL;
+	struct mlme_priv			*pmlmepriv = &(Adapter->mlmepriv);
+	u8	*pframe = precv_frame->u.hdr.rx_data;
+	u32	frame_len = precv_frame->u.hdr.len;
+	u8	*ta;
+	u8	idx, offset;
+
+	/*RTW_INFO("rtw_beamforming_get_report_frame\n");*/
+
+	/*Memory comparison to see if CSI report is the same with previous one*/
+	ta = get_addr2_ptr(pframe);
+	pBeamformEntry = beamforming_get_entry_by_addr(pmlmepriv, ta, &idx);
+	if (pBeamformEntry->beamforming_entry_cap & BEAMFORMER_CAP_VHT_SU)
+		offset = 31;	/*24+(1+1+3)+2  MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2)*/
+	else if (pBeamformEntry->beamforming_entry_cap & BEAMFORMER_CAP_HT_EXPLICIT)
+		offset = 34;	/*24+(1+1+6)+2  MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2)*/
+	else
+		return ret;
+
+	/*RTW_INFO("%s MacId %d offset=%d\n", __func__, pBeamformEntry->mac_id, offset);*/
+
+	if (!memcmp(pBeamformEntry->PreCsiReport + offset, pframe + offset, frame_len - offset) == false)
+		pBeamformEntry->DefaultCsiCnt = 0;
+	else
+		pBeamformEntry->DefaultCsiCnt++;
+
+	memcpy(&pBeamformEntry->PreCsiReport, pframe, frame_len);
+
+	pBeamformEntry->bDefaultCSI = false;
+
+	if (pBeamformEntry->DefaultCsiCnt > 20)
+		pBeamformEntry->bDefaultCSI = true;
+	else
+		pBeamformEntry->bDefaultCSI = false;
+#endif
+	return ret;
+}
+
+void	rtw_beamforming_get_ndpa_frame(PADAPTER	 Adapter, union recv_frame *precv_frame)
+{
+#if (BEAMFORMING_SUPPORT == 1)
+	PHAL_DATA_TYPE	pHalData = GET_HAL_DATA(Adapter);
+	struct PHY_DM_STRUCT		*pDM_Odm = &(pHalData->odmpriv);
+
+	beamforming_get_ndpa_frame(pDM_Odm, precv_frame);
+
+#else /*(BEAMFORMING_SUPPORT == 0)- for drv beamfoming*/
+	u8	*ta;
+	u8	idx, Sequence;
+	u8	*pframe = precv_frame->u.hdr.rx_data;
+	struct mlme_priv			*pmlmepriv = &(Adapter->mlmepriv);
+	struct beamforming_entry	*pBeamformEntry = NULL;
+
+	/*RTW_INFO("rtw_beamforming_get_ndpa_frame\n");*/
+
+	if (IS_HARDWARE_TYPE_8812(Adapter) == false)
+		return;
+	else if (get_frame_sub_type(pframe) != WIFI_NDPA)
+		return;
+
+	ta = get_addr2_ptr(pframe);
+	/*Remove signaling TA. */
+	ta[0] = ta[0] & 0xFE;
+
+	pBeamformEntry = beamforming_get_entry_by_addr(pmlmepriv, ta, &idx);
+
+	if (pBeamformEntry == NULL)
+		return;
+	else if (!(pBeamformEntry->beamforming_entry_cap & BEAMFORMEE_CAP_VHT_SU))
+		return;
+	/*LogSuccess: As long as 8812A receive NDPA and feedback CSI succeed once, clock reset is NO LONGER needed !2015-04-10, Jeffery*/
+	/*ClockResetTimes: While BFer entry always doesn't receive our CSI, clock will reset again and again.So ClockResetTimes is limited to 5 times.2015-04-13, Jeffery*/
+	else if ((pBeamformEntry->LogSuccess == 1) || (pBeamformEntry->ClockResetTimes == 5)) {
+		RTW_INFO("[%s] LogSeq=%d, PreLogSeq=%d\n", __func__, pBeamformEntry->LogSeq, pBeamformEntry->PreLogSeq);
+		return;
+	}
+
+	Sequence = (pframe[16]) >> 2;
+	RTW_INFO("[%s] Start, Sequence=%d, LogSeq=%d, PreLogSeq=%d, LogRetryCnt=%d, ClockResetTimes=%d, LogSuccess=%d\n",
+		__func__, Sequence, pBeamformEntry->LogSeq, pBeamformEntry->PreLogSeq, pBeamformEntry->LogRetryCnt, pBeamformEntry->ClockResetTimes, pBeamformEntry->LogSuccess);
+
+	if ((pBeamformEntry->LogSeq != 0) && (pBeamformEntry->PreLogSeq != 0)) {
+		/*Success condition*/
+		if ((pBeamformEntry->LogSeq != Sequence) && (pBeamformEntry->PreLogSeq != pBeamformEntry->LogSeq)) {
+			/* break option for clcok reset, 2015-03-30, Jeffery */
+			pBeamformEntry->LogRetryCnt = 0;
+			/*As long as 8812A receive NDPA and feedback CSI succeed once, clock reset is no longer needed.*/
+			/*That is, LogSuccess is NOT needed to be reset to zero, 2015-04-13, Jeffery*/
+			pBeamformEntry->LogSuccess = 1;
+
+		} else {/*Fail condition*/
+
+			if (pBeamformEntry->LogRetryCnt == 5) {
+				pBeamformEntry->ClockResetTimes++;
+				pBeamformEntry->LogRetryCnt = 0;
+
+				RTW_INFO("[%s] Clock Reset!!! ClockResetTimes=%d\n",  __func__, pBeamformEntry->ClockResetTimes);
+				beamforming_wk_cmd(Adapter, BEAMFORMING_CTRL_SOUNDING_CLK, NULL, 0, 1);
+
+			} else
+				pBeamformEntry->LogRetryCnt++;
+		}
+	}
+
+	/*Update LogSeq & PreLogSeq*/
+	pBeamformEntry->PreLogSeq = pBeamformEntry->LogSeq;
+	pBeamformEntry->LogSeq = Sequence;
+
+#endif
+
+}
+
+
+
+
+void	beamforming_wk_hdl(_adapter *padapter, u8 type, u8 *pbuf)
+{
+	PHAL_DATA_TYPE	pHalData = GET_HAL_DATA(padapter);
+	struct PHY_DM_STRUCT		*pDM_Odm = &(pHalData->odmpriv);
+
+#if (BEAMFORMING_SUPPORT == 1) /*(BEAMFORMING_SUPPORT == 1)- for PHYDM beamfoming*/
+	switch (type) {
+	case BEAMFORMING_CTRL_ENTER: {
+		struct sta_info	*psta = (void *)pbuf;
+		u16			staIdx = psta->mac_id;
+
+		beamforming_enter(pDM_Odm, staIdx);
+		break;
+	}
+	case BEAMFORMING_CTRL_LEAVE:
+		beamforming_leave(pDM_Odm, pbuf);
+		break;
+	default:
+		break;
+
+	}
+#else /*(BEAMFORMING_SUPPORT == 0)- for drv beamfoming*/
+	switch (type) {
+	case BEAMFORMING_CTRL_ENTER:
+		beamforming_enter(padapter, (void *)pbuf);
+		break;
+
+	case BEAMFORMING_CTRL_LEAVE:
+		beamforming_leave(padapter, pbuf);
+		break;
+
+	case BEAMFORMING_CTRL_SOUNDING_FAIL:
+		beamforming_sounding_fail(padapter);
+		break;
+
+	case BEAMFORMING_CTRL_SOUNDING_CLK:
+		rtw_hal_set_hwreg(padapter, HW_VAR_SOUNDING_CLK, NULL);
+		break;
+
+	default:
+		break;
+	}
+#endif
+}
+
+u8	beamforming_wk_cmd(_adapter *padapter, s32 type, u8 *pbuf, s32 size, u8 enqueue)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+
+	if (enqueue) {
+		u8	*wk_buf;
+
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+		if (pdrvextra_cmd_parm == NULL) {
+			rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+			res = _FAIL;
+			goto exit;
+		}
+
+		if (pbuf != NULL) {
+			wk_buf = rtw_zmalloc(size);
+			if (wk_buf == NULL) {
+				rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
+				rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm));
+				res = _FAIL;
+				goto exit;
+			}
+
+			memcpy(wk_buf, pbuf, size);
+		} else {
+			wk_buf = NULL;
+			size = 0;
+		}
+
+		pdrvextra_cmd_parm->ec_id = BEAMFORMING_WK_CID;
+		pdrvextra_cmd_parm->type = type;
+		pdrvextra_cmd_parm->size = size;
+		pdrvextra_cmd_parm->pbuf = wk_buf;
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else
+		beamforming_wk_hdl(padapter, type, pbuf);
+
+exit:
+
+
+	return res;
+}
+
+void update_attrib_txbf_info(_adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)
+{
+	if (psta) {
+		pattrib->txbf_g_id = psta->txbf_gid;
+		pattrib->txbf_p_aid = psta->txbf_paid;
+	}
+}
+#endif /* !RTW_BEAMFORMING_VERSION_2 */
+
+#endif /* CONFIG_BEAMFORMING */
diff --git a/drivers/staging/rtl8188eu/core/rtw_br_ext.c b/drivers/staging/rtl8188eu/core/rtw_br_ext.c
new file mode 100644
index 000000000000..9cd8d3ba37f2
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_br_ext.c
@@ -0,0 +1,1529 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_BR_EXT_C_
+
+#ifdef __KERNEL__
+	#include <linux/if_arp.h>
+	#include <net/ip.h>
+	#include <net/ipx.h>
+	#include <linux/atalk.h>
+	#include <linux/udp.h>
+	#include <linux/if_pppox.h>
+#endif
+
+#include <drv_types.h>
+#include <usb_osintf.h>
+
+#ifdef CL_IPV6_PASS
+	#ifdef __KERNEL__
+		#include <linux/ipv6.h>
+		#include <linux/icmpv6.h>
+		#include <net/ndisc.h>
+		#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24))
+			#include <net/ip6_checksum.h>
+		#else
+			#include <net/checksum.h>
+		#endif
+	#endif
+#endif
+
+#ifdef CONFIG_BR_EXT
+#include <hal_data.h>
+
+/* #define BR_EXT_DEBUG */
+
+#define NAT25_IPV4		01
+#define NAT25_IPV6		02
+#define NAT25_IPX		03
+#define NAT25_APPLE		04
+#define NAT25_PPPOE		05
+
+#define RTL_RELAY_TAG_LEN (ETH_ALEN)
+#define TAG_HDR_LEN		4
+
+#define MAGIC_CODE		0x8186
+#define MAGIC_CODE_LEN	2
+#define WAIT_TIME_PPPOE	5	/* waiting time for pppoe server in sec */
+
+/*-----------------------------------------------------------------
+  How database records network address:
+           0    1    2    3    4    5    6    7    8    9   10
+        |----|----|----|----|----|----|----|----|----|----|----|
+  IPv4  |type|                             |      IP addr      |
+  IPX   |type|      Net addr     |          Node addr          |
+  IPX   |type|      Net addr     |Sckt addr|
+  Apple |type| Network |node|
+  PPPoE |type|   SID   |           AC MAC            |
+-----------------------------------------------------------------*/
+
+
+/* Find a tag in pppoe frame and return the pointer */
+static __inline__ unsigned char *__nat25_find_pppoe_tag(struct pppoe_hdr *ph, unsigned short type)
+{
+	unsigned char *cur_ptr, *start_ptr;
+	unsigned short tagLen, tagType;
+
+	start_ptr = cur_ptr = (unsigned char *)ph->tag;
+	while ((cur_ptr - start_ptr) < ntohs(ph->length)) {
+		/* prevent un-alignment access */
+		tagType = (unsigned short)((cur_ptr[0] << 8) + cur_ptr[1]);
+		tagLen  = (unsigned short)((cur_ptr[2] << 8) + cur_ptr[3]);
+		if (tagType == type)
+			return cur_ptr;
+		cur_ptr = cur_ptr + TAG_HDR_LEN + tagLen;
+	}
+	return NULL;
+}
+
+static __inline__ int __nat25_add_pppoe_tag(struct sk_buff *skb, struct pppoe_tag *tag)
+{
+	struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
+	int data_len;
+
+	data_len = be16_to_cpu(tag->tag_len) + TAG_HDR_LEN;
+	if (skb_tailroom(skb) < data_len) {
+		_DEBUG_ERR("skb_tailroom() failed in add SID tag!\n");
+		return -1;
+	}
+
+	skb_put(skb, data_len);
+	/* have a room for new tag */
+	memmove(((unsigned char *)ph->tag + data_len), (unsigned char *)ph->tag, ntohs(ph->length));
+	ph->length = htons(ntohs(ph->length) + data_len);
+	memcpy((unsigned char *)ph->tag, tag, data_len);
+	return data_len;
+}
+
+static int skb_pull_and_merge(struct sk_buff *skb, unsigned char *src, int len)
+{
+	int tail_len;
+	unsigned long end, tail;
+
+	if ((src + len) > skb_tail_pointer(skb) || skb->len < len)
+		return -1;
+
+	tail = (unsigned long)skb_tail_pointer(skb);
+	end = (unsigned long)src + len;
+	if (tail < end)
+		return -1;
+
+	tail_len = (int)(tail - end);
+	if (tail_len > 0)
+		memmove(src, src + len, tail_len);
+
+	skb_trim(skb, skb->len - len);
+	return 0;
+}
+
+static __inline__ unsigned long __nat25_timeout(_adapter *priv)
+{
+	unsigned long timeout;
+
+	timeout = jiffies - NAT25_AGEING_TIME * HZ;
+
+	return timeout;
+}
+
+
+static __inline__ int  __nat25_has_expired(_adapter *priv,
+		struct nat25_network_db_entry *fdb)
+{
+	if (time_before_eq(fdb->ageing_timer, __nat25_timeout(priv)))
+		return 1;
+
+	return 0;
+}
+
+
+static __inline__ void __nat25_generate_ipv4_network_addr(unsigned char *networkAddr,
+		u32 *ipAddr)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_IPV4;
+	memcpy(networkAddr + 7, (unsigned char *)ipAddr, 4);
+}
+
+
+static __inline__ void __nat25_generate_ipx_network_addr_with_node(unsigned char *networkAddr,
+		__be32 *ipxNetAddr, unsigned char *ipxNodeAddr)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_IPX;
+	memcpy(networkAddr + 1, (unsigned char *)ipxNetAddr, 4);
+	memcpy(networkAddr + 5, ipxNodeAddr, 6);
+}
+
+
+static __inline__ void __nat25_generate_ipx_network_addr_with_socket(unsigned char *networkAddr,
+		__be32 *ipxNetAddr, __be16 *ipxSocketAddr)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_IPX;
+	memcpy(networkAddr + 1, (unsigned char *)ipxNetAddr, 4);
+	memcpy(networkAddr + 5, (unsigned char *)ipxSocketAddr, 2);
+}
+
+
+static __inline__ void __nat25_generate_apple_network_addr(unsigned char *networkAddr,
+		__be16 *network, unsigned char *node)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_APPLE;
+	memcpy(networkAddr + 1, (unsigned char *)network, 2);
+	networkAddr[3] = *node;
+}
+
+
+static __inline__ void __nat25_generate_pppoe_network_addr(unsigned char *networkAddr,
+		unsigned char *ac_mac, __be16 *sid)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_PPPOE;
+	memcpy(networkAddr + 1, (unsigned char *)sid, 2);
+	memcpy(networkAddr + 3, (unsigned char *)ac_mac, 6);
+}
+
+
+#ifdef CL_IPV6_PASS
+static  void __nat25_generate_ipv6_network_addr(unsigned char *networkAddr,
+		unsigned int *ipAddr)
+{
+	memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
+
+	networkAddr[0] = NAT25_IPV6;
+	memcpy(networkAddr + 1, (unsigned char *)ipAddr, 16);
+}
+
+
+static unsigned char *scan_tlv(unsigned char *data, int len, unsigned char tag, unsigned char len8b)
+{
+	while (len > 0) {
+		if (*data == tag && *(data + 1) == len8b && len >= len8b * 8)
+			return data + 2;
+
+		len -= (*(data + 1)) * 8;
+		data += (*(data + 1)) * 8;
+	}
+	return NULL;
+}
+
+
+static int update_nd_link_layer_addr(unsigned char *data, int len, unsigned char *replace_mac)
+{
+	struct icmp6hdr *icmphdr = (struct icmp6hdr *)data;
+	unsigned char *mac;
+
+	if (icmphdr->icmp6_type == NDISC_ROUTER_SOLICITATION) {
+		if (len >= 8) {
+			mac = scan_tlv(&data[8], len - 8, 1, 1);
+			if (mac) {
+				RTW_INFO("Router Solicitation, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+					mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
+					replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
+				memcpy(mac, replace_mac, 6);
+				return 1;
+			}
+		}
+	} else if (icmphdr->icmp6_type == NDISC_ROUTER_ADVERTISEMENT) {
+		if (len >= 16) {
+			mac = scan_tlv(&data[16], len - 16, 1, 1);
+			if (mac) {
+				RTW_INFO("Router Advertisement, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+					mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
+					replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
+				memcpy(mac, replace_mac, 6);
+				return 1;
+			}
+		}
+	} else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_SOLICITATION) {
+		if (len >= 24) {
+			mac = scan_tlv(&data[24], len - 24, 1, 1);
+			if (mac) {
+				RTW_INFO("Neighbor Solicitation, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+					mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
+					replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
+				memcpy(mac, replace_mac, 6);
+				return 1;
+			}
+		}
+	} else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_ADVERTISEMENT) {
+		if (len >= 24) {
+			mac = scan_tlv(&data[24], len - 24, 2, 1);
+			if (mac) {
+				RTW_INFO("Neighbor Advertisement, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+					mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
+					replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
+				memcpy(mac, replace_mac, 6);
+				return 1;
+			}
+		}
+	} else if (icmphdr->icmp6_type == NDISC_REDIRECT) {
+		if (len >= 40) {
+			mac = scan_tlv(&data[40], len - 40, 2, 1);
+			if (mac) {
+				RTW_INFO("Redirect,  replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
+					mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
+					replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
+				memcpy(mac, replace_mac, 6);
+				return 1;
+			}
+		}
+	}
+	return 0;
+}
+
+
+static void convert_ipv6_mac_to_mc(struct sk_buff *skb)
+{
+	struct ipv6hdr *iph = (struct ipv6hdr *)(skb->data + ETH_HLEN);
+	unsigned char *dst_mac = skb->data;
+
+	/* dst_mac[0] = 0xff; */
+	/* dst_mac[1] = 0xff; */
+	/*modified by qinjunjie,ipv6 multicast address ix 0x33-33-xx-xx-xx-xx*/
+	dst_mac[0] = 0x33;
+	dst_mac[1] = 0x33;
+	memcpy(&dst_mac[2], &iph->daddr.s6_addr32[3], 4);
+#if defined(__LINUX_2_6__)
+	/*modified by qinjunjie,warning:should not remove next line*/
+	skb->pkt_type = PACKET_MULTICAST;
+#endif
+}
+#endif /* CL_IPV6_PASS */
+
+
+static __inline__ int __nat25_network_hash(unsigned char *networkAddr)
+{
+	if (networkAddr[0] == NAT25_IPV4) {
+		unsigned long x;
+
+		x = networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	} else if (networkAddr[0] == NAT25_IPX) {
+		unsigned long x;
+
+		x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^
+		    networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	} else if (networkAddr[0] == NAT25_APPLE) {
+		unsigned long x;
+
+		x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	} else if (networkAddr[0] == NAT25_PPPOE) {
+		unsigned long x;
+
+		x = networkAddr[0] ^ networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^ networkAddr[6] ^ networkAddr[7] ^ networkAddr[8];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	}
+#ifdef CL_IPV6_PASS
+	else if (networkAddr[0] == NAT25_IPV6) {
+		unsigned long x;
+
+		x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^
+		    networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10] ^
+		    networkAddr[11] ^ networkAddr[12] ^ networkAddr[13] ^ networkAddr[14] ^ networkAddr[15] ^
+		    networkAddr[16];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	}
+#endif
+	else {
+		unsigned long x = 0;
+		int i;
+
+		for (i = 0; i < MAX_NETWORK_ADDR_LEN; i++)
+			x ^= networkAddr[i];
+
+		return x & (NAT25_HASH_SIZE - 1);
+	}
+}
+
+
+static __inline__ void __network_hash_link(_adapter *priv,
+		struct nat25_network_db_entry *ent, int hash)
+{
+	/* Caller must _enter_critical_bh already! */
+	/* unsigned long irqL; */
+	/* _enter_critical_bh(&priv->br_ext_lock, &irqL); */
+
+	ent->next_hash = priv->nethash[hash];
+	if (ent->next_hash != NULL)
+		ent->next_hash->pprev_hash = &ent->next_hash;
+	priv->nethash[hash] = ent;
+	ent->pprev_hash = &priv->nethash[hash];
+
+	/* _exit_critical_bh(&priv->br_ext_lock, &irqL); */
+}
+
+
+static __inline__ void __network_hash_unlink(struct nat25_network_db_entry *ent)
+{
+	/* Caller must _enter_critical_bh already! */
+	/* unsigned long irqL; */
+	/* _enter_critical_bh(&priv->br_ext_lock, &irqL); */
+
+	*(ent->pprev_hash) = ent->next_hash;
+	if (ent->next_hash != NULL)
+		ent->next_hash->pprev_hash = ent->pprev_hash;
+	ent->next_hash = NULL;
+	ent->pprev_hash = NULL;
+
+	/* _exit_critical_bh(&priv->br_ext_lock, &irqL); */
+}
+
+
+static int __nat25_db_network_lookup_and_replace(_adapter *priv,
+		struct sk_buff *skb, unsigned char *networkAddr)
+{
+	struct nat25_network_db_entry *db;
+	unsigned long irqL;
+	_enter_critical_bh(&priv->br_ext_lock, &irqL);
+
+	db = priv->nethash[__nat25_network_hash(networkAddr)];
+	while (db != NULL) {
+		if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
+			if (!__nat25_has_expired(priv, db)) {
+				/* replace the destination mac address */
+				memcpy(skb->data, db->macAddr, ETH_ALEN);
+				atomic_inc(&db->use_count);
+
+#ifdef CL_IPV6_PASS
+				RTW_INFO("NAT25: Lookup M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x"
+					 "%02x%02x%02x%02x%02x%02x\n",
+					 db->macAddr[0],
+					 db->macAddr[1],
+					 db->macAddr[2],
+					 db->macAddr[3],
+					 db->macAddr[4],
+					 db->macAddr[5],
+					 db->networkAddr[0],
+					 db->networkAddr[1],
+					 db->networkAddr[2],
+					 db->networkAddr[3],
+					 db->networkAddr[4],
+					 db->networkAddr[5],
+					 db->networkAddr[6],
+					 db->networkAddr[7],
+					 db->networkAddr[8],
+					 db->networkAddr[9],
+					 db->networkAddr[10],
+					 db->networkAddr[11],
+					 db->networkAddr[12],
+					 db->networkAddr[13],
+					 db->networkAddr[14],
+					 db->networkAddr[15],
+					 db->networkAddr[16]);
+#else
+				RTW_INFO("NAT25: Lookup M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
+					 db->macAddr[0],
+					 db->macAddr[1],
+					 db->macAddr[2],
+					 db->macAddr[3],
+					 db->macAddr[4],
+					 db->macAddr[5],
+					 db->networkAddr[0],
+					 db->networkAddr[1],
+					 db->networkAddr[2],
+					 db->networkAddr[3],
+					 db->networkAddr[4],
+					 db->networkAddr[5],
+					 db->networkAddr[6],
+					 db->networkAddr[7],
+					 db->networkAddr[8],
+					 db->networkAddr[9],
+					 db->networkAddr[10]);
+#endif
+			}
+			_exit_critical_bh(&priv->br_ext_lock, &irqL);
+			return 1;
+		}
+
+		db = db->next_hash;
+	}
+
+	_exit_critical_bh(&priv->br_ext_lock, &irqL);
+	return 0;
+}
+
+
+static void __nat25_db_network_insert(_adapter *priv,
+		      unsigned char *macAddr, unsigned char *networkAddr)
+{
+	struct nat25_network_db_entry *db;
+	int hash;
+	unsigned long irqL;
+	_enter_critical_bh(&priv->br_ext_lock, &irqL);
+
+	hash = __nat25_network_hash(networkAddr);
+	db = priv->nethash[hash];
+	while (db != NULL) {
+		if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
+			memcpy(db->macAddr, macAddr, ETH_ALEN);
+			db->ageing_timer = jiffies;
+			_exit_critical_bh(&priv->br_ext_lock, &irqL);
+			return;
+		}
+
+		db = db->next_hash;
+	}
+
+	db = (struct nat25_network_db_entry *) rtw_malloc(sizeof(*db));
+	if (db == NULL) {
+		_exit_critical_bh(&priv->br_ext_lock, &irqL);
+		return;
+	}
+
+	memcpy(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN);
+	memcpy(db->macAddr, macAddr, ETH_ALEN);
+	atomic_set(&db->use_count, 1);
+	db->ageing_timer = jiffies;
+
+	__network_hash_link(priv, db, hash);
+
+	_exit_critical_bh(&priv->br_ext_lock, &irqL);
+}
+
+
+static void __nat25_db_print(_adapter *priv)
+{
+	unsigned long irqL;
+	_enter_critical_bh(&priv->br_ext_lock, &irqL);
+
+#ifdef BR_EXT_DEBUG
+	static int counter = 0;
+	int i, j;
+	struct nat25_network_db_entry *db;
+
+	counter++;
+	if ((counter % 16) != 0)
+		return;
+
+	for (i = 0, j = 0; i < NAT25_HASH_SIZE; i++) {
+		db = priv->nethash[i];
+
+		while (db != NULL) {
+#ifdef CL_IPV6_PASS
+			panic_RTW_INFO("NAT25: DB(%d) H(%02d) C(%d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x"
+				     "%02x%02x%02x%02x%02x%02x\n",
+				     j,
+				     i,
+				     atomic_read(&db->use_count),
+				     db->macAddr[0],
+				     db->macAddr[1],
+				     db->macAddr[2],
+				     db->macAddr[3],
+				     db->macAddr[4],
+				     db->macAddr[5],
+				     db->networkAddr[0],
+				     db->networkAddr[1],
+				     db->networkAddr[2],
+				     db->networkAddr[3],
+				     db->networkAddr[4],
+				     db->networkAddr[5],
+				     db->networkAddr[6],
+				     db->networkAddr[7],
+				     db->networkAddr[8],
+				     db->networkAddr[9],
+				     db->networkAddr[10],
+				     db->networkAddr[11],
+				     db->networkAddr[12],
+				     db->networkAddr[13],
+				     db->networkAddr[14],
+				     db->networkAddr[15],
+				     db->networkAddr[16]);
+#else
+			panic_RTW_INFO("NAT25: DB(%d) H(%02d) C(%d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
+				     j,
+				     i,
+				     atomic_read(&db->use_count),
+				     db->macAddr[0],
+				     db->macAddr[1],
+				     db->macAddr[2],
+				     db->macAddr[3],
+				     db->macAddr[4],
+				     db->macAddr[5],
+				     db->networkAddr[0],
+				     db->networkAddr[1],
+				     db->networkAddr[2],
+				     db->networkAddr[3],
+				     db->networkAddr[4],
+				     db->networkAddr[5],
+				     db->networkAddr[6],
+				     db->networkAddr[7],
+				     db->networkAddr[8],
+				     db->networkAddr[9],
+				     db->networkAddr[10]);
+#endif
+			j++;
+
+			db = db->next_hash;
+		}
+	}
+#endif
+
+	_exit_critical_bh(&priv->br_ext_lock, &irqL);
+}
+
+
+
+
+/*
+ *	NAT2.5 interface
+ */
+
+void nat25_db_cleanup(_adapter *priv)
+{
+	int i;
+	unsigned long irqL;
+	_enter_critical_bh(&priv->br_ext_lock, &irqL);
+
+	for (i = 0; i < NAT25_HASH_SIZE; i++) {
+		struct nat25_network_db_entry *f;
+		f = priv->nethash[i];
+		while (f != NULL) {
+			struct nat25_network_db_entry *g;
+
+			g = f->next_hash;
+			if (priv->scdb_entry == f) {
+				memset(priv->scdb_mac, 0, ETH_ALEN);
+				memset(priv->scdb_ip, 0, 4);
+				priv->scdb_entry = NULL;
+			}
+			__network_hash_unlink(f);
+			rtw_mfree((u8 *) f, sizeof(struct nat25_network_db_entry));
+
+			f = g;
+		}
+	}
+
+	_exit_critical_bh(&priv->br_ext_lock, &irqL);
+}
+
+
+void nat25_db_expire(_adapter *priv)
+{
+	int i;
+	unsigned long irqL;
+	_enter_critical_bh(&priv->br_ext_lock, &irqL);
+
+	/* if(!priv->ethBrExtInfo.nat25_disable) */
+	{
+		for (i = 0; i < NAT25_HASH_SIZE; i++) {
+			struct nat25_network_db_entry *f;
+			f = priv->nethash[i];
+
+			while (f != NULL) {
+				struct nat25_network_db_entry *g;
+				g = f->next_hash;
+
+				if (__nat25_has_expired(priv, f)) {
+					if (atomic_dec_and_test(&f->use_count)) {
+#ifdef BR_EXT_DEBUG
+#ifdef CL_IPV6_PASS
+						panic_RTW_INFO("NAT25 Expire H(%02d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x"
+							"%02x%02x%02x%02x%02x%02x\n",
+							     i,
+							     f->macAddr[0],
+							     f->macAddr[1],
+							     f->macAddr[2],
+							     f->macAddr[3],
+							     f->macAddr[4],
+							     f->macAddr[5],
+							     f->networkAddr[0],
+							     f->networkAddr[1],
+							     f->networkAddr[2],
+							     f->networkAddr[3],
+							     f->networkAddr[4],
+							     f->networkAddr[5],
+							     f->networkAddr[6],
+							     f->networkAddr[7],
+							     f->networkAddr[8],
+							     f->networkAddr[9],
+							     f->networkAddr[10],
+							     f->networkAddr[11],
+							     f->networkAddr[12],
+							     f->networkAddr[13],
+							     f->networkAddr[14],
+							     f->networkAddr[15],
+							f->networkAddr[16]);
+#else
+
+						panic_RTW_INFO("NAT25 Expire H(%02d) M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
+							     i,
+							     f->macAddr[0],
+							     f->macAddr[1],
+							     f->macAddr[2],
+							     f->macAddr[3],
+							     f->macAddr[4],
+							     f->macAddr[5],
+							     f->networkAddr[0],
+							     f->networkAddr[1],
+							     f->networkAddr[2],
+							     f->networkAddr[3],
+							     f->networkAddr[4],
+							     f->networkAddr[5],
+							     f->networkAddr[6],
+							     f->networkAddr[7],
+							     f->networkAddr[8],
+							     f->networkAddr[9],
+							f->networkAddr[10]);
+#endif
+#endif
+						if (priv->scdb_entry == f) {
+							memset(priv->scdb_mac, 0, ETH_ALEN);
+							memset(priv->scdb_ip, 0, 4);
+							priv->scdb_entry = NULL;
+						}
+						__network_hash_unlink(f);
+						rtw_mfree((u8 *) f, sizeof(struct nat25_network_db_entry));
+					}
+				}
+
+				f = g;
+			}
+		}
+	}
+
+	_exit_critical_bh(&priv->br_ext_lock, &irqL);
+}
+
+
+#ifdef SUPPORT_TX_MCAST2UNI
+static int checkIPMcAndReplace(_adapter *priv, struct sk_buff *skb, unsigned int *dst_ip)
+{
+	struct stat_info	*pstat;
+	struct list_head	*phead, *plist;
+	int i;
+
+	phead = &priv->asoc_list;
+	plist = phead->next;
+
+	while (plist != phead) {
+		pstat = list_entry(plist, struct stat_info, asoc_list);
+		plist = plist->next;
+
+		if (pstat->ipmc_num == 0)
+			continue;
+
+		for (i = 0; i < MAX_IP_MC_ENTRY; i++) {
+			if (pstat->ipmc[i].used && !memcmp(&pstat->ipmc[i].mcmac[3], ((unsigned char *)dst_ip) + 1, 3)) {
+				memcpy(skb->data, pstat->ipmc[i].mcmac, ETH_ALEN);
+				return 1;
+			}
+		}
+	}
+	return 0;
+}
+#endif
+
+int nat25_db_handle(_adapter *priv, struct sk_buff *skb, int method)
+{
+	__be16 protocol;
+	unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
+	unsigned int tmp;
+
+	if (skb == NULL)
+		return -1;
+
+	if ((method <= NAT25_MIN) || (method >= NAT25_MAX))
+		return -1;
+
+	protocol = *((__be16 *)(skb->data + 2 * ETH_ALEN));
+
+	/*---------------------------------------------------*/
+	/*                 Handle IP frame                  */
+	/*---------------------------------------------------*/
+	if (protocol == __constant_htons(ETH_P_IP)) {
+		struct iphdr *iph = (struct iphdr *)(skb->data + ETH_HLEN);
+
+		if (((unsigned char *)(iph) + (iph->ihl << 2)) >= (skb->data + ETH_HLEN + skb->len)) {
+			DEBUG_WARN("NAT25: malformed IP packet !\n");
+			return -1;
+		}
+
+		switch (method) {
+		case NAT25_CHECK:
+			return -1;
+
+		case NAT25_INSERT: {
+			/* some muticast with source IP is all zero, maybe other case is illegal */
+			/* in class A, B, C, host address is all zero or all one is illegal */
+			if (iph->saddr == 0)
+				return 0;
+			tmp = be32_to_cpu(iph->saddr);
+			RTW_INFO("NAT25: Insert IP, SA=%08x, DA=%08x\n", iph->saddr, iph->daddr);
+			__nat25_generate_ipv4_network_addr(networkAddr, &tmp);
+			/* record source IP address and , source mac address into db */
+			__nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr);
+
+			__nat25_db_print(priv);
+		}
+		return 0;
+
+		case NAT25_LOOKUP: {
+			RTW_INFO("NAT25: Lookup IP, SA=%08x, DA=%08x\n", iph->saddr, iph->daddr);
+#ifdef SUPPORT_TX_MCAST2UNI
+			if (priv->pshare->rf_ft_var.mc2u_disable ||
+			    ((((OPMODE & (WIFI_STATION_STATE | WIFI_ASOC_STATE))
+			       == (WIFI_STATION_STATE | WIFI_ASOC_STATE)) &&
+			      !checkIPMcAndReplace(priv, skb, &iph->daddr)) ||
+			     (OPMODE & WIFI_ADHOC_STATE)))
+#endif
+			{
+				tmp = be32_to_cpu(iph->daddr);
+				__nat25_generate_ipv4_network_addr(networkAddr, &tmp);
+
+				if (!__nat25_db_network_lookup_and_replace(priv, skb, networkAddr)) {
+					if (*((unsigned char *)&iph->daddr + 3) == 0xff) {
+						/* L2 is unicast but L3 is broadcast, make L2 bacome broadcast */
+						RTW_INFO("NAT25: Set DA as boardcast\n");
+						memset(skb->data, 0xff, ETH_ALEN);
+					} else {
+						/* forward unknow IP packet to upper TCP/IP */
+						RTW_INFO("NAT25: Replace DA with BR's MAC\n");
+						if ((*(u32 *)priv->br_mac) == 0 && (*(u16 *)(priv->br_mac + 4)) == 0) {
+							RTW_INFO("Re-init netdev_br_init() due to br_mac==0!\n");
+							netdev_br_init(priv->pnetdev);
+						}
+						memcpy(skb->data, priv->br_mac, ETH_ALEN);
+					}
+				}
+			}
+		}
+		return 0;
+
+		default:
+			return -1;
+		}
+	}
+
+	/*---------------------------------------------------*/
+	/*                 Handle ARP frame                 */
+	/*---------------------------------------------------*/
+	else if (protocol == __constant_htons(ETH_P_ARP)) {
+		struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
+		unsigned char *arp_ptr = (unsigned char *)(arp + 1);
+		unsigned int *sender, *target;
+
+		if (arp->ar_pro != __constant_htons(ETH_P_IP)) {
+			DEBUG_WARN("NAT25: arp protocol unknown (%4x)!\n", be16_to_cpu(arp->ar_pro));
+			return -1;
+		}
+
+		switch (method) {
+		case NAT25_CHECK:
+			return 0;	/* skb_copy for all ARP frame */
+
+		case NAT25_INSERT: {
+			RTW_INFO("NAT25: Insert ARP, MAC=%02x%02x%02x%02x%02x%02x\n", arp_ptr[0],
+				arp_ptr[1], arp_ptr[2], arp_ptr[3], arp_ptr[4], arp_ptr[5]);
+
+			/* change to ARP sender mac address to wlan STA address */
+			memcpy(arp_ptr, GET_MY_HWADDR(priv), ETH_ALEN);
+
+			arp_ptr += arp->ar_hln;
+			sender = (unsigned int *)arp_ptr;
+
+			__nat25_generate_ipv4_network_addr(networkAddr, sender);
+
+			__nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr);
+
+			__nat25_db_print(priv);
+		}
+		return 0;
+
+		case NAT25_LOOKUP: {
+			RTW_INFO("NAT25: Lookup ARP\n");
+
+			arp_ptr += arp->ar_hln;
+			sender = (unsigned int *)arp_ptr;
+			arp_ptr += (arp->ar_hln + arp->ar_pln);
+			target = (unsigned int *)arp_ptr;
+
+			__nat25_generate_ipv4_network_addr(networkAddr, target);
+
+			__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+
+			/* change to ARP target mac address to Lookup result */
+			arp_ptr = (unsigned char *)(arp + 1);
+			arp_ptr += (arp->ar_hln + arp->ar_pln);
+			memcpy(arp_ptr, skb->data, ETH_ALEN);
+		}
+		return 0;
+
+		default:
+			return -1;
+		}
+	}
+
+	/*---------------------------------------------------*/
+	/*         Handle IPX and Apple Talk frame          */
+	/*---------------------------------------------------*/
+	else if ((protocol == __constant_htons(ETH_P_IPX)) ||
+		 (protocol == __constant_htons(ETH_P_ATALK)) ||
+		 (protocol == __constant_htons(ETH_P_AARP))) {
+		unsigned char ipx_header[2] = {0xFF, 0xFF};
+		struct ipxhdr	*ipx = NULL;
+		struct elapaarp	*ea = NULL;
+		struct ddpehdr	*ddp = NULL;
+		unsigned char *framePtr = skb->data + ETH_HLEN;
+
+		if (protocol == __constant_htons(ETH_P_IPX)) {
+			RTW_INFO("NAT25: Protocol=IPX (Ethernet II)\n");
+			ipx = (struct ipxhdr *)framePtr;
+		} else { /* if(protocol <= __constant_htons(ETH_FRAME_LEN)) */
+			if (!memcmp(ipx_header, framePtr, 2)) {
+				RTW_INFO("NAT25: Protocol=IPX (Ethernet 802.3)\n");
+				ipx = (struct ipxhdr *)framePtr;
+			} else {
+				unsigned char ipx_8022_type =  0xE0;
+				unsigned char snap_8022_type = 0xAA;
+
+				if (*framePtr == snap_8022_type) {
+					unsigned char ipx_snap_id[5] = {0x0, 0x0, 0x0, 0x81, 0x37};		/* IPX SNAP ID */
+					unsigned char aarp_snap_id[5] = {0x00, 0x00, 0x00, 0x80, 0xF3};	/* Apple Talk AARP SNAP ID */
+					unsigned char ddp_snap_id[5] = {0x08, 0x00, 0x07, 0x80, 0x9B};	/* Apple Talk DDP SNAP ID */
+
+					framePtr += 3;	/* eliminate the 802.2 header */
+
+					if (!memcmp(ipx_snap_id, framePtr, 5)) {
+						framePtr += 5;	/* eliminate the SNAP header */
+
+						RTW_INFO("NAT25: Protocol=IPX (Ethernet SNAP)\n");
+						ipx = (struct ipxhdr *)framePtr;
+					} else if (!memcmp(aarp_snap_id, framePtr, 5)) {
+						framePtr += 5;	/* eliminate the SNAP header */
+
+						ea = (struct elapaarp *)framePtr;
+					} else if (!memcmp(ddp_snap_id, framePtr, 5)) {
+						framePtr += 5;	/* eliminate the SNAP header */
+
+						ddp = (struct ddpehdr *)framePtr;
+					} else {
+						DEBUG_WARN("NAT25: Protocol=Ethernet SNAP %02x%02x%02x%02x%02x\n", framePtr[0],
+							framePtr[1], framePtr[2], framePtr[3], framePtr[4]);
+						return -1;
+					}
+				} else if (*framePtr == ipx_8022_type) {
+					framePtr += 3;	/* eliminate the 802.2 header */
+
+					if (!memcmp(ipx_header, framePtr, 2)) {
+						RTW_INFO("NAT25: Protocol=IPX (Ethernet 802.2)\n");
+						ipx = (struct ipxhdr *)framePtr;
+					} else
+						return -1;
+				}
+			}
+		}
+
+		/*   IPX  */
+		if (ipx != NULL) {
+			switch (method) {
+			case NAT25_CHECK:
+				if (!memcmp(skb->data + ETH_ALEN, ipx->ipx_source.node, ETH_ALEN)) {
+					RTW_INFO("NAT25: Check IPX skb_copy\n");
+					return 0;
+				}
+				return -1;
+
+			case NAT25_INSERT: {
+				RTW_INFO("NAT25: Insert IPX, Dest=%08x,%02x%02x%02x%02x%02x%02x,%04x Source=%08x,%02x%02x%02x%02x%02x%02x,%04x\n",
+					 ipx->ipx_dest.net,
+					 ipx->ipx_dest.node[0],
+					 ipx->ipx_dest.node[1],
+					 ipx->ipx_dest.node[2],
+					 ipx->ipx_dest.node[3],
+					 ipx->ipx_dest.node[4],
+					 ipx->ipx_dest.node[5],
+					 ipx->ipx_dest.sock,
+					 ipx->ipx_source.net,
+					 ipx->ipx_source.node[0],
+					 ipx->ipx_source.node[1],
+					 ipx->ipx_source.node[2],
+					 ipx->ipx_source.node[3],
+					 ipx->ipx_source.node[4],
+					 ipx->ipx_source.node[5],
+					 ipx->ipx_source.sock);
+
+				if (!memcmp(skb->data + ETH_ALEN, ipx->ipx_source.node, ETH_ALEN)) {
+					RTW_INFO("NAT25: Use IPX Net, and Socket as network addr\n");
+
+					__nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_source.net, &ipx->ipx_source.sock);
+
+					/* change IPX source node addr to wlan STA address */
+					memcpy(ipx->ipx_source.node, GET_MY_HWADDR(priv), ETH_ALEN);
+				} else
+					__nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_source.net, ipx->ipx_source.node);
+
+				__nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr);
+
+				__nat25_db_print(priv);
+			}
+			return 0;
+
+			case NAT25_LOOKUP: {
+				if (!memcmp(GET_MY_HWADDR(priv), ipx->ipx_dest.node, ETH_ALEN)) {
+					RTW_INFO("NAT25: Lookup IPX, Modify Destination IPX Node addr\n");
+
+					__nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_dest.net, &ipx->ipx_dest.sock);
+
+					__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+
+					/* replace IPX destination node addr with Lookup destination MAC addr */
+					memcpy(ipx->ipx_dest.node, skb->data, ETH_ALEN);
+				} else {
+					__nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_dest.net, ipx->ipx_dest.node);
+
+					__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+				}
+			}
+			return 0;
+
+			default:
+				return -1;
+			}
+		}
+
+		/*   AARP  */
+		else if (ea != NULL) {
+			/* Sanity check fields. */
+			if (ea->hw_len != ETH_ALEN || ea->pa_len != AARP_PA_ALEN) {
+				DEBUG_WARN("NAT25: Appletalk AARP Sanity check fail!\n");
+				return -1;
+			}
+
+			switch (method) {
+			case NAT25_CHECK:
+				return 0;
+
+			case NAT25_INSERT: {
+				/* change to AARP source mac address to wlan STA address */
+				memcpy(ea->hw_src, GET_MY_HWADDR(priv), ETH_ALEN);
+
+				RTW_INFO("NAT25: Insert AARP, Source=%d,%d Destination=%d,%d\n",
+					 ea->pa_src_net,
+					 ea->pa_src_node,
+					 ea->pa_dst_net,
+					 ea->pa_dst_node);
+
+				__nat25_generate_apple_network_addr(networkAddr, &ea->pa_src_net, &ea->pa_src_node);
+
+				__nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr);
+
+				__nat25_db_print(priv);
+			}
+			return 0;
+
+			case NAT25_LOOKUP: {
+				RTW_INFO("NAT25: Lookup AARP, Source=%d,%d Destination=%d,%d\n",
+					 ea->pa_src_net,
+					 ea->pa_src_node,
+					 ea->pa_dst_net,
+					 ea->pa_dst_node);
+
+				__nat25_generate_apple_network_addr(networkAddr, &ea->pa_dst_net, &ea->pa_dst_node);
+
+				__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+
+				/* change to AARP destination mac address to Lookup result */
+				memcpy(ea->hw_dst, skb->data, ETH_ALEN);
+			}
+			return 0;
+
+			default:
+				return -1;
+			}
+		}
+
+		/*   DDP  */
+		else if (ddp != NULL) {
+			switch (method) {
+			case NAT25_CHECK:
+				return -1;
+
+			case NAT25_INSERT: {
+				RTW_INFO("NAT25: Insert DDP, Source=%d,%d Destination=%d,%d\n",
+					 ddp->deh_snet,
+					 ddp->deh_snode,
+					 ddp->deh_dnet,
+					 ddp->deh_dnode);
+
+				__nat25_generate_apple_network_addr(networkAddr, &ddp->deh_snet, &ddp->deh_snode);
+
+				__nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr);
+
+				__nat25_db_print(priv);
+			}
+			return 0;
+
+			case NAT25_LOOKUP: {
+				RTW_INFO("NAT25: Lookup DDP, Source=%d,%d Destination=%d,%d\n",
+					 ddp->deh_snet,
+					 ddp->deh_snode,
+					 ddp->deh_dnet,
+					 ddp->deh_dnode);
+
+				__nat25_generate_apple_network_addr(networkAddr, &ddp->deh_dnet, &ddp->deh_dnode);
+
+				__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+			}
+			return 0;
+
+			default:
+				return -1;
+			}
+		}
+
+		return -1;
+	}
+
+	/*---------------------------------------------------*/
+	/*                Handle PPPoE frame                */
+	/*---------------------------------------------------*/
+	else if ((protocol == __constant_htons(ETH_P_PPP_DISC)) ||
+		 (protocol == __constant_htons(ETH_P_PPP_SES))) {
+		struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
+		__be16 *pMagic;
+
+		switch (method) {
+		case NAT25_CHECK:
+			if (ph->sid == 0)
+				return 0;
+			return 1;
+
+		case NAT25_INSERT:
+			if (ph->sid == 0) {	/* Discovery phase according to tag */
+				if (ph->code == PADI_CODE || ph->code == PADR_CODE) {
+					if (priv->ethBrExtInfo.addPPPoETag) {
+						struct pppoe_tag *tag, *pOldTag;
+						unsigned char tag_buf[40];
+						int old_tag_len = 0;
+
+						tag = (struct pppoe_tag *)tag_buf;
+						pOldTag = (struct pppoe_tag *)__nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID));
+						if (pOldTag) { /* if SID existed, copy old value and delete it */
+							old_tag_len = ntohs(pOldTag->tag_len);
+							if (old_tag_len + TAG_HDR_LEN + MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN > sizeof(tag_buf)) {
+								DEBUG_ERR("SID tag length too long!\n");
+								return -1;
+							}
+
+							memcpy(tag->tag_data + MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN,
+							       pOldTag->tag_data, old_tag_len);
+
+							if (skb_pull_and_merge(skb, (unsigned char *)pOldTag, TAG_HDR_LEN + old_tag_len) < 0) {
+								DEBUG_ERR("call skb_pull_and_merge() failed in PADI/R packet!\n");
+								return -1;
+							}
+							ph->length = htons(ntohs(ph->length) - TAG_HDR_LEN - old_tag_len);
+						}
+
+						tag->tag_type = PTT_RELAY_SID;
+						tag->tag_len = htons(MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN + old_tag_len);
+
+						/* insert the magic_code+client mac in relay tag */
+						pMagic = (__be16 *)tag->tag_data;
+						*pMagic = htons(MAGIC_CODE);
+						memcpy(tag->tag_data + MAGIC_CODE_LEN, skb->data + ETH_ALEN, ETH_ALEN);
+
+						/* Add relay tag */
+						if (__nat25_add_pppoe_tag(skb, tag) < 0)
+							return -1;
+
+						RTW_INFO("NAT25: Insert PPPoE, forward %s packet\n",
+							(ph->code == PADI_CODE ? "PADI" : "PADR"));
+					} else { /* not add relay tag */
+						if (priv->pppoe_connection_in_progress &&
+						    memcmp(skb->data + ETH_ALEN, priv->pppoe_addr, ETH_ALEN))	 {
+							DEBUG_ERR("Discard PPPoE packet due to another PPPoE connection is in progress!\n");
+							return -2;
+						}
+
+						if (priv->pppoe_connection_in_progress == 0)
+							memcpy(priv->pppoe_addr, skb->data + ETH_ALEN, ETH_ALEN);
+
+						priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
+					}
+				} else
+					return -1;
+			} else {	/* session phase */
+				RTW_INFO("NAT25: Insert PPPoE, insert session packet to %s\n", skb->dev->name);
+
+				__nat25_generate_pppoe_network_addr(networkAddr, skb->data, &(ph->sid));
+
+				__nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr);
+
+				__nat25_db_print(priv);
+
+				if (!priv->ethBrExtInfo.addPPPoETag &&
+				    priv->pppoe_connection_in_progress &&
+				    !memcmp(skb->data + ETH_ALEN, priv->pppoe_addr, ETH_ALEN))
+					priv->pppoe_connection_in_progress = 0;
+			}
+			return 0;
+
+		case NAT25_LOOKUP:
+			if (ph->code == PADO_CODE || ph->code == PADS_CODE) {
+				if (priv->ethBrExtInfo.addPPPoETag) {
+					struct pppoe_tag *tag;
+					unsigned char *ptr;
+					unsigned short tagType, tagLen;
+					int offset = 0;
+
+					ptr = __nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID));
+					if (!ptr) {
+						DEBUG_ERR("Fail to find PTT_RELAY_SID in FADO!\n");
+						return -1;
+					}
+
+					tag = (struct pppoe_tag *)ptr;
+					tagType = (unsigned short)((ptr[0] << 8) + ptr[1]);
+					tagLen = (unsigned short)((ptr[2] << 8) + ptr[3]);
+
+					if ((tagType != ntohs(PTT_RELAY_SID)) || (tagLen < (MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN))) {
+						DEBUG_ERR("Invalid PTT_RELAY_SID tag length [%d]!\n", tagLen);
+						return -1;
+					}
+
+					pMagic = (__be16 *)tag->tag_data;
+					if (ntohs(*pMagic) != MAGIC_CODE) {
+						DEBUG_ERR("Can't find MAGIC_CODE in %s packet!\n",
+							(ph->code == PADO_CODE ? "PADO" : "PADS"));
+						return -1;
+					}
+
+					memcpy(skb->data, tag->tag_data + MAGIC_CODE_LEN, ETH_ALEN);
+
+					if (tagLen > MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN)
+						offset = TAG_HDR_LEN;
+
+					if (skb_pull_and_merge(skb, ptr + offset, TAG_HDR_LEN + MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN - offset) < 0) {
+						DEBUG_ERR("call skb_pull_and_merge() failed in PADO packet!\n");
+						return -1;
+					}
+					ph->length = htons(ntohs(ph->length) - (TAG_HDR_LEN + MAGIC_CODE_LEN + RTL_RELAY_TAG_LEN - offset));
+					if (offset > 0)
+						tag->tag_len = htons(tagLen - MAGIC_CODE_LEN - RTL_RELAY_TAG_LEN);
+
+					RTW_INFO("NAT25: Lookup PPPoE, forward %s Packet from %s\n",
+						(ph->code == PADO_CODE ? "PADO" : "PADS"),	skb->dev->name);
+				} else { /* not add relay tag */
+					if (!priv->pppoe_connection_in_progress) {
+						DEBUG_ERR("Discard PPPoE packet due to no connection in progresss!\n");
+						return -1;
+					}
+					memcpy(skb->data, priv->pppoe_addr, ETH_ALEN);
+					priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
+				}
+			} else {
+				if (ph->sid != 0) {
+					RTW_INFO("NAT25: Lookup PPPoE, lookup session packet from %s\n", skb->dev->name);
+					__nat25_generate_pppoe_network_addr(networkAddr, skb->data + ETH_ALEN, &(ph->sid));
+
+					__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
+
+					__nat25_db_print(priv);
+				} else
+					return -1;
+
+			}
+			return 0;
+
+		default:
+			return -1;
+		}
+	}
+
+	/*---------------------------------------------------*/
+	/*                 Handle EAP frame                 */
+	/*---------------------------------------------------*/
+	else if (protocol == __constant_htons(0x888e)) {
+		switch (method) {
+		case NAT25_CHECK:
+			return -1;
+
+		case NAT25_INSERT:
+			return 0;
+
+		case NAT25_LOOKUP:
+			return 0;
+
+		default:
+			return -1;
+		}
+	}
+
+	/*---------------------------------------------------*/
+	/*         Handle C-Media proprietary frame         */
+	/*---------------------------------------------------*/
+	else if ((protocol == __constant_htons(0xe2ae)) ||
+		 (protocol == __constant_htons(0xe2af))) {
+		switch (method) {
+		case NAT25_CHECK:
+			return -1;
+
+		case NAT25_INSERT:
+			return 0;
+
+		case NAT25_LOOKUP:
+			return 0;
+
+		default:
+			return -1;
+		}
+	}
+
+	/*---------------------------------------------------*/
+	/*         Handle IPV6 frame      							 */
+	/*---------------------------------------------------*/
+#ifdef CL_IPV6_PASS
+	else if (protocol == __constant_htons(ETH_P_IPV6)) {
+		struct ipv6hdr *iph = (struct ipv6hdr *)(skb->data + ETH_HLEN);
+
+		if (sizeof(*iph) >= (skb->len - ETH_HLEN)) {
+			DEBUG_WARN("NAT25: malformed IPv6 packet !\n");
+			return -1;
+		}
+
+		switch (method) {
+		case NAT25_CHECK:
+			if (skb->data[0] & 1)
+				return 0;
+			return -1;
+
+		case NAT25_INSERT: {
+			RTW_INFO("NAT25: Insert IP, SA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x,"
+				" DA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n",
+				iph->saddr.s6_addr16[0], iph->saddr.s6_addr16[1], iph->saddr.s6_addr16[2], iph->saddr.s6_addr16[3],
+				iph->saddr.s6_addr16[4], iph->saddr.s6_addr16[5], iph->saddr.s6_addr16[6], iph->saddr.s6_addr16[7],
+				iph->daddr.s6_addr16[0], iph->daddr.s6_addr16[1], iph->daddr.s6_addr16[2], iph->daddr.s6_addr16[3],
+				iph->daddr.s6_addr16[4], iph->daddr.s6_addr16[5], iph->daddr.s6_addr16[6], iph->daddr.s6_addr16[7]);
+
+			if (memcmp(&iph->saddr, "\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0", 16)) {
+				__nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->saddr);
+				__nat25_db_network_insert(priv, skb->data + ETH_ALEN, networkAddr);
+				__nat25_db_print(priv);
+
+				if (iph->nexthdr == IPPROTO_ICMPV6 &&
+				    skb->len > (ETH_HLEN +  sizeof(*iph) + 4)) {
+					if (update_nd_link_layer_addr(skb->data + ETH_HLEN + sizeof(*iph),
+						skb->len - ETH_HLEN - sizeof(*iph), GET_MY_HWADDR(priv))) {
+						struct icmp6hdr  *hdr = (struct icmp6hdr *)(skb->data + ETH_HLEN + sizeof(*iph));
+						hdr->icmp6_cksum = 0;
+						hdr->icmp6_cksum = csum_ipv6_magic(&iph->saddr, &iph->daddr,
+							be16_to_cpu(iph->payload_len),
+							IPPROTO_ICMPV6,
+							csum_partial((__u8 *)hdr, be16_to_cpu(iph->payload_len), 0));
+					}
+				}
+			}
+		}
+		return 0;
+
+		case NAT25_LOOKUP:
+			RTW_INFO("NAT25: Lookup IP, SA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x,"
+				 " DA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n",
+				iph->saddr.s6_addr16[0], iph->saddr.s6_addr16[1], iph->saddr.s6_addr16[2], iph->saddr.s6_addr16[3],
+				iph->saddr.s6_addr16[4], iph->saddr.s6_addr16[5], iph->saddr.s6_addr16[6], iph->saddr.s6_addr16[7],
+				iph->daddr.s6_addr16[0], iph->daddr.s6_addr16[1], iph->daddr.s6_addr16[2], iph->daddr.s6_addr16[3],
+				iph->daddr.s6_addr16[4], iph->daddr.s6_addr16[5], iph->daddr.s6_addr16[6], iph->daddr.s6_addr16[7]);
+
+
+			__nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->daddr);
+			if (!__nat25_db_network_lookup_and_replace(priv, skb, networkAddr)) {
+#ifdef SUPPORT_RX_UNI2MCAST
+				if (iph->daddr.s6_addr[0] == 0xff)
+					convert_ipv6_mac_to_mc(skb);
+#endif
+			}
+			return 0;
+
+		default:
+			return -1;
+		}
+	}
+#endif /* CL_IPV6_PASS */
+
+	return -1;
+}
+
+
+int nat25_handle_frame(_adapter *priv, struct sk_buff *skb)
+{
+#ifdef BR_EXT_DEBUG
+	if ((!priv->ethBrExtInfo.nat25_disable) && (!(skb->data[0] & 1))) {
+		panic_RTW_INFO("NAT25: Input Frame: DA=%02x%02x%02x%02x%02x%02x SA=%02x%02x%02x%02x%02x%02x\n",
+			     skb->data[0],
+			     skb->data[1],
+			     skb->data[2],
+			     skb->data[3],
+			     skb->data[4],
+			     skb->data[5],
+			     skb->data[6],
+			     skb->data[7],
+			     skb->data[8],
+			     skb->data[9],
+			     skb->data[10],
+			     skb->data[11]);
+	}
+#endif
+
+	if (!(skb->data[0] & 1)) {
+		int is_vlan_tag = 0, i, retval = 0;
+		unsigned short vlan_hdr = 0;
+
+		if (*((__be16 *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_8021Q)) {
+			is_vlan_tag = 1;
+			vlan_hdr = *((unsigned short *)(skb->data + ETH_ALEN * 2 + 2));
+			for (i = 0; i < 6; i++)
+				*((unsigned short *)(skb->data + ETH_ALEN * 2 + 2 - i * 2)) = *((unsigned short *)(skb->data + ETH_ALEN * 2 - 2 - i * 2));
+			skb_pull(skb, 4);
+		}
+
+		if (!priv->ethBrExtInfo.nat25_disable) {
+			unsigned long irqL;
+			_enter_critical_bh(&priv->br_ext_lock, &irqL);
+			/*
+			 *	This function look up the destination network address from
+			 *	the NAT2.5 database. Return value = -1 means that the
+			 *	corresponding network protocol is NOT support.
+			 */
+			if (!priv->ethBrExtInfo.nat25sc_disable &&
+			    (*((__be16 *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_IP)) &&
+			    !memcmp(priv->scdb_ip, skb->data + ETH_HLEN + 16, 4)) {
+				memcpy(skb->data, priv->scdb_mac, ETH_ALEN);
+
+				_exit_critical_bh(&priv->br_ext_lock, &irqL);
+			} else {
+				_exit_critical_bh(&priv->br_ext_lock, &irqL);
+
+				retval = nat25_db_handle(priv, skb, NAT25_LOOKUP);
+			}
+		} else {
+			if (((*((__be16 *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_IP)) &&
+			     !memcmp(priv->br_ip, skb->data + ETH_HLEN + 16, 4)) ||
+			    ((*((__be16 *)(skb->data + ETH_ALEN * 2)) == __constant_htons(ETH_P_ARP)) &&
+			     !memcmp(priv->br_ip, skb->data + ETH_HLEN + 24, 4))) {
+				/* for traffic to upper TCP/IP */
+				retval = nat25_db_handle(priv, skb, NAT25_LOOKUP);
+			}
+		}
+
+		if (is_vlan_tag) {
+			skb_push(skb, 4);
+			for (i = 0; i < 6; i++)
+				*((__be16 *)(skb->data + i * 2)) = *((__be16 *)(skb->data + 4 + i * 2));
+			*((__be16 *)(skb->data + ETH_ALEN * 2)) = __constant_htons(ETH_P_8021Q);
+			*((u16 *)(skb->data + ETH_ALEN * 2 + 2)) = vlan_hdr;
+		}
+
+		if (retval == -1) {
+			/* DEBUG_ERR("NAT25: Lookup fail!\n"); */
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+#define SERVER_PORT			67
+#define CLIENT_PORT			68
+#define DHCP_MAGIC			0x63825363
+#define BROADCAST_FLAG		0x8000
+
+struct dhcpMessage {
+	u_int8_t op;
+	u_int8_t htype;
+	u_int8_t hlen;
+	u_int8_t hops;
+	u_int32_t xid;
+	u_int16_t secs;
+	u_int16_t flags;
+	u_int32_t ciaddr;
+	u_int32_t yiaddr;
+	u_int32_t siaddr;
+	u_int32_t giaddr;
+	u_int8_t chaddr[16];
+	u_int8_t sname[64];
+	u_int8_t file[128];
+	u_int32_t cookie;
+	u_int8_t options[308]; /* 312 - cookie */
+};
+
+void dhcp_flag_bcast(_adapter *priv, struct sk_buff *skb)
+{
+	if (!skb)
+		return;
+
+	if (!priv->ethBrExtInfo.dhcp_bcst_disable) {
+		__be16 protocol = *((__be16 *)(skb->data + 2 * ETH_ALEN));
+
+		if (protocol == __constant_htons(ETH_P_IP)) { /* IP */
+			struct iphdr *iph = (struct iphdr *)(skb->data + ETH_HLEN);
+
+			if (iph->protocol == IPPROTO_UDP) { /* UDP */
+				struct udphdr *udph = (struct udphdr *)((SIZE_PTR)iph + (iph->ihl << 2));
+
+				if ((udph->source == __constant_htons(CLIENT_PORT))
+				    && (udph->dest == __constant_htons(SERVER_PORT))) { /* DHCP request */
+					struct dhcpMessage *dhcph =
+						(struct dhcpMessage *)((SIZE_PTR)udph + sizeof(struct udphdr));
+
+					if (dhcph->cookie == DHCP_MAGIC) { /* match magic word */
+						if (!(dhcph->flags & BROADCAST_FLAG)) { /* if not broadcast */
+							register int sum = 0;
+
+							RTW_INFO("DHCP: change flag of DHCP request to broadcast.\n");
+							/* or BROADCAST flag */
+							dhcph->flags |= BROADCAST_FLAG;
+							/* recalculate checksum */
+							sum = ~(udph->check) & 0xffff;
+							sum += dhcph->flags;
+							while (sum >> 16)
+								sum = (sum & 0xffff) + (sum >> 16);
+							udph->check = ~sum;
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
+void *scdb_findEntry(_adapter *priv, unsigned char *macAddr,
+		     unsigned char *ipAddr)
+{
+	unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
+	struct nat25_network_db_entry *db;
+	int hash;
+	/* unsigned long irqL; */
+	/* _enter_critical_bh(&priv->br_ext_lock, &irqL); */
+
+	__nat25_generate_ipv4_network_addr(networkAddr, (unsigned int *)ipAddr);
+	hash = __nat25_network_hash(networkAddr);
+	db = priv->nethash[hash];
+	while (db != NULL) {
+		if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
+			/* _exit_critical_bh(&priv->br_ext_lock, &irqL); */
+			return (void *)db;
+		}
+
+		db = db->next_hash;
+	}
+
+	/* _exit_critical_bh(&priv->br_ext_lock, &irqL); */
+	return NULL;
+}
+
+#endif /* CONFIG_BR_EXT */
diff --git a/drivers/staging/rtl8188eu/core/rtw_btcoex.c b/drivers/staging/rtl8188eu/core/rtw_btcoex.c
new file mode 100644
index 000000000000..f1a2f195fb98
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_btcoex.c
@@ -0,0 +1,1632 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#ifdef CONFIG_BT_COEXIST
+
+#include <drv_types.h>
+#include <hal_btcoex.h>
+#include <hal_data.h>
+
+
+void rtw_btcoex_Initialize(PADAPTER padapter)
+{
+	hal_btcoex_Initialize(padapter);
+}
+
+void rtw_btcoex_PowerOnSetting(PADAPTER padapter)
+{
+	hal_btcoex_PowerOnSetting(padapter);
+}
+
+void rtw_btcoex_PreLoadFirmware(PADAPTER padapter)
+{
+	hal_btcoex_PreLoadFirmware(padapter);
+}
+
+void rtw_btcoex_HAL_Initialize(PADAPTER padapter, u8 bWifiOnly)
+{
+	hal_btcoex_InitHwConfig(padapter, bWifiOnly);
+}
+
+void rtw_btcoex_IpsNotify(PADAPTER padapter, u8 type)
+{
+	PHAL_DATA_TYPE	pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		return;
+
+	hal_btcoex_IpsNotify(padapter, type);
+}
+
+void rtw_btcoex_LpsNotify(PADAPTER padapter, u8 type)
+{
+	PHAL_DATA_TYPE	pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		return;
+
+	hal_btcoex_LpsNotify(padapter, type);
+}
+
+void rtw_btcoex_ScanNotify(PADAPTER padapter, u8 type)
+{
+	PHAL_DATA_TYPE	pHalData;
+#ifdef CONFIG_BT_COEXIST_SOCKET_TRX
+	struct bt_coex_info *pcoex_info = &padapter->coex_info;
+	PBT_MGNT	pBtMgnt = &pcoex_info->BtMgnt;
+#endif /* CONFIG_BT_COEXIST_SOCKET_TRX */
+
+	pHalData = GET_HAL_DATA(padapter);
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		return;
+
+	if (false == type) {
+		#ifdef CONFIG_CONCURRENT_MODE
+		if (rtw_mi_buddy_check_fwstate(padapter, WIFI_SITE_MONITOR))
+			return;
+		#endif
+
+		if (DEV_MGMT_TX_NUM(adapter_to_dvobj(padapter))
+			|| DEV_ROCH_NUM(adapter_to_dvobj(padapter)))
+			return;
+	}
+
+#ifdef CONFIG_BT_COEXIST_SOCKET_TRX
+	if (pBtMgnt->ExtConfig.bEnableWifiScanNotify)
+		rtw_btcoex_SendScanNotify(padapter, type);
+#endif /* CONFIG_BT_COEXIST_SOCKET_TRX	 */
+
+	hal_btcoex_ScanNotify(padapter, type);
+}
+
+void rtw_btcoex_ConnectNotify(PADAPTER padapter, u8 action)
+{
+	PHAL_DATA_TYPE	pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		return;
+
+#ifdef DBG_CONFIG_ERROR_RESET
+	if (rtw_hal_sreset_inprogress(padapter)) {
+		RTW_INFO(FUNC_ADPT_FMT ": [BTCoex] under reset, skip notify!\n",
+			 FUNC_ADPT_ARG(padapter));
+		return;
+	}
+#endif /* DBG_CONFIG_ERROR_RESET */
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (false == action) {
+		if (rtw_mi_buddy_check_fwstate(padapter, WIFI_UNDER_LINKING))
+			return;
+	}
+#endif
+
+	hal_btcoex_ConnectNotify(padapter, action);
+}
+
+void rtw_btcoex_MediaStatusNotify(PADAPTER padapter, u8 mediaStatus)
+{
+	PHAL_DATA_TYPE	pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		return;
+
+#ifdef DBG_CONFIG_ERROR_RESET
+	if (rtw_hal_sreset_inprogress(padapter)) {
+		RTW_INFO(FUNC_ADPT_FMT ": [BTCoex] under reset, skip notify!\n",
+			 FUNC_ADPT_ARG(padapter));
+		return;
+	}
+#endif /* DBG_CONFIG_ERROR_RESET */
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (RT_MEDIA_DISCONNECT == mediaStatus) {
+		if (rtw_mi_buddy_check_fwstate(padapter, WIFI_ASOC_STATE))
+			return;
+	}
+#endif /* CONFIG_CONCURRENT_MODE */
+
+	if ((RT_MEDIA_CONNECT == mediaStatus)
+	    && (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == true))
+		rtw_hal_set_hwreg(padapter, HW_VAR_DL_RSVD_PAGE, NULL);
+
+	hal_btcoex_MediaStatusNotify(padapter, mediaStatus);
+}
+
+void rtw_btcoex_SpecialPacketNotify(PADAPTER padapter, u8 pktType)
+{
+	PHAL_DATA_TYPE	pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		return;
+
+	hal_btcoex_SpecialPacketNotify(padapter, pktType);
+}
+
+void rtw_btcoex_IQKNotify(PADAPTER padapter, u8 state)
+{
+	PHAL_DATA_TYPE	pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		return;
+
+	hal_btcoex_IQKNotify(padapter, state);
+}
+
+void rtw_btcoex_BtInfoNotify(PADAPTER padapter, u8 length, u8 *tmpBuf)
+{
+	PHAL_DATA_TYPE	pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		return;
+
+	hal_btcoex_BtInfoNotify(padapter, length, tmpBuf);
+}
+
+void rtw_btcoex_BtMpRptNotify(PADAPTER padapter, u8 length, u8 *tmpBuf)
+{
+	PHAL_DATA_TYPE	pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		return;
+
+	if (padapter->registrypriv.mp_mode == 1)
+		return;
+
+	hal_btcoex_BtMpRptNotify(padapter, length, tmpBuf);
+}
+
+void rtw_btcoex_SuspendNotify(PADAPTER padapter, u8 state)
+{
+	PHAL_DATA_TYPE	pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		return;
+
+	hal_btcoex_SuspendNotify(padapter, state);
+}
+
+void rtw_btcoex_HaltNotify(PADAPTER padapter)
+{
+	PHAL_DATA_TYPE	pHalData;
+	u8 do_halt = 1;
+
+	pHalData = GET_HAL_DATA(padapter);
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		do_halt = 0;
+
+	if (false == padapter->bup) {
+		RTW_INFO(FUNC_ADPT_FMT ": bup=%d Skip!\n",
+			 FUNC_ADPT_ARG(padapter), padapter->bup);
+		do_halt = 0;
+	}
+
+	if (rtw_is_surprise_removed(padapter)) {
+		RTW_INFO(FUNC_ADPT_FMT ": bSurpriseRemoved=%s Skip!\n",
+			FUNC_ADPT_ARG(padapter), rtw_is_surprise_removed(padapter) ? "True" : "False");
+		do_halt = 0;
+	}
+
+	hal_btcoex_HaltNotify(padapter, do_halt);
+}
+
+void rtw_btcoex_switchband_notify(u8 under_scan, u8 band_type)
+{
+	hal_btcoex_switchband_notify(under_scan, band_type);
+}
+
+void rtw_btcoex_SwitchBtTRxMask(PADAPTER padapter)
+{
+	hal_btcoex_SwitchBtTRxMask(padapter);
+}
+
+void rtw_btcoex_Switch(PADAPTER padapter, u8 enable)
+{
+	hal_btcoex_SetBTCoexist(padapter, enable);
+}
+
+u8 rtw_btcoex_IsBtDisabled(PADAPTER padapter)
+{
+	return hal_btcoex_IsBtDisabled(padapter);
+}
+
+void rtw_btcoex_Handler(PADAPTER padapter)
+{
+	PHAL_DATA_TYPE	pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	if (false == pHalData->EEPROMBluetoothCoexist)
+		return;
+
+	hal_btcoex_Hanlder(padapter);
+}
+
+s32 rtw_btcoex_IsBTCoexRejectAMPDU(PADAPTER padapter)
+{
+	s32 coexctrl;
+
+	coexctrl = hal_btcoex_IsBTCoexRejectAMPDU(padapter);
+
+	return coexctrl;
+}
+
+s32 rtw_btcoex_IsBTCoexCtrlAMPDUSize(PADAPTER padapter)
+{
+	s32 coexctrl;
+
+	coexctrl = hal_btcoex_IsBTCoexCtrlAMPDUSize(padapter);
+
+	return coexctrl;
+}
+
+u32 rtw_btcoex_GetAMPDUSize(PADAPTER padapter)
+{
+	u32 size;
+
+	size = hal_btcoex_GetAMPDUSize(padapter);
+
+	return size;
+}
+
+void rtw_btcoex_SetManualControl(PADAPTER padapter, u8 manual)
+{
+	if (manual)
+		hal_btcoex_SetManualControl(padapter, true);
+	else
+		hal_btcoex_SetManualControl(padapter, false);
+}
+
+u8 rtw_btcoex_1Ant(PADAPTER padapter)
+{
+	return hal_btcoex_1Ant(padapter);
+}
+
+u8 rtw_btcoex_IsBtControlLps(PADAPTER padapter)
+{
+	return hal_btcoex_IsBtControlLps(padapter);
+}
+
+u8 rtw_btcoex_IsLpsOn(PADAPTER padapter)
+{
+	return hal_btcoex_IsLpsOn(padapter);
+}
+
+u8 rtw_btcoex_RpwmVal(PADAPTER padapter)
+{
+	return hal_btcoex_RpwmVal(padapter);
+}
+
+u8 rtw_btcoex_LpsVal(PADAPTER padapter)
+{
+	return hal_btcoex_LpsVal(padapter);
+}
+
+u32 rtw_btcoex_GetRaMask(PADAPTER padapter)
+{
+	return hal_btcoex_GetRaMask(padapter);
+}
+
+void rtw_btcoex_RecordPwrMode(PADAPTER padapter, u8 *pCmdBuf, u8 cmdLen)
+{
+	hal_btcoex_RecordPwrMode(padapter, pCmdBuf, cmdLen);
+}
+
+void rtw_btcoex_DisplayBtCoexInfo(PADAPTER padapter, u8 *pbuf, u32 bufsize)
+{
+	hal_btcoex_DisplayBtCoexInfo(padapter, pbuf, bufsize);
+}
+
+void rtw_btcoex_SetDBG(PADAPTER padapter, u32 *pDbgModule)
+{
+	hal_btcoex_SetDBG(padapter, pDbgModule);
+}
+
+u32 rtw_btcoex_GetDBG(PADAPTER padapter, u8 *pStrBuf, u32 bufSize)
+{
+	return hal_btcoex_GetDBG(padapter, pStrBuf, bufSize);
+}
+
+u8 rtw_btcoex_IncreaseScanDeviceNum(PADAPTER padapter)
+{
+	return hal_btcoex_IncreaseScanDeviceNum(padapter);
+}
+
+u8 rtw_btcoex_IsBtLinkExist(PADAPTER padapter)
+{
+	return hal_btcoex_IsBtLinkExist(padapter);
+}
+
+void rtw_btcoex_SetBtPatchVersion(PADAPTER padapter, u16 btHciVer, u16 btPatchVer)
+{
+	hal_btcoex_SetBtPatchVersion(padapter, btHciVer, btPatchVer);
+}
+
+void rtw_btcoex_SetHciVersion(PADAPTER  padapter, u16 hciVersion)
+{
+	hal_btcoex_SetHciVersion(padapter, hciVersion);
+}
+
+void rtw_btcoex_StackUpdateProfileInfo(void)
+{
+	hal_btcoex_StackUpdateProfileInfo();
+}
+
+void rtw_btcoex_pta_off_on_notify(PADAPTER padapter, u8 bBTON)
+{
+	hal_btcoex_pta_off_on_notify(padapter, bBTON);
+}
+
+/* ==================================================
+ * Below Functions are called by BT-Coex
+ * ================================================== */
+void rtw_btcoex_rx_ampdu_apply(PADAPTER padapter)
+{
+	rtw_rx_ampdu_apply(padapter);
+}
+
+void rtw_btcoex_LPS_Enter(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrpriv;
+	u8 lpsVal;
+
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	pwrpriv->bpower_saving = true;
+	lpsVal = rtw_btcoex_LpsVal(padapter);
+	rtw_set_ps_mode(padapter, PS_MODE_MIN, 0, lpsVal, "BTCOEX");
+}
+
+void rtw_btcoex_LPS_Leave(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) {
+		rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, "BTCOEX");
+		LPS_RF_ON_check(padapter, 100);
+		pwrpriv->bpower_saving = false;
+	}
+}
+
+u16 rtw_btcoex_btreg_read(PADAPTER padapter, u8 type, u16 addr, u32 *data)
+{
+	return hal_btcoex_btreg_read(padapter, type, addr, data);
+}
+
+u16 rtw_btcoex_btreg_write(PADAPTER padapter, u8 type, u16 addr, u16 val)
+{
+	return hal_btcoex_btreg_write(padapter, type, addr, val);
+}
+
+u8 rtw_btcoex_get_bt_coexist(PADAPTER padapter)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+
+	return pHalData->EEPROMBluetoothCoexist;
+}
+
+u8 rtw_btcoex_get_chip_type(PADAPTER padapter)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+
+	return pHalData->EEPROMBluetoothType;
+}
+
+u8 rtw_btcoex_get_pg_ant_num(PADAPTER padapter)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+
+	return pHalData->EEPROMBluetoothAntNum == Ant_x2 ? 2 : 1;
+}
+
+u8 rtw_btcoex_get_pg_single_ant_path(PADAPTER padapter)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+
+	return pHalData->ant_path;
+}
+
+u8 rtw_btcoex_get_pg_rfe_type(PADAPTER padapter)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+
+	return pHalData->rfe_type;
+}
+
+u8 rtw_btcoex_is_tfbga_package_type(PADAPTER padapter)
+{
+	return false;
+}
+
+u8 rtw_btcoex_get_ant_div_cfg(PADAPTER padapter)
+{
+	PHAL_DATA_TYPE pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+	
+	return (pHalData->AntDivCfg == 0) ? false : true;
+}
+
+/* ==================================================
+ * Below Functions are BT-Coex socket related function
+ * ================================================== */
+
+#ifdef CONFIG_BT_COEXIST_SOCKET_TRX
+_adapter *pbtcoexadapter; /* = NULL; */ /* do not initialise globals to 0 or NULL */
+u8 rtw_btcoex_btinfo_cmd(_adapter *adapter, u8 *buf, u16 len)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	u8 *btinfo;
+	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	btinfo = rtw_zmalloc(len);
+	if (btinfo == NULL) {
+		rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
+		rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = BTINFO_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = len;
+	pdrvextra_cmd_parm->pbuf = btinfo;
+
+	memcpy(btinfo, buf, len);
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+u8 rtw_btcoex_send_event_to_BT(_adapter *padapter, u8 status,  u8 event_code, u8 opcode_low, u8 opcode_high, u8 *dbg_msg)
+{
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	rtw_HCI_event *pEvent;
+
+	pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+	pEvent->EventCode = event_code;
+	pEvent->Data[0] = 0x1;	/* packet # */
+	pEvent->Data[1] = opcode_low;
+	pEvent->Data[2] = opcode_high;
+	len = len + 3;
+
+	/* Return parameters starts from here */
+	pRetPar = &pEvent->Data[len];
+	pRetPar[0] = status;		/* status */
+
+	len++;
+	pEvent->Length = len;
+
+	/* total tx event length + EventCode length + sizeof(length) */
+	tx_event_length = pEvent->Length + 2;
+	status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+
+	return status;
+}
+
+/*
+Ref:
+Realtek Wi-Fi Driver
+Host Controller Interface for
+Bluetooth 3.0 + HS V1.4 2013/02/07
+
+Window team code & BT team code
+ */
+
+
+u8 rtw_btcoex_parse_BT_info_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen)
+{
+#define BT_INFO_LENGTH 8
+
+	u8 curPollEnable = pcmd[0];
+	u8 curPollTime = pcmd[1];
+	u8 btInfoReason = pcmd[2];
+	u8 btInfoLen = pcmd[3];
+	u8 btinfo[BT_INFO_LENGTH];
+
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	RTW_HCI_STATUS status = HCI_STATUS_SUCCESS;
+	rtw_HCI_event *pEvent;
+
+	/* RTW_INFO("%s\n",__func__);
+	RTW_INFO("current Poll Enable: %d, currrent Poll Time: %d\n",curPollEnable,curPollTime);
+	RTW_INFO("BT Info reason: %d, BT Info length: %d\n",btInfoReason,btInfoLen);
+	RTW_INFO("%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n"
+		,pcmd[4],pcmd[5],pcmd[6],pcmd[7],pcmd[8],pcmd[9],pcmd[10],pcmd[11]);*/
+
+	memset(btinfo, 0, BT_INFO_LENGTH);
+
+	if (BT_INFO_LENGTH != btInfoLen) {
+		status = HCI_STATUS_INVALID_HCI_CMD_PARA_VALUE;
+		RTW_INFO("Error BT Info Length: %d\n", btInfoLen);
+		/* return _FAIL; */
+	} else {
+		if (0x1 == btInfoReason || 0x2 == btInfoReason) {
+			memcpy(btinfo, &pcmd[4], btInfoLen);
+			btinfo[0] = btInfoReason;
+			rtw_btcoex_btinfo_cmd(padapter, btinfo, btInfoLen);
+		} else
+			RTW_INFO("Other BT info reason\n");
+	}
+
+	/* send complete event to BT */
+	{
+
+		pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+		pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE;
+		pEvent->Data[0] = 0x1;	/* packet # */
+		pEvent->Data[1] = HCIOPCODELOW(HCI_BT_INFO_NOTIFY, OGF_EXTENSION);
+		pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_INFO_NOTIFY, OGF_EXTENSION);
+		len = len + 3;
+
+		/* Return parameters starts from here */
+		pRetPar = &pEvent->Data[len];
+		pRetPar[0] = status;		/* status */
+
+		len++;
+		pEvent->Length = len;
+
+		/* total tx event length + EventCode length + sizeof(length) */
+		tx_event_length = pEvent->Length + 2;
+		status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+
+		return status;
+		/* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */
+	}
+}
+
+u8 rtw_btcoex_parse_BT_patch_ver_info_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen)
+{
+	RTW_HCI_STATUS status = HCI_STATUS_SUCCESS;
+	u16		btPatchVer = 0x0, btHciVer = 0x0;
+	/* u16		*pU2tmp; */
+
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	rtw_HCI_event *pEvent;
+
+	btHciVer = pcmd[0] | pcmd[1] << 8;
+	btPatchVer = pcmd[2] | pcmd[3] << 8;
+
+
+	RTW_INFO("%s, cmd:%02x %02x %02x %02x\n", __func__, pcmd[0] , pcmd[1] , pcmd[2] , pcmd[3]);
+	RTW_INFO("%s, HCI Ver:%d, Patch Ver:%d\n", __func__, btHciVer, btPatchVer);
+
+	rtw_btcoex_SetBtPatchVersion(padapter, btHciVer, btPatchVer);
+
+
+	/* send complete event to BT */
+	{
+		pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+
+		pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE;
+		pEvent->Data[0] = 0x1;	/* packet # */
+		pEvent->Data[1] = HCIOPCODELOW(HCI_BT_PATCH_VERSION_NOTIFY, OGF_EXTENSION);
+		pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_PATCH_VERSION_NOTIFY, OGF_EXTENSION);
+		len = len + 3;
+
+		/* Return parameters starts from here */
+		pRetPar = &pEvent->Data[len];
+		pRetPar[0] = status;		/* status */
+
+		len++;
+		pEvent->Length = len;
+
+		/* total tx event length + EventCode length + sizeof(length) */
+		tx_event_length = pEvent->Length + 2;
+		status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+		return status;
+		/* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */
+	}
+}
+
+u8 rtw_btcoex_parse_HCI_Ver_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen)
+{
+	RTW_HCI_STATUS status = HCI_STATUS_SUCCESS;
+	u16 hciver = pcmd[0] | pcmd[1] << 8;
+
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	rtw_HCI_event *pEvent;
+
+	struct bt_coex_info *pcoex_info = &padapter->coex_info;
+	PBT_MGNT	pBtMgnt = &pcoex_info->BtMgnt;
+	pBtMgnt->ExtConfig.HCIExtensionVer = hciver;
+	RTW_INFO("%s, HCI Version: %d\n", __func__, pBtMgnt->ExtConfig.HCIExtensionVer);
+	if (pBtMgnt->ExtConfig.HCIExtensionVer  < 4) {
+		status = HCI_STATUS_INVALID_HCI_CMD_PARA_VALUE;
+		RTW_INFO("%s, Version = %d, HCI Version < 4\n", __func__, pBtMgnt->ExtConfig.HCIExtensionVer);
+	} else
+		rtw_btcoex_SetHciVersion(padapter, hciver);
+	/* send complete event to BT */
+	{
+		pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+
+		pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE;
+		pEvent->Data[0] = 0x1;	/* packet # */
+		pEvent->Data[1] = HCIOPCODELOW(HCI_EXTENSION_VERSION_NOTIFY, OGF_EXTENSION);
+		pEvent->Data[2] = HCIOPCODEHIGHT(HCI_EXTENSION_VERSION_NOTIFY, OGF_EXTENSION);
+		len = len + 3;
+
+		/* Return parameters starts from here */
+		pRetPar = &pEvent->Data[len];
+		pRetPar[0] = status;		/* status */
+
+		len++;
+		pEvent->Length = len;
+
+		/* total tx event length + EventCode length + sizeof(length) */
+		tx_event_length = pEvent->Length + 2;
+
+		status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+		return status;
+		/* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */
+	}
+
+}
+
+u8 rtw_btcoex_parse_WIFI_scan_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen)
+{
+	RTW_HCI_STATUS status = HCI_STATUS_SUCCESS;
+
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	rtw_HCI_event *pEvent;
+
+	struct bt_coex_info *pcoex_info = &padapter->coex_info;
+	PBT_MGNT	pBtMgnt = &pcoex_info->BtMgnt;
+	pBtMgnt->ExtConfig.bEnableWifiScanNotify = pcmd[0];
+	RTW_INFO("%s, bEnableWifiScanNotify: %d\n", __func__, pBtMgnt->ExtConfig.bEnableWifiScanNotify);
+
+	/* send complete event to BT */
+	{
+		pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+
+		pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE;
+		pEvent->Data[0] = 0x1;	/* packet # */
+		pEvent->Data[1] = HCIOPCODELOW(HCI_ENABLE_WIFI_SCAN_NOTIFY, OGF_EXTENSION);
+		pEvent->Data[2] = HCIOPCODEHIGHT(HCI_ENABLE_WIFI_SCAN_NOTIFY, OGF_EXTENSION);
+		len = len + 3;
+
+		/* Return parameters starts from here */
+		pRetPar = &pEvent->Data[len];
+		pRetPar[0] = status;		/* status */
+
+		len++;
+		pEvent->Length = len;
+
+		/* total tx event length + EventCode length + sizeof(length) */
+		tx_event_length = pEvent->Length + 2;
+
+		status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+		return status;
+		/* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */
+	}
+}
+
+u8 rtw_btcoex_parse_HCI_link_status_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen)
+{
+	RTW_HCI_STATUS	status = HCI_STATUS_SUCCESS;
+	struct bt_coex_info	*pcoex_info = &padapter->coex_info;
+	PBT_MGNT	pBtMgnt = &pcoex_info->BtMgnt;
+	/* PBT_DBG		pBtDbg=&padapter->MgntInfo.BtInfo.BtDbg; */
+	u8		i, numOfHandle = 0, numOfAcl = 0;
+	u16		conHandle;
+	u8		btProfile, btCoreSpec, linkRole;
+	u8		*pTriple;
+
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	rtw_HCI_event *pEvent;
+
+	/* pBtDbg->dbgHciInfo.hciCmdCntLinkStatusNotify++; */
+	/* RT_DISP_DATA(FIOCTL, IOCTL_BT_HCICMD_EXT, "LinkStatusNotify, Hex Data :\n",  */
+	/*		&pHciCmd->Data[0], pHciCmd->Length); */
+
+	RTW_INFO("BTLinkStatusNotify\n");
+
+	/* Current only RTL8723 support this command. */
+	/* pBtMgnt->bSupportProfile = true; */
+	pBtMgnt->bSupportProfile = false;
+
+	pBtMgnt->ExtConfig.NumberOfACL = 0;
+	pBtMgnt->ExtConfig.NumberOfSCO = 0;
+
+	numOfHandle = pcmd[0];
+	/* RT_DISP(FIOCTL, IOCTL_BT_HCICMD_EXT, ("numOfHandle = 0x%x\n", numOfHandle)); */
+	/* RT_DISP(FIOCTL, IOCTL_BT_HCICMD_EXT, ("HCIExtensionVer = %d\n", pBtMgnt->ExtConfig.HCIExtensionVer)); */
+	RTW_INFO("numOfHandle = 0x%x\n", numOfHandle);
+	RTW_INFO("HCIExtensionVer = %d\n", pBtMgnt->ExtConfig.HCIExtensionVer);
+
+	pTriple = &pcmd[1];
+	for (i = 0; i < numOfHandle; i++) {
+		if (pBtMgnt->ExtConfig.HCIExtensionVer < 1) {
+			conHandle = *((u8 *)&pTriple[0]);
+			btProfile = pTriple[2];
+			btCoreSpec = pTriple[3];
+			if (BT_PROFILE_SCO == btProfile)
+				pBtMgnt->ExtConfig.NumberOfSCO++;
+			else {
+				pBtMgnt->ExtConfig.NumberOfACL++;
+				pBtMgnt->ExtConfig.aclLink[i].ConnectHandle = conHandle;
+				pBtMgnt->ExtConfig.aclLink[i].BTProfile = btProfile;
+				pBtMgnt->ExtConfig.aclLink[i].BTCoreSpec = btCoreSpec;
+			}
+			/* RT_DISP(FIOCTL, IOCTL_BT_HCICMD_EXT, */
+			/*	("Connection_Handle=0x%x, BTProfile=%d, BTSpec=%d\n", */
+			/*		conHandle, btProfile, btCoreSpec)); */
+			RTW_INFO("Connection_Handle=0x%x, BTProfile=%d, BTSpec=%d\n", conHandle, btProfile, btCoreSpec);
+			pTriple += 4;
+		} else if (pBtMgnt->ExtConfig.HCIExtensionVer >= 1) {
+			conHandle = *((u16 *)&pTriple[0]);
+			btProfile = pTriple[2];
+			btCoreSpec = pTriple[3];
+			linkRole = pTriple[4];
+			if (BT_PROFILE_SCO == btProfile)
+				pBtMgnt->ExtConfig.NumberOfSCO++;
+			else {
+				pBtMgnt->ExtConfig.NumberOfACL++;
+				pBtMgnt->ExtConfig.aclLink[i].ConnectHandle = conHandle;
+				pBtMgnt->ExtConfig.aclLink[i].BTProfile = btProfile;
+				pBtMgnt->ExtConfig.aclLink[i].BTCoreSpec = btCoreSpec;
+				pBtMgnt->ExtConfig.aclLink[i].linkRole = linkRole;
+			}
+			/* RT_DISP(FIOCTL, IOCTL_BT_HCICMD_EXT, */
+			RTW_INFO("Connection_Handle=0x%x, BTProfile=%d, BTSpec=%d, LinkRole=%d\n",
+				 conHandle, btProfile, btCoreSpec, linkRole);
+			pTriple += 5;
+		}
+	}
+	rtw_btcoex_StackUpdateProfileInfo();
+
+	/* send complete event to BT */
+	{
+		pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+
+		pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE;
+		pEvent->Data[0] = 0x1;	/* packet # */
+		pEvent->Data[1] = HCIOPCODELOW(HCI_LINK_STATUS_NOTIFY, OGF_EXTENSION);
+		pEvent->Data[2] = HCIOPCODEHIGHT(HCI_LINK_STATUS_NOTIFY, OGF_EXTENSION);
+		len = len + 3;
+
+		/* Return parameters starts from here */
+		pRetPar = &pEvent->Data[len];
+		pRetPar[0] = status;		/* status */
+
+		len++;
+		pEvent->Length = len;
+
+		/* total tx event length + EventCode length + sizeof(length) */
+		tx_event_length = pEvent->Length + 2;
+
+		status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+		return status;
+		/* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */
+	}
+
+
+}
+
+u8 rtw_btcoex_parse_HCI_BT_coex_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen)
+{
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	rtw_HCI_event *pEvent;
+	RTW_HCI_STATUS	status = HCI_STATUS_SUCCESS;
+
+	{
+		pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+
+		pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE;
+		pEvent->Data[0] = 0x1;	/* packet # */
+		pEvent->Data[1] = HCIOPCODELOW(HCI_BT_COEX_NOTIFY, OGF_EXTENSION);
+		pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_COEX_NOTIFY, OGF_EXTENSION);
+		len = len + 3;
+
+		/* Return parameters starts from here */
+		pRetPar = &pEvent->Data[len];
+		pRetPar[0] = status;		/* status */
+
+		len++;
+		pEvent->Length = len;
+
+		/* total tx event length + EventCode length + sizeof(length) */
+		tx_event_length = pEvent->Length + 2;
+
+		status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+		return status;
+		/* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */
+	}
+}
+
+u8 rtw_btcoex_parse_HCI_BT_operation_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen)
+{
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	rtw_HCI_event *pEvent;
+	RTW_HCI_STATUS	status = HCI_STATUS_SUCCESS;
+
+	RTW_INFO("%s, OP code: %d\n", __func__, pcmd[0]);
+
+	switch (pcmd[0]) {
+	case HCI_BT_OP_NONE:
+		RTW_INFO("[bt operation] : Operation None!!\n");
+		break;
+	case HCI_BT_OP_INQUIRY_START:
+		RTW_INFO("[bt operation] : Inquiry start!!\n");
+		break;
+	case HCI_BT_OP_INQUIRY_FINISH:
+		RTW_INFO("[bt operation] : Inquiry finished!!\n");
+		break;
+	case HCI_BT_OP_PAGING_START:
+		RTW_INFO("[bt operation] : Paging is started!!\n");
+		break;
+	case HCI_BT_OP_PAGING_SUCCESS:
+		RTW_INFO("[bt operation] : Paging complete successfully!!\n");
+		break;
+	case HCI_BT_OP_PAGING_UNSUCCESS:
+		RTW_INFO("[bt operation] : Paging complete unsuccessfully!!\n");
+		break;
+	case HCI_BT_OP_PAIRING_START:
+		RTW_INFO("[bt operation] : Pairing start!!\n");
+		break;
+	case HCI_BT_OP_PAIRING_FINISH:
+		RTW_INFO("[bt operation] : Pairing finished!!\n");
+		break;
+	case HCI_BT_OP_BT_DEV_ENABLE:
+		RTW_INFO("[bt operation] : BT Device is enabled!!\n");
+		break;
+	case HCI_BT_OP_BT_DEV_DISABLE:
+		RTW_INFO("[bt operation] : BT Device is disabled!!\n");
+		break;
+	default:
+		RTW_INFO("[bt operation] : Unknown, error!!\n");
+		break;
+	}
+
+	/* send complete event to BT */
+	{
+		pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+
+		pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE;
+		pEvent->Data[0] = 0x1;	/* packet # */
+		pEvent->Data[1] = HCIOPCODELOW(HCI_BT_OPERATION_NOTIFY, OGF_EXTENSION);
+		pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_OPERATION_NOTIFY, OGF_EXTENSION);
+		len = len + 3;
+
+		/* Return parameters starts from here */
+		pRetPar = &pEvent->Data[len];
+		pRetPar[0] = status;		/* status */
+
+		len++;
+		pEvent->Length = len;
+
+		/* total tx event length + EventCode length + sizeof(length) */
+		tx_event_length = pEvent->Length + 2;
+
+		status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+		return status;
+		/* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */
+	}
+}
+
+u8 rtw_btcoex_parse_BT_AFH_MAP_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen)
+{
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	rtw_HCI_event *pEvent;
+	RTW_HCI_STATUS	status = HCI_STATUS_SUCCESS;
+
+	{
+		pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+
+		pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE;
+		pEvent->Data[0] = 0x1;	/* packet # */
+		pEvent->Data[1] = HCIOPCODELOW(HCI_BT_AFH_MAP_NOTIFY, OGF_EXTENSION);
+		pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_AFH_MAP_NOTIFY, OGF_EXTENSION);
+		len = len + 3;
+
+		/* Return parameters starts from here */
+		pRetPar = &pEvent->Data[len];
+		pRetPar[0] = status;		/* status */
+
+		len++;
+		pEvent->Length = len;
+
+		/* total tx event length + EventCode length + sizeof(length) */
+		tx_event_length = pEvent->Length + 2;
+
+		status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+		return status;
+		/* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */
+	}
+}
+
+u8 rtw_btcoex_parse_BT_register_val_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen)
+{
+
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	rtw_HCI_event *pEvent;
+	RTW_HCI_STATUS	status = HCI_STATUS_SUCCESS;
+
+	{
+		pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+
+		pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE;
+		pEvent->Data[0] = 0x1;	/* packet # */
+		pEvent->Data[1] = HCIOPCODELOW(HCI_BT_REGISTER_VALUE_NOTIFY, OGF_EXTENSION);
+		pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_REGISTER_VALUE_NOTIFY, OGF_EXTENSION);
+		len = len + 3;
+
+		/* Return parameters starts from here */
+		pRetPar = &pEvent->Data[len];
+		pRetPar[0] = status;		/* status */
+
+		len++;
+		pEvent->Length = len;
+
+		/* total tx event length + EventCode length + sizeof(length) */
+		tx_event_length = pEvent->Length + 2;
+
+		status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+		return status;
+		/* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */
+	}
+}
+
+u8 rtw_btcoex_parse_HCI_BT_abnormal_notify_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen)
+{
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	rtw_HCI_event *pEvent;
+	RTW_HCI_STATUS	status = HCI_STATUS_SUCCESS;
+
+	{
+		pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+
+		pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE;
+		pEvent->Data[0] = 0x1;	/* packet # */
+		pEvent->Data[1] = HCIOPCODELOW(HCI_BT_ABNORMAL_NOTIFY, OGF_EXTENSION);
+		pEvent->Data[2] = HCIOPCODEHIGHT(HCI_BT_ABNORMAL_NOTIFY, OGF_EXTENSION);
+		len = len + 3;
+
+		/* Return parameters starts from here */
+		pRetPar = &pEvent->Data[len];
+		pRetPar[0] = status;		/* status */
+
+		len++;
+		pEvent->Length = len;
+
+		/* total tx event length + EventCode length + sizeof(length) */
+		tx_event_length = pEvent->Length + 2;
+
+		status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+		return status;
+		/* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */
+	}
+}
+
+u8 rtw_btcoex_parse_HCI_query_RF_status_cmd(_adapter *padapter, u8 *pcmd, u16 cmdlen)
+{
+	u8 localBuf[6] = "";
+	u8 *pRetPar;
+	u8	len = 0, tx_event_length = 0;
+	rtw_HCI_event *pEvent;
+	RTW_HCI_STATUS	status = HCI_STATUS_SUCCESS;
+
+	{
+		pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+
+		pEvent->EventCode = HCI_EVENT_COMMAND_COMPLETE;
+		pEvent->Data[0] = 0x1;	/* packet # */
+		pEvent->Data[1] = HCIOPCODELOW(HCI_QUERY_RF_STATUS, OGF_EXTENSION);
+		pEvent->Data[2] = HCIOPCODEHIGHT(HCI_QUERY_RF_STATUS, OGF_EXTENSION);
+		len = len + 3;
+
+		/* Return parameters starts from here */
+		pRetPar = &pEvent->Data[len];
+		pRetPar[0] = status;		/* status */
+
+		len++;
+		pEvent->Length = len;
+
+		/* total tx event length + EventCode length + sizeof(length) */
+		tx_event_length = pEvent->Length + 2;
+
+		status = rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+		return status;
+		/* bthci_IndicateEvent(Adapter, PPacketIrpEvent, len+2); */
+	}
+}
+
+/*****************************************
+* HCI cmd format :
+*| 15 - 0						|
+*| OPcode (OCF|OGF<<10)		|
+*| 15 - 8		|7 - 0			|
+*|Cmd para	|Cmd para Length	|
+*|Cmd para......				|
+******************************************/
+
+/* bit 0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15
+ *	 |	OCF			             |	   OGF       | */
+void rtw_btcoex_parse_hci_extend_cmd(_adapter *padapter, u8 *pcmd, u16 len, const u16 hci_OCF)
+{
+
+	RTW_INFO("%s: OCF: %x\n", __func__, hci_OCF);
+	switch (hci_OCF) {
+	case HCI_EXTENSION_VERSION_NOTIFY:
+		RTW_INFO("HCI_EXTENSION_VERSION_NOTIFY\n");
+		rtw_btcoex_parse_HCI_Ver_notify_cmd(padapter, pcmd, len);
+		break;
+	case HCI_LINK_STATUS_NOTIFY:
+		RTW_INFO("HCI_LINK_STATUS_NOTIFY\n");
+		rtw_btcoex_parse_HCI_link_status_notify_cmd(padapter, pcmd, len);
+		break;
+	case HCI_BT_OPERATION_NOTIFY:
+		/* only for 8723a 2ant */
+		RTW_INFO("HCI_BT_OPERATION_NOTIFY\n");
+		rtw_btcoex_parse_HCI_BT_operation_notify_cmd(padapter, pcmd, len);
+		/*  */
+		break;
+	case HCI_ENABLE_WIFI_SCAN_NOTIFY:
+		RTW_INFO("HCI_ENABLE_WIFI_SCAN_NOTIFY\n");
+		rtw_btcoex_parse_WIFI_scan_notify_cmd(padapter, pcmd, len);
+		break;
+	case HCI_QUERY_RF_STATUS:
+		/* only for 8723b 2ant */
+		RTW_INFO("HCI_QUERY_RF_STATUS\n");
+		rtw_btcoex_parse_HCI_query_RF_status_cmd(padapter, pcmd, len);
+		break;
+	case HCI_BT_ABNORMAL_NOTIFY:
+		RTW_INFO("HCI_BT_ABNORMAL_NOTIFY\n");
+		rtw_btcoex_parse_HCI_BT_abnormal_notify_cmd(padapter, pcmd, len);
+		break;
+	case HCI_BT_INFO_NOTIFY:
+		RTW_INFO("HCI_BT_INFO_NOTIFY\n");
+		rtw_btcoex_parse_BT_info_notify_cmd(padapter, pcmd, len);
+		break;
+	case HCI_BT_COEX_NOTIFY:
+		RTW_INFO("HCI_BT_COEX_NOTIFY\n");
+		rtw_btcoex_parse_HCI_BT_coex_notify_cmd(padapter, pcmd, len);
+		break;
+	case HCI_BT_PATCH_VERSION_NOTIFY:
+		RTW_INFO("HCI_BT_PATCH_VERSION_NOTIFY\n");
+		rtw_btcoex_parse_BT_patch_ver_info_cmd(padapter, pcmd, len);
+		break;
+	case HCI_BT_AFH_MAP_NOTIFY:
+		RTW_INFO("HCI_BT_AFH_MAP_NOTIFY\n");
+		rtw_btcoex_parse_BT_AFH_MAP_notify_cmd(padapter, pcmd, len);
+		break;
+	case HCI_BT_REGISTER_VALUE_NOTIFY:
+		RTW_INFO("HCI_BT_REGISTER_VALUE_NOTIFY\n");
+		rtw_btcoex_parse_BT_register_val_notify_cmd(padapter, pcmd, len);
+		break;
+	default:
+		RTW_INFO("ERROR!!! Unknown OCF: %x\n", hci_OCF);
+		break;
+
+	}
+}
+
+void rtw_btcoex_parse_hci_cmd(_adapter *padapter, u8 *pcmd, u16 len)
+{
+	u16 opcode = pcmd[0] | pcmd[1] << 8;
+	u16 hci_OGF = HCI_OGF(opcode);
+	u16 hci_OCF = HCI_OCF(opcode);
+	u8 cmdlen = len - 3;
+	u8 pare_len = pcmd[2];
+
+	RTW_INFO("%s OGF: %x,OCF: %x\n", __func__, hci_OGF, hci_OCF);
+	switch (hci_OGF) {
+	case OGF_EXTENSION:
+		RTW_INFO("HCI_EXTENSION_CMD_OGF\n");
+		rtw_btcoex_parse_hci_extend_cmd(padapter, &pcmd[3], cmdlen, hci_OCF);
+		break;
+	default:
+		RTW_INFO("Other OGF: %x\n", hci_OGF);
+		break;
+	}
+}
+
+u16 rtw_btcoex_parse_recv_data(u8 *msg, u8 msg_size)
+{
+	u8 cmp_msg1[32] = attend_ack;
+	u8 cmp_msg2[32] = leave_ack;
+	u8 cmp_msg3[32] = bt_leave;
+	u8 cmp_msg4[32] = invite_req;
+	u8 cmp_msg5[32] = attend_req;
+	u8 cmp_msg6[32] = invite_rsp;
+	u8 res = OTHER;
+
+	if (!memcmp(cmp_msg1, msg, msg_size) == true) {
+		/*RTW_INFO("%s, msg:%s\n",__func__,msg);*/
+		res = RX_ATTEND_ACK;
+	} else if (!memcmp(cmp_msg2, msg, msg_size) == true) {
+		/*RTW_INFO("%s, msg:%s\n",__func__,msg);*/
+		res = RX_LEAVE_ACK;
+	} else if (!memcmp(cmp_msg3, msg, msg_size) == true) {
+		/*RTW_INFO("%s, msg:%s\n",__func__,msg);*/
+		res = RX_BT_LEAVE;
+	} else if (!memcmp(cmp_msg4, msg, msg_size) == true) {
+		/*RTW_INFO("%s, msg:%s\n",__func__,msg);*/
+		res = RX_INVITE_REQ;
+	} else if (!memcmp(cmp_msg5, msg, msg_size) == true)
+		res = RX_ATTEND_REQ;
+	else if (!memcmp(cmp_msg6, msg, msg_size) == true)
+		res = RX_INVITE_RSP;
+	else {
+		/*RTW_INFO("%s, %s\n", __func__, msg);*/
+		res = OTHER;
+	}
+
+	/*RTW_INFO("%s, res:%d\n", __func__, res);*/
+
+	return res;
+}
+
+void rtw_btcoex_recvmsgbysocket(void *data)
+{
+	u8 recv_data[255];
+	u8 tx_msg[255] = leave_ack;
+	u32 len = 0;
+	u16 recv_length = 0;
+	u16 parse_res = 0;
+
+	struct bt_coex_info *pcoex_info = NULL;
+	struct sock *sk = NULL;
+	struct sk_buff *skb = NULL;
+
+	/*RTW_INFO("%s\n",__func__);*/
+
+	if (pbtcoexadapter == NULL) {
+		RTW_INFO("%s: btcoexadapter NULL!\n", __func__);
+		return;
+	}
+
+	pcoex_info = &pbtcoexadapter->coex_info;
+	sk = pcoex_info->sk_store;
+
+	if (sk == NULL) {
+		RTW_INFO("%s: critical error when receive socket data!\n", __func__);
+		return;
+	}
+
+	len = skb_queue_len(&sk->sk_receive_queue);
+	while (len > 0) {
+		skb = skb_dequeue(&sk->sk_receive_queue);
+
+		/*important: cut the udp header from skb->data! header length is 8 byte*/
+		recv_length = skb->len - 8;
+		memset(recv_data, 0, sizeof(recv_data));
+		memcpy(recv_data, skb->data + 8, recv_length);
+
+		parse_res = rtw_btcoex_parse_recv_data(recv_data, recv_length);
+		switch (parse_res) {
+		case RX_ATTEND_ACK:
+			/* attend ack */
+			pcoex_info->BT_attend = true;
+			RTW_INFO("RX_ATTEND_ACK!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend);
+			rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend);
+			break;
+
+		case RX_ATTEND_REQ:
+			pcoex_info->BT_attend = true;
+			RTW_INFO("RX_BT_ATTEND_REQ!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend);
+			rtw_btcoex_sendmsgbysocket(pbtcoexadapter, attend_ack, sizeof(attend_ack), false);
+			rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend);
+			break;
+
+		case RX_INVITE_REQ:
+			/* invite req from BT */
+			pcoex_info->BT_attend = true;
+			RTW_INFO("RX_INVITE_REQ!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend);
+			rtw_btcoex_sendmsgbysocket(pbtcoexadapter, invite_rsp, sizeof(invite_rsp), false);
+			rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend);
+			break;
+
+		case RX_INVITE_RSP:
+			/*invite rsp*/
+			pcoex_info->BT_attend = true;
+			RTW_INFO("RX_INVITE_RSP!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend);
+			rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend);
+			break;
+
+		case RX_LEAVE_ACK:
+			/* mean BT know wifi  will leave */
+			pcoex_info->BT_attend = false;
+			RTW_INFO("RX_LEAVE_ACK!,sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend);
+			rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend);
+			break;
+
+		case RX_BT_LEAVE:
+			/* BT leave */
+			rtw_btcoex_sendmsgbysocket(pbtcoexadapter, leave_ack, sizeof(leave_ack), false); /* no ack */
+			pcoex_info->BT_attend = false;
+			RTW_INFO("RX_BT_LEAVE!sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend);
+			rtw_btcoex_pta_off_on_notify(pbtcoexadapter, pcoex_info->BT_attend);
+			break;
+
+		default:
+			if (pcoex_info->BT_attend)
+				rtw_btcoex_parse_hci_cmd(pbtcoexadapter, recv_data, recv_length);
+			else
+				RTW_INFO("ERROR!! BT is UP\n");
+			break;
+
+		}
+
+		len--;
+		kfree_skb(skb);
+	}
+}
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 15, 0))
+	void rtw_btcoex_recvmsg_init(struct sock *sk_in, s32 bytes)
+#else
+	void rtw_btcoex_recvmsg_init(struct sock *sk_in)
+#endif
+{
+	struct bt_coex_info *pcoex_info = NULL;
+
+	if (pbtcoexadapter == NULL) {
+		RTW_INFO("%s: btcoexadapter NULL\n", __func__);
+		return;
+	}
+	pcoex_info = &pbtcoexadapter->coex_info;
+	pcoex_info->sk_store = sk_in;
+	if (pcoex_info->btcoex_wq != NULL)
+		queue_delayed_work(pcoex_info->btcoex_wq, &pcoex_info->recvmsg_work, 0);
+	else
+		RTW_INFO("%s: BTCOEX workqueue NULL\n", __func__);
+}
+
+u8 rtw_btcoex_sendmsgbysocket(_adapter *padapter, u8 *msg, u8 msg_size, bool force)
+{
+	u8 error;
+	struct msghdr	udpmsg;
+	mm_segment_t	oldfs;
+	struct iovec	iov;
+	struct bt_coex_info *pcoex_info = &padapter->coex_info;
+
+	/* RTW_INFO("%s: msg:%s, force:%s\n", __func__, msg, force == true?"true":"false"); */
+	if (false == force) {
+		if (false == pcoex_info->BT_attend) {
+			RTW_INFO("TX Blocked: WiFi-BT disconnected\n");
+			return _FAIL;
+		}
+	}
+
+	iov.iov_base	 = (void *)msg;
+	iov.iov_len	 = msg_size;
+	udpmsg.msg_name	 = &pcoex_info->bt_sockaddr;
+	udpmsg.msg_namelen	= sizeof(struct sockaddr_in);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 19, 0))
+	/* referece:sock_xmit in kernel code
+	 * WRITE for sock_sendmsg, READ for sock_recvmsg
+	 * third parameter for msg_iovlen
+	 * last parameter for iov_len
+	 */
+	iov_iter_init(&udpmsg.msg_iter, WRITE, &iov, 1, msg_size);
+#else
+	udpmsg.msg_iov	 = &iov;
+	udpmsg.msg_iovlen	= 1;
+#endif
+	udpmsg.msg_control	= NULL;
+	udpmsg.msg_controllen = 0;
+	udpmsg.msg_flags	= MSG_DONTWAIT | MSG_NOSIGNAL;
+	oldfs = get_fs();
+	set_fs(KERNEL_DS);
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 1, 0))
+	error = sock_sendmsg(pcoex_info->udpsock, &udpmsg);
+#else
+	error = sock_sendmsg(pcoex_info->udpsock, &udpmsg, msg_size);
+#endif
+	set_fs(oldfs);
+	if (error < 0) {
+		RTW_INFO("Error when sendimg msg, error:%d\n", error);
+		return _FAIL;
+	} else
+		return _SUCCESS;
+}
+
+u8 rtw_btcoex_create_kernel_socket(_adapter *padapter)
+{
+	s8 kernel_socket_err;
+	u8 tx_msg[255] = attend_req;
+	struct bt_coex_info *pcoex_info = &padapter->coex_info;
+	s32 sock_reuse = 1;
+	u8 status = _FAIL;
+
+	RTW_INFO("%s CONNECT_PORT %d\n", __func__, CONNECT_PORT);
+
+	if (NULL == pcoex_info) {
+		RTW_INFO("coex_info: NULL\n");
+		status =  _FAIL;
+	}
+
+	kernel_socket_err = sock_create(PF_INET, SOCK_DGRAM, 0, &pcoex_info->udpsock);
+
+	if (kernel_socket_err < 0) {
+		RTW_INFO("Error during creation of socket error:%d\n", kernel_socket_err);
+		status = _FAIL;
+	} else {
+		memset(&(pcoex_info->wifi_sockaddr), 0, sizeof(pcoex_info->wifi_sockaddr));
+		pcoex_info->wifi_sockaddr.sin_family = AF_INET;
+		pcoex_info->wifi_sockaddr.sin_port = htons(CONNECT_PORT);
+		pcoex_info->wifi_sockaddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+
+		memset(&(pcoex_info->bt_sockaddr), 0, sizeof(pcoex_info->bt_sockaddr));
+		pcoex_info->bt_sockaddr.sin_family = AF_INET;
+		pcoex_info->bt_sockaddr.sin_port = htons(CONNECT_PORT_BT);
+		pcoex_info->bt_sockaddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+
+		pcoex_info->sk_store = NULL;
+		kernel_socket_err = pcoex_info->udpsock->ops->bind(pcoex_info->udpsock, (struct sockaddr *)&pcoex_info->wifi_sockaddr,
+				    sizeof(pcoex_info->wifi_sockaddr));
+		if (kernel_socket_err == 0) {
+			RTW_INFO("binding socket success\n");
+			pcoex_info->udpsock->sk->sk_data_ready = rtw_btcoex_recvmsg_init;
+			pcoex_info->sock_open |=  KERNEL_SOCKET_OK;
+			pcoex_info->BT_attend = false;
+			RTW_INFO("WIFI sending attend_req\n");
+			rtw_btcoex_sendmsgbysocket(padapter, attend_req, sizeof(attend_req), true);
+			status = _SUCCESS;
+		} else {
+			pcoex_info->BT_attend = false;
+			sock_release(pcoex_info->udpsock); /* bind fail release socket */
+			RTW_INFO("Error binding socket: %d\n", kernel_socket_err);
+			status = _FAIL;
+		}
+
+	}
+
+	return status;
+}
+
+void rtw_btcoex_close_kernel_socket(_adapter *padapter)
+{
+	struct bt_coex_info *pcoex_info = &padapter->coex_info;
+	if (pcoex_info->sock_open & KERNEL_SOCKET_OK) {
+		RTW_INFO("release kernel socket\n");
+		sock_release(pcoex_info->udpsock);
+		pcoex_info->sock_open &= ~(KERNEL_SOCKET_OK);
+		if (pcoex_info->BT_attend)
+			pcoex_info->BT_attend = false;
+
+		RTW_INFO("sock_open:%d, BT_attend:%d\n", pcoex_info->sock_open, pcoex_info->BT_attend);
+	}
+}
+
+void rtw_btcoex_init_socket(_adapter *padapter)
+{
+
+	u8 is_invite = false;
+	struct bt_coex_info *pcoex_info = &padapter->coex_info;
+	RTW_INFO("%s\n", __func__);
+	if (false == pcoex_info->is_exist) {
+		memset(pcoex_info, 0, sizeof(struct bt_coex_info));
+		pcoex_info->btcoex_wq = create_workqueue("BTCOEX");
+		INIT_DELAYED_WORK(&pcoex_info->recvmsg_work,
+				  (void *)rtw_btcoex_recvmsgbysocket);
+		pbtcoexadapter = padapter;
+		/* We expect BT is off if BT don't send ack to wifi */
+		RTW_INFO("We expect BT is off if BT send ack to wifi\n");
+		rtw_btcoex_pta_off_on_notify(pbtcoexadapter, false);
+		if (rtw_btcoex_create_kernel_socket(padapter) == _SUCCESS)
+			pcoex_info->is_exist = true;
+		else {
+			pcoex_info->is_exist = false;
+			pbtcoexadapter = NULL;
+		}
+
+		RTW_INFO("%s: pbtcoexadapter:%p, coex_info->is_exist: %s\n"
+			, __func__, pbtcoexadapter, pcoex_info->is_exist == true ? "true" : "false");
+	}
+}
+
+void rtw_btcoex_close_socket(_adapter *padapter)
+{
+	struct bt_coex_info *pcoex_info = &padapter->coex_info;
+
+	RTW_INFO("%s--coex_info->is_exist: %s, pcoex_info->BT_attend:%s\n"
+		, __func__, pcoex_info->is_exist == true ? "true" : "false", pcoex_info->BT_attend == true ? "true" : "false");
+
+	if (pcoex_info->is_exist) {
+		if (pcoex_info->BT_attend) {
+			/*inform BT wifi leave*/
+			rtw_btcoex_sendmsgbysocket(padapter, wifi_leave, sizeof(wifi_leave), false);
+			msleep(50);
+		}
+
+		if (pcoex_info->btcoex_wq != NULL) {
+			flush_workqueue(pcoex_info->btcoex_wq);
+			destroy_workqueue(pcoex_info->btcoex_wq);
+		}
+
+		rtw_btcoex_close_kernel_socket(padapter);
+		pbtcoexadapter = NULL;
+		pcoex_info->is_exist = false;
+	}
+}
+
+void rtw_btcoex_dump_tx_msg(u8 *tx_msg, u8 len, u8 *msg_name)
+{
+	u8	i = 0;
+	RTW_INFO("======> Msg name: %s\n", msg_name);
+	for (i = 0; i < len; i++)
+		RTW_INFO("%02x ", tx_msg[i]);
+	RTW_INFO("\n");
+	RTW_INFO("Msg name: %s <======\n", msg_name);
+}
+
+/* Porting from Windows team */
+void rtw_btcoex_SendEventExtBtCoexControl(PADAPTER padapter, u8 bNeedDbgRsp, u8 dataLen, void *pData)
+{
+	u8			len = 0, tx_event_length = 0;
+	u8 			localBuf[32] = "";
+	u8			*pRetPar;
+	u8			opCode = 0;
+	u8			*pInBuf = (u8 *)pData;
+	u8			*pOpCodeContent;
+	rtw_HCI_event *pEvent;
+
+	opCode = pInBuf[0];
+
+	RTW_INFO("%s, OPCode:%02x\n", __func__, opCode);
+
+	pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+	/* len += bthci_ExtensionEventHeaderRtk(&localBuf[0], */
+	/*	HCI_EVENT_EXT_BT_COEX_CONTROL); */
+	pEvent->EventCode = HCI_EVENT_EXTENSION_RTK;
+	pEvent->Data[0] = HCI_EVENT_EXT_BT_COEX_CONTROL;	/* extension event code */
+	len++;
+
+	/* Return parameters starts from here */
+	pRetPar = &pEvent->Data[len];
+	memcpy(&pRetPar[0], pData, dataLen);
+
+	len += dataLen;
+
+	pEvent->Length = len;
+
+	/* total tx event length + EventCode length + sizeof(length) */
+	tx_event_length = pEvent->Length + 2;
+	rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+}
+
+/* Porting from Windows team */
+void rtw_btcoex_SendEventExtBtInfoControl(PADAPTER padapter, u8 dataLen, void *pData)
+{
+	rtw_HCI_event *pEvent;
+	u8			*pRetPar;
+	u8			len = 0, tx_event_length = 0;
+	u8 			localBuf[32] = "";
+
+	struct bt_coex_info *pcoex_info = &padapter->coex_info;
+	PBT_MGNT		pBtMgnt = &pcoex_info->BtMgnt;
+
+	/* RTW_INFO("%s\n",__func__);*/
+	if (pBtMgnt->ExtConfig.HCIExtensionVer < 4) { /* not support */
+		RTW_INFO("ERROR: HCIExtensionVer = %d, HCIExtensionVer<4 !!!!\n", pBtMgnt->ExtConfig.HCIExtensionVer);
+		return;
+	}
+
+	pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+	/* len += bthci_ExtensionEventHeaderRtk(&localBuf[0], */
+	/*		HCI_EVENT_EXT_BT_INFO_CONTROL); */
+	pEvent->EventCode = HCI_EVENT_EXTENSION_RTK;
+	pEvent->Data[0] = HCI_EVENT_EXT_BT_INFO_CONTROL;		/* extension event code */
+	len++;
+
+	/* Return parameters starts from here */
+	pRetPar = &pEvent->Data[len];
+	memcpy(&pRetPar[0], pData, dataLen);
+
+	len += dataLen;
+
+	pEvent->Length = len;
+
+	/* total tx event length + EventCode length + sizeof(length) */
+	tx_event_length = pEvent->Length + 2;
+	rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+}
+
+void rtw_btcoex_SendScanNotify(PADAPTER padapter, u8 scanType)
+{
+	u8	len = 0, tx_event_length = 0;
+	u8 	localBuf[7] = "";
+	u8	*pRetPar;
+	u8	*pu1Temp;
+	rtw_HCI_event *pEvent;
+	struct bt_coex_info *pcoex_info = &padapter->coex_info;
+	PBT_MGNT		pBtMgnt = &pcoex_info->BtMgnt;
+
+	/*	if(!pBtMgnt->BtOperationOn)
+	 *		return; */
+
+	pEvent = (rtw_HCI_event *)(&localBuf[0]);
+
+	/*	len += bthci_ExtensionEventHeaderRtk(&localBuf[0],
+	 *			HCI_EVENT_EXT_WIFI_SCAN_NOTIFY); */
+
+	pEvent->EventCode = HCI_EVENT_EXTENSION_RTK;
+	pEvent->Data[0] = HCI_EVENT_EXT_WIFI_SCAN_NOTIFY;		/* extension event code */
+	len++;
+
+	/* Return parameters starts from here */
+	/* pRetPar = &PPacketIrpEvent->Data[len]; */
+	/* pu1Temp = (u8 *)&pRetPar[0]; */
+	/* *pu1Temp = scanType; */
+	pEvent->Data[len] = scanType;
+	len += 1;
+
+	pEvent->Length = len;
+
+	/* total tx event length + EventCode length + sizeof(length) */
+	tx_event_length = pEvent->Length + 2;
+	rtw_btcoex_sendmsgbysocket(padapter, (u8 *)pEvent, tx_event_length, false);
+}
+#endif /* CONFIG_BT_COEXIST_SOCKET_TRX */
+#endif /* CONFIG_BT_COEXIST */
diff --git a/drivers/staging/rtl8188eu/core/rtw_btcoex_wifionly.c b/drivers/staging/rtl8188eu/core/rtw_btcoex_wifionly.c
new file mode 100644
index 000000000000..3dc651a95264
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_btcoex_wifionly.c
@@ -0,0 +1,26 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#include <drv_types.h>
+#include <hal_btcoex_wifionly.h>
+#include <hal_data.h>
+
+void rtw_btcoex_wifionly_switchband_notify(PADAPTER padapter)
+{
+	hal_btcoex_wifionly_switchband_notify(padapter);
+}
+
+void rtw_btcoex_wifionly_scan_notify(PADAPTER padapter)
+{
+	hal_btcoex_wifionly_scan_notify(padapter);
+}
+
+void rtw_btcoex_wifionly_hw_config(PADAPTER padapter)
+{
+	hal_btcoex_wifionly_hw_config(padapter);
+}
+
+void rtw_btcoex_wifionly_initialize(PADAPTER padapter)
+{
+	hal_btcoex_wifionly_initlizevariables(padapter);
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_cmd.c b/drivers/staging/rtl8188eu/core/rtw_cmd.c
new file mode 100644
index 000000000000..162725e43e45
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_cmd.c
@@ -0,0 +1,4433 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_CMD_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#ifndef DBG_CMD_EXECUTE
+	#define DBG_CMD_EXECUTE 0
+#endif
+
+/* Caller and the rtw_cmd_thread can protect cmd_q by spin_lock.
+ * No irqsave is necessary.
+ */
+
+sint	_rtw_init_cmd_priv(struct	cmd_priv *pcmdpriv)
+{
+	sint res = _SUCCESS;
+
+	sema_init(&(pcmdpriv->cmd_queue_sema), 0);
+	sema_init(&(pcmdpriv->terminate_cmdthread_sema), 0);
+
+
+	_rtw_init_queue(&(pcmdpriv->cmd_queue));
+
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+	pcmdpriv->cmd_seq = 1;
+
+	pcmdpriv->cmd_allocated_buf = rtw_zmalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ);
+
+	if (!pcmdpriv->cmd_allocated_buf) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf  +  CMDBUFF_ALIGN_SZ - ((SIZE_PTR)(pcmdpriv->cmd_allocated_buf) & (CMDBUFF_ALIGN_SZ - 1));
+
+	pcmdpriv->rsp_allocated_buf = rtw_zmalloc(MAX_RSPSZ + 4);
+
+	if (!pcmdpriv->rsp_allocated_buf) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf  +  4 - ((SIZE_PTR)(pcmdpriv->rsp_allocated_buf) & 3);
+
+	pcmdpriv->cmd_issued_cnt = pcmdpriv->cmd_done_cnt = pcmdpriv->rsp_cnt = 0;
+
+	_rtw_mutex_init(&pcmdpriv->sctx_mutex);
+exit:
+
+
+	return res;
+
+}
+
+#ifdef CONFIG_C2H_WK
+static void c2h_wk_callback(_workitem *work)
+{
+	struct evt_priv *evtpriv = container_of(work, struct evt_priv, c2h_wk);
+	_adapter *adapter = container_of(evtpriv, _adapter, evtpriv);
+	u8 *c2h_evt;
+	c2h_id_filter direct_hdl_filter = rtw_hal_c2h_id_handle_directly;
+	u8 id, seq, plen;
+	u8 *payload;
+
+	evtpriv->c2h_wk_alive = true;
+
+	while (!rtw_cbuf_empty(evtpriv->c2h_queue)) {
+		c2h_evt = (u8 *)rtw_cbuf_pop(evtpriv->c2h_queue);
+		if (c2h_evtL) {
+			/* This C2H event is read, clear it */
+			c2h_evt_clear(adapter);
+		} else {
+			c2h_evt = (u8 *)rtw_malloc(C2H_REG_LEN);
+			if (!c2h_evt) {
+				rtw_warn_on(1);
+				continue;
+			}
+
+			/* This C2H event is not read, read & clear now */
+			if (rtw_hal_c2h_evt_read(adapter, c2h_evt) != _SUCCESS) {
+				rtw_mfree(c2h_evt, C2H_REG_LEN);
+				continue;
+			}
+		}
+
+		/* Special pointer to trigger c2h_evt_clear only */
+		if ((void *)c2h_evt == (void *)evtpriv)
+			continue;
+
+		if (!rtw_hal_c2h_valid(adapter, c2h_evt)
+			|| rtw_hal_c2h_reg_hdr_parse(adapter, c2h_evt, &id, &seq, &plen, &payload) != _SUCCESS
+		) {
+			rtw_mfree(c2h_evt, C2H_REG_LEN);
+			continue;
+		}
+
+		if (direct_hdl_filter(adapter, id, seq, plen, payload) == true) {
+			/* Handle directly */
+			rtw_hal_c2h_handler(adapter, id, seq, plen, payload);
+			rtw_mfree(c2h_evt, C2H_REG_LEN);
+		} else {
+			/* Enqueue into cmd_thread for others */
+			rtw_c2h_reg_wk_cmd(adapter, c2h_evt);
+			rtw_mfree(c2h_evt, C2H_REG_LEN);
+		}
+	}
+
+	evtpriv->c2h_wk_alive = false;
+}
+#endif /* CONFIG_C2H_WK */
+
+sint _rtw_init_evt_priv(struct evt_priv *pevtpriv)
+{
+	sint res = _SUCCESS;
+
+
+#ifdef CONFIG_H2CLBK
+	sema_init(&(pevtpriv->lbkevt_done), 0);
+	pevtpriv->lbkevt_limit = 0;
+	pevtpriv->lbkevt_num = 0;
+	pevtpriv->cmdevt_parm = NULL;
+#endif
+
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+	ATOMIC_SET(&pevtpriv->event_seq, 0);
+	pevtpriv->evt_done_cnt = 0;
+
+#ifdef CONFIG_EVENT_THREAD_MODE
+
+	sema_init(&(pevtpriv->evt_notify), 0);
+	sema_init(&(pevtpriv->terminate_evtthread_sema), 0);
+
+	pevtpriv->evt_allocated_buf = rtw_zmalloc(MAX_EVTSZ + 4);
+	if (!pevtpriv->evt_allocated_buf) {
+		res = _FAIL;
+		goto exit;
+	}
+	pevtpriv->evt_buf = pevtpriv->evt_allocated_buf  +  4 - ((unsigned int)(pevtpriv->evt_allocated_buf) & 3);
+
+
+#if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
+	pevtpriv->allocated_c2h_mem = rtw_zmalloc(C2H_MEM_SZ + 4);
+
+	if (!pevtpriv->allocated_c2h_mem) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pevtpriv->c2h_mem = pevtpriv->allocated_c2h_mem +  4 -
+			    ((u32)(pevtpriv->allocated_c2h_mem) & 3);
+#endif /* end of CONFIG_SDIO_HCI */
+
+	_rtw_init_queue(&(pevtpriv->evt_queue));
+
+exit:
+
+#endif /* end of CONFIG_EVENT_THREAD_MODE */
+
+#ifdef CONFIG_C2H_WK
+	_init_workitem(&pevtpriv->c2h_wk, c2h_wk_callback, NULL);
+	pevtpriv->c2h_wk_alive = false;
+	pevtpriv->c2h_queue = rtw_cbuf_alloc(C2H_QUEUE_MAX_LEN + 1);
+#endif
+
+
+	return res;
+}
+
+void _rtw_free_evt_priv(struct	evt_priv *pevtpriv)
+{
+#ifdef CONFIG_EVENT_THREAD_MODE
+	if (pevtpriv->evt_allocated_buf)
+		rtw_mfree(pevtpriv->evt_allocated_buf, MAX_EVTSZ + 4);
+#endif
+
+#ifdef CONFIG_C2H_WK
+	_cancel_workitem_sync(&pevtpriv->c2h_wk);
+	while (pevtpriv->c2h_wk_alive)
+		rtw_msleep_os(10);
+
+	while (!rtw_cbuf_empty(pevtpriv->c2h_queue)) {
+		void *c2h;
+
+		c2h = rtw_cbuf_pop(pevtpriv->c2h_queue);
+		if (c2h && c2h != (void *)pevtpriv)
+			rtw_mfree(c2h, 16);
+	}
+	rtw_cbuf_free(pevtpriv->c2h_queue);
+#endif
+}
+
+void _rtw_free_cmd_priv(struct	cmd_priv *pcmdpriv)
+{
+	if (pcmdpriv) {
+		if (pcmdpriv->cmd_allocated_buf)
+			rtw_mfree(pcmdpriv->cmd_allocated_buf, MAX_CMDSZ + CMDBUFF_ALIGN_SZ);
+
+		if (pcmdpriv->rsp_allocated_buf)
+			rtw_mfree(pcmdpriv->rsp_allocated_buf, MAX_RSPSZ + 4);
+
+		_rtw_mutex_free(&pcmdpriv->sctx_mutex);
+	}
+}
+
+/* Calling Context:
+ * rtw_enqueue_cmd can only be called between kernel thread,
+ * since only spin_lock is used.
+
+ * ISR/Call-Back functions can't call this sub-function.
+ */
+#ifdef DBG_CMD_QUEUE
+extern u8 dump_cmd_id;
+#endif
+
+sint _rtw_enqueue_cmd(_queue *queue, struct cmd_obj *obj, bool to_head)
+{
+	unsigned long irqL;
+
+	if (!obj)
+		goto exit;
+
+	_enter_critical(&queue->lock, &irqL);
+
+	if (to_head)
+		list_add(&obj->list, &queue->queue);
+	else
+		list_add_tail(&obj->list, &queue->queue);
+
+#ifdef DBG_CMD_QUEUE
+	if (dump_cmd_id) {
+		RTW_INFO("%s===> cmdcode:0x%02x\n", __func__, obj->cmdcode);
+		if (obj->cmdcode == GEN_CMD_CODE(_Set_MLME_EVT)) {
+			if (obj->parmbuf) {
+				struct C2HEvent_Header *pc2h_evt_hdr = (struct C2HEvent_Header *)(obj->parmbuf);
+
+				RTW_INFO("pc2h_evt_hdr->ID:0x%02x(%d)\n", pc2h_evt_hdr->ID, pc2h_evt_hdr->ID);
+			}
+		}
+		if (obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
+			if (obj->parmbuf) {
+				struct drvextra_cmd_parm *pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)(obj->parmbuf);
+
+				RTW_INFO("pdrvextra_cmd_parm->ec_id:0x%02x\n", pdrvextra_cmd_parm->ec_id);
+			}
+		}
+	}
+
+	if (queue->queue.prev->next != &queue->queue) {
+		RTW_INFO("[%d] head %p, tail %p, tail->prev->next %p[tail], tail->next %p[head]\n", __LINE__,
+			&queue->queue, queue->queue.prev, queue->queue.prev->prev->next, queue->queue.prev->next);
+
+		RTW_INFO("==========%s============\n", __func__);
+		RTW_INFO("head:%p,obj_addr:%p\n", &queue->queue, obj);
+		RTW_INFO("padapter: %p\n", obj->padapter);
+		RTW_INFO("cmdcode: 0x%02x\n", obj->cmdcode);
+		RTW_INFO("res: %d\n", obj->res);
+		RTW_INFO("parmbuf: %p\n", obj->parmbuf);
+		RTW_INFO("cmdsz: %d\n", obj->cmdsz);
+		RTW_INFO("rsp: %p\n", obj->rsp);
+		RTW_INFO("rspsz: %d\n", obj->rspsz);
+		RTW_INFO("sctx: %p\n", obj->sctx);
+		RTW_INFO("list->next: %p\n", obj->list.next);
+		RTW_INFO("list->prev: %p\n", obj->list.prev);
+	}
+#endif /* DBG_CMD_QUEUE */
+
+	_exit_critical(&queue->lock, &irqL);
+exit:
+	return _SUCCESS;
+}
+
+struct	cmd_obj	*_rtw_dequeue_cmd(_queue *queue)
+{
+	unsigned long irqL;
+	struct cmd_obj *obj;
+
+	_enter_critical(&queue->lock, &irqL);
+
+#ifdef DBG_CMD_QUEUE
+	if (queue->queue.prev->next != &queue->queue) {
+		RTW_INFO("[%d] head %p, tail %p, tail->prev->next %p[tail], tail->next %p[head]\n", __LINE__,
+			&queue->queue, queue->queue.prev, queue->queue.prev->prev->next, queue->queue.prev->next);
+	}
+#endif /* DBG_CMD_QUEUE */
+
+	if (list_empty(&(queue->queue))) {
+		obj = NULL;
+	} else {
+		obj = LIST_CONTAINOR(get_next(&(queue->queue)), struct cmd_obj, list);
+#ifdef DBG_CMD_QUEUE
+		if (queue->queue.prev->next != &queue->queue) {
+			RTW_INFO("==========%s============\n", __func__);
+			RTW_INFO("head:%p,obj_addr:%p\n", &queue->queue, obj);
+			RTW_INFO("padapter: %p\n", obj->padapter);
+			RTW_INFO("cmdcode: 0x%02x\n", obj->cmdcode);
+			RTW_INFO("res: %d\n", obj->res);
+			RTW_INFO("parmbuf: %p\n", obj->parmbuf);
+			RTW_INFO("cmdsz: %d\n", obj->cmdsz);
+			RTW_INFO("rsp: %p\n", obj->rsp);
+			RTW_INFO("rspsz: %d\n", obj->rspsz);
+			RTW_INFO("sctx: %p\n", obj->sctx);
+			RTW_INFO("list->next: %p\n", obj->list.next);
+			RTW_INFO("list->prev: %p\n", obj->list.prev);
+		}
+
+		if (dump_cmd_id) {
+			RTW_INFO("%s===> cmdcode:0x%02x\n", __func__, obj->cmdcode);
+			if (obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
+				if (obj->parmbuf) {
+					struct drvextra_cmd_parm *pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)(obj->parmbuf);
+
+					RTW_INFO("pdrvextra_cmd_parm->ec_id:0x%02x\n", pdrvextra_cmd_parm->ec_id);
+				}
+			}
+
+		}
+#endif /* DBG_CMD_QUEUE */
+
+		list_del_init(&obj->list);
+	}
+	_exit_critical(&queue->lock, &irqL);
+
+	return obj;
+}
+
+u32	rtw_init_cmd_priv(struct cmd_priv *pcmdpriv)
+{
+	u32	res;
+
+	res = _rtw_init_cmd_priv(pcmdpriv);
+	return res;
+}
+
+u32	rtw_init_evt_priv(struct	evt_priv *pevtpriv)
+{
+	int	res;
+
+	res = _rtw_init_evt_priv(pevtpriv);
+	return res;
+}
+
+void rtw_free_evt_priv(struct	evt_priv *pevtpriv)
+{
+	_rtw_free_evt_priv(pevtpriv);
+}
+
+void rtw_free_cmd_priv(struct	cmd_priv *pcmdpriv)
+{
+	_rtw_free_cmd_priv(pcmdpriv);
+}
+
+int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+	u8 bAllow = false; /* set to true to allow enqueuing cmd when hw_init_completed is false */
+
+#ifdef SUPPORT_HW_RFOFF_DETECTED
+	/* To decide allow or not */
+	if ((adapter_to_pwrctl(pcmdpriv->padapter)->bHWPwrPindetect) &&
+	    (!pcmdpriv->padapter->registrypriv.usbss_enable)) {
+		if (cmd_obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
+			struct drvextra_cmd_parm *pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)cmd_obj->parmbuf;
+
+			if (pdrvextra_cmd_parm->ec_id == POWER_SAVING_CTRL_WK_CID)
+				bAllow = true;
+		}
+	}
+#endif
+
+	if (cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan))
+		bAllow = true;
+
+	if (cmd_obj->no_io)
+		bAllow = true;
+
+	if ((!rtw_is_hw_init_completed(pcmdpriv->padapter) && (bAllow == false))
+	    || ATOMIC_READ(&(pcmdpriv->cmdthd_running)) == false	/* com_thread not running */
+	   ) {
+		if (DBG_CMD_EXECUTE)
+			RTW_INFO(ADPT_FMT" drop "CMD_FMT" hw_init_completed:%u, cmdthd_running:%u\n", ADPT_ARG(cmd_obj->padapter)
+				, CMD_ARG(cmd_obj), rtw_get_hw_init_completed(cmd_obj->padapter), ATOMIC_READ(&pcmdpriv->cmdthd_running));
+		if (0)
+			rtw_warn_on(1);
+
+		return _FAIL;
+	}
+	return _SUCCESS;
+}
+
+
+
+u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+	int res = _FAIL;
+	PADAPTER padapter = pcmdpriv->padapter;
+
+
+	if (!cmd_obj)
+		goto exit;
+
+	cmd_obj->padapter = padapter;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	/* change pcmdpriv to primary's pcmdpriv */
+	if (padapter->adapter_type != PRIMARY_ADAPTER)
+		pcmdpriv = &(GET_PRIMARY_ADAPTER(padapter)->cmdpriv);
+#endif
+
+	res = rtw_cmd_filter(pcmdpriv, cmd_obj);
+	if ((res == _FAIL) || (cmd_obj->cmdsz > MAX_CMDSZ)) {
+		if (cmd_obj->cmdsz > MAX_CMDSZ) {
+			RTW_INFO("%s failed due to obj->cmdsz(%d) > MAX_CMDSZ(%d)\n", __func__, cmd_obj->cmdsz, MAX_CMDSZ);
+			rtw_warn_on(1);
+		}
+
+		if (cmd_obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
+			struct drvextra_cmd_parm *extra_parm = (struct drvextra_cmd_parm *)cmd_obj->parmbuf;
+
+			if (extra_parm->pbuf && extra_parm->size > 0)
+				rtw_mfree(extra_parm->pbuf, extra_parm->size);
+		}
+		rtw_free_cmd_obj(cmd_obj);
+		goto exit;
+	}
+
+	res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj, 0);
+
+	if (res == _SUCCESS)
+		up(&pcmdpriv->cmd_queue_sema);
+
+exit:
+
+
+	return res;
+}
+
+struct	cmd_obj	*rtw_dequeue_cmd(struct cmd_priv *pcmdpriv)
+{
+	struct cmd_obj *cmd_obj;
+
+
+	cmd_obj = _rtw_dequeue_cmd(&pcmdpriv->cmd_queue);
+
+	return cmd_obj;
+}
+
+void rtw_cmd_clr_isr(struct	cmd_priv *pcmdpriv)
+{
+	pcmdpriv->cmd_done_cnt++;
+	/* up(&(pcmdpriv->cmd_done_sema)); */
+}
+
+void rtw_free_cmd_obj(struct cmd_obj *pcmd)
+{
+	struct drvextra_cmd_parm *extra_parm = NULL;
+
+	if (pcmd->parmbuf) {
+		/* free parmbuf in cmd_obj */
+		rtw_mfree((unsigned char *)pcmd->parmbuf, pcmd->cmdsz);
+	}
+	if (pcmd->rsp) {
+		if (pcmd->rspsz != 0) {
+			/* free rsp in cmd_obj */
+			rtw_mfree((unsigned char *)pcmd->rsp, pcmd->rspsz);
+		}
+	}
+
+	/* free cmd_obj */
+	rtw_mfree((unsigned char *)pcmd, sizeof(struct cmd_obj));
+}
+
+
+void rtw_stop_cmd_thread(_adapter *adapter)
+{
+	if (adapter->cmdThread &&
+	    ATOMIC_READ(&(adapter->cmdpriv.cmdthd_running)) == true &&
+	    adapter->cmdpriv.stop_req == 0) {
+		adapter->cmdpriv.stop_req = 1;
+		up(&adapter->cmdpriv.cmd_queue_sema);
+		_rtw_down_sema(&adapter->cmdpriv.terminate_cmdthread_sema);
+	}
+}
+
+thread_return rtw_cmd_thread(thread_context context)
+{
+	u8 ret;
+	struct cmd_obj *pcmd;
+	u8 *pcmdbuf, *prspbuf;
+	u32 cmd_start_time;
+	u32 cmd_process_time;
+	u8 (*cmd_hdl)(_adapter *padapter, u8 *pbuf);
+	void (*pcmd_callback)(_adapter *dev, struct cmd_obj *pcmd);
+	PADAPTER padapter = (PADAPTER)context;
+	struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
+	struct drvextra_cmd_parm *extra_parm = NULL;
+	unsigned long irqL;
+
+	thread_enter("RTW_CMD_THREAD");
+
+	pcmdbuf = pcmdpriv->cmd_buf;
+	prspbuf = pcmdpriv->rsp_buf;
+
+	pcmdpriv->stop_req = 0;
+	ATOMIC_SET(&(pcmdpriv->cmdthd_running), true);
+	up(&pcmdpriv->terminate_cmdthread_sema);
+
+
+	while (1) {
+		if (_rtw_down_sema(&pcmdpriv->cmd_queue_sema) == _FAIL) {
+			RTW_PRINT(FUNC_ADPT_FMT" _rtw_down_sema(&pcmdpriv->cmd_queue_sema) return _FAIL, break\n", FUNC_ADPT_ARG(padapter));
+			break;
+		}
+
+		if (RTW_CANNOT_RUN(padapter)) {
+			RTW_PRINT("%s: DriverStopped(%s) SurpriseRemoved(%s) break at line %d\n",
+				  __func__
+				, rtw_is_drv_stopped(padapter) ? "True" : "False"
+				, rtw_is_surprise_removed(padapter) ? "True" : "False"
+				  , __LINE__);
+			break;
+		}
+
+		if (pcmdpriv->stop_req) {
+			RTW_PRINT(FUNC_ADPT_FMT" stop_req:%u, break\n", FUNC_ADPT_ARG(padapter), pcmdpriv->stop_req);
+			break;
+		}
+
+		_enter_critical(&pcmdpriv->cmd_queue.lock, &irqL);
+		if (list_empty(&(pcmdpriv->cmd_queue.queue))) {
+			/* RTW_INFO("%s: cmd queue is empty!\n", __func__); */
+			_exit_critical(&pcmdpriv->cmd_queue.lock, &irqL);
+			continue;
+		}
+		_exit_critical(&pcmdpriv->cmd_queue.lock, &irqL);
+
+_next:
+		if (RTW_CANNOT_RUN(padapter)) {
+			RTW_PRINT("%s: DriverStopped(%s) SurpriseRemoved(%s) break at line %d\n",
+				  __func__
+				, rtw_is_drv_stopped(padapter) ? "True" : "False"
+				, rtw_is_surprise_removed(padapter) ? "True" : "False"
+				  , __LINE__);
+			break;
+		}
+
+		pcmd = rtw_dequeue_cmd(pcmdpriv);
+		if (!pcmd) {
+#ifdef CONFIG_LPS_LCLK
+			rtw_unregister_cmd_alive(padapter);
+#endif
+			continue;
+		}
+
+		cmd_start_time = jiffies;
+		pcmdpriv->cmd_issued_cnt++;
+
+		if (pcmd->cmdsz > MAX_CMDSZ) {
+			RTW_ERR("%s cmdsz:%d > MAX_CMDSZ:%d\n", __func__, pcmd->cmdsz, MAX_CMDSZ);
+			pcmd->res = H2C_PARAMETERS_ERROR;
+			goto post_process;
+		}
+
+		if (pcmd->cmdcode >= (sizeof(wlancmds) / sizeof(struct cmd_hdl))) {
+			RTW_ERR("%s undefined cmdcode:%d\n", __func__, pcmd->cmdcode);
+			pcmd->res = H2C_PARAMETERS_ERROR;
+			goto post_process;
+		}
+
+		cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
+		if (!cmd_hdl) {
+			RTW_ERR("%s no cmd_hdl for cmdcode:%d\n", __func__, pcmd->cmdcode);
+			pcmd->res = H2C_PARAMETERS_ERROR;
+			goto post_process;
+		}
+
+		if (rtw_cmd_filter(pcmdpriv, pcmd) == _FAIL) {
+			pcmd->res = H2C_DROPPED;
+			if (pcmd->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
+				extra_parm = (struct drvextra_cmd_parm *)pcmd->parmbuf;
+				if (extra_parm && extra_parm->pbuf && extra_parm->size > 0)
+					rtw_mfree(extra_parm->pbuf, extra_parm->size);
+			}
+			goto post_process;
+		}
+
+#ifdef CONFIG_LPS_LCLK
+		if (pcmd->no_io)
+			rtw_unregister_cmd_alive(padapter);
+		else {
+			if (rtw_register_cmd_alive(padapter) != _SUCCESS) {
+				if (DBG_CMD_EXECUTE)
+					RTW_PRINT("%s: wait to leave LPS_LCLK\n", __func__);
+
+				pcmd->res = H2C_ENQ_HEAD;
+				ret = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, pcmd, 1);
+				if (ret == _SUCCESS) {
+					if (DBG_CMD_EXECUTE)
+						RTW_INFO(ADPT_FMT" "CMD_FMT" ENQ_HEAD\n", ADPT_ARG(pcmd->padapter), CMD_ARG(pcmd));
+					continue;
+				}
+
+				RTW_INFO(ADPT_FMT" "CMD_FMT" ENQ_HEAD_FAIL\n", ADPT_ARG(pcmd->padapter), CMD_ARG(pcmd));
+				pcmd->res = H2C_ENQ_HEAD_FAIL;
+				rtw_warn_on(1);
+			}
+		}
+#endif /* CONFIG_LPS_LCLK */
+
+		if (DBG_CMD_EXECUTE)
+			RTW_INFO(ADPT_FMT" "CMD_FMT" %sexecute\n", ADPT_ARG(pcmd->padapter), CMD_ARG(pcmd)
+				, pcmd->res == H2C_ENQ_HEAD ? "ENQ_HEAD " : (pcmd->res == H2C_ENQ_HEAD_FAIL ? "ENQ_HEAD_FAIL " : ""));
+
+		memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
+		ret = cmd_hdl(pcmd->padapter, pcmdbuf);
+		pcmd->res = ret;
+
+		pcmdpriv->cmd_seq++;
+
+post_process:
+
+		_enter_critical_mutex(&(pcmd->padapter->cmdpriv.sctx_mutex), NULL);
+		if (pcmd->sctx) {
+			if (0)
+				RTW_PRINT(FUNC_ADPT_FMT" pcmd->sctx\n",
+					  FUNC_ADPT_ARG(pcmd->padapter));
+			if (pcmd->res == H2C_SUCCESS)
+				rtw_sctx_done(&pcmd->sctx);
+			else
+				rtw_sctx_done_err(&pcmd->sctx, RTW_SCTX_DONE_CMD_ERROR);
+		}
+		_exit_critical_mutex(&(pcmd->padapter->cmdpriv.sctx_mutex), NULL);
+
+		cmd_process_time = rtw_get_passing_time_ms(cmd_start_time);
+		if (cmd_process_time > 1000) {
+			RTW_INFO(ADPT_FMT" "CMD_FMT" process_time=%d\n", ADPT_ARG(pcmd->padapter), CMD_ARG(pcmd), cmd_process_time);
+			if (0)
+				rtw_warn_on(1);
+		}
+
+		/* call callback function for post-processed */
+		if (pcmd->cmdcode < (sizeof(rtw_cmd_callback) / sizeof(struct _cmd_callback))) {
+			pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback;
+			if (!pcmd_callback) {
+				rtw_free_cmd_obj(pcmd);
+			} else {
+				/* todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!=NULL) */
+				pcmd_callback(pcmd->padapter, pcmd);/* need conider that free cmd_obj in rtw_cmd_callback */
+			}
+		} else {
+			rtw_free_cmd_obj(pcmd);
+		}
+
+		flush_signals_thread();
+
+		goto _next;
+
+	}
+
+#ifdef CONFIG_LPS_LCLK
+	rtw_unregister_cmd_alive(padapter);
+#endif
+
+	/* to avoid enqueue cmd after free all cmd_obj */
+	ATOMIC_SET(&(pcmdpriv->cmdthd_running), false);
+
+	/* free all cmd_obj resources */
+	do {
+		pcmd = rtw_dequeue_cmd(pcmdpriv);
+		if (!pcmd)
+			break;
+
+		if (pcmd->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
+			extra_parm = (struct drvextra_cmd_parm *)pcmd->parmbuf;
+			if (extra_parm->pbuf && extra_parm->size > 0)
+				rtw_mfree(extra_parm->pbuf, extra_parm->size);
+		}
+
+		rtw_free_cmd_obj(pcmd);
+	} while (1);
+
+	up(&pcmdpriv->terminate_cmdthread_sema);
+
+
+	thread_exit();
+
+}
+
+
+#ifdef CONFIG_EVENT_THREAD_MODE
+u32 rtw_enqueue_evt(struct evt_priv *pevtpriv, struct evt_obj *obj)
+{
+	unsigned long irqL;
+	int	res;
+	_queue *queue = &pevtpriv->evt_queue;
+
+
+	res = _SUCCESS;
+
+	if (!obj) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	list_add_tail(&obj->list, &queue->queue);
+
+	_exit_critical_bh(&queue->lock, &irqL);
+
+exit:
+
+	return res;
+}
+
+struct evt_obj *rtw_dequeue_evt(_queue *queue)
+{
+	unsigned long irqL;
+	struct	evt_obj	*pevtobj;
+
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	if (list_empty(&(queue->queue)))
+		pevtobj = NULL;
+	else {
+		pevtobj = LIST_CONTAINOR(get_next(&(queue->queue)), struct evt_obj, list);
+		list_del_init(&pevtobj->list);
+	}
+
+	_exit_critical_bh(&queue->lock, &irqL);
+
+
+	return pevtobj;
+}
+
+void rtw_free_evt_obj(struct evt_obj *pevtobj)
+{
+
+	if (pevtobj->parmbuf)
+		rtw_mfree((unsigned char *)pevtobj->parmbuf, pevtobj->evtsz);
+
+	rtw_mfree((unsigned char *)pevtobj, sizeof(struct evt_obj));
+
+}
+
+void rtw_evt_notify_isr(struct evt_priv *pevtpriv)
+{
+	pevtpriv->evt_done_cnt++;
+	up(&(pevtpriv->evt_notify));
+}
+#endif
+
+u8 rtw_setstandby_cmd(_adapter *padapter, uint action)
+{
+	struct cmd_obj			*ph2c;
+	struct usb_suspend_parm	*psetusbsuspend;
+	struct cmd_priv			*pcmdpriv = &padapter->cmdpriv;
+	u8 ret = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		ret = _FAIL;
+		goto exit;
+	}
+	psetusbsuspend = (struct usb_suspend_parm *)rtw_zmalloc(sizeof(struct usb_suspend_parm));
+	if (psetusbsuspend == NULL) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		ret = _FAIL;
+		goto exit;
+	}
+	psetusbsuspend->action = action;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetusbsuspend, GEN_CMD_CODE(_SetUsbSuspend));
+
+	ret = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return ret;
+}
+
+/* ### NOTE:#### (!!!!)
+ * MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
+ */
+u8 rtw_sitesurvey_cmd(_adapter  *padapter, NDIS_802_11_SSID *ssid, int ssid_num,
+		      struct rtw_ieee80211_channel *ch, int ch_num)
+{
+	u8 res = _FAIL;
+	struct cmd_obj		*ph2c;
+	struct sitesurvey_parm	*psurveyPara;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_P2P */
+
+#ifdef CONFIG_LPS
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 1);
+#endif
+
+#ifdef CONFIG_P2P_PS
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+		p2p_ps_wk_cmd(padapter, P2P_PS_SCAN, 1);
+#endif /* CONFIG_P2P_PS */
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c)
+		return _FAIL;
+
+	psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm));
+	if (!psurveyPara) {
+		rtw_mfree((unsigned char *) ph2c, sizeof(struct cmd_obj));
+		return _FAIL;
+	}
+
+	rtw_free_network_queue(padapter, false);
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+
+	psurveyPara->scan_mode = pmlmepriv->scan_mode;
+
+	/* prepare ssid list */
+	if (ssid) {
+		int i;
+
+		for (i = 0; i < ssid_num && i < RTW_SSID_SCAN_AMOUNT; i++) {
+			if (ssid[i].SsidLength) {
+				memcpy(&psurveyPara->ssid[i], &ssid[i], sizeof(NDIS_802_11_SSID));
+				psurveyPara->ssid_num++;
+			}
+		}
+	}
+
+	/* prepare channel list */
+	if (ch) {
+		int i;
+
+		for (i = 0; i < ch_num && i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
+			if (ch[i].hw_value && !(ch[i].flags & RTW_IEEE80211_CHAN_DISABLED)) {
+				memcpy(&psurveyPara->ch[i], &ch[i], sizeof(struct rtw_ieee80211_channel));
+				psurveyPara->ch_num++;
+			}
+		}
+	}
+
+	set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+	if (res == _SUCCESS) {
+
+		pmlmepriv->scan_start_time = jiffies;
+
+#ifdef CONFIG_SCAN_BACKOP
+		if (rtw_mi_buddy_check_mlmeinfo_state(padapter, WIFI_FW_AP_STATE)) {
+			if (is_supported_5g(padapter->registrypriv.wireless_mode)
+			    && IsSupported24G(padapter->registrypriv.wireless_mode)) /* dual band */
+				mlme_set_scan_to_timer(pmlmepriv, CONC_SCANNING_TIMEOUT_DUAL_BAND);
+			else /* single band */
+				mlme_set_scan_to_timer(pmlmepriv, CONC_SCANNING_TIMEOUT_SINGLE_BAND);
+		} else
+#endif /* CONFIG_SCAN_BACKOP */
+			mlme_set_scan_to_timer(pmlmepriv, SCANNING_TIMEOUT);
+
+		rtw_led_control(padapter, LED_CTL_SITE_SURVEY);
+	} else
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+	return res;
+}
+
+u8 rtw_setdatarate_cmd(_adapter *padapter, u8 *rateset)
+{
+	struct cmd_obj *ph2c;
+	struct setdatarate_parm	*pbsetdataratepara;
+	struct cmd_priv	 *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pbsetdataratepara = (struct setdatarate_parm *)rtw_zmalloc(sizeof(struct setdatarate_parm));
+	if (!pbsetdataratepara) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pbsetdataratepara, GEN_CMD_CODE(_SetDataRate));
+#ifdef MP_FIRMWARE_OFFLOAD
+	pbsetdataratepara->curr_rateidx = *(u32 *)rateset;
+#else
+	pbsetdataratepara->mac_id = 5;
+	memcpy(pbsetdataratepara->datarates, rateset, NumRates);
+#endif
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+u8 rtw_setbasicrate_cmd(_adapter *padapter, u8 *rateset)
+{
+	struct cmd_obj *ph2c;
+	struct setbasicrate_parm *pssetbasicratepara;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+	pssetbasicratepara = (struct setbasicrate_parm *)rtw_zmalloc(sizeof(struct setbasicrate_parm));
+
+	if (!pssetbasicratepara) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pssetbasicratepara, _SetBasicRate_CMD_);
+
+	memcpy(pssetbasicratepara->basicrates, rateset, NumRates);
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+	return res;
+}
+
+/* unsigned char rtw_setphy_cmd(unsigned char  *adapter)
+ * 1.  be called only after rtw_update_registrypriv_dev_network( ~) or mp testing program
+ * 2.  for AdHoc/Ap mode or mp mode?
+ */
+u8 rtw_setphy_cmd(_adapter *padapter, u8 modem, u8 ch)
+{
+	struct cmd_obj			*ph2c;
+	struct setphy_parm		*psetphypara;
+	struct cmd_priv			*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+	psetphypara = (struct setphy_parm *)rtw_zmalloc(sizeof(struct setphy_parm));
+
+	if (!psetphypara) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetphypara, _SetPhy_CMD_);
+
+	psetphypara->modem = modem;
+	psetphypara->rfchannel = ch;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+u8 rtw_getmacreg_cmd(_adapter *padapter, u8 len, u32 addr)
+{
+	struct cmd_obj *ph2c;
+	struct readMAC_parm *preadmacparm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+	preadmacparm = (struct readMAC_parm *)rtw_zmalloc(sizeof(struct readMAC_parm));
+
+	if (!preadmacparm) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+	init_h2fwcmd_w_parm_no_rsp(ph2c, preadmacparm, GEN_CMD_CODE(_GetMACReg));
+
+	preadmacparm->len = len;
+	preadmacparm->addr = addr;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+void rtw_usb_catc_trigger_cmd(_adapter *padapter, const char *caller)
+{
+	RTW_INFO("%s caller:%s\n", __func__, caller);
+	rtw_getmacreg_cmd(padapter, 1, 0x1c4);
+}
+
+u8 rtw_setbbreg_cmd(_adapter *padapter, u8 offset, u8 val)
+{
+	struct cmd_obj *ph2c;
+	struct writeBB_parm *pwritebbparm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+	pwritebbparm = (struct writeBB_parm *)rtw_zmalloc(sizeof(struct writeBB_parm));
+
+	if (!pwritebbparm) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pwritebbparm, GEN_CMD_CODE(_SetBBReg));
+	pwritebbparm->offset = offset;
+	pwritebbparm->value = val;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+u8 rtw_getbbreg_cmd(_adapter  *padapter, u8 offset, u8 *pval)
+{
+	struct cmd_obj *ph2c;
+	struct readBB_parm *prdbbparm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+	prdbbparm = (struct readBB_parm *)rtw_zmalloc(sizeof(struct readBB_parm));
+
+	if (!prdbbparm) {
+		rtw_mfree((unsigned char *) ph2c, sizeof(struct	cmd_obj));
+		return _FAIL;
+	}
+	INIT_LIST_HEAD(&ph2c->list);
+	ph2c->cmdcode = GEN_CMD_CODE(_GetBBReg);
+	ph2c->parmbuf = (unsigned char *)prdbbparm;
+	ph2c->cmdsz =  sizeof(struct readBB_parm);
+	ph2c->rsp = pval;
+	ph2c->rspsz = sizeof(struct readBB_rsp);
+	prdbbparm->offset = offset;
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+u8 rtw_setrfreg_cmd(_adapter  *padapter, u8 offset, u32 val)
+{
+	struct cmd_obj *ph2c;
+	struct writeRF_parm *pwriterfparm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+	pwriterfparm = (struct writeRF_parm *)rtw_zmalloc(sizeof(struct writeRF_parm));
+
+	if (!pwriterfparm) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pwriterfparm, GEN_CMD_CODE(_SetRFReg));
+
+	pwriterfparm->offset = offset;
+	pwriterfparm->value = val;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+u8 rtw_getrfreg_cmd(_adapter  *padapter, u8 offset, u8 *pval)
+{
+	struct cmd_obj *ph2c;
+	struct readRF_parm *prdrfparm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+	prdrfparm = (struct readRF_parm *)rtw_zmalloc(sizeof(struct readRF_parm));
+	if (!prdrfparm) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+	INIT_LIST_HEAD(&ph2c->list);
+	ph2c->cmdcode = GEN_CMD_CODE(_GetRFReg);
+	ph2c->parmbuf = (unsigned char *)prdrfparm;
+	ph2c->cmdsz =  sizeof(struct readRF_parm);
+	ph2c->rsp = pval;
+	ph2c->rspsz = sizeof(struct readRF_rsp);
+
+	prdrfparm->offset = offset;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+void rtw_getbbrfreg_cmdrsp_callback(_adapter *padapter, struct cmd_obj *pcmd)
+{
+	rtw_mfree((unsigned char *) pcmd->parmbuf, pcmd->cmdsz);
+	rtw_mfree((unsigned char *) pcmd, sizeof(struct cmd_obj));
+
+#ifdef CONFIG_MP_INCLUDED
+	if (padapter->registrypriv.mp_mode == 1)
+		padapter->mppriv.workparam.bcompleted = true;
+#endif
+}
+
+void rtw_readtssi_cmdrsp_callback(_adapter *padapter, struct cmd_obj *pcmd)
+{
+	rtw_mfree((unsigned char *) pcmd->parmbuf, pcmd->cmdsz);
+	rtw_mfree((unsigned char *) pcmd, sizeof(struct cmd_obj));
+
+#ifdef CONFIG_MP_INCLUDED
+	if (padapter->registrypriv.mp_mode == 1)
+		padapter->mppriv.workparam.bcompleted = true;
+#endif
+
+}
+
+static u8 rtw_createbss_cmd(_adapter  *adapter, int flags, bool adhoc,
+			    s16 req_ch, s8 req_bw, s8 req_offset)
+{
+	struct cmd_obj *cmdobj;
+	struct createbss_parm *parm;
+	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct submit_ctx sctx;
+	u8 res = _SUCCESS;
+
+	if (req_ch > 0 && req_bw >= 0 && req_offset >= 0) {
+		if (!rtw_chset_is_chbw_valid(adapter->mlmeextpriv.channel_set, req_ch, req_bw, req_offset)) {
+			res = _FAIL;
+			goto exit;
+		}
+	}
+
+	/* prepare cmd parameter */
+	parm = (struct createbss_parm *)rtw_zmalloc(sizeof(*parm));
+	if (!parm) {
+		res = _FAIL;
+		goto exit;
+	}
+	if (adhoc) {
+		/* for now, adhoc doesn't support ch,bw,offset request */
+		parm->adhoc = 1;
+	} else {
+		parm->adhoc = 0;
+		parm->req_ch = req_ch;
+		parm->req_bw = req_bw;
+		parm->req_offset = req_offset;
+	}
+
+	if (flags & RTW_CMDF_DIRECTLY) {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (createbss_hdl(adapter, (u8 *)parm) != H2C_SUCCESS)
+			res = _FAIL;
+		rtw_mfree((u8 *)parm, sizeof(*parm));
+	} else {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj));
+		if (cmdobj == NULL) {
+			res = _FAIL;
+			rtw_mfree((u8 *)parm, sizeof(*parm));
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, GEN_CMD_CODE(_CreateBss));
+
+		if (flags & RTW_CMDF_WAIT_ACK) {
+			cmdobj->sctx = &sctx;
+			rtw_sctx_init(&sctx, 2000);
+		}
+
+		res = rtw_enqueue_cmd(pcmdpriv, cmdobj);
+
+		if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) {
+			rtw_sctx_wait(&sctx, __func__);
+			_enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
+			if (sctx.status == RTW_SCTX_SUBMITTED)
+				cmdobj->sctx = NULL;
+			_exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
+		}
+	}
+
+exit:
+	return res;
+}
+
+inline u8 rtw_create_ibss_cmd(_adapter *adapter, int flags)
+{
+/* for now, adhoc doesn't support ch,bw,offset request */
+	return rtw_createbss_cmd(adapter, flags, 1,
+				 0, -1, -1);
+}
+
+inline u8 rtw_startbss_cmd(_adapter *adapter, int flags)
+{
+/* excute entire AP setup cmd */
+	return rtw_createbss_cmd(adapter, flags, 0, 0, -1, -1);
+}
+
+inline u8 rtw_change_bss_chbw_cmd(_adapter *adapter, int flags, s16 req_ch, s8 req_bw, s8 req_offset)
+{
+	return rtw_createbss_cmd(adapter, flags, 0,
+				 req_ch, req_bw, req_offset);
+}
+
+u8 rtw_joinbss_cmd(_adapter  *padapter, struct wlan_network *pnetwork)
+{
+	u8 *auth, res = _SUCCESS;
+	uint t_len = 0;
+	WLAN_BSSID_EX *psecnetwork;
+	struct cmd_obj *pcmd;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+	NDIS_802_11_NETWORK_INFRASTRUCTURE ndis_network_mode = pnetwork->network.InfrastructureMode;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u32 tmp_len;
+	u8 *ptmp = NULL;
+#ifdef CONFIG_RTW_80211R
+	struct _ft_priv *pftpriv = &pmlmepriv->ftpriv;
+#endif
+
+	rtw_led_control(padapter, LED_CTL_START_TO_LINK);
+
+	pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!pcmd) {
+		res = _FAIL;
+		goto exit;
+	}
+	/* for IEs is fix buf size */
+	t_len = sizeof(WLAN_BSSID_EX);
+
+	/* for hidden ap to set fw_state here */
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE) != true) {
+		switch (ndis_network_mode) {
+		case Ndis802_11IBSS:
+			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			break;
+		case Ndis802_11Infrastructure:
+			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+			break;
+		case Ndis802_11APMode:
+		case Ndis802_11AutoUnknown:
+		case Ndis802_11InfrastructureMax:
+		case Ndis802_11Monitor:
+			break;
+		}
+	}
+
+	pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.IEs, pnetwork->network.IELength);
+
+	/* Solution for allocating a new WLAN_BSSID_EX to avoid race condition issue between disconnect and joinbss
+	 */
+	psecnetwork = (WLAN_BSSID_EX *)rtw_zmalloc(sizeof(WLAN_BSSID_EX));
+	if (!psecnetwork) {
+		if (pcmd)
+			rtw_mfree((unsigned char *)pcmd, sizeof(struct	cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	memset(psecnetwork, 0, t_len);
+
+	memcpy(psecnetwork, &pnetwork->network, get_WLAN_BSSID_EX_sz(&pnetwork->network));
+
+	auth = &psecuritypriv->authenticator_ie[0];
+	psecuritypriv->authenticator_ie[0] = (unsigned char)psecnetwork->IELength;
+
+	if ((psecnetwork->IELength - 12) < (256 - 1))
+		memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], psecnetwork->IELength - 12);
+	else
+		memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], (256 - 1));
+
+	psecnetwork->IELength = 0;
+	/* If the driver wants to use the bssid to create the connection. */
+	/* If not,  we have to copy the connecting AP's MAC address to it so that */
+	/* the driver just has the bssid information for PMKIDList searching. */
+
+	if (!pmlmepriv->assoc_by_bssid)
+		memcpy(&pmlmepriv->assoc_bssid[0], &pnetwork->network.MacAddress[0], ETH_ALEN);
+
+	psecnetwork->IELength = rtw_restruct_sec_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength);
+
+	pqospriv->qos_option = 0;
+
+	if (pregistrypriv->wmm_enable) {
+		tmp_len = rtw_restruct_wmm_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength, psecnetwork->IELength);
+
+		if (psecnetwork->IELength != tmp_len) {
+			psecnetwork->IELength = tmp_len;
+			pqospriv->qos_option = 1; /* There is WMM IE in this corresp. beacon */
+		} else {
+			pqospriv->qos_option = 0;/* There is no WMM IE in this corresp. beacon */
+		}
+	}
+
+	phtpriv->ht_option = false;
+	ptmp = rtw_get_ie(&pnetwork->network.IEs[12], _HT_CAPABILITY_IE_, &tmp_len, pnetwork->network.IELength - 12);
+	if (pregistrypriv->ht_enable && ptmp && tmp_len > 0) {
+		/*	For the WEP mode, we will use the bg mode to do the connection to avoid some IOT issue. */
+		/*	Especially for Realtek 8192u SoftAP. */
+		if ((padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_) &&
+		    (padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_) &&
+		    (padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_)) {
+			rtw_ht_use_default_setting(padapter);
+
+			rtw_build_wmm_ie_ht(padapter, &psecnetwork->IEs[0], &psecnetwork->IELength);
+
+			/* rtw_restructure_ht_ie */
+			rtw_restructure_ht_ie(padapter, &pnetwork->network.IEs[12], &psecnetwork->IEs[0],
+				pnetwork->network.IELength - 12, &psecnetwork->IELength,
+				pnetwork->network.Configuration.DSConfig);
+		}
+	}
+	rtw_append_exented_cap(padapter, &psecnetwork->IEs[0], &psecnetwork->IELength);
+
+#ifdef CONFIG_RTW_80211R
+	/*IEEE802.11-2012 Std. Table 8-101�XAKM suite selectors*/
+	if ((rtw_chk_ft_flags(padapter, RTW_FT_STA_SUPPORTED)) &&
+	    ((psecuritypriv->rsn_akm_suite_type == 3) ||
+	     (psecuritypriv->rsn_akm_suite_type == 4))) {
+		ptmp = rtw_get_ie(&pnetwork->network.IEs[12], _MDIE_, &tmp_len, pnetwork->network.IELength-12);
+		if (ptmp) {
+			memcpy(&pftpriv->mdid, ptmp+2, 2);
+			pftpriv->ft_cap = *(ptmp+4);
+
+			RTW_INFO("FT: Target AP "MAC_FMT" MDID=(0x%2x), capacity=(0x%2x)\n", MAC_ARG(pnetwork->network.MacAddress), pftpriv->mdid, pftpriv->ft_cap);
+			rtw_set_ft_flags(padapter, RTW_FT_SUPPORTED);
+			if ((rtw_chk_ft_flags(padapter, RTW_FT_STA_OVER_DS_SUPPORTED)) && (pftpriv->ft_roam_on_expired == false) && (pftpriv->ft_cap & 0x01))
+				rtw_set_ft_flags(padapter, RTW_FT_OVER_DS_SUPPORTED);
+		} else {
+			/*Don't use FT roaming if Target AP cannot support FT*/
+			RTW_INFO("FT: Target AP "MAC_FMT" could not support FT\n", MAC_ARG(pnetwork->network.MacAddress));
+			rtw_clr_ft_flags(padapter, RTW_FT_SUPPORTED|RTW_FT_OVER_DS_SUPPORTED);
+			rtw_reset_ft_status(padapter);
+		}
+	} else {
+		/*It could be a non-FT connection*/
+		RTW_INFO("FT: %s\n", __func__);
+		rtw_clr_ft_flags(padapter, RTW_FT_SUPPORTED|RTW_FT_OVER_DS_SUPPORTED);
+		rtw_reset_ft_status(padapter);
+	}
+#endif
+	pcmd->cmdsz = sizeof(WLAN_BSSID_EX);
+	INIT_LIST_HEAD(&pcmd->list);
+	pcmd->cmdcode = _JoinBss_CMD_;/* GEN_CMD_CODE(_JoinBss) */
+	pcmd->parmbuf = (unsigned char *)psecnetwork;
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+exit:
+	return res;
+}
+
+u8 rtw_disassoc_cmd(_adapter *padapter, u32 deauth_timeout_ms, bool enqueue) /* for sta_mode */
+{
+	struct cmd_obj *cmdobj = NULL;
+	struct disconnect_parm *param = NULL;
+	struct cmd_priv *cmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	/* prepare cmd parameter */
+	param = (struct disconnect_parm *)rtw_zmalloc(sizeof(*param));
+	if (!param) {
+		res = _FAIL;
+		goto exit;
+	}
+	param->deauth_timeout_ms = deauth_timeout_ms;
+
+	if (enqueue) {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj));
+		if (!cmdobj) {
+			res = _FAIL;
+			rtw_mfree((u8 *)param, sizeof(*param));
+			goto exit;
+		}
+		init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_);
+		res = rtw_enqueue_cmd(cmdpriv, cmdobj);
+	} else {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (disconnect_hdl(padapter, (u8 *)param) != H2C_SUCCESS)
+			res = _FAIL;
+		rtw_mfree((u8 *)param, sizeof(*param));
+	}
+
+exit:
+	return res;
+}
+
+u8 rtw_setopmode_cmd(_adapter  *padapter, NDIS_802_11_NETWORK_INFRASTRUCTURE networktype, bool enqueue)
+{
+	struct	cmd_obj	*ph2c;
+	struct	setopmode_parm *psetop;
+	struct	cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	psetop = (struct setopmode_parm *)rtw_zmalloc(sizeof(struct setopmode_parm));
+	if (!psetop) {
+		res = _FAIL;
+		goto exit;
+	}
+	psetop->mode = (u8)networktype;
+
+	if (enqueue) {
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (!ph2c) {
+			rtw_mfree((u8 *)psetop, sizeof(*psetop));
+			res = _FAIL;
+			goto exit;
+		}
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else {
+		setopmode_hdl(padapter, (u8 *)psetop);
+		rtw_mfree((u8 *)psetop, sizeof(*psetop));
+	}
+exit:
+	return res;
+}
+
+u8 rtw_setstakey_cmd(_adapter *padapter, struct sta_info *sta, u8 key_type, bool enqueue)
+{
+	struct cmd_obj *ph2c;
+	struct set_stakey_parm *psetstakey_para;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct set_stakey_rsp *psetstakey_rsp = NULL;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	u8 res = _SUCCESS;
+
+	psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm));
+	if (!psetstakey_para) {
+		res = _FAIL;
+		goto exit;
+	}
+	memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		psetstakey_para->algorithm = (unsigned char) psecuritypriv->dot11PrivacyAlgrthm;
+	else
+		GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm, false);
+
+	if (key_type == GROUP_KEY)
+		memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, 16);
+	else if (key_type == UNICAST_KEY)
+		memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
+#ifdef CONFIG_TDLS
+	else if (key_type == TDLS_KEY) {
+		memcpy(&psetstakey_para->key, sta->tpk.tk, 16);
+		psetstakey_para->algorithm = (u8)sta->dot118021XPrivacy;
+	}
+#endif /* CONFIG_TDLS */
+
+	/* jeff: set this becasue at least sw key is ready */
+	padapter->securitypriv.busetkipkey = true;
+
+	if (enqueue) {
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (!ph2c) {
+			rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm));
+			res = _FAIL;
+			goto exit;
+		}
+		psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp));
+		if (!psetstakey_rsp) {
+			rtw_mfree((u8 *) ph2c, sizeof(struct cmd_obj));
+			rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm));
+			res = _FAIL;
+			goto exit;
+		}
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+		ph2c->rsp = (u8 *) psetstakey_rsp;
+		ph2c->rspsz = sizeof(struct set_stakey_rsp);
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else {
+		set_stakey_hdl(padapter, (u8 *)psetstakey_para);
+		rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm));
+	}
+exit:
+	return res;
+}
+
+u8 rtw_clearstakey_cmd(_adapter *padapter, struct sta_info *sta, u8 enqueue)
+{
+	struct cmd_obj *ph2c;
+	struct set_stakey_parm *psetstakey_para;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct set_stakey_rsp *psetstakey_rsp = NULL;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	s16 cam_id = 0;
+	u8 res = _SUCCESS;
+
+	if (!enqueue) {
+		while ((cam_id = rtw_camid_search(padapter, sta->hwaddr, -1, -1)) >= 0) {
+			RTW_PRINT("clear key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(sta->hwaddr), cam_id);
+			clear_cam_entry(padapter, cam_id);
+			rtw_camid_free(padapter, cam_id);
+		}
+	} else {
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (!ph2c) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm));
+		if (!psetstakey_para) {
+			rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+			res = _FAIL;
+			goto exit;
+		}
+		psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp));
+		if (!psetstakey_rsp) {
+			rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+			rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm));
+			res = _FAIL;
+			goto exit;
+		}
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
+		ph2c->rsp = (u8 *) psetstakey_rsp;
+		ph2c->rspsz = sizeof(struct set_stakey_rsp);
+
+		memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
+
+		psetstakey_para->algorithm = _NO_PRIVACY_;
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	}
+
+exit:
+	return res;
+}
+
+u8 rtw_setrttbl_cmd(_adapter  *padapter, struct setratable_parm *prate_table)
+{
+	struct cmd_obj *ph2c;
+	struct setratable_parm *psetrttblparm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+	psetrttblparm = (struct setratable_parm *)rtw_zmalloc(sizeof(struct setratable_parm));
+
+	if (!psetrttblparm) {
+		rtw_mfree((unsigned char *) ph2c, sizeof(struct	cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetrttblparm, GEN_CMD_CODE(_SetRaTable));
+
+	memcpy(psetrttblparm, prate_table, sizeof(struct setratable_parm));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+
+}
+
+u8 rtw_getrttbl_cmd(_adapter  *padapter, struct getratable_rsp *pval)
+{
+	struct cmd_obj *ph2c;
+	struct getratable_parm *pgetrttblparm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+	pgetrttblparm = (struct getratable_parm *)rtw_zmalloc(sizeof(struct getratable_parm));
+
+	if (!pgetrttblparm) {
+		rtw_mfree((unsigned char *) ph2c, sizeof(struct	cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+	INIT_LIST_HEAD(&ph2c->list);
+	ph2c->cmdcode = GEN_CMD_CODE(_GetRaTable);
+	ph2c->parmbuf = (unsigned char *)pgetrttblparm;
+	ph2c->cmdsz =  sizeof(struct getratable_parm);
+	ph2c->rsp = (u8 *)pval;
+	ph2c->rspsz = sizeof(struct getratable_rsp);
+
+	pgetrttblparm->rsvd = 0x0;
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+u8 rtw_setassocsta_cmd(_adapter  *padapter, u8 *mac_addr)
+{
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct cmd_obj *ph2c;
+	struct set_assocsta_parm *psetassocsta_para;
+	struct set_stakey_rsp *psetassocsta_rsp = NULL;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+	psetassocsta_para = (struct set_assocsta_parm *)rtw_zmalloc(sizeof(struct set_assocsta_parm));
+	if (!psetassocsta_para) {
+		rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	psetassocsta_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_assocsta_rsp));
+	if (!psetassocsta_rsp) {
+		rtw_mfree((u8 *) ph2c, sizeof(struct	cmd_obj));
+		rtw_mfree((u8 *) psetassocsta_para, sizeof(struct set_assocsta_parm));
+		return _FAIL;
+	}
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psetassocsta_para, _SetAssocSta_CMD_);
+	ph2c->rsp = (u8 *)psetassocsta_rsp;
+	ph2c->rspsz = sizeof(struct set_assocsta_rsp);
+
+	memcpy(psetassocsta_para->addr, mac_addr, ETH_ALEN);
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+u8 rtw_addbareq_cmd(_adapter *padapter, u8 tid, u8 *addr)
+{
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct cmd_obj *ph2c;
+	struct addBaReq_parm *paddbareq_parm;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	paddbareq_parm = (struct addBaReq_parm *)rtw_zmalloc(sizeof(struct addBaReq_parm));
+	if (paddbareq_parm == NULL) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct	cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+	paddbareq_parm->tid = tid;
+	memcpy(paddbareq_parm->addr, addr, ETH_ALEN);
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm, GEN_CMD_CODE(_AddBAReq));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+u8 rtw_addbarsp_cmd(_adapter *padapter, u8 *addr, u16 tid, u8 status, u8 size, u16 start_seq)
+{
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct cmd_obj *ph2c;
+	struct addBaRsp_parm *paddBaRsp_parm;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	paddBaRsp_parm = (struct addBaRsp_parm *)rtw_zmalloc(sizeof(struct addBaRsp_parm));
+
+	if (!paddBaRsp_parm) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	memcpy(paddBaRsp_parm->addr, addr, ETH_ALEN);
+	paddBaRsp_parm->tid = tid;
+	paddBaRsp_parm->status = status;
+	paddBaRsp_parm->size = size;
+	paddBaRsp_parm->start_seq = start_seq;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, paddBaRsp_parm, GEN_CMD_CODE(_AddBARsp));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+/* add for CONFIG_IEEE80211W, none 11w can use it */
+u8 rtw_reset_securitypriv_cmd(_adapter *padapter)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = RESET_SECURITYPRIV;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+u8 rtw_free_assoc_resources_cmd(_adapter *padapter)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = FREE_ASSOC_RESOURCES;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+u8 rtw_dynamic_chk_wk_cmd(_adapter *padapter)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	/* only  primary padapter does this cmd */
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = DYNAMIC_CHK_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+u8 rtw_set_ch_cmd(_adapter *padapter, u8 ch, u8 bw, u8 ch_offset, u8 enqueue)
+{
+	struct cmd_obj *pcmdobj;
+	struct set_ch_parm *set_ch_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	RTW_INFO(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
+		 FUNC_NDEV_ARG(padapter->pnetdev), ch, bw, ch_offset);
+
+	/* prepare cmd parameter */
+	set_ch_parm = (struct set_ch_parm *)rtw_zmalloc(sizeof(*set_ch_parm));
+	if (!set_ch_parm) {
+		res = _FAIL;
+		goto exit;
+	}
+	set_ch_parm->ch = ch;
+	set_ch_parm->bw = bw;
+	set_ch_parm->ch_offset = ch_offset;
+
+	if (enqueue) {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
+		if (!pcmdobj) {
+			rtw_mfree((u8 *)set_ch_parm, sizeof(*set_ch_parm));
+			res = _FAIL;
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(pcmdobj, set_ch_parm, GEN_CMD_CODE(_SetChannel));
+		res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+	} else {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (set_ch_hdl(padapter, (u8 *)set_ch_parm) != H2C_SUCCESS)
+			res = _FAIL;
+
+		rtw_mfree((u8 *)set_ch_parm, sizeof(*set_ch_parm));
+	}
+
+exit:
+
+	RTW_INFO(FUNC_NDEV_FMT" res:%u\n", FUNC_NDEV_ARG(padapter->pnetdev), res);
+
+	return res;
+}
+
+static u8 _rtw_set_chplan_cmd(_adapter *adapter, int flags, u8 chplan, const struct country_chplan *country_ent, u8 swconfig)
+{
+	struct cmd_obj *cmdobj;
+	struct	SetChannelPlan_param *parm;
+	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct submit_ctx sctx;
+	u8 res = _SUCCESS;
+
+
+	/* check if allow software config */
+	if (swconfig && rtw_hal_is_disable_sw_channel_plan(adapter)) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	/* if country_entry is provided, replace chplan */
+	if (country_ent)
+		chplan = country_ent->chplan;
+
+	/* check input parameter */
+	if (!rtw_is_channel_plan_valid(chplan)) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	/* prepare cmd parameter */
+	parm = (struct SetChannelPlan_param *)rtw_zmalloc(sizeof(*parm));
+	if (!parm) {
+		res = _FAIL;
+		goto exit;
+	}
+	parm->country_ent = country_ent;
+	parm->channel_plan = chplan;
+
+	if (flags & RTW_CMDF_DIRECTLY) {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (set_chplan_hdl(adapter, (u8 *)parm) != H2C_SUCCESS)
+			res = _FAIL;
+		rtw_mfree((u8 *)parm, sizeof(*parm));
+	} else {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj));
+		if (!cmdobj) {
+			res = _FAIL;
+			rtw_mfree((u8 *)parm, sizeof(*parm));
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, GEN_CMD_CODE(_SetChannelPlan));
+
+		if (flags & RTW_CMDF_WAIT_ACK) {
+			cmdobj->sctx = &sctx;
+			rtw_sctx_init(&sctx, 2000);
+		}
+
+		res = rtw_enqueue_cmd(pcmdpriv, cmdobj);
+
+		if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) {
+			rtw_sctx_wait(&sctx, __func__);
+			_enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
+			if (sctx.status == RTW_SCTX_SUBMITTED)
+				cmdobj->sctx = NULL;
+			_exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
+		}
+	}
+
+exit:
+	return res;
+}
+
+inline u8 rtw_set_chplan_cmd(_adapter *adapter, int flags, u8 chplan, u8 swconfig)
+{
+	return _rtw_set_chplan_cmd(adapter, flags, chplan, NULL, swconfig);
+}
+
+inline u8 rtw_set_country_cmd(_adapter *adapter, int flags, const char *country_code, u8 swconfig)
+{
+	const struct country_chplan *ent;
+
+	if (!is_alpha(country_code[0]) || !is_alpha(country_code[1])) {
+		RTW_PRINT("%s input country_code is not alpha2\n", __func__);
+		return _FAIL;
+	}
+
+	ent = rtw_get_chplan_from_country(country_code);
+
+	if (!ent) {
+		RTW_PRINT("%s unsupported country_code:\"%c%c\"\n", __func__, country_code[0], country_code[1]);
+		return _FAIL;
+	}
+
+	RTW_PRINT("%s country_code:\"%c%c\" mapping to chplan:0x%02x\n", __func__, country_code[0], country_code[1], ent->chplan);
+
+	return _rtw_set_chplan_cmd(adapter, flags, RTW_CHPLAN_MAX, ent, swconfig);
+}
+
+u8 rtw_led_blink_cmd(_adapter *padapter, void *pLed)
+{
+	struct	cmd_obj	*pcmdobj;
+	struct	LedBlink_param *ledBlink_param;
+	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	pcmdobj = (struct	cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
+	if (!pcmdobj) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	ledBlink_param = (struct LedBlink_param *)rtw_zmalloc(sizeof(struct LedBlink_param));
+	if (!ledBlink_param) {
+		rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+	ledBlink_param->pLed = pLed;
+
+	init_h2fwcmd_w_parm_no_rsp(pcmdobj, ledBlink_param, GEN_CMD_CODE(_LedBlink));
+	res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+
+exit:
+	return res;
+}
+
+u8 rtw_set_csa_cmd(_adapter *padapter, u8 new_ch_no)
+{
+	struct	cmd_obj	*pcmdobj;
+	struct	SetChannelSwitch_param *setChannelSwitch_param;
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
+	if (!pcmdobj) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	setChannelSwitch_param = (struct SetChannelSwitch_param *)rtw_zmalloc(sizeof(struct	SetChannelSwitch_param));
+	if (!setChannelSwitch_param) {
+		rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+	setChannelSwitch_param->new_ch_no = new_ch_no;
+
+	init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelSwitch_param, GEN_CMD_CODE(_SetChannelSwitch));
+	res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+
+exit:
+	return res;
+}
+
+u8 rtw_tdls_cmd(_adapter *padapter, u8 *addr, u8 option)
+{
+	struct cmd_obj	*pcmdobj;
+	struct TDLSoption_param	*TDLSoption;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+#ifdef CONFIG_TDLS
+	pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
+	if (!pcmdobj) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	TDLSoption = (struct TDLSoption_param *)rtw_zmalloc(sizeof(struct TDLSoption_param));
+	if (!TDLSoption) {
+		rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	spin_lock(&(padapter->tdlsinfo.cmd_lock));
+	if (!addr)
+		memcpy(TDLSoption->addr, addr, 6);
+	TDLSoption->option = option;
+	spin_unlock(&(padapter->tdlsinfo.cmd_lock));
+	init_h2fwcmd_w_parm_no_rsp(pcmdobj, TDLSoption, GEN_CMD_CODE(_TDLS));
+	res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+
+#endif /* CONFIG_TDLS */
+
+exit:
+	return res;
+}
+
+u8 rtw_enable_hw_update_tsf_cmd(_adapter *padapter)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+	pdrvextra_cmd_parm->ec_id = EN_HW_UPDATE_TSF_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+/* from_timer == 1 means driver is in LPS */
+u8 traffic_status_watchdog(_adapter *padapter, u8 from_timer)
+{
+	u8 bEnterPS = false;
+	u16 BusyThresholdHigh;
+	u16 BusyThresholdLow;
+	u16 BusyThreshold;
+	u8 bBusyTraffic = false, bTxBusyTraffic = false, bRxBusyTraffic = false;
+	u8 bHigherBusyTraffic = false, bHigherBusyRxTraffic = false, bHigherBusyTxTraffic = false;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &(padapter->tdlsinfo);
+	struct tdls_txmgmt txmgmt;
+	u8 baddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+#endif /* CONFIG_TDLS */
+
+	RT_LINK_DETECT_T *link_detect = &pmlmepriv->LinkDetectInfo;
+
+#ifdef CONFIG_BT_COEXIST
+	if (padapter->registrypriv.wifi_spec != 1) {
+		BusyThresholdHigh = 25;
+		BusyThresholdLow = 10;
+	} else
+#endif /* CONFIG_BT_COEXIST */
+	{
+		BusyThresholdHigh = 100;
+		BusyThresholdLow = 75;
+	}
+	BusyThreshold = BusyThresholdHigh;
+
+
+	/* Determine if our traffic is busy now */
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		/* if we raise bBusyTraffic in last watchdog, using lower threshold. */
+		if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
+			BusyThreshold = BusyThresholdLow;
+
+		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > BusyThreshold ||
+		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold) {
+			bBusyTraffic = true;
+
+			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+				bRxBusyTraffic = true;
+			else
+				bTxBusyTraffic = true;
+		}
+
+		/* Higher Tx/Rx data. */
+		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 4000 ||
+		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 4000) {
+			bHigherBusyTraffic = true;
+
+			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
+				bHigherBusyRxTraffic = true;
+			else
+				bHigherBusyTxTraffic = true;
+		}
+
+#ifdef CONFIG_TRAFFIC_PROTECT
+#define TX_ACTIVE_TH 10
+#define RX_ACTIVE_TH 20
+#define TRAFFIC_PROTECT_PERIOD_MS 4500
+
+		if (link_detect->NumTxOkInPeriod > TX_ACTIVE_TH
+		    || link_detect->NumRxUnicastOkInPeriod > RX_ACTIVE_TH) {
+
+			RTW_INFO(FUNC_ADPT_FMT" acqiure wake_lock for %u ms(tx:%d,rx_unicast:%d)\n",
+				 FUNC_ADPT_ARG(padapter),
+				 TRAFFIC_PROTECT_PERIOD_MS,
+				 link_detect->NumTxOkInPeriod,
+				 link_detect->NumRxUnicastOkInPeriod);
+
+			rtw_lock_traffic_suspend_timeout(TRAFFIC_PROTECT_PERIOD_MS);
+		}
+#endif
+
+#ifdef CONFIG_TDLS
+#ifdef CONFIG_TDLS_AUTOSETUP
+		/* TDLS_WATCHDOG_PERIOD * 2sec, periodically send */
+		if (hal_chk_wl_func(padapter, WL_FUNC_TDLS) == true) {
+			if ((ptdlsinfo->watchdog_count % TDLS_WATCHDOG_PERIOD) == 0) {
+				memcpy(txmgmt.peer, baddr, ETH_ALEN);
+				issue_tdls_dis_req(padapter, &txmgmt);
+			}
+			ptdlsinfo->watchdog_count++;
+		}
+#endif /* CONFIG_TDLS_AUTOSETUP */
+#endif /* CONFIG_TDLS */
+
+#ifdef CONFIG_LPS
+		/* check traffic for  powersaving. */
+		if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
+#ifdef CONFIG_LPS_SLOW_TRANSITION
+		    (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2)
+#else /* CONFIG_LPS_SLOW_TRANSITION */
+		    (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 4)
+#endif /* CONFIG_LPS_SLOW_TRANSITION */
+		   ) {
+#ifdef DBG_RX_COUNTER_DUMP
+			if (padapter->dump_rx_cnt_mode & DUMP_DRV_TRX_COUNTER_DATA)
+				RTW_INFO("(-)Tx = %d, Rx = %d\n", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod);
+#endif
+			bEnterPS = false;
+#ifdef CONFIG_LPS_SLOW_TRANSITION
+			if (bBusyTraffic == true) {
+				if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount <= 4)
+					pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 4;
+
+				pmlmepriv->LinkDetectInfo.TrafficTransitionCount++;
+
+				/* RTW_INFO("Set TrafficTransitionCount to %d\n", pmlmepriv->LinkDetectInfo.TrafficTransitionCount); */
+
+				if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount > 30/*TrafficTransitionLevel*/)
+					pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 30;
+			}
+#endif /* CONFIG_LPS_SLOW_TRANSITION */
+
+		} else {
+#ifdef DBG_RX_COUNTER_DUMP
+			if (padapter->dump_rx_cnt_mode & DUMP_DRV_TRX_COUNTER_DATA)
+				RTW_INFO("(+)Tx = %d, Rx = %d\n", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod);
+#endif
+#ifdef CONFIG_LPS_SLOW_TRANSITION
+			if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount >= 2)
+				pmlmepriv->LinkDetectInfo.TrafficTransitionCount -= 2;
+			else
+				pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
+
+			if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount == 0)
+				bEnterPS = true;
+#else /* CONFIG_LPS_SLOW_TRANSITION */
+			bEnterPS = true;
+#endif /* CONFIG_LPS_SLOW_TRANSITION */
+		}
+
+#ifdef CONFIG_DYNAMIC_DTIM
+		if (pmlmepriv->LinkDetectInfo.LowPowerTransitionCount == 8)
+			bEnterPS = false;
+
+		RTW_INFO("LowPowerTransitionCount=%d\n", pmlmepriv->LinkDetectInfo.LowPowerTransitionCount);
+#endif /* CONFIG_DYNAMIC_DTIM */
+
+		/* LeisurePS only work in infra mode. */
+		if (bEnterPS) {
+			if (!from_timer) {
+#ifdef CONFIG_DYNAMIC_DTIM
+				if (pmlmepriv->LinkDetectInfo.LowPowerTransitionCount < 8)
+					adapter_to_pwrctl(padapter)->dtim = 1;
+				else
+					adapter_to_pwrctl(padapter)->dtim = 3;
+#endif /* CONFIG_DYNAMIC_DTIM */
+				LPS_Enter(padapter, "TRAFFIC_IDLE");
+			} else {
+				/* do this at caller */
+				/* rtw_lps_ctrl_wk_cmd(adapter, LPS_CTRL_ENTER, 1); */
+				/* rtw_hal_dm_watchdog_in_lps(padapter); */
+			}
+#ifdef CONFIG_DYNAMIC_DTIM
+			if (adapter_to_pwrctl(padapter)->bFwCurrentInPSMode == true)
+				pmlmepriv->LinkDetectInfo.LowPowerTransitionCount++;
+#endif /* CONFIG_DYNAMIC_DTIM */
+		} else {
+#ifdef CONFIG_DYNAMIC_DTIM
+			if (pmlmepriv->LinkDetectInfo.LowPowerTransitionCount != 8)
+				pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
+			else
+				pmlmepriv->LinkDetectInfo.LowPowerTransitionCount++;
+#endif /* CONFIG_DYNAMIC_DTIM			 */
+			if (!from_timer)
+				LPS_Leave(padapter, "TRAFFIC_BUSY");
+			else {
+#ifdef CONFIG_CONCURRENT_MODE
+				#ifndef CONFIG_FW_MULTI_PORT_SUPPORT
+				if (padapter->hw_port == HW_PORT0)
+				#endif
+#endif
+					rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_TRAFFIC_BUSY, 1);
+			}
+		}
+
+#endif /* CONFIG_LPS */
+	} else {
+#ifdef CONFIG_LPS
+		struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+		int n_assoc_iface = 0;
+		int i;
+
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			if (check_fwstate(&(dvobj->padapters[i]->mlmepriv), WIFI_ASOC_STATE))
+				n_assoc_iface++;
+		}
+
+		if (!from_timer && n_assoc_iface == 0)
+			LPS_Leave(padapter, "NON_LINKED");
+#endif
+	}
+
+	session_tracker_chk_cmd(padapter, NULL);
+
+#ifdef CONFIG_BEAMFORMING
+#ifdef RTW_BEAMFORMING_VERSION_2
+	rtw_bf_update_traffic(padapter);
+#endif /* RTW_BEAMFORMING_VERSION_2 */
+#endif /* CONFIG_BEAMFORMING */
+
+	pmlmepriv->LinkDetectInfo.NumRxOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.NumTxOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod = 0;
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = bBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bTxBusyTraffic = bTxBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bRxBusyTraffic = bRxBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyTraffic = bHigherBusyTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyRxTraffic = bHigherBusyRxTraffic;
+	pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic;
+
+	return bEnterPS;
+
+}
+
+
+/* for 11n Logo 4.2.31/4.2.32 */
+static void dynamic_update_bcn_check(_adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if (!padapter->registrypriv.wifi_spec)
+		return;
+
+	if (!MLME_IS_AP(padapter))
+		return;
+
+	if (pmlmeext->bstart_bss) {
+		/* In 10 * 2 = 20s, there are no legacy AP, update HT info  */
+		static u8 count = 1;
+
+		if (count % 10 == 0) {
+			count = 1;
+
+			if (false == ATOMIC_READ(&pmlmepriv->olbc)
+				&& false == ATOMIC_READ(&pmlmepriv->olbc_ht)) {
+
+				if (rtw_ht_operation_update(padapter) > 0) {
+					update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, false);
+					update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, true);
+				}
+			}
+		}
+
+		/* In 2s, there are any legacy AP, update HT info, and then reset count  */
+
+		if (ATOMIC_READ(&pmlmepriv->olbc) && ATOMIC_READ(&pmlmepriv->olbc_ht)) {
+			if (rtw_ht_operation_update(padapter) > 0) {
+				update_beacon(padapter, _HT_CAPABILITY_IE_, NULL, false);
+				update_beacon(padapter, _HT_ADD_INFO_IE_, NULL, true);
+
+			}
+			ATOMIC_SET(&pmlmepriv->olbc, false);
+			ATOMIC_SET(&pmlmepriv->olbc_ht, false);
+			count = 0;
+		}
+
+		count++;
+	}
+}
+void rtw_iface_dynamic_chk_wk_hdl(_adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+	#ifdef CONFIG_AP_MODE
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+		expire_timeout_chk(padapter);
+	#endif
+	#endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */
+	dynamic_update_bcn_check(padapter);
+
+	linked_status_chk(padapter, 0);
+	traffic_status_watchdog(padapter, 0);
+
+	/* for debug purpose */
+	_linked_info_dump(padapter);
+
+	#ifdef CONFIG_BEAMFORMING
+	#ifndef RTW_BEAMFORMING_VERSION_2
+	#if (BEAMFORMING_SUPPORT == 0) /*for diver defined beamforming*/
+	beamforming_watchdog(padapter);
+	#endif
+	#endif /* !RTW_BEAMFORMING_VERSION_2 */
+	#endif
+
+}
+static void rtw_dynamic_chk_wk_hdl(_adapter *padapter)
+{
+	rtw_mi_dynamic_chk_wk_hdl(padapter);
+
+#ifdef DBG_CONFIG_ERROR_DETECT
+	rtw_hal_sreset_xmit_status_check(padapter);
+	rtw_hal_sreset_linked_status_check(padapter);
+#endif
+
+	/* if(check_fwstate(pmlmepriv, _FW_UNDER_LINKING|_FW_UNDER_SURVEY)==false) */
+	{
+#ifdef DBG_RX_COUNTER_DUMP
+		rtw_dump_rx_counters(padapter);
+#endif
+		dm_DynamicUsbTxAgg(padapter, 0);
+	}
+	rtw_hal_dm_watchdog(padapter);
+
+	/* check_hw_pbc(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type); */
+
+#ifdef CONFIG_BT_COEXIST
+	/* BT-Coexist */
+	rtw_btcoex_Handler(padapter);
+#endif
+
+#ifdef CONFIG_IPS_CHECK_IN_WD
+	/* always call rtw_ps_processor() at last one. */
+	rtw_ps_processor(padapter);
+#endif
+
+#ifdef CONFIG_MCC_MODE
+	rtw_hal_mcc_sw_status_check(padapter);
+#endif /* CONFIG_MCC_MODE */
+
+}
+
+#ifdef CONFIG_LPS
+
+void lps_ctrl_wk_hdl(_adapter *padapter, u8 lps_ctrl_type);
+void lps_ctrl_wk_hdl(_adapter *padapter, u8 lps_ctrl_type)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	u8	mstatus;
+
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)
+	    || (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
+		return;
+
+	switch (lps_ctrl_type) {
+	case LPS_CTRL_SCAN:
+		/* RTW_INFO("LPS_CTRL_SCAN\n"); */
+#ifdef CONFIG_BT_COEXIST
+		rtw_btcoex_ScanNotify(padapter, true);
+#endif /* CONFIG_BT_COEXIST */
+		if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+			/* connect */
+			LPS_Leave(padapter, "LPS_CTRL_SCAN");
+		}
+		break;
+	case LPS_CTRL_JOINBSS:
+		/* RTW_INFO("LPS_CTRL_JOINBSS\n"); */
+		LPS_Leave(padapter, "LPS_CTRL_JOINBSS");
+		break;
+	case LPS_CTRL_CONNECT:
+		/* RTW_INFO("LPS_CTRL_CONNECT\n"); */
+		mstatus = 1;/* connect */
+		/* Reset LPS Setting */
+		pwrpriv->LpsIdleCount = 0;
+		rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
+#ifdef CONFIG_BT_COEXIST
+		rtw_btcoex_MediaStatusNotify(padapter, mstatus);
+#endif /* CONFIG_BT_COEXIST */
+		break;
+	case LPS_CTRL_DISCONNECT:
+		/* RTW_INFO("LPS_CTRL_DISCONNECT\n"); */
+		mstatus = 0;/* disconnect */
+#ifdef CONFIG_BT_COEXIST
+		rtw_btcoex_MediaStatusNotify(padapter, mstatus);
+#endif /* CONFIG_BT_COEXIST */
+		LPS_Leave(padapter, "LPS_CTRL_DISCONNECT");
+		rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
+		break;
+	case LPS_CTRL_SPECIAL_PACKET:
+		/* RTW_INFO("LPS_CTRL_SPECIAL_PACKET\n"); */
+		pwrpriv->DelayLPSLastTimeStamp = jiffies;
+#ifdef CONFIG_BT_COEXIST
+		rtw_btcoex_SpecialPacketNotify(padapter, PACKET_DHCP);
+#endif /* CONFIG_BT_COEXIST */
+		LPS_Leave(padapter, "LPS_CTRL_SPECIAL_PACKET");
+		break;
+	case LPS_CTRL_LEAVE:
+		LPS_Leave(padapter, "LPS_CTRL_LEAVE");
+		break;
+	case LPS_CTRL_LEAVE_CFG80211_PWRMGMT:
+		LPS_Leave(padapter, "CFG80211_PWRMGMT");
+		break;
+	case LPS_CTRL_TRAFFIC_BUSY:
+		LPS_Leave(padapter, "LPS_CTRL_TRAFFIC_BUSY");
+		break;
+	case LPS_CTRL_TX_TRAFFIC_LEAVE:
+		LPS_Leave(padapter, "LPS_CTRL_TX_TRAFFIC_LEAVE");
+		break;
+	case LPS_CTRL_RX_TRAFFIC_LEAVE:
+		LPS_Leave(padapter, "LPS_CTRL_RX_TRAFFIC_LEAVE");
+		break;
+	case LPS_CTRL_ENTER:
+		LPS_Enter(padapter, "TRAFFIC_IDLE_1");
+		break;
+	default:
+		break;
+	}
+
+}
+
+u8 rtw_lps_ctrl_wk_cmd(_adapter *padapter, u8 lps_ctrl_type, u8 enqueue)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	/* struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter); */
+	u8	res = _SUCCESS;
+
+
+	/* if(!pwrctrlpriv->bLeisurePs) */
+	/*	return res; */
+
+	if (enqueue) {
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+		if (pdrvextra_cmd_parm == NULL) {
+			rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm->ec_id = LPS_CTRL_WK_CID;
+		pdrvextra_cmd_parm->type = lps_ctrl_type;
+		pdrvextra_cmd_parm->size = 0;
+		pdrvextra_cmd_parm->pbuf = NULL;
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else
+		lps_ctrl_wk_hdl(padapter, lps_ctrl_type);
+
+exit:
+
+
+	return res;
+
+}
+
+static void rtw_dm_in_lps_hdl(_adapter *padapter)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_DM_IN_LPS, NULL);
+}
+
+u8 rtw_dm_in_lps_wk_cmd(_adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = DM_IN_LPS_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+
+}
+
+static void rtw_lps_change_dtim_hdl(_adapter *padapter, u8 dtim)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+	if (dtim <= 0 || dtim > 16)
+		return;
+
+#ifdef CONFIG_BT_COEXIST
+	if (rtw_btcoex_IsBtControlLps(padapter) == true)
+		return;
+#endif
+
+#ifdef CONFIG_LPS_LCLK
+	_enter_pwrlock(&pwrpriv->lock);
+#endif
+
+	if (pwrpriv->dtim != dtim) {
+		RTW_INFO("change DTIM from %d to %d, bFwCurrentInPSMode=%d, ps_mode=%d\n", pwrpriv->dtim, dtim,
+			 pwrpriv->bFwCurrentInPSMode, pwrpriv->pwr_mode);
+
+		pwrpriv->dtim = dtim;
+	}
+
+	if ((pwrpriv->bFwCurrentInPSMode == true) && (pwrpriv->pwr_mode > PS_MODE_ACTIVE)) {
+		u8 ps_mode = pwrpriv->pwr_mode;
+
+		/* RTW_INFO("change DTIM from %d to %d, ps_mode=%d\n", pwrpriv->dtim, dtim, ps_mode); */
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+	}
+
+#ifdef CONFIG_LPS_LCLK
+	_exit_pwrlock(&pwrpriv->lock);
+#endif
+
+}
+
+#endif
+
+u8 rtw_lps_change_dtim_cmd(_adapter *padapter, u8 dtim)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!pdrvextra_cmd_parm) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = LPS_CHANGE_DTIM_CID;
+	pdrvextra_cmd_parm->type = dtim;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+}
+
+#if (RATE_ADAPTIVE_SUPPORT == 1)
+static void rpt_timer_setting_wk_hdl(_adapter *padapter, u16 minRptTime)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_RPT_TIMER_SETTING, (u8 *)(&minRptTime));
+}
+
+u8 rtw_rpt_timer_cfg_cmd(_adapter *padapter, u16 minRptTime)
+{
+	struct cmd_obj		*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+
+	u8	res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = RTP_TIMER_CFG_WK_CID;
+	pdrvextra_cmd_parm->type = minRptTime;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+
+	return res;
+
+}
+
+#endif
+
+#ifdef CONFIG_ANTENNA_DIVERSITY
+void antenna_select_wk_hdl(_adapter *padapter, u8 antenna)
+{
+	rtw_hal_set_odm_var(padapter, HAL_ODM_ANTDIV_SELECT, &antenna, true);
+}
+
+u8 rtw_antenna_select_cmd(_adapter *padapter, u8 antenna, u8 enqueue)
+{
+	struct cmd_obj		*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	u8	bSupportAntDiv = false;
+	u8	res = _SUCCESS;
+	int	i;
+
+	rtw_hal_get_def_var(padapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &(bSupportAntDiv));
+	if (false == bSupportAntDiv)
+		return _FAIL;
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		if (rtw_linked_check(dvobj->padapters[i]))
+			return _FAIL;
+	}
+
+	if (enqueue) {
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+		if (pdrvextra_cmd_parm == NULL) {
+			rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm->ec_id = ANT_SELECT_WK_CID;
+		pdrvextra_cmd_parm->type = antenna;
+		pdrvextra_cmd_parm->size = 0;
+		pdrvextra_cmd_parm->pbuf = NULL;
+		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else
+		antenna_select_wk_hdl(padapter, antenna);
+exit:
+
+
+	return res;
+
+}
+#endif
+
+static void rtw_dm_ra_mask_hdl(_adapter *padapter, struct sta_info *psta)
+{
+	if (psta)
+		set_sta_rate(padapter, psta);
+}
+
+u8 rtw_dm_ra_mask_wk_cmd(_adapter *padapter, u8 *psta)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = DM_RA_MSK_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = psta;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+
+}
+
+static void power_saving_wk_hdl(_adapter *padapter)
+{
+	rtw_ps_processor(padapter);
+}
+
+/* add for CONFIG_IEEE80211W, none 11w can use it */
+static void reset_securitypriv_hdl(_adapter *padapter)
+{
+	rtw_reset_securitypriv(padapter);
+}
+
+static void free_assoc_resources_hdl(_adapter *padapter)
+{
+	rtw_free_assoc_resources(padapter, 1);
+}
+
+#ifdef CONFIG_P2P
+u8 p2p_protocol_wk_cmd(_adapter *padapter, int intCmdType)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return res;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = P2P_PROTO_WK_CID;
+	pdrvextra_cmd_parm->type = intCmdType;	/*	As the command tppe. */
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;		/*	Must be NULL here */
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+
+	return res;
+
+}
+
+#ifdef CONFIG_IOCTL_CFG80211
+static u8 _p2p_roch_cmd(_adapter *adapter, u64 cookie,
+			struct wireless_dev *wdev,
+			struct ieee80211_channel *ch,
+			enum nl80211_channel_type ch_type,
+			unsigned int duration, u8 flags)
+{
+	struct cmd_obj *cmdobj;
+	struct drvextra_cmd_parm *parm;
+	struct p2p_roch_parm *roch_parm;
+	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
+	struct submit_ctx sctx;
+	u8 cancel = duration ? 0 : 1;
+	u8 res = _SUCCESS;
+
+	roch_parm = (struct p2p_roch_parm *)rtw_zmalloc(sizeof(struct p2p_roch_parm));
+	if (!roch_parm) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	roch_parm->cookie = cookie;
+	roch_parm->wdev = wdev;
+	if (!cancel) {
+		memcpy(&roch_parm->ch, ch, sizeof(struct ieee80211_channel));
+		roch_parm->ch_type = ch_type;
+		roch_parm->duration = duration;
+	}
+
+	if (flags & RTW_CMDF_DIRECTLY) {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (p2p_protocol_wk_hdl(adapter,
+					cancel ? P2P_CANCEL_RO_CH_WK : P2P_RO_CH_WK,
+					(u8 *)roch_parm) != H2C_SUCCESS)
+			res = _FAIL;
+		rtw_mfree((u8 *)roch_parm, sizeof(*roch_parm));
+	} else {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+		if (parm == NULL) {
+			rtw_mfree((u8 *)roch_parm, sizeof(*roch_parm));
+			res = _FAIL;
+			goto exit;
+		}
+
+		parm->ec_id = P2P_PROTO_WK_CID;
+		parm->type = cancel ? P2P_CANCEL_RO_CH_WK : P2P_RO_CH_WK;
+		parm->size = sizeof(*roch_parm);
+		parm->pbuf = (u8 *)roch_parm;
+
+		cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj));
+		if (cmdobj == NULL) {
+			res = _FAIL;
+			rtw_mfree((u8 *)roch_parm, sizeof(*roch_parm));
+			rtw_mfree((u8 *)parm, sizeof(*parm));
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		if (flags & RTW_CMDF_WAIT_ACK) {
+			cmdobj->sctx = &sctx;
+			rtw_sctx_init(&sctx, 10 * 1000);
+		}
+
+		res = rtw_enqueue_cmd(pcmdpriv, cmdobj);
+
+		if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) {
+			rtw_sctx_wait(&sctx, __func__);
+			_enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
+			if (sctx.status == RTW_SCTX_SUBMITTED)
+				cmdobj->sctx = NULL;
+			_exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
+			if (sctx.status != RTW_SCTX_DONE_SUCCESS)
+				res = _FAIL;
+		}
+	}
+
+exit:
+	return res;
+}
+
+inline u8 p2p_roch_cmd(_adapter *adapter
+	, u64 cookie, struct wireless_dev *wdev
+	, struct ieee80211_channel *ch, enum nl80211_channel_type ch_type
+	, unsigned int duration
+	, u8 flags
+)
+{
+	return _p2p_roch_cmd(adapter, cookie, wdev, ch, ch_type, duration, flags);
+}
+
+inline u8 p2p_cancel_roch_cmd(_adapter *adapter, u64 cookie, struct wireless_dev *wdev, u8 flags)
+{
+	return _p2p_roch_cmd(adapter, cookie, wdev, NULL, 0, 0, flags);
+}
+#endif /* CONFIG_IOCTL_CFG80211 */
+#endif /* CONFIG_P2P */
+
+u8 rtw_ps_cmd(_adapter *padapter)
+{
+	struct cmd_obj		*ppscmd;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+
+	u8	res = _SUCCESS;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (padapter->adapter_type != PRIMARY_ADAPTER)
+		goto exit;
+#endif
+
+	ppscmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ppscmd == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((unsigned char *)ppscmd, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+	init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ppscmd);
+
+exit:
+
+
+	return res;
+
+}
+
+#ifdef CONFIG_AP_MODE
+
+static void rtw_chk_hi_queue_hdl(_adapter *padapter)
+{
+	struct sta_info *psta_bmc;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u32 start = jiffies;
+	u8 empty = false;
+
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+	if (!psta_bmc)
+		return;
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty);
+
+	while (false == empty && rtw_get_passing_time_ms(start) < rtw_get_wait_hiq_empty_ms()) {
+		rtw_msleep_os(100);
+		rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty);
+	}
+
+	if (psta_bmc->sleepq_len == 0) {
+		if (empty == _SUCCESS) {
+			bool update_tim = false;
+
+			if (pstapriv->tim_bitmap & BIT(0))
+				update_tim = true;
+
+			pstapriv->tim_bitmap &= ~BIT(0);
+			pstapriv->sta_dz_bitmap &= ~BIT(0);
+
+			if (update_tim == true)
+				_update_beacon(padapter, _TIM_IE_, NULL, true, "bmc sleepq and HIQ empty");
+		} else /* re check again */
+			rtw_chk_hi_queue_cmd(padapter);
+
+	}
+
+}
+
+u8 rtw_chk_hi_queue_cmd(_adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = CHECK_HIQ_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = 0;
+	pdrvextra_cmd_parm->pbuf = NULL;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+
+}
+
+#ifdef CONFIG_DFS_MASTER
+u8 rtw_dfs_master_hdl(_adapter *adapter)
+{
+	struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter);
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+
+	if (!rfctl->dfs_master_enabled)
+		goto exit;
+
+	if (rtw_get_on_cur_ch_time(adapter) == 0
+		|| rtw_get_passing_time_ms(rtw_get_on_cur_ch_time(adapter)) < 300
+	) {
+		/* offchannel , bypass radar detect */
+		goto cac_status_chk;
+	}
+
+	if (IS_CH_WAITING(rfctl) && !IS_UNDER_CAC(rfctl)) {
+		/* non_ocp, bypass radar detect */
+		goto cac_status_chk;
+	}
+
+	if (!rfctl->dbg_dfs_master_fake_radar_detect_cnt
+		&& rtw_odm_radar_detect(adapter) != true)
+		goto cac_status_chk;
+
+	if (rfctl->dbg_dfs_master_fake_radar_detect_cnt != 0) {
+		RTW_INFO(FUNC_ADPT_FMT" fake radar detect, cnt:%d\n", FUNC_ADPT_ARG(adapter)
+			, rfctl->dbg_dfs_master_fake_radar_detect_cnt);
+		rfctl->dbg_dfs_master_fake_radar_detect_cnt--;
+	}
+
+	if (rfctl->dbg_dfs_master_radar_detect_trigger_non) {
+		/* radar detect debug mode, trigger no mlme flow */
+		if (0)
+			RTW_INFO(FUNC_ADPT_FMT" radar detected, trigger no mlme flow for debug\n", FUNC_ADPT_ARG(adapter));
+	} else {
+		/* TODO: move timer to rfctl */
+		struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+		int i;
+
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			if (!dvobj->padapters[i])
+				continue;
+			if (check_fwstate(&dvobj->padapters[i]->mlmepriv, WIFI_AP_STATE)
+				&& check_fwstate(&dvobj->padapters[i]->mlmepriv, WIFI_ASOC_STATE))
+				break;
+		}
+
+		if (i >= dvobj->iface_nums) {
+			/* what? */
+			rtw_warn_on(1);
+		} else {
+			rtw_chset_update_non_ocp(dvobj->padapters[i]->mlmeextpriv.channel_set
+				, rfctl->radar_detect_ch, rfctl->radar_detect_bw, rfctl->radar_detect_offset);
+			rfctl->radar_detected = 1;
+
+			/* trigger channel selection */
+			rtw_change_bss_chbw_cmd(dvobj->padapters[i], RTW_CMDF_DIRECTLY, -1, dvobj->padapters[i]->mlmepriv.ori_bw, -1);
+		}
+
+		if (rfctl->dfs_master_enabled)
+			goto set_timer;
+		goto exit;
+	}
+
+cac_status_chk:
+
+	if (!IS_CH_WAITING(rfctl) && !IS_CAC_STOPPED(rfctl)) {
+		u8 pause = 0x00;
+
+		rtw_hal_set_hwreg(adapter, HW_VAR_TXPAUSE, &pause);
+		rfctl->cac_start_time = rfctl->cac_end_time = RTW_CAC_STOPPED;
+	}
+
+set_timer:
+	_set_timer(&mlme->dfs_master_timer, DFS_MASTER_TIMER_MS);
+
+exit:
+	return H2C_SUCCESS;
+}
+
+u8 rtw_dfs_master_cmd(_adapter *adapter, bool enqueue)
+{
+	struct cmd_obj *cmdobj;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
+	u8 res = _FAIL;
+
+	if (enqueue) {
+		cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (cmdobj == NULL)
+			goto exit;
+
+		pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+		if (pdrvextra_cmd_parm == NULL) {
+			rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj));
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm->ec_id = DFS_MASTER_WK_CID;
+		pdrvextra_cmd_parm->type = 0;
+		pdrvextra_cmd_parm->size = 0;
+		pdrvextra_cmd_parm->pbuf = NULL;
+
+		init_h2fwcmd_w_parm_no_rsp(cmdobj, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+		res = rtw_enqueue_cmd(pcmdpriv, cmdobj);
+	} else {
+		rtw_dfs_master_hdl(adapter);
+		res = _SUCCESS;
+	}
+
+exit:
+	return res;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+void rtw_dfs_master_timer_hdl(RTW_TIMER_HDL_ARGS)
+#else
+void rtw_dfs_master_timer_hdl(struct timer_list *t)
+#endif
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_adapter *adapter = (_adapter *)FunctionContext;
+#else
+	_adapter *adapter = from_timer(adapter, t, dfs_master_timer);
+#endif
+	rtw_dfs_master_cmd(adapter, true);
+}
+
+void rtw_dfs_master_enable(_adapter *adapter, u8 ch, u8 bw, u8 offset)
+{
+	struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter);
+
+	/* TODO: move timer to rfctl */
+	adapter = GET_PRIMARY_ADAPTER(adapter);
+
+	RTW_INFO(FUNC_ADPT_FMT" on %u,%u,%u\n", FUNC_ADPT_ARG(adapter), ch, bw, offset);
+
+	if (rtw_is_cac_reset_needed(adapter, ch, bw, offset) == true)
+		rtw_reset_cac(adapter, ch, bw, offset);
+
+	rfctl->radar_detect_by_others = false;
+	rfctl->radar_detect_ch = ch;
+	rfctl->radar_detect_bw = bw;
+	rfctl->radar_detect_offset = offset;
+
+	rfctl->radar_detected = 0;
+
+	if (!rfctl->dfs_master_enabled) {
+		RTW_INFO(FUNC_ADPT_FMT" set dfs_master_enabled\n", FUNC_ADPT_ARG(adapter));
+		rfctl->dfs_master_enabled = 1;
+		_set_timer(&adapter->mlmepriv.dfs_master_timer, DFS_MASTER_TIMER_MS);
+
+		if (rtw_rfctl_overlap_radar_detect_ch(rfctl)) {
+			if (IS_CH_WAITING(rfctl)) {
+				u8 pause = 0xFF;
+
+				rtw_hal_set_hwreg(adapter, HW_VAR_TXPAUSE, &pause);
+			}
+			rtw_odm_radar_detect_enable(adapter);
+		}
+	}
+}
+
+void rtw_dfs_master_disable(_adapter *adapter, u8 ch, u8 bw, u8 offset, bool by_others)
+{
+	struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter);
+
+	/* TODO: move timer to rfctl */
+	adapter = GET_PRIMARY_ADAPTER(adapter);
+
+	rfctl->radar_detect_by_others = by_others;
+
+	if (rfctl->dfs_master_enabled) {
+		bool overlap_radar_detect_ch = rtw_rfctl_overlap_radar_detect_ch(rfctl);
+
+		RTW_INFO(FUNC_ADPT_FMT" clear dfs_master_enabled\n", FUNC_ADPT_ARG(adapter));
+
+		rfctl->dfs_master_enabled = 0;
+		rfctl->radar_detected = 0;
+		rfctl->radar_detect_ch = 0;
+		rfctl->radar_detect_bw = 0;
+		rfctl->radar_detect_offset = 0;
+		rfctl->cac_start_time = rfctl->cac_end_time = RTW_CAC_STOPPED;
+		_cancel_timer_ex(&adapter->mlmepriv.dfs_master_timer);
+
+		if (overlap_radar_detect_ch) {
+			u8 pause = 0x00;
+
+			rtw_hal_set_hwreg(adapter, HW_VAR_TXPAUSE, &pause);
+			rtw_odm_radar_detect_disable(adapter);
+		}
+	}
+
+	if (by_others) {
+		rfctl->radar_detect_ch = ch;
+		rfctl->radar_detect_bw = bw;
+		rfctl->radar_detect_offset = offset;
+	}
+}
+
+void rtw_dfs_master_status_apply(_adapter *adapter, u8 self_action)
+{
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+	struct mi_state mstate;
+	u8 u_ch, u_bw, u_offset;
+	bool ld_sta_in_dfs = false;
+	bool sync_ch = false; /* false: asign channel directly */
+	bool needed = false;
+
+	rtw_mi_status_no_self(adapter, &mstate);
+	rtw_mi_get_ch_setting_union_no_self(adapter, &u_ch, &u_bw, &u_offset);
+	if (u_ch != 0)
+		sync_ch = true;
+
+	switch (self_action) {
+	case MLME_STA_CONNECTING:
+		MSTATE_STA_LG_NUM(&mstate)++;
+		break;
+	case MLME_STA_CONNECTED:
+		MSTATE_STA_LD_NUM(&mstate)++;
+		break;
+	case MLME_AP_STARTED:
+		MSTATE_AP_NUM(&mstate)++;
+		break;
+	case MLME_AP_STOPPED:
+	case MLME_STA_DISCONNECTED:
+	default:
+		break;
+	}
+
+	if (sync_ch == true) {
+		if (!rtw_is_chbw_grouped(mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset, u_ch, u_bw, u_offset)) {
+			RTW_INFO(FUNC_ADPT_FMT" can't sync %u,%u,%u with %u,%u,%u\n", FUNC_ADPT_ARG(adapter)
+				, mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset, u_ch, u_bw, u_offset);
+			goto apply;
+		}
+
+		rtw_sync_chbw(&mlmeext->cur_channel, &mlmeext->cur_bwmode, &mlmeext->cur_ch_offset
+			, &u_ch, &u_bw, &u_offset);
+	} else {
+		u_ch = mlmeext->cur_channel;
+		u_bw = mlmeext->cur_bwmode;
+		u_offset = mlmeext->cur_ch_offset;
+	}
+
+	if (MSTATE_STA_LD_NUM(&mstate) > 0) {
+		/* rely on AP on which STA mode connects */
+		if (rtw_is_dfs_ch(u_ch, u_bw, u_offset))
+			ld_sta_in_dfs = true;
+		goto apply;
+	}
+
+	if (MSTATE_STA_LG_NUM(&mstate) > 0) {
+		/* STA mode is linking */
+		goto apply;
+	}
+
+	if (MSTATE_AP_NUM(&mstate) == 0) {
+		/* No working AP mode */
+		goto apply;
+	}
+
+	if (rtw_is_dfs_ch(u_ch, u_bw, u_offset))
+		needed = true;
+
+apply:
+
+	RTW_INFO(FUNC_ADPT_FMT" needed:%d, self_action:%u\n"
+		, FUNC_ADPT_ARG(adapter), needed, self_action);
+	RTW_INFO(FUNC_ADPT_FMT" ld_sta_num:%u, lg_sta_num:%u, ap_num:%u, %u,%u,%u\n"
+		, FUNC_ADPT_ARG(adapter), MSTATE_STA_LD_NUM(&mstate), MSTATE_STA_LG_NUM(&mstate), MSTATE_AP_NUM(&mstate)
+		, u_ch, u_bw, u_offset);
+
+	if (needed == true)
+		rtw_dfs_master_enable(adapter, u_ch, u_bw, u_offset);
+	else
+		rtw_dfs_master_disable(adapter, u_ch, u_bw, u_offset, ld_sta_in_dfs);
+}
+#endif /* CONFIG_DFS_MASTER */
+
+#endif /* CONFIG_AP_MODE */
+
+#ifdef CONFIG_BT_COEXIST
+struct btinfo {
+	u8 cid;
+	u8 len;
+
+	u8 bConnection:1;
+	u8 bSCOeSCO:1;
+	u8 bInQPage:1;
+	u8 bACLBusy:1;
+	u8 bSCOBusy:1;
+	u8 bHID:1;
+	u8 bA2DP:1;
+	u8 bFTP:1;
+
+	u8 retry_cnt:4;
+	u8 rsvd_34:1;
+	u8 rsvd_35:1;
+	u8 rsvd_36:1;
+	u8 rsvd_37:1;
+
+	u8 rssi;
+
+	u8 rsvd_50:1;
+	u8 rsvd_51:1;
+	u8 rsvd_52:1;
+	u8 rsvd_53:1;
+	u8 rsvd_54:1;
+	u8 rsvd_55:1;
+	u8 eSCO_SCO:1;
+	u8 Master_Slave:1;
+
+	u8 rsvd_6;
+	u8 rsvd_7;
+};
+
+void btinfo_evt_dump(void *sel, void *buf)
+{
+	struct btinfo *info = (struct btinfo *)buf;
+
+	RTW_PRINT_SEL(sel, "cid:0x%02x, len:%u\n", info->cid, info->len);
+
+	if (info->len > 2)
+		RTW_PRINT_SEL(sel, "byte2:%s%s%s%s%s%s%s%s\n"
+			      , info->bConnection ? "bConnection " : ""
+			      , info->bSCOeSCO ? "bSCOeSCO " : ""
+			      , info->bInQPage ? "bInQPage " : ""
+			      , info->bACLBusy ? "bACLBusy " : ""
+			      , info->bSCOBusy ? "bSCOBusy " : ""
+			      , info->bHID ? "bHID " : ""
+			      , info->bA2DP ? "bA2DP " : ""
+			      , info->bFTP ? "bFTP" : ""
+			     );
+
+	if (info->len > 3)
+		RTW_PRINT_SEL(sel, "retry_cnt:%u\n", info->retry_cnt);
+
+	if (info->len > 4)
+		RTW_PRINT_SEL(sel, "rssi:%u\n", info->rssi);
+
+	if (info->len > 5)
+		RTW_PRINT_SEL(sel, "byte5:%s%s\n"
+			      , info->eSCO_SCO ? "eSCO_SCO " : ""
+			      , info->Master_Slave ? "Master_Slave " : ""
+			     );
+}
+
+static void rtw_btinfo_hdl(_adapter *adapter, u8 *buf, u16 buf_len)
+{
+#define BTINFO_WIFI_FETCH 0x23
+#define BTINFO_BT_AUTO_RPT 0x27
+#ifdef CONFIG_BT_COEXIST_SOCKET_TRX
+	struct btinfo_8761ATV *info = (struct btinfo_8761ATV *)buf;
+#else /* !CONFIG_BT_COEXIST_SOCKET_TRX */
+	struct btinfo *info = (struct btinfo *)buf;
+#endif /* CONFIG_BT_COEXIST_SOCKET_TRX */
+	u8 cmd_idx;
+	u8 len;
+
+	cmd_idx = info->cid;
+
+	if (info->len > buf_len - 2) {
+		rtw_warn_on(1);
+		len = buf_len - 2;
+	} else
+		len = info->len;
+
+	/* #define DBG_PROC_SET_BTINFO_EVT */
+#ifdef DBG_PROC_SET_BTINFO_EVT
+#ifdef CONFIG_BT_COEXIST_SOCKET_TRX
+	RTW_INFO("%s: btinfo[0]=%x,btinfo[1]=%x,btinfo[2]=%x,btinfo[3]=%x btinfo[4]=%x,btinfo[5]=%x,btinfo[6]=%x,btinfo[7]=%x\n"
+		, __func__, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
+#else/* !CONFIG_BT_COEXIST_SOCKET_TRX */
+	btinfo_evt_dump(RTW_DBGDUMP, info);
+#endif /* CONFIG_BT_COEXIST_SOCKET_TRX */
+#endif /* DBG_PROC_SET_BTINFO_EVT */
+
+	/* transform BT-FW btinfo to WiFI-FW C2H format and notify */
+	if (cmd_idx == BTINFO_WIFI_FETCH)
+		buf[1] = 0;
+	else if (cmd_idx == BTINFO_BT_AUTO_RPT)
+		buf[1] = 2;
+#ifdef CONFIG_BT_COEXIST_SOCKET_TRX
+	else if (cmd_idx == 0x01 || cmd_idx == 0x02)
+		buf[1] = buf[0];
+#endif /* CONFIG_BT_COEXIST_SOCKET_TRX */
+	rtw_btcoex_BtInfoNotify(adapter, len + 1, &buf[1]);
+}
+
+u8 rtw_btinfo_cmd(_adapter *adapter, u8 *buf, u16 len)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	u8 *btinfo;
+	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	btinfo = rtw_zmalloc(len);
+	if (btinfo == NULL) {
+		rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
+		rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = BTINFO_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = len;
+	pdrvextra_cmd_parm->pbuf = btinfo;
+
+	memcpy(btinfo, buf, len);
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+#endif /* CONFIG_BT_COEXIST */
+
+u8 rtw_test_h2c_cmd(_adapter *adapter, u8 *buf, u8 len)
+{
+	struct cmd_obj *pcmdobj;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	u8 *ph2c_content;
+	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+	pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmdobj == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	ph2c_content = rtw_zmalloc(len);
+	if (ph2c_content == NULL) {
+		rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj));
+		rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = TEST_H2C_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = len;
+	pdrvextra_cmd_parm->pbuf = ph2c_content;
+
+	memcpy(ph2c_content, buf, len);
+
+	init_h2fwcmd_w_parm_no_rsp(pcmdobj, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+
+exit:
+	return res;
+}
+
+static s32 rtw_mp_cmd_hdl(_adapter *padapter, u8 mp_cmd_id)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	int ret = H2C_SUCCESS;
+	u8 rfreg0;
+
+	if (mp_cmd_id == MP_START) {
+		if (padapter->registrypriv.mp_mode == 0) {
+			rtw_hal_deinit(padapter);
+			padapter->registrypriv.mp_mode = 1;
+#ifdef CONFIG_RF_POWER_TRIM
+			if (!IS_HARDWARE_TYPE_8814A(padapter) && !IS_HARDWARE_TYPE_8822B(padapter)) {
+				padapter->registrypriv.RegPwrTrimEnable = 1;
+				rtw_hal_read_chip_info(padapter);
+			}
+#endif /*CONFIG_RF_POWER_TRIM*/
+			rtw_hal_init(padapter);
+#ifdef RTW_HALMAC /*for New IC*/
+			MPT_InitializeAdapter(padapter, 1);
+#endif /* CONFIG_MP_INCLUDED */
+		}
+
+		if (padapter->registrypriv.mp_mode == 0) {
+			ret = H2C_REJECTED;
+			goto exit;
+		}
+
+		if (padapter->mppriv.mode == MP_OFF) {
+			if (mp_start_test(padapter) == _FAIL) {
+				ret = H2C_REJECTED;
+				goto exit;
+			}
+			padapter->mppriv.mode = MP_ON;
+			MPT_PwrCtlDM(padapter, 0);
+		}
+		padapter->mppriv.bmac_filter = false;
+		odm_write_dig(&pHalData->odmpriv, 0x20);
+	} else if (mp_cmd_id == MP_STOP) {
+		if (padapter->registrypriv.mp_mode == 1) {
+			MPT_DeInitAdapter(padapter);
+			rtw_hal_deinit(padapter);
+			padapter->registrypriv.mp_mode = 0;
+			rtw_hal_init(padapter);
+		}
+
+		if (padapter->mppriv.mode != MP_OFF) {
+			mp_stop_test(padapter);
+			padapter->mppriv.mode = MP_OFF;
+		}
+	} else {
+		RTW_INFO(FUNC_ADPT_FMT"invalid id:%d\n", FUNC_ADPT_ARG(padapter), mp_cmd_id);
+		ret = H2C_PARAMETERS_ERROR;
+		rtw_warn_on(1);
+	}
+
+exit:
+	return ret;
+}
+
+u8 rtw_mp_cmd(_adapter *adapter, u8 mp_cmd_id, u8 flags)
+{
+	struct cmd_obj *cmdobj;
+	struct drvextra_cmd_parm *parm;
+	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
+	struct submit_ctx sctx;
+	u8 res = _SUCCESS;
+
+	parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (!parm) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	parm->ec_id = MP_CMD_WK_CID;
+	parm->type = mp_cmd_id;
+	parm->size = 0;
+	parm->pbuf = NULL;
+
+	if (flags & RTW_CMDF_DIRECTLY) {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (rtw_mp_cmd_hdl(adapter, mp_cmd_id) != H2C_SUCCESS)
+			res = _FAIL;
+		rtw_mfree((u8 *)parm, sizeof(*parm));
+	} else {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj));
+		if (cmdobj == NULL) {
+			res = _FAIL;
+			rtw_mfree((u8 *)parm, sizeof(*parm));
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		if (flags & RTW_CMDF_WAIT_ACK) {
+			cmdobj->sctx = &sctx;
+			rtw_sctx_init(&sctx, 10 * 1000);
+		}
+
+		res = rtw_enqueue_cmd(pcmdpriv, cmdobj);
+
+		if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) {
+			rtw_sctx_wait(&sctx, __func__);
+			_enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
+			if (sctx.status == RTW_SCTX_SUBMITTED)
+				cmdobj->sctx = NULL;
+			_exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
+			if (sctx.status != RTW_SCTX_DONE_SUCCESS)
+				res = _FAIL;
+		}
+	}
+
+exit:
+	return res;
+}
+
+#ifdef CONFIG_RTW_CUSTOMER_STR
+static s32 rtw_customer_str_cmd_hdl(_adapter *adapter, u8 write, const u8 *cstr)
+{
+	int ret = H2C_SUCCESS;
+
+	if (write)
+		ret = rtw_hal_h2c_customer_str_write(adapter, cstr);
+	else
+		ret = rtw_hal_h2c_customer_str_req(adapter);
+
+	return ret == _SUCCESS ? H2C_SUCCESS : H2C_REJECTED;
+}
+
+static u8 rtw_customer_str_cmd(_adapter *adapter, u8 write, const u8 *cstr)
+{
+	struct cmd_obj *cmdobj;
+	struct drvextra_cmd_parm *parm;
+	u8 *str = NULL;
+	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
+	struct submit_ctx sctx;
+	u8 res = _SUCCESS;
+
+	parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (parm == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	if (write) {
+		str = rtw_zmalloc(RTW_CUSTOMER_STR_LEN);
+		if (str == NULL) {
+			rtw_mfree((u8 *)parm, sizeof(struct drvextra_cmd_parm));
+			res = _FAIL;
+			goto exit;
+		}
+	}
+
+	parm->ec_id = CUSTOMER_STR_WK_CID;
+	parm->type = write;
+	parm->size = write ? RTW_CUSTOMER_STR_LEN : 0;
+	parm->pbuf = write ? str : NULL;
+
+	if (write)
+		memcpy(str, cstr, RTW_CUSTOMER_STR_LEN);
+
+	/* need enqueue, prepare cmd_obj and enqueue */
+	cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj));
+	if (cmdobj == NULL) {
+		res = _FAIL;
+		rtw_mfree((u8 *)parm, sizeof(*parm));
+		if (write)
+			rtw_mfree(str, RTW_CUSTOMER_STR_LEN);
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(cmdobj, parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	cmdobj->sctx = &sctx;
+	rtw_sctx_init(&sctx, 2 * 1000);
+
+	res = rtw_enqueue_cmd(pcmdpriv, cmdobj);
+
+	if (res == _SUCCESS) {
+		rtw_sctx_wait(&sctx, __func__);
+		_enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
+		if (sctx.status == RTW_SCTX_SUBMITTED)
+			cmdobj->sctx = NULL;
+		_exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
+		if (sctx.status != RTW_SCTX_DONE_SUCCESS)
+			res = _FAIL;
+	}
+
+exit:
+	return res;
+}
+
+inline u8 rtw_customer_str_req_cmd(_adapter *adapter)
+{
+	return rtw_customer_str_cmd(adapter, 0, NULL);
+}
+
+inline u8 rtw_customer_str_write_cmd(_adapter *adapter, const u8 *cstr)
+{
+	return rtw_customer_str_cmd(adapter, 1, cstr);
+}
+#endif /* CONFIG_RTW_CUSTOMER_STR */
+
+static u8 rtw_c2h_wk_cmd(PADAPTER padapter, u8 *pbuf, u16 length, u8 type)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 *extra_cmd_buf;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	extra_cmd_buf = rtw_zmalloc(length);
+	if (extra_cmd_buf == NULL) {
+		rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
+		rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm));
+		res = _FAIL;
+		goto exit;
+	}
+
+	memcpy(extra_cmd_buf, pbuf, length);
+	pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
+	pdrvextra_cmd_parm->type = type;
+	pdrvextra_cmd_parm->size = length;
+	pdrvextra_cmd_parm->pbuf = extra_cmd_buf;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+#ifdef CONFIG_FW_C2H_REG
+inline u8 rtw_c2h_reg_wk_cmd(_adapter *adapter, u8 *c2h_evt)
+{
+	return rtw_c2h_wk_cmd(adapter, c2h_evt, c2h_evt ? C2H_REG_LEN : 0, C2H_TYPE_REG);
+}
+#endif
+
+#ifdef CONFIG_FW_C2H_PKT
+inline u8 rtw_c2h_packet_wk_cmd(_adapter *adapter, u8 *c2h_evt, u16 length)
+{
+	return rtw_c2h_wk_cmd(adapter, c2h_evt, length, C2H_TYPE_PKT);
+}
+#endif
+
+u8 rtw_run_in_thread_cmd(PADAPTER padapter, void (*func)(void *), void *context)
+{
+	struct cmd_priv *pcmdpriv;
+	struct cmd_obj *ph2c;
+	struct RunInThread_param *parm;
+	s32 res = _SUCCESS;
+
+	pcmdpriv = &padapter->cmdpriv;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (!ph2c) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	parm = (struct RunInThread_param *)rtw_zmalloc(sizeof(struct RunInThread_param));
+	if (!parm) {
+		rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	parm->func = func;
+	parm->context = context;
+	init_h2fwcmd_w_parm_no_rsp(ph2c, parm, GEN_CMD_CODE(_RunInThreadCMD));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+
+
+	return res;
+}
+
+#ifdef CONFIG_FW_C2H_REG
+s32 c2h_evt_hdl(_adapter *adapter, u8 *c2h_evt, c2h_id_filter filter)
+{
+	s32 ret = _FAIL;
+	u8 buf[C2H_REG_LEN] = {0};
+	u8 id, seq, plen;
+	u8 *payload;
+
+	if (!c2h_evt) {
+		/* No c2h event in cmd_obj, read c2h event before handling*/
+		if (rtw_hal_c2h_evt_read(adapter, buf) != _SUCCESS)
+			goto exit;
+		c2h_evt = buf;
+	}
+
+	rtw_hal_c2h_reg_hdr_parse(adapter, c2h_evt, &id, &seq, &plen, &payload);
+
+	if (filter && filter(adapter, id, seq, plen, payload) == false)
+		goto exit;
+
+	ret = rtw_hal_c2h_handler(adapter, id, seq, plen, payload);
+
+exit:
+	return ret;
+}
+#endif /* CONFIG_FW_C2H_REG */
+
+static u8 session_tracker_cmd(_adapter *adapter, u8 cmd, struct sta_info *sta, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port)
+{
+	struct cmd_priv	*cmdpriv = &adapter->cmdpriv;
+	struct cmd_obj *cmdobj;
+	struct drvextra_cmd_parm *cmd_parm;
+	struct st_cmd_parm *st_parm;
+	u8	res = _SUCCESS;
+
+	cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (cmdobj == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (cmd_parm == NULL) {
+		rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	st_parm = (struct st_cmd_parm *)rtw_zmalloc(sizeof(struct st_cmd_parm));
+	if (st_parm == NULL) {
+		rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj));
+		rtw_mfree((u8 *)cmd_parm, sizeof(struct drvextra_cmd_parm));
+		res = _FAIL;
+		goto exit;
+	}
+
+	st_parm->cmd = cmd;
+	st_parm->sta = sta;
+	if (cmd != ST_CMD_CHK) {
+		memcpy(&st_parm->local_naddr, local_naddr, 4);
+		memcpy(&st_parm->local_port, local_port, 2);
+		memcpy(&st_parm->remote_naddr, remote_naddr, 4);
+		memcpy(&st_parm->remote_port, remote_port, 2);
+	}
+
+	cmd_parm->ec_id = SESSION_TRACKER_WK_CID;
+	cmd_parm->type = 0;
+	cmd_parm->size = sizeof(struct st_cmd_parm);
+	cmd_parm->pbuf = (u8 *)st_parm;
+	init_h2fwcmd_w_parm_no_rsp(cmdobj, cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+	cmdobj->no_io = 1;
+
+	res = rtw_enqueue_cmd(cmdpriv, cmdobj);
+
+exit:
+	return res;
+}
+
+inline u8 session_tracker_chk_cmd(_adapter *adapter, struct sta_info *sta)
+{
+	return session_tracker_cmd(adapter, ST_CMD_CHK, sta, NULL, NULL, NULL, NULL);
+}
+
+inline u8 session_tracker_add_cmd(_adapter *adapter, struct sta_info *sta, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port)
+{
+	return session_tracker_cmd(adapter, ST_CMD_ADD, sta, local_naddr, local_port, remote_naddr, remote_port);
+}
+
+inline u8 session_tracker_del_cmd(_adapter *adapter, struct sta_info *sta, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port)
+{
+	return session_tracker_cmd(adapter, ST_CMD_DEL, sta, local_naddr, local_port, remote_naddr, remote_port);
+}
+
+static void session_tracker_chk_for_sta(_adapter *adapter, struct sta_info *sta)
+{
+	struct st_ctl_t *st_ctl = &sta->st_ctl;
+	int i;
+	unsigned long irqL;
+	_list *plist, *phead, *pnext;
+	_list dlist;
+	struct session_tracker *st = NULL;
+	u8 op_wfd_mode = MIRACAST_DISABLED;
+
+	if (DBG_SESSION_TRACKER)
+		RTW_INFO(FUNC_ADPT_FMT" sta:%p\n", FUNC_ADPT_ARG(adapter), sta);
+
+	if (!(sta->state & _FW_LINKED))
+		goto exit;
+
+	for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++) {
+		if (st_ctl->reg[i].s_proto != 0)
+			break;
+	}
+	if (i >= SESSION_TRACKER_REG_ID_NUM)
+		goto chk_sta;
+
+	INIT_LIST_HEAD(&dlist);
+
+	_enter_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+
+	phead = &st_ctl->tracker_q.queue;
+	plist = get_next(phead);
+	pnext = get_next(plist);
+	while (rtw_end_of_queue_search(phead, plist) == false) {
+		st = LIST_CONTAINOR(plist, struct session_tracker, list);
+		plist = pnext;
+		pnext = get_next(pnext);
+
+		if (st->status != ST_STATUS_ESTABLISH
+			&& rtw_get_passing_time_ms(st->set_time) > ST_EXPIRE_MS
+		) {
+			list_del_init(&st->list);
+			list_add_tail(&st->list, &dlist);
+		}
+
+		/* TODO: check OS for status update */
+		if (st->status == ST_STATUS_CHECK)
+			st->status = ST_STATUS_ESTABLISH;
+
+		if (st->status != ST_STATUS_ESTABLISH)
+			continue;
+
+		#ifdef CONFIG_WFD
+		if (ntohs(st->local_port) == adapter->wfd_info.rtsp_ctrlport)
+			op_wfd_mode |= MIRACAST_SINK;
+		if (ntohs(st->local_port) == adapter->wfd_info.tdls_rtsp_ctrlport)
+			op_wfd_mode |= MIRACAST_SINK;
+		if (ntohs(st->remote_port) == adapter->wfd_info.peer_rtsp_ctrlport)
+			op_wfd_mode |= MIRACAST_SOURCE;
+		#endif
+	}
+
+	_exit_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+
+	plist = get_next(&dlist);
+	while (rtw_end_of_queue_search(&dlist, plist) == false) {
+		st = LIST_CONTAINOR(plist, struct session_tracker, list);
+		plist = get_next(plist);
+		rtw_mfree((u8 *)st, sizeof(struct session_tracker));
+	}
+
+chk_sta:
+	if (STA_OP_WFD_MODE(sta) != op_wfd_mode) {
+		STA_SET_OP_WFD_MODE(sta, op_wfd_mode);
+		rtw_sta_media_status_rpt_cmd(adapter, sta, 1);
+	}
+
+exit:
+	return;
+}
+
+static void session_tracker_chk_for_adapter(_adapter *adapter)
+{
+	struct sta_priv *stapriv = &adapter->stapriv;
+	struct sta_info *sta;
+	int i;
+	unsigned long irqL;
+	_list *plist, *phead;
+	u8 op_wfd_mode = MIRACAST_DISABLED;
+
+	_enter_critical_bh(&stapriv->sta_hash_lock, &irqL);
+
+	for (i = 0; i < NUM_STA; i++) {
+		phead = &(stapriv->sta_hash[i]);
+		plist = get_next(phead);
+
+		while ((rtw_end_of_queue_search(phead, plist)) == false) {
+			sta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+			plist = get_next(plist);
+
+			session_tracker_chk_for_sta(adapter, sta);
+
+			op_wfd_mode |= STA_OP_WFD_MODE(sta);
+		}
+	}
+
+	_exit_critical_bh(&stapriv->sta_hash_lock, &irqL);
+
+#ifdef CONFIG_WFD
+	adapter->wfd_info.op_wfd_mode = MIRACAST_MODE_REVERSE(op_wfd_mode);
+#endif
+}
+
+static void session_tracker_cmd_hdl(_adapter *adapter, struct st_cmd_parm *parm)
+{
+	u8 cmd = parm->cmd;
+	struct sta_info *sta = parm->sta;
+
+	if (cmd == ST_CMD_CHK) {
+		if (sta)
+			session_tracker_chk_for_sta(adapter, sta);
+		else
+			session_tracker_chk_for_adapter(adapter);
+
+		goto exit;
+
+	} else if (cmd == ST_CMD_ADD || cmd == ST_CMD_DEL) {
+		struct st_ctl_t *st_ctl;
+		u32 local_naddr = parm->local_naddr;
+		u16 local_port = parm->local_port;
+		u32 remote_naddr = parm->remote_naddr;
+		u16 remote_port = parm->remote_port;
+		struct session_tracker *st = NULL;
+		unsigned long irqL;
+		_list *plist, *phead;
+		u8 free_st = 0;
+		u8 alloc_st = 0;
+
+		if (DBG_SESSION_TRACKER)
+			RTW_INFO(FUNC_ADPT_FMT" cmd:%u, sta:%p, local:"IP_FMT":"PORT_FMT", remote:"IP_FMT":"PORT_FMT"\n"
+				, FUNC_ADPT_ARG(adapter), cmd, sta
+				, IP_ARG(&local_naddr), PORT_ARG(&local_port)
+				, IP_ARG(&remote_naddr), PORT_ARG(&remote_port)
+			);
+
+		if (!(sta->state & _FW_LINKED))
+			goto exit;
+
+		st_ctl = &sta->st_ctl;
+
+		_enter_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+
+		phead = &st_ctl->tracker_q.queue;
+		plist = get_next(phead);
+		while (rtw_end_of_queue_search(phead, plist) == false) {
+			st = LIST_CONTAINOR(plist, struct session_tracker, list);
+
+			if (st->local_naddr == local_naddr
+				&& st->local_port == cpu_to_be16(local_port)
+				&& st->remote_naddr == remote_naddr
+				&& st->remote_port == cpu_to_be16(remote_port))
+				break;
+
+			plist = get_next(plist);
+		}
+
+		if (rtw_end_of_queue_search(phead, plist) == true)
+			st = NULL;
+
+		switch (cmd) {
+		case ST_CMD_DEL:
+			if (st) {
+				list_del_init(plist);
+				free_st = 1;
+			}
+			goto unlock;
+		case ST_CMD_ADD:
+			if (!st)
+				alloc_st = 1;
+		}
+
+unlock:
+		_exit_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+
+		if (free_st) {
+			rtw_mfree((u8 *)st, sizeof(struct session_tracker));
+			goto exit;
+		}
+
+		if (alloc_st) {
+			st = (struct session_tracker *)rtw_zmalloc(sizeof(struct session_tracker));
+			if (!st)
+				goto exit;
+
+			st->local_naddr = local_naddr;
+			st->local_port = cpu_to_be16(local_port);
+			st->remote_naddr = remote_naddr;
+			st->remote_port = cpu_to_be16(remote_port);
+			st->set_time = jiffies;
+			st->status = ST_STATUS_CHECK;
+
+			_enter_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+			list_add_tail(&st->list, phead);
+			_exit_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+		}
+	}
+
+exit:
+	return;
+}
+
+u8 rtw_drvextra_cmd_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	int ret = H2C_SUCCESS;
+	struct drvextra_cmd_parm *pdrvextra_cmd;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	pdrvextra_cmd = (struct drvextra_cmd_parm *)pbuf;
+
+	switch (pdrvextra_cmd->ec_id) {
+	case STA_MSTATUS_RPT_WK_CID:
+		rtw_sta_media_status_rpt_cmd_hdl(padapter, (struct sta_media_status_rpt_cmd_parm *)pdrvextra_cmd->pbuf);
+		break;
+
+	case DYNAMIC_CHK_WK_CID:/*only  primary padapter go to this cmd, but execute dynamic_chk_wk_hdl() for two interfaces */
+		rtw_dynamic_chk_wk_hdl(padapter);
+		break;
+	case POWER_SAVING_CTRL_WK_CID:
+		power_saving_wk_hdl(padapter);
+		break;
+#ifdef CONFIG_LPS
+	case LPS_CTRL_WK_CID:
+		lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type);
+		break;
+	case DM_IN_LPS_WK_CID:
+		rtw_dm_in_lps_hdl(padapter);
+		break;
+	case LPS_CHANGE_DTIM_CID:
+		rtw_lps_change_dtim_hdl(padapter, (u8)pdrvextra_cmd->type);
+		break;
+#endif
+#if (RATE_ADAPTIVE_SUPPORT == 1)
+	case RTP_TIMER_CFG_WK_CID:
+		rpt_timer_setting_wk_hdl(padapter, pdrvextra_cmd->type);
+		break;
+#endif
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	case ANT_SELECT_WK_CID:
+		antenna_select_wk_hdl(padapter, pdrvextra_cmd->type);
+		break;
+#endif
+#ifdef CONFIG_P2P_PS
+	case P2P_PS_WK_CID:
+		p2p_ps_wk_hdl(padapter, pdrvextra_cmd->type);
+		break;
+#endif
+#ifdef CONFIG_P2P
+	case P2P_PROTO_WK_CID:
+		/* I used the type_size as the type command */
+		ret = p2p_protocol_wk_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf);
+		break;
+#endif
+#ifdef CONFIG_AP_MODE
+	case CHECK_HIQ_WK_CID:
+		rtw_chk_hi_queue_hdl(padapter);
+		break;
+#endif
+#ifdef CONFIG_INTEL_WIDI
+	case INTEl_WIDI_WK_CID:
+		intel_widi_wk_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf);
+		break;
+#endif
+	/* add for CONFIG_IEEE80211W, none 11w can use it */
+	case RESET_SECURITYPRIV:
+		reset_securitypriv_hdl(padapter);
+		break;
+	case FREE_ASSOC_RESOURCES:
+		free_assoc_resources_hdl(padapter);
+		break;
+	case C2H_WK_CID:
+		switch (pdrvextra_cmd->type) {
+		#ifdef CONFIG_FW_C2H_REG
+		case C2H_TYPE_REG:
+			c2h_evt_hdl(padapter, pdrvextra_cmd->pbuf, NULL);
+			break;
+		#endif
+		#ifdef CONFIG_FW_C2H_PKT
+		case C2H_TYPE_PKT:
+			rtw_hal_c2h_pkt_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->size);
+			break;
+		#endif
+		default:
+			RTW_ERR("unknown C2H type:%d\n", pdrvextra_cmd->type);
+			rtw_warn_on(1);
+			break;
+		}
+		break;
+#ifdef CONFIG_BEAMFORMING
+	case BEAMFORMING_WK_CID:
+		beamforming_wk_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf);
+		break;
+#endif
+	case DM_RA_MSK_WK_CID:
+		rtw_dm_ra_mask_hdl(padapter, (struct sta_info *)pdrvextra_cmd->pbuf);
+		break;
+#ifdef CONFIG_BT_COEXIST
+	case BTINFO_WK_CID:
+		rtw_btinfo_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->size);
+		break;
+#endif
+#ifdef CONFIG_DFS_MASTER
+	case DFS_MASTER_WK_CID:
+		rtw_dfs_master_hdl(padapter);
+		break;
+#endif
+	case SESSION_TRACKER_WK_CID:
+		session_tracker_cmd_hdl(padapter, (struct st_cmd_parm *)pdrvextra_cmd->pbuf);
+		break;
+	case EN_HW_UPDATE_TSF_WK_CID:
+		rtw_hal_set_hwreg(padapter, HW_VAR_EN_HW_UPDATE_TSF, NULL);
+		break;
+	case TEST_H2C_CID:
+		rtw_hal_fill_h2c_cmd(padapter, pdrvextra_cmd->pbuf[0], pdrvextra_cmd->size - 1, &pdrvextra_cmd->pbuf[1]);
+		break;
+	case MP_CMD_WK_CID:
+		ret = rtw_mp_cmd_hdl(padapter, pdrvextra_cmd->type);
+		break;
+#ifdef CONFIG_RTW_CUSTOMER_STR
+	case CUSTOMER_STR_WK_CID:
+		ret = rtw_customer_str_cmd_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf);
+		break;
+#endif
+	default:
+		break;
+	}
+
+	if (pdrvextra_cmd->pbuf && pdrvextra_cmd->size > 0)
+		rtw_mfree(pdrvextra_cmd->pbuf, pdrvextra_cmd->size);
+
+	return ret;
+}
+
+void rtw_survey_cmd_callback(_adapter *padapter, struct cmd_obj *pcmd)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+
+	if (pcmd->res == H2C_DROPPED) {
+		/* TODO: cancel timer and do timeout handler directly... */
+		/* need to make timeout handlerOS independent */
+		mlme_set_scan_to_timer(pmlmepriv, 1);
+	} else if (pcmd->res != H2C_SUCCESS) {
+		mlme_set_scan_to_timer(pmlmepriv, 1);
+	}
+
+	/* free cmd */
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_disassoc_cmd_callback(_adapter	*padapter,  struct cmd_obj *pcmd)
+{
+	unsigned long	irqL;
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+
+	if (pcmd->res != H2C_SUCCESS) {
+		_enter_critical_bh(&pmlmepriv->lock, &irqL);
+		set_fwstate(pmlmepriv, _FW_LINKED);
+		_exit_critical_bh(&pmlmepriv->lock, &irqL);
+		goto exit;
+	}
+#ifdef CONFIG_BR_EXT
+	else /* clear bridge database */
+		nat25_db_cleanup(padapter);
+#endif /* CONFIG_BR_EXT */
+
+	/* free cmd */
+	rtw_free_cmd_obj(pcmd);
+
+exit:
+	return;
+}
+
+void rtw_getmacreg_cmdrsp_callback(_adapter *padapter,  struct cmd_obj *pcmd)
+{
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_joinbss_cmd_callback(_adapter	*padapter,  struct cmd_obj *pcmd)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (pcmd->res == H2C_DROPPED) {
+		/* TODO: cancel timer and do timeout handler directly... */
+		/* need to make timeout handlerOS independent */
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+	} else if (pcmd->res != H2C_SUCCESS)
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+
+	rtw_free_cmd_obj(pcmd);
+
+}
+
+void rtw_create_ibss_post_hdl(_adapter *padapter, int status)
+{
+	u8 timer_cancelled;
+	struct sta_info *psta = NULL;
+	struct wlan_network *pwlan = NULL;
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	WLAN_BSSID_EX *pdev_network = &padapter->registrypriv.dev_network;
+	struct wlan_network *mlme_cur_network = &(pmlmepriv->cur_network);
+	unsigned long irqL, irqL2;
+
+	if (status != H2C_SUCCESS)
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+
+	_cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	pwlan = _rtw_alloc_network(pmlmepriv);
+	_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL2);
+	if (!pwlan) {
+		pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue);
+		if (!pwlan) {
+			_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL2);
+			goto createbss_cmd_fail;
+		}
+		pwlan->last_scanned = jiffies;
+	} else {
+		list_add_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue);
+	}
+	pdev_network->Length = get_WLAN_BSSID_EX_sz(pdev_network);
+	memcpy(&(pwlan->network), pdev_network, pdev_network->Length);
+
+	/* copy pdev_network information to pmlmepriv->cur_network */
+	memcpy(&mlme_cur_network->network, pdev_network, (get_WLAN_BSSID_EX_sz(pdev_network)));
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+	_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL2);
+
+createbss_cmd_fail:
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+exit:
+	return;
+}
+
+void rtw_setstaKey_cmdrsp_callback(_adapter *padapter, struct cmd_obj *pcmd)
+{
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_setassocsta_cmdrsp_callback(_adapter	*padapter,  struct cmd_obj *pcmd)
+{
+	unsigned long	irqL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct set_assocsta_parm *passocsta_parm = (struct set_assocsta_parm *)(pcmd->parmbuf);
+	struct set_assocsta_rsp *passocsta_rsp = (struct set_assocsta_rsp *)(pcmd->rsp);
+	struct sta_info	*psta = rtw_get_stainfo(pstapriv, passocsta_parm->addr);
+
+	if (!psta)
+		goto exit;
+
+	psta->aid = psta->mac_id = passocsta_rsp->cam_id;
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE) &&
+	    check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+	set_fwstate(pmlmepriv, _FW_LINKED);
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+exit:
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_getrttbl_cmd_cmdrsp_callback(_adapter	*padapter,  struct cmd_obj *pcmd)
+{
+
+	rtw_free_cmd_obj(pcmd);
+#ifdef CONFIG_MP_INCLUDED
+	if (padapter->registrypriv.mp_mode == 1)
+		padapter->mppriv.workparam.bcompleted = true;
+#endif
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_debug.c b/drivers/staging/rtl8188eu/core/rtw_debug.c
new file mode 100644
index 000000000000..8c458700288c
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_debug.c
@@ -0,0 +1,4800 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_DEBUG_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#ifdef CONFIG_RTW_DEBUG
+static const char *rtw_log_level_str[] = {
+	"_DRV_NONE_ = 0",
+	"_DRV_ALWAYS_ = 1",
+	"_DRV_ERR_ = 2",
+	"_DRV_WARNING_ = 3",
+	"_DRV_INFO_ = 4",
+	"_DRV_DEBUG_ = 5",
+	"_DRV_MAX_ = 6",
+};
+#endif
+
+#ifdef CONFIG_DEBUG_RTL871X
+	u64 GlobalDebugComponents = 0;
+#endif /* CONFIG_DEBUG_RTL871X */
+
+#include <rtw_version.h>
+
+#ifdef CONFIG_TDLS
+	#define TDLS_DBG_INFO_SPACE_BTWN_ITEM_AND_VALUE	41
+#endif
+
+void dump_drv_version(void *sel)
+{
+	RTW_PRINT_SEL(sel, "%s %s\n", DRV_NAME, DRIVERVERSION);
+}
+
+void dump_drv_cfg(void *sel)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24))
+	char *kernel_version = utsname()->release;
+
+	RTW_PRINT_SEL(sel, "\nKernel Version: %s\n", kernel_version);
+#endif
+
+	RTW_PRINT_SEL(sel, "Driver Version: %s\n", DRIVERVERSION);
+	RTW_PRINT_SEL(sel, "------------------------------------------------\n");
+#ifdef CONFIG_IOCTL_CFG80211
+	RTW_PRINT_SEL(sel, "CFG80211\n");
+#ifdef RTW_USE_CFG80211_STA_EVENT
+	RTW_PRINT_SEL(sel, "RTW_USE_CFG80211_STA_EVENT\n");
+#endif
+	#ifdef CONFIG_RADIO_WORK
+	RTW_PRINT_SEL(sel, "CONFIG_RADIO_WORK\n");
+	#endif
+#else
+	RTW_PRINT_SEL(sel, "WEXT\n");
+#endif
+
+	RTW_PRINT_SEL(sel, "DBG:%d\n", DBG);
+#ifdef CONFIG_RTW_DEBUG
+	RTW_PRINT_SEL(sel, "CONFIG_RTW_DEBUG\n");
+#endif
+
+#ifdef CONFIG_CONCURRENT_MODE
+	RTW_PRINT_SEL(sel, "CONFIG_CONCURRENT_MODE\n");
+#endif
+
+#ifdef CONFIG_POWER_SAVING
+	RTW_PRINT_SEL(sel, "CONFIG_POWER_SAVING\n");
+#endif
+
+#ifdef CONFIG_LOAD_PHY_PARA_FROM_FILE
+	RTW_PRINT_SEL(sel, "LOAD_PHY_PARA_FROM_FILE - REALTEK_CONFIG_PATH=%s\n", REALTEK_CONFIG_PATH);
+	#if defined(CONFIG_MULTIDRV) || defined(REALTEK_CONFIG_PATH_WITH_IC_NAME_FOLDER)
+	RTW_PRINT_SEL(sel, "LOAD_PHY_PARA_FROM_FILE - REALTEK_CONFIG_PATH_WITH_IC_NAME_FOLDER\n");
+	#endif
+
+/* configurations about TX power */
+#ifdef CONFIG_CALIBRATE_TX_POWER_BY_REGULATORY
+	RTW_PRINT_SEL(sel, "CONFIG_CALIBRATE_TX_POWER_BY_REGULATORY\n");
+#endif
+#ifdef CONFIG_CALIBRATE_TX_POWER_TO_MAX
+	RTW_PRINT_SEL(sel, "CONFIG_CALIBRATE_TX_POWER_TO_MAX\n");
+#endif
+#endif
+	RTW_PRINT_SEL(sel, "RTW_DEF_MODULE_REGULATORY_CERT=0x%02x\n", RTW_DEF_MODULE_REGULATORY_CERT);
+
+	RTW_PRINT_SEL(sel, "CONFIG_TXPWR_BY_RATE_EN=%d\n", CONFIG_TXPWR_BY_RATE_EN);
+	RTW_PRINT_SEL(sel, "CONFIG_TXPWR_LIMIT_EN=%d\n", CONFIG_TXPWR_LIMIT_EN);
+
+
+#ifdef CONFIG_DISABLE_ODM
+	RTW_PRINT_SEL(sel, "CONFIG_DISABLE_ODM\n");
+#endif
+
+#ifdef CONFIG_MINIMAL_MEMORY_USAGE
+	RTW_PRINT_SEL(sel, "CONFIG_MINIMAL_MEMORY_USAGE\n");
+#endif
+
+	RTW_PRINT_SEL(sel, "CONFIG_RTW_ADAPTIVITY_EN = %d\n", CONFIG_RTW_ADAPTIVITY_EN);
+#if (CONFIG_RTW_ADAPTIVITY_EN)
+	RTW_PRINT_SEL(sel, "ADAPTIVITY_MODE = %s\n", (CONFIG_RTW_ADAPTIVITY_MODE) ? "carrier_sense" : "normal");
+#endif
+
+#ifdef CONFIG_WOWLAN
+	RTW_PRINT_SEL(sel, "CONFIG_WOWLAN - ");
+
+#ifdef CONFIG_GPIO_WAKEUP
+	RTW_PRINT_SEL(sel, "CONFIG_GPIO_WAKEUP - WAKEUP_GPIO_IDX:%d\n", WAKEUP_GPIO_IDX);
+#endif
+#endif
+
+#ifdef CONFIG_TDLS
+	RTW_PRINT_SEL(sel, "CONFIG_TDLS\n");
+#endif
+
+#ifdef CONFIG_RTW_80211R
+	RTW_PRINT_SEL(sel, "CONFIG_RTW_80211R\n");
+#endif
+
+#ifdef CONFIG_SUPPORT_USB_INT
+	RTW_PRINT_SEL(sel, "CONFIG_SUPPORT_USB_INT\n");
+#endif
+#ifdef CONFIG_USB_INTERRUPT_IN_PIPE
+	RTW_PRINT_SEL(sel, "CONFIG_USB_INTERRUPT_IN_PIPE\n");
+#endif
+#ifdef CONFIG_USB_TX_AGGREGATION
+	RTW_PRINT_SEL(sel, "CONFIG_USB_TX_AGGREGATION\n");
+#endif
+#ifdef CONFIG_USB_RX_AGGREGATION
+	RTW_PRINT_SEL(sel, "CONFIG_USB_RX_AGGREGATION\n");
+#endif
+#ifdef CONFIG_USE_USB_BUFFER_ALLOC_TX
+	RTW_PRINT_SEL(sel, "CONFIG_USE_USB_BUFFER_ALLOC_TX\n");
+#endif
+#ifdef CONFIG_USE_USB_BUFFER_ALLOC_RX
+	RTW_PRINT_SEL(sel, "CONFIG_USE_USB_BUFFER_ALLOC_RX\n");
+#endif
+#ifdef CONFIG_PREALLOC_RECV_SKB
+	RTW_PRINT_SEL(sel, "CONFIG_PREALLOC_RECV_SKB\n");
+#endif
+#ifdef CONFIG_FIX_NR_BULKIN_BUFFER
+	RTW_PRINT_SEL(sel, "CONFIG_FIX_NR_BULKIN_BUFFER\n");
+#endif
+	RTW_PRINT_SEL(sel, "MAX_XMITBUF_SZ = %d\n", MAX_XMITBUF_SZ);
+	RTW_PRINT_SEL(sel, "MAX_RECVBUF_SZ = %d\n", MAX_RECVBUF_SZ);
+}
+
+void dump_log_level(void *sel)
+{
+#ifdef CONFIG_RTW_DEBUG
+	int i;
+
+	RTW_PRINT_SEL(sel, "drv_log_level:%d\n", rtw_drv_log_level);
+	for (i = 0; i <= _DRV_MAX_; i++) {
+		if (rtw_log_level_str[i])
+			RTW_PRINT_SEL(sel, "%c %s = %d\n",
+				(rtw_drv_log_level == i) ? '+' : ' ', rtw_log_level_str[i], i);
+	}
+#else
+	RTW_PRINT_SEL(sel, "CONFIG_RTW_DEBUG is disabled\n");
+#endif
+}
+
+void mac_reg_dump(void *sel, _adapter *adapter)
+{
+	int i, j = 1;
+
+	RTW_PRINT_SEL(sel, "======= MAC REG =======\n");
+
+	for (i = 0x0; i < 0x800; i += 4) {
+		if (j % 4 == 1)
+			RTW_PRINT_SEL(sel, "0x%04x", i);
+		_RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i));
+		if ((j++) % 4 == 0)
+			_RTW_PRINT_SEL(sel, "\n");
+	}
+}
+
+void bb_reg_dump(void *sel, _adapter *adapter)
+{
+	int i, j = 1;
+
+	RTW_PRINT_SEL(sel, "======= BB REG =======\n");
+	for (i = 0x800; i < 0x1000; i += 4) {
+		if (j % 4 == 1)
+			RTW_PRINT_SEL(sel, "0x%04x", i);
+		_RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i));
+		if ((j++) % 4 == 0)
+			_RTW_PRINT_SEL(sel, "\n");
+	}
+}
+
+void bb_reg_dump_ex(void *sel, _adapter *adapter)
+{
+	int i, j = 1;
+
+	RTW_PRINT_SEL(sel, "======= BB REG =======\n");
+	for (i = 0x800; i < 0x1000; i += 4) {
+		RTW_PRINT_SEL(sel, "0x%04x", i);
+		_RTW_PRINT_SEL(sel, " 0x%08x ", rtw_read32(adapter, i));
+		_RTW_PRINT_SEL(sel, "\n");
+	}
+}
+
+void rf_reg_dump(void *sel, _adapter *adapter)
+{
+	int i, j = 1, path;
+	u32 value;
+	u8 rf_type = 0;
+	u8 path_nums = 0;
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	if ((RF_1T2R == rf_type) || (RF_1T1R == rf_type))
+		path_nums = 1;
+	else
+		path_nums = 2;
+
+	RTW_PRINT_SEL(sel, "======= RF REG =======\n");
+
+	for (path = 0; path < path_nums; path++) {
+		RTW_PRINT_SEL(sel, "RF_Path(%x)\n", path);
+		for (i = 0; i < 0x100; i++) {
+			value = rtw_hal_read_rfreg(adapter, path, i, 0xffffffff);
+			if (j % 4 == 1)
+				RTW_PRINT_SEL(sel, "0x%02x ", i);
+			_RTW_PRINT_SEL(sel, " 0x%08x ", value);
+			if ((j++) % 4 == 0)
+				_RTW_PRINT_SEL(sel, "\n");
+		}
+	}
+}
+
+static u8 fwdl_test_chksum_fail = 0;
+static u8 fwdl_test_wintint_rdy_fail = 0;
+
+bool rtw_fwdl_test_trigger_chksum_fail(void)
+{
+	if (fwdl_test_chksum_fail) {
+		RTW_PRINT("fwdl test case: trigger chksum_fail\n");
+		fwdl_test_chksum_fail--;
+		return true;
+	}
+	return false;
+}
+
+bool rtw_fwdl_test_trigger_wintint_rdy_fail(void)
+{
+	if (fwdl_test_wintint_rdy_fail) {
+		RTW_PRINT("fwdl test case: trigger wintint_rdy_fail\n");
+		fwdl_test_wintint_rdy_fail--;
+		return true;
+	}
+	return false;
+}
+
+static u32 g_wait_hiq_empty_ms = 0;
+
+u32 rtw_get_wait_hiq_empty_ms(void)
+{
+	return g_wait_hiq_empty_ms;
+}
+
+static u8 del_rx_ampdu_test_no_tx_fail = 0;
+
+bool rtw_del_rx_ampdu_test_trigger_no_tx_fail(void)
+{
+	if (del_rx_ampdu_test_no_tx_fail) {
+		RTW_PRINT("del_rx_ampdu test case: trigger no_tx_fail\n");
+		del_rx_ampdu_test_no_tx_fail--;
+		return true;
+	}
+	return false;
+}
+
+void rtw_sink_rtp_seq_dbg(_adapter *adapter, _pkt *pkt)
+{
+	struct recv_priv *precvpriv = &(adapter->recvpriv);
+	if (precvpriv->sink_udpport > 0) {
+		if (*((__be16 *)((pkt->data) + 0x24)) == cpu_to_be16(precvpriv->sink_udpport)) {
+			precvpriv->pre_rtp_rxseq = precvpriv->cur_rtp_rxseq;
+			precvpriv->cur_rtp_rxseq = be16_to_cpu(*((__be16 *)((pkt->data) + 0x2C)));
+			if (precvpriv->pre_rtp_rxseq + 1 != precvpriv->cur_rtp_rxseq)
+				RTW_INFO("%s : RTP Seq num from %d to %d\n", __func__, precvpriv->pre_rtp_rxseq, precvpriv->cur_rtp_rxseq);
+		}
+	}
+}
+
+void sta_rx_reorder_ctl_dump(void *sel, struct sta_info *sta)
+{
+	struct recv_reorder_ctrl *reorder_ctl;
+	int i;
+
+	for (i = 0; i < 16; i++) {
+		reorder_ctl = &sta->recvreorder_ctrl[i];
+		if (reorder_ctl->ampdu_size != RX_AMPDU_SIZE_INVALID || reorder_ctl->indicate_seq != 0xFFFF) {
+			RTW_PRINT_SEL(sel, "tid=%d, enable=%d, ampdu_size=%u, indicate_seq=%u\n"
+				, i, reorder_ctl->enable, reorder_ctl->ampdu_size, reorder_ctl->indicate_seq
+				     );
+		}
+	}
+}
+
+void dump_tx_rate_bmp(void *sel, struct dvobj_priv *dvobj)
+{
+	_adapter *adapter = dvobj_get_primary_adapter(dvobj);
+	struct rf_ctl_t *rfctl = dvobj_to_rfctl(dvobj);
+	u8 bw;
+
+	RTW_PRINT_SEL(sel, "%-6s", "bw");
+	if (hal_chk_proto_cap(adapter, PROTO_CAP_11AC))
+		_RTW_PRINT_SEL(sel, " %-11s", "vht");
+
+	_RTW_PRINT_SEL(sel, " %-11s %-4s %-3s\n", "ht", "ofdm", "cck");
+
+	for (bw = CHANNEL_WIDTH_20; bw <= CHANNEL_WIDTH_160; bw++) {
+		if (!hal_is_bw_support(adapter, bw))
+			continue;
+
+		RTW_PRINT_SEL(sel, "%6s", ch_width_str(bw));
+		if (hal_chk_proto_cap(adapter, PROTO_CAP_11AC)) {
+			_RTW_PRINT_SEL(sel, " %03x %03x %03x"
+				, RATE_BMP_GET_VHT_3SS(rfctl->rate_bmp_vht_by_bw[bw])
+				, RATE_BMP_GET_VHT_2SS(rfctl->rate_bmp_vht_by_bw[bw])
+				, RATE_BMP_GET_VHT_1SS(rfctl->rate_bmp_vht_by_bw[bw])
+			);
+		}
+
+		_RTW_PRINT_SEL(sel, " %02x %02x %02x %02x"
+			, bw <= CHANNEL_WIDTH_40 ? RATE_BMP_GET_HT_4SS(rfctl->rate_bmp_ht_by_bw[bw]) : 0
+			, bw <= CHANNEL_WIDTH_40 ? RATE_BMP_GET_HT_3SS(rfctl->rate_bmp_ht_by_bw[bw]) : 0
+			, bw <= CHANNEL_WIDTH_40 ? RATE_BMP_GET_HT_2SS(rfctl->rate_bmp_ht_by_bw[bw]) : 0
+			, bw <= CHANNEL_WIDTH_40 ? RATE_BMP_GET_HT_1SS(rfctl->rate_bmp_ht_by_bw[bw]) : 0
+		);
+
+		_RTW_PRINT_SEL(sel, "  %03x   %01x\n"
+			, bw <= CHANNEL_WIDTH_20 ? RATE_BMP_GET_OFDM(rfctl->rate_bmp_cck_ofdm) : 0
+			, bw <= CHANNEL_WIDTH_20 ? RATE_BMP_GET_CCK(rfctl->rate_bmp_cck_ofdm) : 0
+		);
+	}
+}
+
+void dump_adapters_status(void *sel, struct dvobj_priv *dvobj)
+{
+	struct rf_ctl_t *rfctl = dvobj_to_rfctl(dvobj);
+	int i;
+	_adapter *iface;
+	u8 u_ch, u_bw, u_offset;
+
+	dump_mi_status(sel, dvobj);
+
+#ifdef CONFIG_FW_MULTI_PORT_SUPPORT
+	RTW_PRINT_SEL(sel, "default port id:%d\n\n", dvobj->default_port_id);
+#endif /* CONFIG_FW_MULTI_PORT_SUPPORT */
+
+	RTW_PRINT_SEL(sel, "dev status:%s%s\n\n"
+		, dev_is_surprise_removed(dvobj) ? " SR" : ""
+		, dev_is_drv_stopped(dvobj) ? " DS" : ""
+	);
+
+#ifdef CONFIG_P2P
+#define P2P_INFO_TITLE_FMT	" %-3s %-4s"
+#define P2P_INFO_TITLE_ARG	, "lch", "p2ps"
+#ifdef CONFIG_IOCTL_CFG80211
+#define P2P_INFO_VALUE_FMT	" %3u %c%3u"
+#define P2P_INFO_VALUE_ARG	, iface->wdinfo.listen_channel, iface->wdev_data.p2p_enabled ? 'e' : ' ', rtw_p2p_state(&iface->wdinfo)
+#else
+#define P2P_INFO_VALUE_FMT	" %3u %4u"
+#define P2P_INFO_VALUE_ARG	, iface->wdinfo.listen_channel, rtw_p2p_state(&iface->wdinfo)
+#endif
+#define P2P_INFO_DASH		"---------"
+#else
+#define P2P_INFO_TITLE_FMT	""
+#define P2P_INFO_TITLE_ARG
+#define P2P_INFO_VALUE_FMT	""
+#define P2P_INFO_VALUE_ARG
+#define P2P_INFO_DASH
+#endif
+
+	RTW_PRINT_SEL(sel, "%-2s %-15s %c %-3s %-3s %-3s %-17s %-4s %-7s"
+		P2P_INFO_TITLE_FMT
+		" %s\n"
+		, "id", "ifname", ' ', "bup", "nup", "ncd", "macaddr", "port", "ch"
+		P2P_INFO_TITLE_ARG
+		, "status");
+
+	RTW_PRINT_SEL(sel, "---------------------------------------------------------------"
+		P2P_INFO_DASH
+		"-------\n");
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		if (iface) {
+			RTW_PRINT_SEL(sel, "%2d %-15s %c %3u %3u %3u "MAC_FMT" %4hhu %3u,%u,%u"
+				P2P_INFO_VALUE_FMT
+				" "MLME_STATE_FMT"\n"
+				, i, iface->registered ? ADPT_ARG(iface) : NULL
+				, iface->registered ? 'R' : ' '
+				, iface->bup
+				, iface->netif_up
+				, iface->net_closed
+				, MAC_ARG(adapter_mac_addr(iface))
+				, get_hw_port(iface)
+				, iface->mlmeextpriv.cur_channel
+				, iface->mlmeextpriv.cur_bwmode
+				, iface->mlmeextpriv.cur_ch_offset
+				P2P_INFO_VALUE_ARG
+				, MLME_STATE_ARG(iface)
+			);
+		}
+	}
+
+	RTW_PRINT_SEL(sel, "---------------------------------------------------------------"
+		P2P_INFO_DASH
+		"-------\n");
+
+	rtw_mi_get_ch_setting_union(dvobj_get_primary_adapter(dvobj), &u_ch, &u_bw, &u_offset);
+	RTW_PRINT_SEL(sel, "%55s %3u,%u,%u\n"
+		, "union:"
+		, u_ch, u_bw, u_offset
+	);
+
+	RTW_PRINT_SEL(sel, "%55s %3u,%u,%u\n"
+		, "oper:"
+		, dvobj->oper_channel
+		, dvobj->oper_bwmode
+		, dvobj->oper_ch_offset
+	);
+
+#ifdef CONFIG_DFS_MASTER
+	if (rfctl->radar_detect_ch != 0) {
+		u32 non_ocp_ms;
+		u32 cac_ms;
+
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			if (!dvobj->padapters[i])
+				continue;
+			if (check_fwstate(&dvobj->padapters[i]->mlmepriv, WIFI_AP_STATE)
+				&& check_fwstate(&dvobj->padapters[i]->mlmepriv, WIFI_ASOC_STATE))
+				break;
+		}
+
+		if (i >= dvobj->iface_nums) {
+			RTW_PRINT_SEL(sel, "DFS master enable without AP mode???");
+			goto end_dfs_master;
+		}
+
+		RTW_PRINT_SEL(sel, "%55s %3u,%u,%u"
+			, "radar_detect:"
+			, rfctl->radar_detect_ch
+			, rfctl->radar_detect_bw
+			, rfctl->radar_detect_offset
+		);
+
+		_RTW_PRINT_SEL(sel, ", domain:%u", rtw_odm_get_dfs_domain(dvobj->padapters[IFACE_ID0]));
+
+		rtw_get_ch_waiting_ms(dvobj->padapters[i]
+			, rfctl->radar_detect_ch
+			, rfctl->radar_detect_bw
+			, rfctl->radar_detect_offset
+			, &non_ocp_ms
+			, &cac_ms
+		);
+
+		if (non_ocp_ms)
+			_RTW_PRINT_SEL(sel, ", non_ocp:%d", non_ocp_ms);
+		if (cac_ms)
+			_RTW_PRINT_SEL(sel, ", cac:%d", cac_ms);
+end_dfs_master:
+		_RTW_PRINT_SEL(sel, "\n");
+	}
+#endif /* CONFIG_DFS_MASTER */
+}
+
+#define SEC_CAM_ENT_ID_TITLE_FMT "%-2s"
+#define SEC_CAM_ENT_ID_TITLE_ARG "id"
+#define SEC_CAM_ENT_ID_VALUE_FMT "%2u"
+#define SEC_CAM_ENT_ID_VALUE_ARG(id) (id)
+
+#define SEC_CAM_ENT_TITLE_FMT "%-6s %-17s %-32s %-3s %-7s %-2s %-2s %-5s"
+#define SEC_CAM_ENT_TITLE_ARG "ctrl", "addr", "key", "kid", "type", "MK", "GK", "valid"
+#define SEC_CAM_ENT_VALUE_FMT "0x%04x "MAC_FMT" "KEY_FMT" %3u %-7s %2u %2u %5u"
+#define SEC_CAM_ENT_VALUE_ARG(ent) \
+	(ent)->ctrl \
+	, MAC_ARG((ent)->mac) \
+	, KEY_ARG((ent)->key) \
+	, ((ent)->ctrl) & 0x03 \
+	, security_type_str((((ent)->ctrl) >> 2) & 0x07) \
+	, (((ent)->ctrl) >> 5) & 0x01 \
+	, (((ent)->ctrl) >> 6) & 0x01 \
+	, (((ent)->ctrl) >> 15) & 0x01
+
+void dump_sec_cam_ent(void *sel, struct sec_cam_ent *ent, int id)
+{
+	if (id >= 0) {
+		RTW_PRINT_SEL(sel, SEC_CAM_ENT_ID_VALUE_FMT " " SEC_CAM_ENT_VALUE_FMT"\n"
+			, SEC_CAM_ENT_ID_VALUE_ARG(id), SEC_CAM_ENT_VALUE_ARG(ent));
+	} else
+		RTW_PRINT_SEL(sel, SEC_CAM_ENT_VALUE_FMT"\n", SEC_CAM_ENT_VALUE_ARG(ent));
+}
+
+void dump_sec_cam_ent_title(void *sel, u8 has_id)
+{
+	if (has_id) {
+		RTW_PRINT_SEL(sel, SEC_CAM_ENT_ID_TITLE_FMT " " SEC_CAM_ENT_TITLE_FMT"\n"
+			, SEC_CAM_ENT_ID_TITLE_ARG, SEC_CAM_ENT_TITLE_ARG);
+	} else
+		RTW_PRINT_SEL(sel, SEC_CAM_ENT_TITLE_FMT"\n", SEC_CAM_ENT_TITLE_ARG);
+}
+
+void dump_sec_cam(void *sel, _adapter *adapter)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	struct sec_cam_ent ent;
+	int i;
+
+	RTW_PRINT_SEL(sel, "HW sec cam:\n");
+	dump_sec_cam_ent_title(sel, 1);
+	for (i = 0; i < cam_ctl->num; i++) {
+		rtw_sec_read_cam_ent(adapter, i, (u8 *)(&ent.ctrl), ent.mac, ent.key);
+		dump_sec_cam_ent(sel , &ent, i);
+	}
+}
+
+void dump_sec_cam_cache(void *sel, _adapter *adapter)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	int i;
+
+	RTW_PRINT_SEL(sel, "SW sec cam cache:\n");
+	dump_sec_cam_ent_title(sel, 1);
+	for (i = 0; i < cam_ctl->num; i++) {
+		if (dvobj->cam_cache[i].ctrl != 0)
+			dump_sec_cam_ent(sel, &dvobj->cam_cache[i], i);
+	}
+
+}
+
+#ifdef CONFIG_PROC_DEBUG
+ssize_t proc_set_write_reg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 addr, val, len;
+
+	if (count < 3) {
+		RTW_INFO("argument size is less than 3\n");
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%x %x %x", &addr, &val, &len);
+
+		if (num !=  3) {
+			RTW_INFO("invalid write_reg parameter!\n");
+			return count;
+		}
+
+		switch (len) {
+		case 1:
+			rtw_write8(padapter, addr, (u8)val);
+			break;
+		case 2:
+			rtw_write16(padapter, addr, (u16)val);
+			break;
+		case 4:
+			rtw_write32(padapter, addr, val);
+			break;
+		default:
+			RTW_INFO("error write length=%d", len);
+			break;
+		}
+
+	}
+
+	return count;
+
+}
+
+static u32 proc_get_read_addr = 0xeeeeeeee;
+static u32 proc_get_read_len = 0x4;
+
+int proc_get_read_reg(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+
+	if (proc_get_read_addr == 0xeeeeeeee) {
+		RTW_PRINT_SEL(m, "address not initialized\n");
+		return 0;
+	}
+
+	switch (proc_get_read_len) {
+	case 1:
+		RTW_PRINT_SEL(m, "rtw_read8(0x%x)=0x%x\n", proc_get_read_addr, rtw_read8(padapter, proc_get_read_addr));
+		break;
+	case 2:
+		RTW_PRINT_SEL(m, "rtw_read16(0x%x)=0x%x\n", proc_get_read_addr, rtw_read16(padapter, proc_get_read_addr));
+		break;
+	case 4:
+		RTW_PRINT_SEL(m, "rtw_read32(0x%x)=0x%x\n", proc_get_read_addr, rtw_read32(padapter, proc_get_read_addr));
+		break;
+	default:
+		RTW_PRINT_SEL(m, "error read length=%d\n", proc_get_read_len);
+		break;
+	}
+
+	return 0;
+}
+
+ssize_t proc_set_read_reg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	char tmp[16];
+	u32 addr, len;
+
+	if (count < 2) {
+		RTW_INFO("argument size is less than 2\n");
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%x %x", &addr, &len);
+
+		if (num !=  2) {
+			RTW_INFO("invalid read_reg parameter!\n");
+			return count;
+		}
+
+		proc_get_read_addr = addr;
+
+		proc_get_read_len = len;
+	}
+
+	return count;
+
+}
+
+int proc_get_rx_stat(struct seq_file *m, void *v)
+{
+	unsigned long	 irqL;
+	_list	*plist, *phead;
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct sta_info *psta = NULL;
+	struct stainfo_stats	*pstats = NULL;
+	struct sta_priv		*pstapriv = &(adapter->stapriv);
+	u32 i, j;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 null_addr[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+	for (i = 0; i < NUM_STA; i++) {
+		phead = &(pstapriv->sta_hash[i]);
+		plist = get_next(phead);
+		while ((rtw_end_of_queue_search(phead, plist)) == false) {
+			psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+			plist = get_next(plist);
+			pstats = &psta->sta_stats;
+
+			if (pstats == NULL)
+				continue;
+			if ((memcmp(psta->hwaddr, bc_addr, 6))
+				&& (memcmp(psta->hwaddr, null_addr, 6))
+				&& (memcmp(psta->hwaddr, adapter_mac_addr(adapter), 6))) {
+				RTW_PRINT_SEL(m, "MAC :\t\t"MAC_FMT "\n", MAC_ARG(psta->hwaddr));
+				RTW_PRINT_SEL(m, "data_rx_cnt :\t%llu\n", pstats->rx_data_pkts - pstats->rx_data_last_pkts);
+				pstats->rx_data_last_pkts = pstats->rx_data_pkts;
+				RTW_PRINT_SEL(m, "duplicate_cnt :\t%u\n", pstats->duplicate_cnt);
+				pstats->duplicate_cnt = 0;
+				RTW_PRINT_SEL(m, "rx_per_rate_cnt :\n");
+
+				for (j = 0; j < 0x60; j++) {
+					RTW_PRINT_SEL(m, "%08u  ", pstats->rxratecnt[j]);
+					pstats->rxratecnt[j] = 0;
+					if ((j%8) == 7)
+						RTW_PRINT_SEL(m, "\n");
+				}
+				RTW_PRINT_SEL(m, "\n");
+			}
+		}
+	}
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+	return 0;
+}
+
+int proc_get_tx_stat(struct seq_file *m, void *v)
+{
+	unsigned long	irqL;
+	_list	*plist, *phead;
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct sta_info *psta = NULL, *sta_rec[NUM_STA];
+	struct stainfo_stats	*pstats = NULL;
+	struct sta_priv	*pstapriv = &(adapter->stapriv);
+	u32 i, macid_rec_idx = 0;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 null_addr[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	struct submit_ctx gotc2h;
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+	for (i = 0; i < NUM_STA; i++) {
+		sta_rec[i] = NULL;
+		phead = &(pstapriv->sta_hash[i]);
+		plist = get_next(phead);
+		while ((rtw_end_of_queue_search(phead, plist)) == false) {
+			psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+			plist = get_next(plist);
+			if ((memcmp(psta->hwaddr, bc_addr, 6))
+				&& (memcmp(psta->hwaddr, null_addr, 6))
+				&& (memcmp(psta->hwaddr, adapter_mac_addr(adapter), 6))) {
+				sta_rec[macid_rec_idx++] = psta;
+			}
+		}
+	}
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+	for (i = 0; i < macid_rec_idx; i++) {
+		pstats = &(sta_rec[i]->sta_stats);
+		if (pstats == NULL)
+			continue;
+		pstapriv->c2h_sta = sta_rec[i];
+		rtw_hal_reqtxrpt(adapter, sta_rec[i]->mac_id);
+		rtw_sctx_init(&gotc2h, 60);
+		pstapriv->gotc2h = &gotc2h;
+		if (rtw_sctx_wait(&gotc2h, __func__)) {
+			RTW_PRINT_SEL(m, "MAC :\t\t"MAC_FMT "\n", MAC_ARG(sta_rec[i]->hwaddr));
+			RTW_PRINT_SEL(m, "data_sent_cnt :\t%u\n", pstats->tx_ok_cnt + pstats->tx_fail_cnt);
+			RTW_PRINT_SEL(m, "success_cnt :\t%u\n", pstats->tx_ok_cnt);
+			RTW_PRINT_SEL(m, "failure_cnt :\t%u\n", pstats->tx_fail_cnt);
+			RTW_PRINT_SEL(m, "retry_cnt :\t%u\n\n", pstats->tx_retry_cnt);
+		} else {
+			RTW_PRINT_SEL(m, "Warming : Query timeout, operation abort!!\n");
+			RTW_PRINT_SEL(m, "\n");
+			break;
+		}
+	}
+	return 0;
+}
+
+int proc_get_fwstate(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	RTW_PRINT_SEL(m, "fwstate=0x%x\n", get_fwstate(pmlmepriv));
+
+	return 0;
+}
+
+int proc_get_sec_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct security_priv *sec = &padapter->securitypriv;
+
+	RTW_PRINT_SEL(m, "auth_alg=0x%x, enc_alg=0x%x, auth_type=0x%x, enc_type=0x%x\n",
+		sec->dot11AuthAlgrthm, sec->dot11PrivacyAlgrthm,
+		sec->ndisauthtype, sec->ndisencryptstatus);
+
+	RTW_PRINT_SEL(m, "hw_decrypted=%d\n", sec->hw_decrypted);
+
+#ifdef DBG_SW_SEC_CNT
+	RTW_PRINT_SEL(m, "wep_sw_enc_cnt=%llu, %llu, %llu\n"
+		, sec->wep_sw_enc_cnt_bc , sec->wep_sw_enc_cnt_mc, sec->wep_sw_enc_cnt_uc);
+	RTW_PRINT_SEL(m, "wep_sw_dec_cnt=%llu, %llu, %llu\n"
+		, sec->wep_sw_dec_cnt_bc , sec->wep_sw_dec_cnt_mc, sec->wep_sw_dec_cnt_uc);
+
+	RTW_PRINT_SEL(m, "tkip_sw_enc_cnt=%llu, %llu, %llu\n"
+		, sec->tkip_sw_enc_cnt_bc , sec->tkip_sw_enc_cnt_mc, sec->tkip_sw_enc_cnt_uc);
+	RTW_PRINT_SEL(m, "tkip_sw_dec_cnt=%llu, %llu, %llu\n"
+		, sec->tkip_sw_dec_cnt_bc , sec->tkip_sw_dec_cnt_mc, sec->tkip_sw_dec_cnt_uc);
+
+	RTW_PRINT_SEL(m, "aes_sw_enc_cnt=%llu, %llu, %llu\n"
+		, sec->aes_sw_enc_cnt_bc , sec->aes_sw_enc_cnt_mc, sec->aes_sw_enc_cnt_uc);
+	RTW_PRINT_SEL(m, "aes_sw_dec_cnt=%llu, %llu, %llu\n"
+		, sec->aes_sw_dec_cnt_bc , sec->aes_sw_dec_cnt_mc, sec->aes_sw_dec_cnt_uc);
+#endif /* DBG_SW_SEC_CNT */
+
+	return 0;
+}
+
+int proc_get_mlmext_state(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	RTW_PRINT_SEL(m, "pmlmeinfo->state=0x%x\n", pmlmeinfo->state);
+
+	return 0;
+}
+
+#ifdef CONFIG_LAYER2_ROAMING
+int proc_get_roam_flags(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+
+	RTW_PRINT_SEL(m, "0x%02x\n", rtw_roam_flags(adapter));
+
+	return 0;
+}
+
+ssize_t proc_set_roam_flags(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+
+	char tmp[32];
+	u8 flags;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%hhx", &flags);
+
+		if (num == 1)
+			rtw_assign_roam_flags(adapter, flags);
+	}
+
+	return count;
+
+}
+
+int proc_get_roam_param(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+
+	RTW_PRINT_SEL(m, "%12s %12s %11s %14s\n", "rssi_diff_th", "scanr_exp_ms", "scan_int_ms", "rssi_threshold");
+	RTW_PRINT_SEL(m, "%-12u %-12u %-11u %-14u\n"
+		, mlme->roam_rssi_diff_th
+		, mlme->roam_scanr_exp_ms
+		, mlme->roam_scan_int_ms
+		, mlme->roam_rssi_threshold
+	);
+
+	return 0;
+}
+
+ssize_t proc_set_roam_param(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+
+	char tmp[32];
+	u8 rssi_diff_th;
+	u32 scanr_exp_ms;
+	u32 scan_int_ms;
+	u8 rssi_threshold;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%hhu %u %u %hhu", &rssi_diff_th, &scanr_exp_ms, &scan_int_ms, &rssi_threshold);
+
+		if (num >= 1)
+			mlme->roam_rssi_diff_th = rssi_diff_th;
+		if (num >= 2)
+			mlme->roam_scanr_exp_ms = scanr_exp_ms;
+		if (num >= 3)
+			mlme->roam_scan_int_ms = scan_int_ms;
+		if (num >= 4)
+			mlme->roam_rssi_threshold = rssi_threshold;
+	}
+
+	return count;
+
+}
+
+ssize_t proc_set_roam_tgt_addr(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+
+	char tmp[32];
+	u8 addr[ETH_ALEN];
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", addr, addr + 1, addr + 2, addr + 3, addr + 4, addr + 5);
+		if (num == 6)
+			memcpy(adapter->mlmepriv.roam_tgt_addr, addr, ETH_ALEN);
+
+		RTW_INFO("set roam_tgt_addr to "MAC_FMT"\n", MAC_ARG(adapter->mlmepriv.roam_tgt_addr));
+	}
+
+	return count;
+}
+#endif /* CONFIG_LAYER2_ROAMING */
+
+#ifdef CONFIG_RTW_80211R
+ssize_t proc_set_ft_flags(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+
+	char tmp[32];
+	u8 flags;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int num = sscanf(tmp, "%hhx", &flags);
+
+		if (num == 1)
+			adapter->mlmepriv.ftpriv.ft_flags = flags;
+	}
+
+	return count;
+
+}
+
+int proc_get_ft_flags(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+
+	RTW_PRINT_SEL(m, "0x%02x\n", adapter->mlmepriv.ftpriv.ft_flags);
+
+	return 0;
+}
+#endif
+
+int proc_get_qos_option(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	RTW_PRINT_SEL(m, "qos_option=%d\n", pmlmepriv->qospriv.qos_option);
+
+	return 0;
+}
+
+int proc_get_ht_option(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	RTW_PRINT_SEL(m, "ht_option=%d\n", pmlmepriv->htpriv.ht_option);
+	return 0;
+}
+
+int proc_get_rf_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	RTW_PRINT_SEL(m, "cur_ch=%d, cur_bw=%d, cur_ch_offet=%d\n",
+		pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+
+	RTW_PRINT_SEL(m, "oper_ch=%d, oper_bw=%d, oper_ch_offet=%d\n",
+		rtw_get_oper_ch(padapter), rtw_get_oper_bw(padapter),  rtw_get_oper_choffset(padapter));
+
+	return 0;
+}
+
+int proc_get_scan_param(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+	struct ss_res *ss = &mlmeext->sitesurvey_res;
+
+#define SCAN_PARAM_TITLE_FMT "%10s"
+#define SCAN_PARAM_VALUE_FMT "%-10u"
+#define SCAN_PARAM_TITLE_ARG , "scan_ch_ms"
+#define SCAN_PARAM_VALUE_ARG , ss->scan_ch_ms
+#define SCAN_PARAM_TITLE_FMT_HT " %15s %13s"
+#define SCAN_PARAM_VALUE_FMT_HT " %-15u %-13u"
+#define SCAN_PARAM_TITLE_ARG_HT , "rx_ampdu_accept", "rx_ampdu_size"
+#define SCAN_PARAM_VALUE_ARG_HT , ss->rx_ampdu_accept, ss->rx_ampdu_size
+#ifdef CONFIG_SCAN_BACKOP
+#define SCAN_PARAM_TITLE_FMT_BACKOP " %9s %12s"
+#define SCAN_PARAM_VALUE_FMT_BACKOP " %-9u %-12u"
+#define SCAN_PARAM_TITLE_ARG_BACKOP , "backop_ms", "scan_cnt_max"
+#define SCAN_PARAM_VALUE_ARG_BACKOP , ss->backop_ms, ss->scan_cnt_max
+#else
+#define SCAN_PARAM_TITLE_FMT_BACKOP ""
+#define SCAN_PARAM_VALUE_FMT_BACKOP ""
+#define SCAN_PARAM_TITLE_ARG_BACKOP
+#define SCAN_PARAM_VALUE_ARG_BACKOP
+#endif
+
+	RTW_PRINT_SEL(m,
+		SCAN_PARAM_TITLE_FMT
+		SCAN_PARAM_TITLE_FMT_HT
+		SCAN_PARAM_TITLE_FMT_BACKOP
+		"\n"
+		SCAN_PARAM_TITLE_ARG
+		SCAN_PARAM_TITLE_ARG_HT
+		SCAN_PARAM_TITLE_ARG_BACKOP
+	);
+
+	RTW_PRINT_SEL(m,
+		SCAN_PARAM_VALUE_FMT
+		SCAN_PARAM_VALUE_FMT_HT
+		SCAN_PARAM_VALUE_FMT_BACKOP
+		"\n"
+		SCAN_PARAM_VALUE_ARG
+		SCAN_PARAM_VALUE_ARG_HT
+		SCAN_PARAM_VALUE_ARG_BACKOP
+	);
+
+	return 0;
+}
+
+ssize_t proc_set_scan_param(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+	struct ss_res *ss = &mlmeext->sitesurvey_res;
+
+	char tmp[32] = {0};
+
+	u16 scan_ch_ms;
+#define SCAN_PARAM_INPUT_FMT "%hu"
+#define SCAN_PARAM_INPUT_ARG , &scan_ch_ms
+	u8 rx_ampdu_accept;
+	u8 rx_ampdu_size;
+#define SCAN_PARAM_INPUT_FMT_HT " %hhu %hhu"
+#define SCAN_PARAM_INPUT_ARG_HT , &rx_ampdu_accept, &rx_ampdu_size
+#ifdef CONFIG_SCAN_BACKOP
+	u16 backop_ms;
+	u8 scan_cnt_max;
+#define SCAN_PARAM_INPUT_FMT_BACKOP " %hu %hhu"
+#define SCAN_PARAM_INPUT_ARG_BACKOP , &backop_ms, &scan_cnt_max
+#else
+#define SCAN_PARAM_INPUT_FMT_BACKOP ""
+#define SCAN_PARAM_INPUT_ARG_BACKOP
+#endif
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp,
+			SCAN_PARAM_INPUT_FMT
+			SCAN_PARAM_INPUT_FMT_HT
+			SCAN_PARAM_INPUT_FMT_BACKOP
+			SCAN_PARAM_INPUT_ARG
+			SCAN_PARAM_INPUT_ARG_HT
+			SCAN_PARAM_INPUT_ARG_BACKOP
+		);
+
+		if (num-- > 0)
+			ss->scan_ch_ms = scan_ch_ms;
+		if (num-- > 0)
+			ss->rx_ampdu_accept = rx_ampdu_accept;
+		if (num-- > 0)
+			ss->rx_ampdu_size = rx_ampdu_size;
+#ifdef CONFIG_SCAN_BACKOP
+		if (num-- > 0)
+			ss->backop_ms = backop_ms;
+		if (num-- > 0)
+			ss->scan_cnt_max = scan_cnt_max;
+#endif
+	}
+
+	return count;
+}
+
+int proc_get_scan_abort(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	u32 pass_ms;
+
+	pass_ms = rtw_scan_abort_timeout(adapter, 10000);
+
+	RTW_PRINT_SEL(m, "%u\n", pass_ms);
+
+	return 0;
+}
+
+#ifdef CONFIG_SCAN_BACKOP
+int proc_get_backop_flags_sta(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+
+	RTW_PRINT_SEL(m, "0x%02x\n", mlmeext_scan_backop_flags_sta(mlmeext));
+
+	return 0;
+}
+
+ssize_t proc_set_backop_flags_sta(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+
+	char tmp[32];
+	u8 flags;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%hhx", &flags);
+
+		if (num == 1)
+			mlmeext_assign_scan_backop_flags_sta(mlmeext, flags);
+	}
+
+	return count;
+}
+
+int proc_get_backop_flags_ap(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+
+	RTW_PRINT_SEL(m, "0x%02x\n", mlmeext_scan_backop_flags_ap(mlmeext));
+
+	return 0;
+}
+
+ssize_t proc_set_backop_flags_ap(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+
+	char tmp[32];
+	u8 flags;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%hhx", &flags);
+
+		if (num == 1)
+			mlmeext_assign_scan_backop_flags_ap(mlmeext, flags);
+	}
+
+	return count;
+}
+
+#endif /* CONFIG_SCAN_BACKOP */
+
+int proc_get_survey_info(struct seq_file *m, void *v)
+{
+	unsigned long irqL;
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	_queue	*queue	= &(pmlmepriv->scanned_queue);
+	struct wlan_network	*pnetwork = NULL;
+	_list	*plist, *phead;
+	s32 notify_signal;
+	s16 notify_noise = 0;
+	u16  index = 0, ie_cap = 0;
+	unsigned char *ie_wpa = NULL, *ie_wpa2 = NULL, *ie_wps = NULL;
+	unsigned char *ie_p2p = NULL, *ssid = NULL;
+	char flag_str[64];
+	int ielen = 0;
+	u32 wpsielen = 0;
+
+	_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+	phead = get_list_head(queue);
+	if (!phead)
+		goto exit;
+	plist = get_next(phead);
+	if (!plist)
+		goto exit;
+
+	RTW_PRINT_SEL(m, "%5s  %-17s  %3s  %-3s  %-4s  %-4s  %5s  %32s  %32s\n", "index", "bssid", "ch", "RSSI", "SdBm", "Noise", "age", "flag", "ssid");
+	while (1) {
+		if (rtw_end_of_queue_search(phead, plist) == true)
+			break;
+
+		pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+		if (!pnetwork)
+			break;
+
+		if (check_fwstate(pmlmepriv, _FW_LINKED) == true &&
+		    is_same_network(&pmlmepriv->cur_network.network, &pnetwork->network, 0)) {
+			notify_signal = translate_percentage_to_dbm(padapter->recvpriv.signal_strength);/* dbm */
+		} else {
+			notify_signal = translate_percentage_to_dbm(pnetwork->network.PhyInfo.SignalStrength);/* dbm */
+		}
+
+#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+		rtw_hal_get_odm_var(padapter, HAL_ODM_NOISE_MONITOR, &(pnetwork->network.Configuration.DSConfig), &(notify_noise));
+#endif
+
+		ie_wpa = rtw_get_wpa_ie(&pnetwork->network.IEs[12], &ielen, pnetwork->network.IELength - 12);
+		ie_wpa2 = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &ielen, pnetwork->network.IELength - 12);
+		ie_cap = rtw_get_capability(&pnetwork->network);
+		ie_wps = rtw_get_wps_ie(&pnetwork->network.IEs[12], pnetwork->network.IELength - 12, NULL, &wpsielen);
+		ie_p2p = rtw_get_p2p_ie(&pnetwork->network.IEs[12], pnetwork->network.IELength - 12, NULL, &ielen);
+		ssid = pnetwork->network.Ssid.Ssid;
+		sprintf(flag_str, "%s%s%s%s%s%s%s",
+			(ie_wpa) ? "[WPA]" : "",
+			(ie_wpa2) ? "[WPA2]" : "",
+			(!ie_wpa && !ie_wpa && ie_cap & BIT(4)) ? "[WEP]" : "",
+			(ie_wps) ? "[WPS]" : "",
+			(pnetwork->network.InfrastructureMode == Ndis802_11IBSS) ? "[IBSS]" : "",
+			(ie_cap & BIT(0)) ? "[ESS]" : "",
+			(ie_p2p) ? "[P2P]" : "");
+		RTW_PRINT_SEL(m, "%5d  "MAC_FMT"  %3d  %3d  %4d  %4d    %5d  %32s  %32s\n",
+			      ++index,
+			      MAC_ARG(pnetwork->network.MacAddress),
+			      pnetwork->network.Configuration.DSConfig,
+			      (int)pnetwork->network.Rssi,
+			      notify_signal,
+			      notify_noise,
+			rtw_get_passing_time_ms((u32)pnetwork->last_scanned),
+			      flag_str,
+			      pnetwork->network.Ssid.Ssid);
+		plist = get_next(plist);
+	}
+exit:
+	_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+	return 0;
+}
+
+ssize_t proc_set_survey_info(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	unsigned long irqL;
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	bool need_indicate_scan_done = false;
+	u8 _status = false;
+	NDIS_802_11_SSID ssid[RTW_SSID_SCAN_AMOUNT];
+
+	if (count < 1)
+		return -EFAULT;
+
+#ifdef CONFIG_MP_INCLUDED
+	if (rtw_mi_mp_mode_check(padapter)) {
+		RTW_INFO("MP mode block Scan request\n");
+		goto exit;
+	}
+#endif
+	if (rtw_is_scan_deny(padapter)) {
+		RTW_INFO(FUNC_ADPT_FMT  ": scan deny\n", FUNC_ADPT_ARG(padapter));
+		goto exit;
+	}
+
+	rtw_ps_deny(padapter, PS_DENY_SCAN);
+	if (_FAIL == rtw_pwr_wakeup(padapter))
+		goto cancel_ps_deny;
+
+	if (!rtw_is_adapter_up(padapter)) {
+		RTW_INFO("scan abort!! adapter cannot use\n");
+		goto cancel_ps_deny;
+	}
+
+	if (rtw_mi_busy_traffic_check(padapter, false)) {
+		RTW_INFO("scan abort!! BusyTraffic == true\n");
+		goto cancel_ps_deny;
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) && check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+		RTW_INFO("scan abort!! AP mode process WPS\n");
+		goto cancel_ps_deny;
+	}
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING) == true) {
+		RTW_INFO("scan abort!! fwstate=0x%x\n", pmlmepriv->fw_state);
+		goto cancel_ps_deny;
+	}
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (rtw_mi_buddy_check_fwstate(padapter,
+		       _FW_UNDER_SURVEY | _FW_UNDER_LINKING | WIFI_UNDER_WPS)) {
+		RTW_INFO("scan abort!! buddy_fwstate check failed\n");
+		goto cancel_ps_deny;
+	}
+#endif
+	_status = rtw_set_802_11_bssid_list_scan(padapter, NULL, 0, NULL, 0);
+
+cancel_ps_deny:
+	rtw_ps_deny_cancel(padapter, PS_DENY_SCAN);
+exit:
+	return count;
+}
+
+int proc_get_ap_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct sta_info *psta;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct wlan_network *cur_network = &(pmlmepriv->cur_network);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	psta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress);
+	if (psta) {
+		int i;
+		struct recv_reorder_ctrl *preorder_ctrl;
+
+		RTW_PRINT_SEL(m, "SSID=%s\n", cur_network->network.Ssid.Ssid);
+		RTW_PRINT_SEL(m, "sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->hwaddr));
+		RTW_PRINT_SEL(m, "cur_channel=%d, cur_bwmode=%d, cur_ch_offset=%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+		RTW_PRINT_SEL(m, "wireless_mode=0x%x, rtsen=%d, cts2slef=%d\n", psta->wireless_mode, psta->rtsen, psta->cts2self);
+		RTW_PRINT_SEL(m, "state=0x%x, aid=%d, macid=%d, raid=%d\n", psta->state, psta->aid, psta->mac_id, psta->raid);
+		RTW_PRINT_SEL(m, "qos_en=%d, ht_en=%d, init_rate=%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate);
+		RTW_PRINT_SEL(m, "bwmode=%d, ch_offset=%d, sgi_20m=%d,sgi_40m=%d\n", psta->bw_mode, psta->htpriv.ch_offset, psta->htpriv.sgi_20m, psta->htpriv.sgi_40m);
+		RTW_PRINT_SEL(m, "ampdu_enable = %d\n", psta->htpriv.ampdu_enable);
+		RTW_PRINT_SEL(m, "agg_enable_bitmap=%x, candidate_tid_bitmap=%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap);
+		RTW_PRINT_SEL(m, "ldpc_cap=0x%x, stbc_cap=0x%x, beamform_cap=0x%x\n", psta->htpriv.ldpc_cap, psta->htpriv.stbc_cap, psta->htpriv.beamform_cap);
+		sta_rx_reorder_ctl_dump(m, psta);
+	} else
+		RTW_PRINT_SEL(m, "can't get sta's macaddr, cur_network's macaddr:" MAC_FMT "\n", MAC_ARG(cur_network->network.MacAddress));
+
+	return 0;
+}
+
+ssize_t proc_reset_trx_info(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	char cmd[32] = {0};
+	u8 cnt = 0;
+
+	if (count > sizeof(cmd)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(cmd, buffer, count)) {
+		int num = sscanf(cmd, "%hhx", &cnt);
+
+		if (0 == cnt) {
+			pdbgpriv->dbg_rx_ampdu_drop_count = 0;
+			pdbgpriv->dbg_rx_ampdu_forced_indicate_count = 0;
+			pdbgpriv->dbg_rx_ampdu_loss_count = 0;
+			pdbgpriv->dbg_rx_dup_mgt_frame_drop_count = 0;
+			pdbgpriv->dbg_rx_ampdu_window_shift_cnt = 0;
+			pdbgpriv->dbg_rx_conflic_mac_addr_cnt = 0;
+		}
+	}
+
+	return count;
+}
+
+int proc_get_trx_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	int i;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct recv_priv  *precvpriv = &padapter->recvpriv;
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	struct hw_xmit *phwxmit;
+
+	dump_os_queue(m, padapter);
+
+	RTW_PRINT_SEL(m, "free_xmitbuf_cnt=%d, free_xmitframe_cnt=%d\n"
+		, pxmitpriv->free_xmitbuf_cnt, pxmitpriv->free_xmitframe_cnt);
+	RTW_PRINT_SEL(m, "free_ext_xmitbuf_cnt=%d, free_xframe_ext_cnt=%d\n"
+		, pxmitpriv->free_xmit_extbuf_cnt, pxmitpriv->free_xframe_ext_cnt);
+	RTW_PRINT_SEL(m, "free_recvframe_cnt=%d\n"
+		      , precvpriv->free_recvframe_cnt);
+
+	for (i = 0; i < 4; i++) {
+		phwxmit = pxmitpriv->hwxmits + i;
+		RTW_PRINT_SEL(m, "%d, hwq.accnt=%d\n", i, phwxmit->accnt);
+	}
+
+	RTW_PRINT_SEL(m, "rx_urb_pending_cn=%d\n", ATOMIC_READ(&(precvpriv->rx_pending_cnt)));
+
+	/* Folowing are RX info */
+	/* Counts of packets whose seq_num is less than preorder_ctrl->indicate_seq, Ex delay, retransmission, redundant packets and so on */
+	RTW_PRINT_SEL(m, "Rx: Counts of Packets Whose Seq_Num Less Than Reorder Control Seq_Num: %llu\n", (unsigned long long)pdbgpriv->dbg_rx_ampdu_drop_count);
+	/* How many times the Rx Reorder Timer is triggered. */
+	RTW_PRINT_SEL(m, "Rx: Reorder Time-out Trigger Counts: %llu\n", (unsigned long long)pdbgpriv->dbg_rx_ampdu_forced_indicate_count);
+	/* Total counts of packets loss */
+	RTW_PRINT_SEL(m, "Rx: Packet Loss Counts: %llu\n", (unsigned long long)pdbgpriv->dbg_rx_ampdu_loss_count);
+	RTW_PRINT_SEL(m, "Rx: Duplicate Management Frame Drop Count: %llu\n", (unsigned long long)pdbgpriv->dbg_rx_dup_mgt_frame_drop_count);
+	RTW_PRINT_SEL(m, "Rx: AMPDU BA window shift Count: %llu\n", (unsigned long long)pdbgpriv->dbg_rx_ampdu_window_shift_cnt);
+	/*The same mac addr counts*/
+	RTW_PRINT_SEL(m, "Rx: Conflict MAC Address Frames Count: %llu\n", (unsigned long long)pdbgpriv->dbg_rx_conflic_mac_addr_cnt);
+	return 0;
+}
+
+int proc_get_dis_pwt(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	u8 dis_pwt = 0;
+	rtw_hal_get_def_var(padapter, HAL_DEF_DBG_DIS_PWT, &(dis_pwt));
+	RTW_PRINT_SEL(m, " Tx Power training mode:%s\n", (dis_pwt == true) ? "Disable" : "Enable");
+	return 0;
+}
+ssize_t proc_set_dis_pwt(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[4] = {0};
+	u8 dis_pwt = 0;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%hhx", &dis_pwt);
+		RTW_INFO("Set Tx Power training mode:%s\n", (dis_pwt == true) ? "Disable" : "Enable");
+
+		if (num >= 1)
+			rtw_hal_set_def_var(padapter, HAL_DEF_DBG_DIS_PWT, &(dis_pwt));
+	}
+
+	return count;
+
+}
+
+int proc_get_rate_ctl(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	int i;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	u8 data_rate = 0, sgi = 0, data_fb = 0;
+
+	if (adapter->fix_rate != 0xff) {
+		data_rate = adapter->fix_rate & 0x7F;
+		sgi = adapter->fix_rate >> 7;
+		data_fb = adapter->data_fb ? 1 : 0;
+		RTW_PRINT_SEL(m, "FIXED %s%s%s\n"
+			, HDATA_RATE(data_rate)
+			, data_rate > DESC_RATE54M ? (sgi ? " SGI" : " LGI") : ""
+			, data_fb ? " FB" : ""
+		);
+		RTW_PRINT_SEL(m, "0x%02x %u\n", adapter->fix_rate, adapter->data_fb);
+	} else
+		RTW_PRINT_SEL(m, "RA\n");
+
+	return 0;
+}
+
+ssize_t proc_set_rate_ctl(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u8 fix_rate;
+	u8 data_fb;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%hhx %hhu", &fix_rate, &data_fb);
+
+		if (num >= 1) {
+			u8 fix_rate_ori = adapter->fix_rate;
+
+			adapter->fix_rate = fix_rate;
+			if (adapter->fix_bw != 0xFF && fix_rate_ori != fix_rate)
+				rtw_update_tx_rate_bmp(adapter_to_dvobj(adapter));
+		}
+		if (num >= 2)
+			adapter->data_fb = data_fb ? 1 : 0;
+	}
+
+	return count;
+}
+
+int proc_get_tx_power_offset(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	int i;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+
+	RTW_PRINT_SEL(m, "Tx power offset - %u\n", adapter->power_offset);
+	return 0;
+}
+
+ssize_t proc_set_tx_power_offset(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u8 power_offset = 0;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%hhu", &power_offset);
+
+		if (num >= 1) {
+			if (power_offset > 5)
+				power_offset = 0;
+
+			adapter->power_offset = power_offset;
+		}
+	}
+
+	return count;
+}
+
+int proc_get_bw_ctl(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	u8 data_bw = 0;
+
+	if (adapter->fix_bw != 0xff) {
+		data_bw = adapter->fix_bw;
+		RTW_PRINT_SEL(m, "FIXED %s\n", ch_width_str(data_bw));
+	} else
+		RTW_PRINT_SEL(m, "Auto\n");
+
+	return 0;
+}
+
+ssize_t proc_set_bw_ctl(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u8 fix_bw;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int num = sscanf(tmp, "%hhu", &fix_bw);
+
+		if (num >= 1) {
+			u8 fix_bw_ori = adapter->fix_bw;
+
+			adapter->fix_bw = fix_bw;
+
+			if (adapter->fix_rate != 0xFF && fix_bw_ori != fix_bw)
+				rtw_update_tx_rate_bmp(adapter_to_dvobj(adapter));
+		}
+	}
+
+	return count;
+}
+
+#ifdef DBG_RX_COUNTER_DUMP
+int proc_get_rx_cnt_dump(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	int i;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+
+	RTW_PRINT_SEL(m, "BIT0- Dump RX counters of DRV\n");
+	RTW_PRINT_SEL(m, "BIT1- Dump RX counters of MAC\n");
+	RTW_PRINT_SEL(m, "BIT2- Dump RX counters of PHY\n");
+	RTW_PRINT_SEL(m, "BIT3- Dump TRX data frame of DRV\n");
+	RTW_PRINT_SEL(m, "dump_rx_cnt_mode = 0x%02x\n", adapter->dump_rx_cnt_mode);
+
+	return 0;
+}
+ssize_t proc_set_rx_cnt_dump(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u8 dump_rx_cnt_mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%hhx", &dump_rx_cnt_mode);
+
+		rtw_dump_phy_rxcnts_preprocess(adapter, dump_rx_cnt_mode);
+		adapter->dump_rx_cnt_mode = dump_rx_cnt_mode;
+
+	}
+
+	return count;
+}
+#endif
+ssize_t proc_set_fwdl_test_case(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	int num;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count))
+		num = sscanf(tmp, "%hhu %hhu", &fwdl_test_chksum_fail, &fwdl_test_wintint_rdy_fail);
+
+	return count;
+}
+
+ssize_t proc_set_del_rx_ampdu_test_case(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	int num;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count))
+		num = sscanf(tmp, "%hhu", &del_rx_ampdu_test_no_tx_fail);
+
+	return count;
+}
+
+#ifdef CONFIG_DFS_MASTER
+int proc_get_dfs_master_test_case(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter);
+
+	RTW_PRINT_SEL(m, "%-24s %-19s\n", "radar_detect_trigger_non", "choose_dfs_ch_first");
+	RTW_PRINT_SEL(m, "%24hhu %19hhu\n"
+		, rfctl->dbg_dfs_master_radar_detect_trigger_non
+		, rfctl->dbg_dfs_master_choose_dfs_ch_first
+	);
+
+	return 0;
+}
+
+ssize_t proc_set_dfs_master_test_case(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+	struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter);
+	char tmp[32];
+	u8 radar_detect_trigger_non;
+	u8 choose_dfs_ch_first;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int num = sscanf(tmp, "%hhu %hhu", &radar_detect_trigger_non, &choose_dfs_ch_first);
+
+		if (num >= 1)
+			rfctl->dbg_dfs_master_radar_detect_trigger_non = radar_detect_trigger_non;
+		if (num >= 2)
+			rfctl->dbg_dfs_master_choose_dfs_ch_first = choose_dfs_ch_first;
+	}
+
+	return count;
+}
+#endif /* CONFIG_DFS_MASTER */
+
+ssize_t proc_set_wait_hiq_empty(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	int num;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count))
+		num = sscanf(tmp, "%u", &g_wait_hiq_empty_ms);
+
+	return count;
+}
+
+int proc_get_suspend_resume_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct dvobj_priv *dvobj = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &dvobj->drv_dbg;
+
+	RTW_PRINT_SEL(m, "dbg_sdio_alloc_irq_cnt=%d\n", pdbgpriv->dbg_sdio_alloc_irq_cnt);
+	RTW_PRINT_SEL(m, "dbg_sdio_free_irq_cnt=%d\n", pdbgpriv->dbg_sdio_free_irq_cnt);
+	RTW_PRINT_SEL(m, "dbg_sdio_alloc_irq_error_cnt=%d\n", pdbgpriv->dbg_sdio_alloc_irq_error_cnt);
+	RTW_PRINT_SEL(m, "dbg_sdio_free_irq_error_cnt=%d\n", pdbgpriv->dbg_sdio_free_irq_error_cnt);
+	RTW_PRINT_SEL(m, "dbg_sdio_init_error_cnt=%d\n", pdbgpriv->dbg_sdio_init_error_cnt);
+	RTW_PRINT_SEL(m, "dbg_sdio_deinit_error_cnt=%d\n", pdbgpriv->dbg_sdio_deinit_error_cnt);
+	RTW_PRINT_SEL(m, "dbg_suspend_error_cnt=%d\n", pdbgpriv->dbg_suspend_error_cnt);
+	RTW_PRINT_SEL(m, "dbg_suspend_cnt=%d\n", pdbgpriv->dbg_suspend_cnt);
+	RTW_PRINT_SEL(m, "dbg_resume_cnt=%d\n", pdbgpriv->dbg_resume_cnt);
+	RTW_PRINT_SEL(m, "dbg_resume_error_cnt=%d\n", pdbgpriv->dbg_resume_error_cnt);
+	RTW_PRINT_SEL(m, "dbg_deinit_fail_cnt=%d\n", pdbgpriv->dbg_deinit_fail_cnt);
+	RTW_PRINT_SEL(m, "dbg_carddisable_cnt=%d\n", pdbgpriv->dbg_carddisable_cnt);
+	RTW_PRINT_SEL(m, "dbg_ps_insuspend_cnt=%d\n", pdbgpriv->dbg_ps_insuspend_cnt);
+	RTW_PRINT_SEL(m, "dbg_dev_unload_inIPS_cnt=%d\n", pdbgpriv->dbg_dev_unload_inIPS_cnt);
+	RTW_PRINT_SEL(m, "dbg_scan_pwr_state_cnt=%d\n", pdbgpriv->dbg_scan_pwr_state_cnt);
+	RTW_PRINT_SEL(m, "dbg_downloadfw_pwr_state_cnt=%d\n", pdbgpriv->dbg_downloadfw_pwr_state_cnt);
+	RTW_PRINT_SEL(m, "dbg_carddisable_error_cnt=%d\n", pdbgpriv->dbg_carddisable_error_cnt);
+	RTW_PRINT_SEL(m, "dbg_fw_read_ps_state_fail_cnt=%d\n", pdbgpriv->dbg_fw_read_ps_state_fail_cnt);
+	RTW_PRINT_SEL(m, "dbg_leave_ips_fail_cnt=%d\n", pdbgpriv->dbg_leave_ips_fail_cnt);
+	RTW_PRINT_SEL(m, "dbg_leave_lps_fail_cnt=%d\n", pdbgpriv->dbg_leave_lps_fail_cnt);
+	RTW_PRINT_SEL(m, "dbg_h2c_leave32k_fail_cnt=%d\n", pdbgpriv->dbg_h2c_leave32k_fail_cnt);
+	RTW_PRINT_SEL(m, "dbg_diswow_dload_fw_fail_cnt=%d\n", pdbgpriv->dbg_diswow_dload_fw_fail_cnt);
+	RTW_PRINT_SEL(m, "dbg_enwow_dload_fw_fail_cnt=%d\n", pdbgpriv->dbg_enwow_dload_fw_fail_cnt);
+	RTW_PRINT_SEL(m, "dbg_ips_drvopen_fail_cnt=%d\n", pdbgpriv->dbg_ips_drvopen_fail_cnt);
+	RTW_PRINT_SEL(m, "dbg_poll_fail_cnt=%d\n", pdbgpriv->dbg_poll_fail_cnt);
+	RTW_PRINT_SEL(m, "dbg_rpwm_toogle_cnt=%d\n", pdbgpriv->dbg_rpwm_toogle_cnt);
+	RTW_PRINT_SEL(m, "dbg_rpwm_timeout_fail_cnt=%d\n", pdbgpriv->dbg_rpwm_timeout_fail_cnt);
+	RTW_PRINT_SEL(m, "dbg_sreset_cnt=%d\n", pdbgpriv->dbg_sreset_cnt);
+
+	return 0;
+}
+
+#ifdef CONFIG_DBG_COUNTER
+
+int proc_get_rx_logs(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct rx_logs *rx_logs = &padapter->rx_logs;
+
+	RTW_PRINT_SEL(m,
+		      "intf_rx=%d\n"
+		      "intf_rx_err_recvframe=%d\n"
+		      "intf_rx_err_skb=%d\n"
+		      "intf_rx_report=%d\n"
+		      "core_rx=%d\n"
+		      "core_rx_pre=%d\n"
+		      "core_rx_pre_ver_err=%d\n"
+		      "core_rx_pre_mgmt=%d\n"
+		      "core_rx_pre_mgmt_err_80211w=%d\n"
+		      "core_rx_pre_mgmt_err=%d\n"
+		      "core_rx_pre_ctrl=%d\n"
+		      "core_rx_pre_ctrl_err=%d\n"
+		      "core_rx_pre_data=%d\n"
+		      "core_rx_pre_data_wapi_seq_err=%d\n"
+		      "core_rx_pre_data_wapi_key_err=%d\n"
+		      "core_rx_pre_data_handled=%d\n"
+		      "core_rx_pre_data_err=%d\n"
+		      "core_rx_pre_data_unknown=%d\n"
+		      "core_rx_pre_unknown=%d\n"
+		      "core_rx_enqueue=%d\n"
+		      "core_rx_dequeue=%d\n"
+		      "core_rx_post=%d\n"
+		      "core_rx_post_decrypt=%d\n"
+		      "core_rx_post_decrypt_wep=%d\n"
+		      "core_rx_post_decrypt_tkip=%d\n"
+		      "core_rx_post_decrypt_aes=%d\n"
+		      "core_rx_post_decrypt_wapi=%d\n"
+		      "core_rx_post_decrypt_hw=%d\n"
+		      "core_rx_post_decrypt_unknown=%d\n"
+		      "core_rx_post_decrypt_err=%d\n"
+		      "core_rx_post_defrag_err=%d\n"
+		      "core_rx_post_portctrl_err=%d\n"
+		      "core_rx_post_indicate=%d\n"
+		      "core_rx_post_indicate_in_oder=%d\n"
+		      "core_rx_post_indicate_reoder=%d\n"
+		      "core_rx_post_indicate_err=%d\n"
+		      "os_indicate=%d\n"
+		      "os_indicate_ap_mcast=%d\n"
+		      "os_indicate_ap_forward=%d\n"
+		      "os_indicate_ap_self=%d\n"
+		      "os_indicate_err=%d\n"
+		      "os_netif_ok=%d\n"
+		      "os_netif_err=%d\n",
+		      rx_logs->intf_rx,
+		      rx_logs->intf_rx_err_recvframe,
+		      rx_logs->intf_rx_err_skb,
+		      rx_logs->intf_rx_report,
+		      rx_logs->core_rx,
+		      rx_logs->core_rx_pre,
+		      rx_logs->core_rx_pre_ver_err,
+		      rx_logs->core_rx_pre_mgmt,
+		      rx_logs->core_rx_pre_mgmt_err_80211w,
+		      rx_logs->core_rx_pre_mgmt_err,
+		      rx_logs->core_rx_pre_ctrl,
+		      rx_logs->core_rx_pre_ctrl_err,
+		      rx_logs->core_rx_pre_data,
+		      rx_logs->core_rx_pre_data_wapi_seq_err,
+		      rx_logs->core_rx_pre_data_wapi_key_err,
+		      rx_logs->core_rx_pre_data_handled,
+		      rx_logs->core_rx_pre_data_err,
+		      rx_logs->core_rx_pre_data_unknown,
+		      rx_logs->core_rx_pre_unknown,
+		      rx_logs->core_rx_enqueue,
+		      rx_logs->core_rx_dequeue,
+		      rx_logs->core_rx_post,
+		      rx_logs->core_rx_post_decrypt,
+		      rx_logs->core_rx_post_decrypt_wep,
+		      rx_logs->core_rx_post_decrypt_tkip,
+		      rx_logs->core_rx_post_decrypt_aes,
+		      rx_logs->core_rx_post_decrypt_wapi,
+		      rx_logs->core_rx_post_decrypt_hw,
+		      rx_logs->core_rx_post_decrypt_unknown,
+		      rx_logs->core_rx_post_decrypt_err,
+		      rx_logs->core_rx_post_defrag_err,
+		      rx_logs->core_rx_post_portctrl_err,
+		      rx_logs->core_rx_post_indicate,
+		      rx_logs->core_rx_post_indicate_in_oder,
+		      rx_logs->core_rx_post_indicate_reoder,
+		      rx_logs->core_rx_post_indicate_err,
+		      rx_logs->os_indicate,
+		      rx_logs->os_indicate_ap_mcast,
+		      rx_logs->os_indicate_ap_forward,
+		      rx_logs->os_indicate_ap_self,
+		      rx_logs->os_indicate_err,
+		      rx_logs->os_netif_ok,
+		      rx_logs->os_netif_err
+		     );
+
+	return 0;
+}
+
+int proc_get_tx_logs(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct tx_logs *tx_logs = &padapter->tx_logs;
+
+	RTW_PRINT_SEL(m,
+		      "os_tx=%d\n"
+		      "os_tx_err_up=%d\n"
+		      "os_tx_err_xmit=%d\n"
+		      "os_tx_m2u=%d\n"
+		      "os_tx_m2u_ignore_fw_linked=%d\n"
+		      "os_tx_m2u_ignore_self=%d\n"
+		      "os_tx_m2u_entry=%d\n"
+		      "os_tx_m2u_entry_err_xmit=%d\n"
+		      "os_tx_m2u_entry_err_skb=%d\n"
+		      "os_tx_m2u_stop=%d\n"
+		      "core_tx=%d\n"
+		      "core_tx_err_pxmitframe=%d\n"
+		      "core_tx_err_brtx=%d\n"
+		      "core_tx_upd_attrib=%d\n"
+		      "core_tx_upd_attrib_adhoc=%d\n"
+		      "core_tx_upd_attrib_sta=%d\n"
+		      "core_tx_upd_attrib_ap=%d\n"
+		      "core_tx_upd_attrib_unknown=%d\n"
+		      "core_tx_upd_attrib_dhcp=%d\n"
+		      "core_tx_upd_attrib_icmp=%d\n"
+		      "core_tx_upd_attrib_active=%d\n"
+		      "core_tx_upd_attrib_err_ucast_sta=%d\n"
+		      "core_tx_upd_attrib_err_ucast_ap_link=%d\n"
+		      "core_tx_upd_attrib_err_sta=%d\n"
+		      "core_tx_upd_attrib_err_link=%d\n"
+		      "core_tx_upd_attrib_err_sec=%d\n"
+		      "core_tx_ap_enqueue_warn_fwstate=%d\n"
+		      "core_tx_ap_enqueue_warn_sta=%d\n"
+		      "core_tx_ap_enqueue_warn_nosta=%d\n"
+		      "core_tx_ap_enqueue_warn_link=%d\n"
+		      "core_tx_ap_enqueue_warn_trigger=%d\n"
+		      "core_tx_ap_enqueue_mcast=%d\n"
+		      "core_tx_ap_enqueue_ucast=%d\n"
+		      "core_tx_ap_enqueue=%d\n"
+		      "intf_tx=%d\n"
+		      "intf_tx_pending_ac=%d\n"
+		      "intf_tx_pending_fw_under_survey=%d\n"
+		      "intf_tx_pending_fw_under_linking=%d\n"
+		      "intf_tx_pending_xmitbuf=%d\n"
+		      "intf_tx_enqueue=%d\n"
+		      "core_tx_enqueue=%d\n"
+		      "core_tx_enqueue_class=%d\n"
+		      "core_tx_enqueue_class_err_sta=%d\n"
+		      "core_tx_enqueue_class_err_nosta=%d\n"
+		      "core_tx_enqueue_class_err_fwlink=%d\n"
+		      "intf_tx_direct=%d\n"
+		      "intf_tx_direct_err_coalesce=%d\n"
+		      "intf_tx_dequeue=%d\n"
+		      "intf_tx_dequeue_err_coalesce=%d\n"
+		      "intf_tx_dump_xframe=%d\n"
+		      "intf_tx_dump_xframe_err_txdesc=%d\n"
+		      "intf_tx_dump_xframe_err_port=%d\n",
+		      tx_logs->os_tx,
+		      tx_logs->os_tx_err_up,
+		      tx_logs->os_tx_err_xmit,
+		      tx_logs->os_tx_m2u,
+		      tx_logs->os_tx_m2u_ignore_fw_linked,
+		      tx_logs->os_tx_m2u_ignore_self,
+		      tx_logs->os_tx_m2u_entry,
+		      tx_logs->os_tx_m2u_entry_err_xmit,
+		      tx_logs->os_tx_m2u_entry_err_skb,
+		      tx_logs->os_tx_m2u_stop,
+		      tx_logs->core_tx,
+		      tx_logs->core_tx_err_pxmitframe,
+		      tx_logs->core_tx_err_brtx,
+		      tx_logs->core_tx_upd_attrib,
+		      tx_logs->core_tx_upd_attrib_adhoc,
+		      tx_logs->core_tx_upd_attrib_sta,
+		      tx_logs->core_tx_upd_attrib_ap,
+		      tx_logs->core_tx_upd_attrib_unknown,
+		      tx_logs->core_tx_upd_attrib_dhcp,
+		      tx_logs->core_tx_upd_attrib_icmp,
+		      tx_logs->core_tx_upd_attrib_active,
+		      tx_logs->core_tx_upd_attrib_err_ucast_sta,
+		      tx_logs->core_tx_upd_attrib_err_ucast_ap_link,
+		      tx_logs->core_tx_upd_attrib_err_sta,
+		      tx_logs->core_tx_upd_attrib_err_link,
+		      tx_logs->core_tx_upd_attrib_err_sec,
+		      tx_logs->core_tx_ap_enqueue_warn_fwstate,
+		      tx_logs->core_tx_ap_enqueue_warn_sta,
+		      tx_logs->core_tx_ap_enqueue_warn_nosta,
+		      tx_logs->core_tx_ap_enqueue_warn_link,
+		      tx_logs->core_tx_ap_enqueue_warn_trigger,
+		      tx_logs->core_tx_ap_enqueue_mcast,
+		      tx_logs->core_tx_ap_enqueue_ucast,
+		      tx_logs->core_tx_ap_enqueue,
+		      tx_logs->intf_tx,
+		      tx_logs->intf_tx_pending_ac,
+		      tx_logs->intf_tx_pending_fw_under_survey,
+		      tx_logs->intf_tx_pending_fw_under_linking,
+		      tx_logs->intf_tx_pending_xmitbuf,
+		      tx_logs->intf_tx_enqueue,
+		      tx_logs->core_tx_enqueue,
+		      tx_logs->core_tx_enqueue_class,
+		      tx_logs->core_tx_enqueue_class_err_sta,
+		      tx_logs->core_tx_enqueue_class_err_nosta,
+		      tx_logs->core_tx_enqueue_class_err_fwlink,
+		      tx_logs->intf_tx_direct,
+		      tx_logs->intf_tx_direct_err_coalesce,
+		      tx_logs->intf_tx_dequeue,
+		      tx_logs->intf_tx_dequeue_err_coalesce,
+		      tx_logs->intf_tx_dump_xframe,
+		      tx_logs->intf_tx_dump_xframe_err_txdesc,
+		      tx_logs->intf_tx_dump_xframe_err_port
+		     );
+
+	return 0;
+}
+
+int proc_get_int_logs(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+
+	RTW_PRINT_SEL(m,
+		      "all=%d\n"
+		      "err=%d\n"
+		      "tbdok=%d\n"
+		      "tbder=%d\n"
+		      "bcnderr=%d\n"
+		      "bcndma=%d\n"
+		      "bcndma_e=%d\n"
+		      "rx=%d\n"
+		      "rx_rdu=%d\n"
+		      "rx_fovw=%d\n"
+		      "txfovw=%d\n"
+		      "mgntok=%d\n"
+		      "highdok=%d\n"
+		      "bkdok=%d\n"
+		      "bedok=%d\n"
+		      "vidok=%d\n"
+		      "vodok=%d\n",
+		      padapter->int_logs.all,
+		      padapter->int_logs.err,
+		      padapter->int_logs.tbdok,
+		      padapter->int_logs.tbder,
+		      padapter->int_logs.bcnderr,
+		      padapter->int_logs.bcndma,
+		      padapter->int_logs.bcndma_e,
+		      padapter->int_logs.rx,
+		      padapter->int_logs.rx_rdu,
+		      padapter->int_logs.rx_fovw,
+		      padapter->int_logs.txfovw,
+		      padapter->int_logs.mgntok,
+		      padapter->int_logs.highdok,
+		      padapter->int_logs.bkdok,
+		      padapter->int_logs.bedok,
+		      padapter->int_logs.vidok,
+		      padapter->int_logs.vodok
+		     );
+
+	return 0;
+}
+
+#endif /* CONFIG_DBG_COUNTER */
+
+int proc_get_hw_status(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct dvobj_priv *dvobj = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &dvobj->drv_dbg;
+	struct registry_priv *regsty = dvobj_to_regsty(dvobj);
+
+	if (regsty->check_hw_status == 0)
+		RTW_PRINT_SEL(m, "RX FIFO full count: not check in watch dog\n");
+	else if (pdbgpriv->dbg_rx_fifo_last_overflow == 1
+	    && pdbgpriv->dbg_rx_fifo_curr_overflow == 1
+	    && pdbgpriv->dbg_rx_fifo_diff_overflow == 1
+	   )
+		RTW_PRINT_SEL(m, "RX FIFO full count: no implementation\n");
+	else {
+		RTW_PRINT_SEL(m, "RX FIFO full count: last_time=%llu, current_time=%llu, differential=%llu\n"
+			, pdbgpriv->dbg_rx_fifo_last_overflow, pdbgpriv->dbg_rx_fifo_curr_overflow, pdbgpriv->dbg_rx_fifo_diff_overflow);
+	}
+
+	return 0;
+}
+
+ssize_t proc_set_hw_status(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct dvobj_priv *dvobj = padapter->dvobj;
+	struct registry_priv *regsty = dvobj_to_regsty(dvobj);
+	char tmp[32];
+	u32 enable;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &enable);
+
+		if (regsty && enable <= 1) {
+			regsty->check_hw_status = enable;
+			RTW_INFO("check_hw_status=%d\n", regsty->check_hw_status);
+		}
+	}
+
+	return count;
+}
+
+int proc_get_trx_info_debug(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+
+	int i;
+
+
+	/*============  tx info ============	*/
+	rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, m);
+
+	/*============  rx info ============	*/
+	rtw_hal_set_odm_var(padapter, HAL_ODM_RX_INFO_DUMP, m, false);
+
+
+	return 0;
+}
+
+int proc_get_rx_signal(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	RTW_PRINT_SEL(m, "rssi:%d\n", padapter->recvpriv.rssi);
+	/* RTW_PRINT_SEL(m, "rxpwdb:%d\n", padapter->recvpriv.rxpwdb); */
+	RTW_PRINT_SEL(m, "signal_strength:%u\n", padapter->recvpriv.signal_strength);
+	RTW_PRINT_SEL(m, "signal_qual:%u\n", padapter->recvpriv.signal_qual);
+	if (padapter->registrypriv.mp_mode == 1) {
+		if (padapter->mppriv.antenna_rx == ANTENNA_A)
+			RTW_PRINT_SEL(m, "Antenna: A\n");
+		else if (padapter->mppriv.antenna_rx == ANTENNA_B)
+			RTW_PRINT_SEL(m, "Antenna: B\n");
+		else if (padapter->mppriv.antenna_rx == ANTENNA_C)
+			RTW_PRINT_SEL(m, "Antenna: C\n");
+		else if (padapter->mppriv.antenna_rx == ANTENNA_D)
+			RTW_PRINT_SEL(m, "Antenna: D\n");
+		else if (padapter->mppriv.antenna_rx == ANTENNA_AB)
+			RTW_PRINT_SEL(m, "Antenna: AB\n");
+		else if (padapter->mppriv.antenna_rx == ANTENNA_BC)
+			RTW_PRINT_SEL(m, "Antenna: BC\n");
+		else if (padapter->mppriv.antenna_rx == ANTENNA_CD)
+			RTW_PRINT_SEL(m, "Antenna: CD\n");
+		else
+			RTW_PRINT_SEL(m, "Antenna: __\n");
+		return 0;
+	}
+
+	rtw_get_noise(padapter);
+	RTW_PRINT_SEL(m, "noise:%d\n", padapter->recvpriv.noise);
+#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+	rtw_odm_get_perpkt_rssi(m, padapter);
+	rtw_get_raw_rssi_info(m, padapter);
+#endif
+	return 0;
+}
+
+ssize_t proc_set_rx_signal(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 is_signal_dbg, signal_strength;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%u %u", &is_signal_dbg, &signal_strength);
+
+		is_signal_dbg = is_signal_dbg == 0 ? 0 : 1;
+
+		if (is_signal_dbg && num != 2)
+			return count;
+
+		signal_strength = signal_strength > 100 ? 100 : signal_strength;
+
+		padapter->recvpriv.is_signal_dbg = is_signal_dbg;
+		padapter->recvpriv.signal_strength_dbg = signal_strength;
+
+		if (is_signal_dbg)
+			RTW_INFO("set %s %u\n", "DBG_SIGNAL_STRENGTH", signal_strength);
+		else
+			RTW_INFO("set %s\n", "HW_SIGNAL_STRENGTH");
+
+	}
+
+	return count;
+
+}
+
+int proc_get_ht_enable(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		RTW_PRINT_SEL(m, "%d\n", pregpriv->ht_enable);
+
+	return 0;
+}
+
+ssize_t proc_set_ht_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && mode < 2) {
+			pregpriv->ht_enable = mode;
+			RTW_INFO("ht_enable=%d\n", pregpriv->ht_enable);
+		}
+	}
+
+	return count;
+
+}
+
+int proc_get_bw_mode(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		RTW_PRINT_SEL(m, "0x%02x\n", pregpriv->bw_mode);
+
+	return 0;
+}
+
+ssize_t proc_set_bw_mode(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+	u8 bw_2g;
+	u8 bw_5g;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%x ", &mode);
+		bw_5g = mode >> 4;
+		bw_2g = mode & 0x0f;
+
+		if (pregpriv && bw_2g <= 4 && bw_5g <= 4) {
+
+			pregpriv->bw_mode = mode;
+			RTW_INFO("bw_mode=0x%x\n", mode);
+
+		}
+	}
+
+	return count;
+
+}
+
+int proc_get_ampdu_enable(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		RTW_PRINT_SEL(m, "%d\n", pregpriv->ampdu_enable);
+
+	return 0;
+}
+
+ssize_t proc_set_ampdu_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && mode < 2) {
+			pregpriv->ampdu_enable = mode;
+			RTW_INFO("ampdu_enable=%d\n", mode);
+		}
+
+	}
+
+	return count;
+
+}
+
+int proc_get_mac_rptbuf(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	u16 i;
+	u16 mac_id;
+	u32 shcut_addr = 0;
+	u32 read_addr = 0;
+	return 0;
+}
+
+int proc_get_rx_ampdu(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+
+	_RTW_PRINT_SEL(m, "accept: ");
+	if (padapter->fix_rx_ampdu_accept == RX_AMPDU_ACCEPT_INVALID)
+		RTW_PRINT_SEL(m, "%u%s\n", rtw_rx_ampdu_is_accept(padapter), "(auto)");
+	else
+		RTW_PRINT_SEL(m, "%u%s\n", padapter->fix_rx_ampdu_accept, "(fixed)");
+
+	_RTW_PRINT_SEL(m, "size: ");
+	if (padapter->fix_rx_ampdu_size == RX_AMPDU_SIZE_INVALID)
+		RTW_PRINT_SEL(m, "%u%s\n", rtw_rx_ampdu_size(padapter), "(auto)");
+	else
+		RTW_PRINT_SEL(m, "%u%s\n", padapter->fix_rx_ampdu_size, "(fixed)");
+
+	RTW_PRINT_SEL(m, "%19s %17s\n", "fix_rx_ampdu_accept", "fix_rx_ampdu_size");
+
+	_RTW_PRINT_SEL(m, "%-19d %-17u\n"
+		, padapter->fix_rx_ampdu_accept
+		, padapter->fix_rx_ampdu_size);
+
+	return 0;
+}
+
+ssize_t proc_set_rx_ampdu(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	char tmp[32];
+	u8 accept;
+	u8 size;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%hhu %hhu", &accept, &size);
+
+		if (num >= 1)
+			rtw_rx_ampdu_set_accept(padapter, accept, RX_AMPDU_DRV_FIXED);
+		if (num >= 2)
+			rtw_rx_ampdu_set_size(padapter, size, RX_AMPDU_DRV_FIXED);
+
+		rtw_rx_ampdu_apply(padapter);
+	}
+
+exit:
+	return count;
+}
+int proc_get_rx_ampdu_factor(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+
+
+	if (padapter)
+		RTW_PRINT_SEL(m, "rx ampdu factor = %x\n", padapter->driver_rx_ampdu_factor);
+
+	return 0;
+}
+
+ssize_t proc_set_rx_ampdu_factor(struct file *file, const char __user *buffer
+				 , size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 factor;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &factor);
+
+		if (padapter && (num == 1)) {
+			RTW_INFO("padapter->driver_rx_ampdu_factor = %x\n", factor);
+
+			if (factor  > 0x03)
+				padapter->driver_rx_ampdu_factor = 0xFF;
+			else
+				padapter->driver_rx_ampdu_factor = factor;
+		}
+	}
+
+	return count;
+}
+
+int proc_get_rx_ampdu_density(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+
+
+	if (padapter)
+		RTW_PRINT_SEL(m, "rx ampdu densityg = %x\n", padapter->driver_rx_ampdu_spacing);
+
+	return 0;
+}
+
+ssize_t proc_set_rx_ampdu_density(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 density;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &density);
+
+		if (padapter && (num == 1)) {
+			RTW_INFO("padapter->driver_rx_ampdu_spacing = %x\n", density);
+
+			if (density > 0x07)
+				padapter->driver_rx_ampdu_spacing = 0xFF;
+			else
+				padapter->driver_rx_ampdu_spacing = density;
+		}
+	}
+
+	return count;
+}
+
+int proc_get_tx_ampdu_density(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+
+
+	if (padapter)
+		RTW_PRINT_SEL(m, "tx ampdu density = %x\n", padapter->driver_ampdu_spacing);
+
+	return 0;
+}
+
+ssize_t proc_set_tx_ampdu_density(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 density;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &density);
+
+		if (padapter && (num == 1)) {
+			RTW_INFO("padapter->driver_ampdu_spacing = %x\n", density);
+
+			if (density > 0x07)
+				padapter->driver_ampdu_spacing = 0xFF;
+			else
+				padapter->driver_ampdu_spacing = density;
+		}
+	}
+
+	return count;
+}
+
+#ifdef CONFIG_TX_AMSDU
+int proc_get_tx_amsdu(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	if (padapter)
+	{
+		RTW_PRINT_SEL(m, "tx amsdu = %d\n", padapter->tx_amsdu);
+		RTW_PRINT_SEL(m, "amsdu set timer conut = %u\n", pxmitpriv->amsdu_debug_set_timer);
+		RTW_PRINT_SEL(m, "amsdu  time out count = %u\n", pxmitpriv->amsdu_debug_timeout);
+		RTW_PRINT_SEL(m, "amsdu coalesce one count = %u\n", pxmitpriv->amsdu_debug_coalesce_one);
+		RTW_PRINT_SEL(m, "amsdu coalesce two count = %u\n", pxmitpriv->amsdu_debug_coalesce_two);
+	}
+
+	return 0;
+}
+
+ssize_t proc_set_tx_amsdu(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	char tmp[32];
+	u32 amsdu;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &amsdu);
+
+		if (padapter && (num == 1)) {
+			RTW_INFO("padapter->tx_amsdu = %x\n", amsdu);
+
+			if (amsdu > 3)
+				padapter->tx_amsdu = 0;
+			else if(amsdu == 3)
+			{
+				pxmitpriv->amsdu_debug_set_timer = 0;
+				pxmitpriv->amsdu_debug_timeout = 0;
+				pxmitpriv->amsdu_debug_coalesce_one = 0;
+				pxmitpriv->amsdu_debug_coalesce_two = 0;
+			}
+			else
+				padapter->tx_amsdu = amsdu;
+		}
+	}
+
+	return count;
+}
+
+int proc_get_tx_amsdu_rate(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+
+	if (padapter)
+		RTW_PRINT_SEL(m, "tx amsdu rate = %d Mbps\n", padapter->tx_amsdu_rate);
+
+	return 0;
+}
+
+ssize_t proc_set_tx_amsdu_rate(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 amsdu_rate;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &amsdu_rate);
+
+		if (padapter && (num == 1)) {
+			RTW_INFO("padapter->tx_amsdu_rate = %x\n", amsdu_rate);
+			padapter->tx_amsdu_rate = amsdu_rate;
+		}
+	}
+
+	return count;
+}
+#endif /* CONFIG_TX_AMSDU */
+
+int proc_get_en_fwps(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pregpriv)
+		RTW_PRINT_SEL(m, "check_fw_ps = %d , 1:enable get FW PS state , 0: disable get FW PS state\n"
+			      , pregpriv->check_fw_ps);
+
+	return 0;
+}
+
+ssize_t proc_set_en_fwps(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv &&  mode < 2) {
+			pregpriv->check_fw_ps = mode;
+			RTW_INFO("pregpriv->check_fw_ps=%d\n", pregpriv->check_fw_ps);
+		}
+
+	}
+
+	return count;
+}
+
+/*
+int proc_get_two_path_rssi(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+
+	if(padapter)
+		RTW_PRINT_SEL(m, "%d %d\n",
+			padapter->recvpriv.RxRssi[0], padapter->recvpriv.RxRssi[1]);
+
+	return 0;
+}
+*/
+void rtw_dump_dft_phy_cap(void *sel, _adapter *adapter)
+{
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct ht_priv	*phtpriv = &pmlmepriv->htpriv;
+
+	RTW_PRINT_SEL(sel, "[DFT CAP] HT STBC Tx : %s\n", (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX)) ? "V" : "X");
+	RTW_PRINT_SEL(sel, "[DFT CAP] HT STBC Rx : %s\n\n", (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_RX)) ? "V" : "X");
+
+	RTW_PRINT_SEL(sel, "[DFT CAP] HT LDPC Tx : %s\n", (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX)) ? "V" : "X");
+	RTW_PRINT_SEL(sel, "[DFT CAP] HT LDPC Rx : %s\n\n", (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_RX)) ? "V" : "X");
+
+	#ifdef CONFIG_BEAMFORMING
+	RTW_PRINT_SEL(sel, "[DFT CAP] HT Bfer : %s\n", (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE))  ? "V" : "X");
+	RTW_PRINT_SEL(sel, "[DFT CAP] HT Bfee : %s\n", (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE)) ? "V" : "X");
+	#endif
+}
+
+void rtw_get_dft_phy_cap(void *sel, _adapter *adapter)
+{
+	RTW_PRINT_SEL(sel, "\n ======== PHY CAP protocol ========\n");
+	rtw_ht_use_default_setting(adapter);
+	rtw_dump_dft_phy_cap(sel, adapter);
+}
+
+void rtw_dump_drv_phy_cap(void *sel, _adapter *adapter)
+{
+	struct registry_priv	*pregistry_priv = &adapter->registrypriv;
+
+	RTW_PRINT_SEL(sel, "\n ======== DRV's configuration ========\n");
+	RTW_PRINT_SEL(sel, "[DRV CAP] STBC Capability : 0x%02x\n", pregistry_priv->stbc_cap);
+	RTW_PRINT_SEL(sel, "[DRV CAP] VHT STBC Tx : %s\n", (TEST_FLAG(pregistry_priv->stbc_cap, BIT1)) ? "V" : "X"); /*BIT1: Enable VHT STBC Tx*/
+	RTW_PRINT_SEL(sel, "[DRV CAP] VHT STBC Rx : %s\n", (TEST_FLAG(pregistry_priv->stbc_cap, BIT0)) ? "V" : "X"); /*BIT0: Enable VHT STBC Rx*/
+	RTW_PRINT_SEL(sel, "[DRV CAP] HT STBC Tx : %s\n", (TEST_FLAG(pregistry_priv->stbc_cap, BIT5)) ? "V" : "X"); /*BIT5: Enable HT STBC Tx*/
+	RTW_PRINT_SEL(sel, "[DRV CAP] HT STBC Rx : %s\n\n", (TEST_FLAG(pregistry_priv->stbc_cap, BIT4)) ? "V" : "X"); /*BIT4: Enable HT STBC Rx*/
+
+	RTW_PRINT_SEL(sel, "[DRV CAP] LDPC Capability : 0x%02x\n", pregistry_priv->ldpc_cap);
+	RTW_PRINT_SEL(sel, "[DRV CAP] VHT LDPC Tx : %s\n", (TEST_FLAG(pregistry_priv->ldpc_cap, BIT1)) ? "V" : "X"); /*BIT1: Enable VHT LDPC Tx*/
+	RTW_PRINT_SEL(sel, "[DRV CAP] VHT LDPC Rx : %s\n", (TEST_FLAG(pregistry_priv->ldpc_cap, BIT0)) ? "V" : "X"); /*BIT0: Enable VHT LDPC Rx*/
+	RTW_PRINT_SEL(sel, "[DRV CAP] HT LDPC Tx : %s\n", (TEST_FLAG(pregistry_priv->ldpc_cap, BIT5)) ? "V" : "X"); /*BIT5: Enable HT LDPC Tx*/
+	RTW_PRINT_SEL(sel, "[DRV CAP] HT LDPC Rx : %s\n\n", (TEST_FLAG(pregistry_priv->ldpc_cap, BIT4)) ? "V" : "X"); /*BIT4: Enable HT LDPC Rx*/
+	#ifdef CONFIG_BEAMFORMING
+
+	/*
+	 * BIT0: Enable VHT SU Beamformer
+	 * BIT1: Enable VHT SU Beamformee
+	 * BIT2: Enable VHT MU Beamformer, depend on VHT SU Beamformer
+	 * BIT3: Enable VHT MU Beamformee, depend on VHT SU Beamformee
+	 * BIT4: Enable HT Beamformer
+	 * BIT5: Enable HT Beamformee
+	 */
+	RTW_PRINT_SEL(sel, "[DRV CAP] TxBF Capability : 0x%02x\n", pregistry_priv->beamform_cap);
+	RTW_PRINT_SEL(sel, "[DRV CAP] VHT MU Bfer : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT2)) ? "V" : "X");
+	RTW_PRINT_SEL(sel, "[DRV CAP] VHT MU Bfee : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT3)) ? "V" : "X");
+	RTW_PRINT_SEL(sel, "[DRV CAP] VHT SU Bfer : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT0)) ? "V" : "X");
+	RTW_PRINT_SEL(sel, "[DRV CAP] VHT SU Bfee : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT1)) ? "V" : "X");
+	RTW_PRINT_SEL(sel, "[DRV CAP] HT Bfer : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT4))  ? "V" : "X");
+	RTW_PRINT_SEL(sel, "[DRV CAP] HT Bfee : %s\n", (TEST_FLAG(pregistry_priv->beamform_cap, BIT5)) ? "V" : "X");
+
+	RTW_PRINT_SEL(sel, "[DRV CAP] Tx Bfer rf_num : %d\n", pregistry_priv->beamformer_rf_num);
+	RTW_PRINT_SEL(sel, "[DRV CAP] Tx Bfee rf_num : %d\n", pregistry_priv->beamformee_rf_num);
+	#endif
+}
+
+int proc_get_stbc_cap(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		RTW_PRINT_SEL(m, "0x%02x\n", pregpriv->stbc_cap);
+
+	return 0;
+}
+
+ssize_t proc_set_stbc_cap(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv) {
+			pregpriv->stbc_cap = mode;
+			RTW_INFO("stbc_cap = 0x%02x\n", mode);
+		}
+	}
+
+	return count;
+}
+int proc_get_rx_stbc(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		RTW_PRINT_SEL(m, "%d\n", pregpriv->rx_stbc);
+
+	return 0;
+}
+
+ssize_t proc_set_rx_stbc(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && (mode == 0 || mode == 1 || mode == 2 || mode == 3)) {
+			pregpriv->rx_stbc = mode;
+			RTW_INFO("rx_stbc=%d\n", mode);
+		}
+	}
+
+	return count;
+
+}
+int proc_get_ldpc_cap(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		RTW_PRINT_SEL(m, "0x%02x\n", pregpriv->ldpc_cap);
+
+	return 0;
+}
+
+ssize_t proc_set_ldpc_cap(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv) {
+			pregpriv->ldpc_cap = mode;
+			RTW_INFO("ldpc_cap = 0x%02x\n", mode);
+		}
+	}
+
+	return count;
+}
+#ifdef CONFIG_BEAMFORMING
+int proc_get_txbf_cap(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		RTW_PRINT_SEL(m, "0x%02x\n", pregpriv->beamform_cap);
+
+	return 0;
+}
+
+ssize_t proc_set_txbf_cap(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv) {
+			pregpriv->beamform_cap = mode;
+			RTW_INFO("beamform_cap = 0x%02x\n", mode);
+		}
+	}
+
+	return count;
+}
+#endif
+
+#ifdef CONFIG_AP_MODE
+
+int proc_get_all_sta_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	unsigned long irqL;
+	struct sta_info *psta;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	int i;
+	_list	*plist, *phead;
+
+	RTW_PRINT_SEL(m, "sta_dz_bitmap=0x%x, tim_bitmap=0x%x\n", pstapriv->sta_dz_bitmap, pstapriv->tim_bitmap);
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	for (i = 0; i < NUM_STA; i++) {
+		phead = &(pstapriv->sta_hash[i]);
+		plist = get_next(phead);
+
+		while ((rtw_end_of_queue_search(phead, plist)) == false) {
+			psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+			plist = get_next(plist);
+
+			/* if(extra_arg == psta->aid) */
+			{
+				RTW_PRINT_SEL(m, "==============================\n");
+				RTW_PRINT_SEL(m, "sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->hwaddr));
+				RTW_PRINT_SEL(m, "rtsen=%d, cts2slef=%d\n", psta->rtsen, psta->cts2self);
+				RTW_PRINT_SEL(m, "state=0x%x, aid=%d, macid=%d, raid=%d\n", psta->state, psta->aid, psta->mac_id, psta->raid);
+				RTW_PRINT_SEL(m, "qos_en=%d, ht_en=%d, init_rate=%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate);
+				RTW_PRINT_SEL(m, "bwmode=%d, ch_offset=%d, sgi_20m=%d,sgi_40m=%d\n", psta->bw_mode, psta->htpriv.ch_offset, psta->htpriv.sgi_20m, psta->htpriv.sgi_40m);
+				RTW_PRINT_SEL(m, "ampdu_enable = %d\n", psta->htpriv.ampdu_enable);
+				RTW_PRINT_SEL(m, "tx_amsdu_enable = %d\n", psta->htpriv.tx_amsdu_enable);
+				RTW_PRINT_SEL(m, "agg_enable_bitmap=%x, candidate_tid_bitmap=%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap);
+				RTW_PRINT_SEL(m, "sleepq_len=%d\n", psta->sleepq_len);
+				RTW_PRINT_SEL(m, "sta_xmitpriv.vo_q_qcnt=%d\n", psta->sta_xmitpriv.vo_q.qcnt);
+				RTW_PRINT_SEL(m, "sta_xmitpriv.vi_q_qcnt=%d\n", psta->sta_xmitpriv.vi_q.qcnt);
+				RTW_PRINT_SEL(m, "sta_xmitpriv.be_q_qcnt=%d\n", psta->sta_xmitpriv.be_q.qcnt);
+				RTW_PRINT_SEL(m, "sta_xmitpriv.bk_q_qcnt=%d\n", psta->sta_xmitpriv.bk_q.qcnt);
+
+				RTW_PRINT_SEL(m, "capability=0x%x\n", psta->capability);
+				RTW_PRINT_SEL(m, "flags=0x%x\n", psta->flags);
+				RTW_PRINT_SEL(m, "wpa_psk=0x%x\n", psta->wpa_psk);
+				RTW_PRINT_SEL(m, "wpa2_group_cipher=0x%x\n", psta->wpa2_group_cipher);
+				RTW_PRINT_SEL(m, "wpa2_pairwise_cipher=0x%x\n", psta->wpa2_pairwise_cipher);
+				RTW_PRINT_SEL(m, "qos_info=0x%x\n", psta->qos_info);
+				RTW_PRINT_SEL(m, "dot118021XPrivacy=0x%x\n", psta->dot118021XPrivacy);
+
+				sta_rx_reorder_ctl_dump(m, psta);
+
+#ifdef CONFIG_TDLS
+				RTW_PRINT_SEL(m, "tdls_sta_state=0x%08x\n", psta->tdls_sta_state);
+				RTW_PRINT_SEL(m, "PeerKey_Lifetime=%d\n", psta->TDLS_PeerKey_Lifetime);
+				RTW_PRINT_SEL(m, "rx_data_pkts=%llu\n", psta->sta_stats.rx_data_pkts);
+				RTW_PRINT_SEL(m, "rx_bytes=%llu\n", psta->sta_stats.rx_bytes);
+				RTW_PRINT_SEL(m, "tx_data_pkts=%llu\n", psta->sta_stats.tx_pkts);
+				RTW_PRINT_SEL(m, "tx_bytes=%llu\n", psta->sta_stats.tx_bytes);
+#endif /* CONFIG_TDLS */
+
+				dump_st_ctl(m, &psta->st_ctl);
+
+				if (STA_OP_WFD_MODE(psta))
+					RTW_PRINT_SEL(m, "op_wfd_mode:0x%02x\n", STA_OP_WFD_MODE(psta));
+
+				RTW_PRINT_SEL(m, "==============================\n");
+			}
+
+		}
+
+	}
+
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	return 0;
+}
+
+#endif
+
+#ifdef CONFIG_PREALLOC_RX_SKB_BUFFER
+int proc_get_rtkm_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct recv_priv	*precvpriv = &padapter->recvpriv;
+	struct recv_buf *precvbuf;
+
+	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
+
+	RTW_PRINT_SEL(m, "============[RTKM Info]============\n");
+	RTW_PRINT_SEL(m, "MAX_RTKM_NR_PREALLOC_RECV_SKB: %d\n", rtw_rtkm_get_nr_recv_skb());
+	RTW_PRINT_SEL(m, "MAX_RTKM_RECVBUF_SZ: %d\n", rtw_rtkm_get_buff_size());
+
+	RTW_PRINT_SEL(m, "============[Driver Info]============\n");
+	RTW_PRINT_SEL(m, "NR_PREALLOC_RECV_SKB: %d\n", NR_PREALLOC_RECV_SKB);
+	RTW_PRINT_SEL(m, "MAX_RECVBUF_SZ: %d\n", precvbuf->alloc_sz);
+
+	return 0;
+}
+#endif /* CONFIG_PREALLOC_RX_SKB_BUFFER */
+
+#ifdef CONFIG_FIND_BEST_CHANNEL
+int proc_get_best_channel(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	u32 i, best_channel_24G = 1, best_channel_5G = 36, index_24G = 0, index_5G = 0;
+
+	for (i = 0; pmlmeext->channel_set[i].ChannelNum != 0; i++) {
+		if (pmlmeext->channel_set[i].ChannelNum == 1)
+			index_24G = i;
+		if (pmlmeext->channel_set[i].ChannelNum == 36)
+			index_5G = i;
+	}
+
+	for (i = 0; (i < MAX_CHANNEL_NUM) && (pmlmeext->channel_set[i].ChannelNum != 0) ; i++) {
+		/* 2.4G */
+		if (pmlmeext->channel_set[i].ChannelNum == 6) {
+			if (pmlmeext->channel_set[i].rx_count < pmlmeext->channel_set[index_24G].rx_count) {
+				index_24G = i;
+				best_channel_24G = pmlmeext->channel_set[i].ChannelNum;
+			}
+		}
+
+		/* 5G */
+		if (pmlmeext->channel_set[i].ChannelNum >= 36
+		    && pmlmeext->channel_set[i].ChannelNum < 140) {
+			/* Find primary channel */
+			if (((pmlmeext->channel_set[i].ChannelNum - 36) % 8 == 0)
+			    && (pmlmeext->channel_set[i].rx_count < pmlmeext->channel_set[index_5G].rx_count)) {
+				index_5G = i;
+				best_channel_5G = pmlmeext->channel_set[i].ChannelNum;
+			}
+		}
+
+		if (pmlmeext->channel_set[i].ChannelNum >= 149
+		    && pmlmeext->channel_set[i].ChannelNum < 165) {
+			/* find primary channel */
+			if (((pmlmeext->channel_set[i].ChannelNum - 149) % 8 == 0)
+			    && (pmlmeext->channel_set[i].rx_count < pmlmeext->channel_set[index_5G].rx_count)) {
+				index_5G = i;
+				best_channel_5G = pmlmeext->channel_set[i].ChannelNum;
+			}
+		}
+		RTW_PRINT_SEL(m, "The rx cnt of channel %3d = %d\n",
+			pmlmeext->channel_set[i].ChannelNum, pmlmeext->channel_set[i].rx_count);
+	}
+
+	RTW_PRINT_SEL(m, "best_channel_5G = %d\n", best_channel_5G);
+	RTW_PRINT_SEL(m, "best_channel_24G = %d\n", best_channel_24G);
+
+	return 0;
+}
+
+ssize_t proc_set_best_channel(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	char tmp[32];
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int i;
+		for (i = 0; pmlmeext->channel_set[i].ChannelNum != 0; i++)
+			pmlmeext->channel_set[i].rx_count = 0;
+
+		RTW_INFO("set %s\n", "Clean Best Channel Count");
+	}
+
+	return count;
+}
+#endif /* CONFIG_FIND_BEST_CHANNEL */
+
+#ifdef CONFIG_BT_COEXIST
+int proc_get_btcoex_dbg(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	PADAPTER padapter;
+	char buf[512] = {0};
+	padapter = (PADAPTER)rtw_netdev_priv(dev);
+
+	rtw_btcoex_GetDBG(padapter, buf, 512);
+
+	_RTW_PRINT_SEL(m, "%s", buf);
+
+	return 0;
+}
+
+ssize_t proc_set_btcoex_dbg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	PADAPTER padapter;
+	u8 tmp[80] = {0};
+	u32 module[2] = {0};
+	u32 num;
+
+	padapter = (PADAPTER)rtw_netdev_priv(dev);
+
+	/*	RTW_INFO("+" FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(padapter)); */
+
+	if (NULL == buffer) {
+		RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n",
+			 FUNC_ADPT_ARG(padapter));
+
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n",
+			 FUNC_ADPT_ARG(padapter));
+
+		return -EFAULT;
+	}
+
+	num = count;
+	if (num > (sizeof(tmp) - 1))
+		num = (sizeof(tmp) - 1);
+
+	if (copy_from_user(tmp, buffer, num)) {
+		RTW_INFO(FUNC_ADPT_FMT ": copy buffer from user space FAIL!\n",
+			 FUNC_ADPT_ARG(padapter));
+
+		return -EFAULT;
+	}
+
+	num = sscanf(tmp, "%x %x", module, module + 1);
+	if (1 == num) {
+		if (0 == module[0])
+			memset(module, 0, sizeof(module));
+		else
+			memset(module, 0xFF, sizeof(module));
+	} else if (2 != num) {
+		RTW_INFO(FUNC_ADPT_FMT ": input(\"%s\") format incorrect!\n",
+			 FUNC_ADPT_ARG(padapter), tmp);
+
+		if (0 == num)
+			return -EFAULT;
+	}
+
+	RTW_INFO(FUNC_ADPT_FMT ": input 0x%08X 0x%08X\n",
+		 FUNC_ADPT_ARG(padapter), module[0], module[1]);
+	rtw_btcoex_SetDBG(padapter, module);
+
+	return count;
+}
+
+int proc_get_btcoex_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	PADAPTER padapter;
+	const u32 bufsize = 30 * 100;
+	u8 *pbuf = NULL;
+
+	padapter = (PADAPTER)rtw_netdev_priv(dev);
+
+	pbuf = rtw_zmalloc(bufsize);
+	if (NULL == pbuf)
+		return -ENOMEM;
+
+	rtw_btcoex_DisplayBtCoexInfo(padapter, pbuf, bufsize);
+
+	_RTW_PRINT_SEL(m, "%s\n", pbuf);
+
+	rtw_mfree(pbuf, bufsize);
+
+	return 0;
+}
+#endif /* CONFIG_BT_COEXIST */
+
+#if defined(DBG_CONFIG_ERROR_DETECT)
+int proc_get_sreset(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	return 0;
+}
+
+ssize_t proc_set_sreset(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	s32 trigger_point;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		int num = sscanf(tmp, "%d", &trigger_point);
+
+		if (trigger_point == SRESET_TGP_NULL)
+			rtw_hal_sreset_reset(padapter);
+		else
+			sreset_set_trigger_point(padapter, trigger_point);
+	}
+
+	return count;
+
+}
+#endif /* DBG_CONFIG_ERROR_DETECT */
+
+#ifdef CONFIG_WOWLAN
+int proc_get_pattern_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	u8 pattern_num = 0, val8;
+	char str_1[128];
+	char *p_str;
+	int i = 0 , j = 0, k = 0;
+	int len = 0, max_len = 0, total = 0;
+
+	p_str = str_1;
+	max_len = sizeof(str_1);
+
+	total = pwrpriv->wowlan_pattern_idx;
+
+	rtw_set_default_pattern(padapter);
+
+	/*show pattern*/
+	RTW_PRINT_SEL(m, "\n======[Pattern Info.]======\n");
+	RTW_PRINT_SEL(m, "pattern number: %d\n", total);
+	RTW_PRINT_SEL(m, "support default patterns: %c\n",
+		      (pregistrypriv->default_patterns_en) ? 'Y' : 'N');
+
+	for (k = 0; k < total ; k++) {
+		RTW_PRINT_SEL(m, "\npattern idx: %d\n", k);
+		RTW_PRINT_SEL(m, "pattern content:\n");
+
+		p_str = str_1;
+		max_len = sizeof(str_1);
+		for (i = 0 ; i < MAX_WKFM_PATTERN_SIZE / 8 ; i++) {
+			memset(p_str, 0, max_len);
+			len = 0;
+			for (j = 0 ; j < 8 ; j++) {
+				val8 = pwrpriv->patterns[k].content[i * 8 + j];
+				len += snprintf(p_str + len, max_len - len,
+						"%02x ", val8);
+			}
+			RTW_PRINT_SEL(m, "%s\n", p_str);
+		}
+		RTW_PRINT_SEL(m, "\npattern mask:\n");
+		for (i = 0 ; i < MAX_WKFM_SIZE / 8 ; i++) {
+			memset(p_str, 0, max_len);
+			len = 0;
+			for (j = 0 ; j < 8 ; j++) {
+				val8 = pwrpriv->patterns[k].mask[i * 8 + j];
+				len += snprintf(p_str + len, max_len - len,
+						"%02x ", val8);
+			}
+			RTW_PRINT_SEL(m, "%s\n", p_str);
+		}
+
+		RTW_PRINT_SEL(m, "\npriv_pattern_len:\n");
+		RTW_PRINT_SEL(m, "pattern_len: %d\n", pwrpriv->patterns[k].len);
+		RTW_PRINT_SEL(m, "*****************\n");
+	}
+
+	return 0;
+}
+ssize_t proc_set_pattern_info(struct file *file, const char __user *buffer,
+			      size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct wowlan_ioctl_param poidparam;
+	u8 tmp[MAX_WKFM_PATTERN_SIZE] = {0};
+	int ret = 0, num = 0;
+	u8 index = 0;
+
+	poidparam.subcode = 0;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (pwrpriv->wowlan_pattern_idx >= MAX_WKFM_CAM_NUM) {
+		RTW_INFO("WARNING: priv-pattern is full(idx: %d)\n",
+			 pwrpriv->wowlan_pattern_idx);
+		RTW_INFO("WARNING: please clean priv-pattern first\n");
+		return -ENOMEM;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		if (strncmp(tmp, "clean", 5) == 0) {
+			poidparam.subcode = WOWLAN_PATTERN_CLEAN;
+			rtw_hal_set_hwreg(padapter,
+					  HW_VAR_WOWLAN, (u8 *)&poidparam);
+		} else {
+			index = pwrpriv->wowlan_pattern_idx;
+			ret = rtw_wowlan_parser_pattern_cmd(tmp,
+					    pwrpriv->patterns[index].content,
+					    &pwrpriv->patterns[index].len,
+					    pwrpriv->patterns[index].mask);
+			if (ret == true)
+				pwrpriv->wowlan_pattern_idx++;
+		}
+	}
+
+	return count;
+}
+
+int proc_get_wakeup_reason(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	u8 val = pwrpriv->wowlan_last_wake_reason;
+
+	RTW_PRINT_SEL(m, "last wake reason: %#02x\n", val);
+	return 0;
+}
+#endif /*CONFIG_WOWLAN*/
+
+#ifdef CONFIG_GPIO_WAKEUP
+int proc_get_wowlan_gpio_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	u8 val = pwrpriv->is_high_active;
+
+	RTW_PRINT_SEL(m, "wakeup_gpio_idx: %d\n", WAKEUP_GPIO_IDX);
+	RTW_PRINT_SEL(m, "high_active: %d\n", val);
+
+	return 0;
+}
+
+ssize_t proc_set_wowlan_gpio_info(struct file *file, const char __user *buffer,
+				  size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	char tmp[32] = {0};
+	int num = 0;
+	u32 is_high_active = 0;
+	u8 val8 = 0;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+
+		num = sscanf(tmp, "%u", &is_high_active);
+
+		is_high_active = is_high_active == 0 ? 0 : 1;
+
+		pwrpriv->is_high_active = is_high_active;
+
+		rtw_ps_deny(padapter, PS_DENY_IOCTL);
+		LeaveAllPowerSaveModeDirect(padapter);
+		val8 = (pwrpriv->is_high_active == 0) ? 1 : 0;
+		rtw_hal_set_output_gpio(padapter, WAKEUP_GPIO_IDX, val8);
+		rtw_ps_deny_cancel(padapter, PS_DENY_IOCTL);
+
+		RTW_INFO("set %s %d\n", "gpio_high_active",
+			 pwrpriv->is_high_active);
+		RTW_INFO("%s: set GPIO_%d %d as default.\n",
+			 __func__, WAKEUP_GPIO_IDX, val8);
+	}
+
+	return count;
+}
+#endif /* CONFIG_GPIO_WAKEUP */
+
+#ifdef CONFIG_P2P_WOWLAN
+int proc_get_p2p_wowlan_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	struct p2p_wowlan_info	 peerinfo = pwdinfo->p2p_wow_info;
+	if (peerinfo.is_trigger) {
+		RTW_PRINT_SEL(m, "is_trigger: true\n");
+		switch (peerinfo.wowlan_recv_frame_type) {
+		case P2P_WOWLAN_RECV_NEGO_REQ:
+			RTW_PRINT_SEL(m, "Frame Type: Nego Request\n");
+			break;
+		case P2P_WOWLAN_RECV_INVITE_REQ:
+			RTW_PRINT_SEL(m, "Frame Type: Invitation Request\n");
+			break;
+		case P2P_WOWLAN_RECV_PROVISION_REQ:
+			RTW_PRINT_SEL(m, "Frame Type: Provision Request\n");
+			break;
+		default:
+			break;
+		}
+		RTW_PRINT_SEL(m, "Peer Addr: "MAC_FMT"\n", MAC_ARG(peerinfo.wowlan_peer_addr));
+		RTW_PRINT_SEL(m, "Peer WPS Config: %x\n", peerinfo.wowlan_peer_wpsconfig);
+		RTW_PRINT_SEL(m, "Persistent Group: %d\n", peerinfo.wowlan_peer_is_persistent);
+		RTW_PRINT_SEL(m, "Intivation Type: %d\n", peerinfo.wowlan_peer_invitation_type);
+	} else
+		RTW_PRINT_SEL(m, "is_trigger: False\n");
+	return 0;
+}
+#endif /* CONFIG_P2P_WOWLAN */
+
+int proc_get_new_bcn_max(struct seq_file *m, void *v)
+{
+	extern int new_bcn_max;
+
+	RTW_PRINT_SEL(m, "%d", new_bcn_max);
+	return 0;
+}
+
+ssize_t proc_set_new_bcn_max(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	char tmp[32];
+	extern int new_bcn_max;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count))
+		sscanf(tmp, "%d ", &new_bcn_max);
+
+	return count;
+}
+
+#ifdef CONFIG_POWER_SAVING
+int proc_get_ps_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	u8 ips_mode = pwrpriv->ips_mode;
+	u8 lps_mode = pwrpriv->power_mgnt;
+	char *str = "";
+
+	RTW_PRINT_SEL(m, "======Power Saving Info:======\n");
+	RTW_PRINT_SEL(m, "*IPS:\n");
+
+	if (ips_mode == IPS_NORMAL) {
+#ifdef CONFIG_FWLPS_IN_IPS
+		str = "FW_LPS_IN_IPS";
+#else
+		str = "Card Disable";
+#endif
+	} else if (ips_mode == IPS_NONE)
+		str = "NO IPS";
+	else if (ips_mode == IPS_LEVEL_2)
+		str = "IPS_LEVEL_2";
+	else
+		str = "invalid ips_mode";
+
+	RTW_PRINT_SEL(m, " IPS mode: %s\n", str);
+	RTW_PRINT_SEL(m, " IPS enter count:%d, IPS leave count:%d\n",
+		      pwrpriv->ips_enter_cnts, pwrpriv->ips_leave_cnts);
+	RTW_PRINT_SEL(m, "------------------------------\n");
+	RTW_PRINT_SEL(m, "*LPS:\n");
+
+	if (lps_mode == PS_MODE_ACTIVE)
+		str = "NO LPS";
+	else if (lps_mode == PS_MODE_MIN)
+		str = "MIN";
+	else if (lps_mode == PS_MODE_MAX)
+		str = "MAX";
+	else if (lps_mode == PS_MODE_DTIM)
+		str = "DTIM";
+	else
+		sprintf(str, "%d", lps_mode);
+
+	RTW_PRINT_SEL(m, " LPS mode: %s\n", str);
+
+	if (pwrpriv->dtim != 0)
+		RTW_PRINT_SEL(m, " DTIM: %d\n", pwrpriv->dtim);
+	RTW_PRINT_SEL(m, " LPS enter count:%d, LPS leave count:%d\n",
+		      pwrpriv->lps_enter_cnts, pwrpriv->lps_leave_cnts);
+	RTW_PRINT_SEL(m, "=============================\n");
+	return 0;
+}
+#endif /* CONFIG_POWER_SAVING */
+
+#ifdef CONFIG_TDLS
+static int proc_tdls_display_tdls_function_info(struct seq_file *m)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	u8 SpaceBtwnItemAndValue = TDLS_DBG_INFO_SPACE_BTWN_ITEM_AND_VALUE;
+	u8 SpaceBtwnItemAndValueTmp = 0;
+	bool FirstMatchFound = false;
+	int j = 0;
+
+	RTW_PRINT_SEL(m, "============[TDLS Function Info]============\n");
+	RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Prohibited", (ptdlsinfo->ap_prohibited == true) ? "true" : "false");
+	RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Channel Switch Prohibited", (ptdlsinfo->ch_switch_prohibited == true) ? "true" : "false");
+	RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Link Established", (ptdlsinfo->link_established == true) ? "true" : "false");
+	RTW_PRINT_SEL(m, "%-*s = %d/%d\n", SpaceBtwnItemAndValue, "TDLS STA Num (Linked/Allowed)", ptdlsinfo->sta_cnt, MAX_ALLOWED_TDLS_STA_NUM);
+	RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Allowed STA Num Reached", (ptdlsinfo->sta_maximum == true) ? "true" : "false");
+
+#ifdef CONFIG_TDLS_CH_SW
+	RTW_PRINT_SEL(m, "%-*s =", SpaceBtwnItemAndValue, "TDLS CH SW State");
+	if (ptdlsinfo->chsw_info.ch_sw_state == TDLS_STATE_NONE)
+		RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_STATE_NONE");
+	else {
+		for (j = 0; j < 32; j++) {
+			if (ptdlsinfo->chsw_info.ch_sw_state & BIT(j)) {
+				if (FirstMatchFound ==  false) {
+					SpaceBtwnItemAndValueTmp = 1;
+					FirstMatchFound = true;
+				} else
+					SpaceBtwnItemAndValueTmp = SpaceBtwnItemAndValue + 3;
+				switch (BIT(j)) {
+				case TDLS_INITIATOR_STATE:
+					RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_INITIATOR_STATE");
+					break;
+				case TDLS_RESPONDER_STATE:
+					RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_RESPONDER_STATE");
+					break;
+				case TDLS_LINKED_STATE:
+					RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_LINKED_STATE");
+					break;
+				case TDLS_WAIT_PTR_STATE:
+					RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_WAIT_PTR_STATE");
+					break;
+				case TDLS_ALIVE_STATE:
+					RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_ALIVE_STATE");
+					break;
+				case TDLS_CH_SWITCH_ON_STATE:
+					RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_CH_SWITCH_ON_STATE");
+					break;
+				case TDLS_PEER_AT_OFF_STATE:
+					RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_PEER_AT_OFF_STATE");
+					break;
+				case TDLS_CH_SW_INITIATOR_STATE:
+					RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_CH_SW_INITIATOR_STATE");
+					break;
+				case TDLS_WAIT_CH_RSP_STATE:
+					RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValue, " ", "TDLS_WAIT_CH_RSP_STATE");
+					break;
+				default:
+					RTW_PRINT_SEL(m, "%-*sBIT(%d)\n", SpaceBtwnItemAndValueTmp, " ", j);
+					break;
+				}
+			}
+		}
+	}
+
+	RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS CH SW On", (ATOMIC_READ(&ptdlsinfo->chsw_info.chsw_on) == true) ? "true" : "false");
+	RTW_PRINT_SEL(m, "%-*s = %d\n", SpaceBtwnItemAndValue, "TDLS CH SW Off-Channel Num", ptdlsinfo->chsw_info.off_ch_num);
+	RTW_PRINT_SEL(m, "%-*s = %d\n", SpaceBtwnItemAndValue, "TDLS CH SW Channel Offset", ptdlsinfo->chsw_info.ch_offset);
+	RTW_PRINT_SEL(m, "%-*s = %d\n", SpaceBtwnItemAndValue, "TDLS CH SW Current Time", ptdlsinfo->chsw_info.cur_time);
+	RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS CH SW Delay Switch Back", (ptdlsinfo->chsw_info.delay_switch_back == true) ? "true" : "false");
+	RTW_PRINT_SEL(m, "%-*s = %d\n", SpaceBtwnItemAndValue, "TDLS CH SW Dump Back", ptdlsinfo->chsw_info.dump_stack);
+#endif
+
+	RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Device Discovered", (ptdlsinfo->dev_discovered == true) ? "true" : "false");
+	RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Enable", (ptdlsinfo->tdls_enable == true) ? "true" : "false");
+	RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "TDLS Driver Setup", (ptdlsinfo->driver_setup == true) ? "true" : "false");
+
+	return 0;
+}
+
+static int proc_tdls_display_network_info(struct seq_file *m)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct wlan_network *cur_network = &(pmlmepriv->cur_network);
+	int i = 0;
+	u8 SpaceBtwnItemAndValue = TDLS_DBG_INFO_SPACE_BTWN_ITEM_AND_VALUE;
+
+	/* Display the linked AP/GO info */
+	RTW_PRINT_SEL(m, "============[Associated AP/GO Info]============\n");
+
+	if ((pmlmepriv->fw_state & WIFI_STATION_STATE) && (pmlmepriv->fw_state & _FW_LINKED)) {
+		RTW_PRINT_SEL(m, "%-*s = %s\n", SpaceBtwnItemAndValue, "BSSID", cur_network->network.Ssid.Ssid);
+		RTW_PRINT_SEL(m, "%-*s = "MAC_FMT"\n", SpaceBtwnItemAndValue, "Mac Address", MAC_ARG(cur_network->network.MacAddress));
+
+		RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Wireless Mode");
+		for (i = 0; i < 8; i++) {
+			if (pmlmeext->cur_wireless_mode & BIT(i)) {
+				switch (BIT(i)) {
+				case WIRELESS_11B:
+					RTW_PRINT_SEL(m, "%4s", "11B ");
+					break;
+				case WIRELESS_11G:
+					RTW_PRINT_SEL(m, "%4s", "11G ");
+					break;
+				case WIRELESS_11A:
+					RTW_PRINT_SEL(m, "%4s", "11A ");
+					break;
+				case WIRELESS_11_24N:
+					RTW_PRINT_SEL(m, "%7s", "11_24N ");
+					break;
+				case WIRELESS_11_5N:
+					RTW_PRINT_SEL(m, "%6s", "11_5N ");
+					break;
+				case WIRELESS_AUTO:
+					RTW_PRINT_SEL(m, "%5s", "AUTO ");
+					break;
+				case WIRELESS_11AC:
+					RTW_PRINT_SEL(m, "%5s", "11AC ");
+					break;
+				}
+			}
+		}
+		RTW_PRINT_SEL(m, "\n");
+
+		RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Privacy");
+		switch (padapter->securitypriv.dot11PrivacyAlgrthm) {
+		case _NO_PRIVACY_:
+			RTW_PRINT_SEL(m, "%s\n", "NO PRIVACY");
+			break;
+		case _WEP40_:
+			RTW_PRINT_SEL(m, "%s\n", "WEP 40");
+			break;
+		case _TKIP_:
+			RTW_PRINT_SEL(m, "%s\n", "TKIP");
+			break;
+		case _TKIP_WTMIC_:
+			RTW_PRINT_SEL(m, "%s\n", "TKIP WTMIC");
+			break;
+		case _AES_:
+			RTW_PRINT_SEL(m, "%s\n", "AES");
+			break;
+		case _WEP104_:
+			RTW_PRINT_SEL(m, "%s\n", "WEP 104");
+			break;
+		case _WEP_WPA_MIXED_:
+			RTW_PRINT_SEL(m, "%s\n", "WEP/WPA Mixed");
+			break;
+		case _SMS4_:
+			RTW_PRINT_SEL(m, "%s\n", "SMS4");
+			break;
+#ifdef CONFIG_IEEE80211W
+		case _BIP_:
+			RTW_PRINT_SEL(m, "%s\n", "BIP");
+			break;
+#endif /* CONFIG_IEEE80211W */
+		}
+
+		RTW_PRINT_SEL(m, "%-*s = %d\n", SpaceBtwnItemAndValue, "Channel", pmlmeext->cur_channel);
+		RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Channel Offset");
+		switch (pmlmeext->cur_ch_offset) {
+		case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
+			RTW_PRINT_SEL(m, "%s\n", "N/A");
+			break;
+		case HAL_PRIME_CHNL_OFFSET_LOWER:
+			RTW_PRINT_SEL(m, "%s\n", "Lower");
+			break;
+		case HAL_PRIME_CHNL_OFFSET_UPPER:
+			RTW_PRINT_SEL(m, "%s\n", "Upper");
+			break;
+		}
+
+		RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Bandwidth Mode");
+		switch (pmlmeext->cur_bwmode) {
+		case CHANNEL_WIDTH_20:
+			RTW_PRINT_SEL(m, "%s\n", "20MHz");
+			break;
+		case CHANNEL_WIDTH_40:
+			RTW_PRINT_SEL(m, "%s\n", "40MHz");
+			break;
+		case CHANNEL_WIDTH_80:
+			RTW_PRINT_SEL(m, "%s\n", "80MHz");
+			break;
+		case CHANNEL_WIDTH_160:
+			RTW_PRINT_SEL(m, "%s\n", "160MHz");
+			break;
+		case CHANNEL_WIDTH_80_80:
+			RTW_PRINT_SEL(m, "%s\n", "80MHz + 80MHz");
+			break;
+		}
+	} else
+		RTW_PRINT_SEL(m, "No association with AP/GO exists!\n");
+
+	return 0;
+}
+
+static int proc_tdls_display_tdls_sta_info(struct seq_file *m)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	struct sta_info *psta;
+	int i = 0, j = 0;
+	unsigned long irqL;
+	_list	*plist, *phead;
+	u8 SpaceBtwnItemAndValue = TDLS_DBG_INFO_SPACE_BTWN_ITEM_AND_VALUE;
+	u8 SpaceBtwnItemAndValueTmp = 0;
+	u8 NumOfTdlsStaToShow = 0;
+	bool FirstMatchFound = false;
+
+	/* Search for TDLS sta info to display */
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+	for (i = 0; i < NUM_STA; i++) {
+		phead = &(pstapriv->sta_hash[i]);
+		plist = get_next(phead);
+		while ((rtw_end_of_queue_search(phead, plist)) == false) {
+			psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+			plist = get_next(plist);
+			if (psta->tdls_sta_state != TDLS_STATE_NONE) {
+				/* We got one TDLS sta info to show */
+				RTW_PRINT_SEL(m, "============[TDLS Peer STA Info: STA %d]============\n", ++NumOfTdlsStaToShow);
+				RTW_PRINT_SEL(m, "%-*s = "MAC_FMT"\n", SpaceBtwnItemAndValue, "Mac Address", MAC_ARG(psta->hwaddr));
+				RTW_PRINT_SEL(m, "%-*s =", SpaceBtwnItemAndValue, "TDLS STA State");
+				SpaceBtwnItemAndValueTmp = 0;
+				FirstMatchFound = false;
+				for (j = 0; j < 32; j++) {
+					if (psta->tdls_sta_state & BIT(j)) {
+						if (FirstMatchFound ==  false) {
+							SpaceBtwnItemAndValueTmp = 1;
+							FirstMatchFound = true;
+						} else
+							SpaceBtwnItemAndValueTmp = SpaceBtwnItemAndValue + 3;
+						switch (BIT(j)) {
+						case TDLS_INITIATOR_STATE:
+							RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_INITIATOR_STATE");
+							break;
+						case TDLS_RESPONDER_STATE:
+							RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_RESPONDER_STATE");
+							break;
+						case TDLS_LINKED_STATE:
+							RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_LINKED_STATE");
+							break;
+						case TDLS_WAIT_PTR_STATE:
+							RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_WAIT_PTR_STATE");
+							break;
+						case TDLS_ALIVE_STATE:
+							RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_ALIVE_STATE");
+							break;
+						case TDLS_CH_SWITCH_ON_STATE:
+							RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_CH_SWITCH_ON_STATE");
+							break;
+						case TDLS_PEER_AT_OFF_STATE:
+							RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_PEER_AT_OFF_STATE");
+							break;
+						case TDLS_CH_SW_INITIATOR_STATE:
+							RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValueTmp, " ", "TDLS_CH_SW_INITIATOR_STATE");
+							break;
+						case TDLS_WAIT_CH_RSP_STATE:
+							RTW_PRINT_SEL(m, "%-*s%s\n", SpaceBtwnItemAndValue, " ", "TDLS_WAIT_CH_RSP_STATE");
+							break;
+						default:
+							RTW_PRINT_SEL(m, "%-*sBIT(%d)\n", SpaceBtwnItemAndValueTmp, " ", j);
+							break;
+						}
+					}
+				}
+
+				RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Wireless Mode");
+				for (j = 0; j < 8; j++) {
+					if (psta->wireless_mode & BIT(j)) {
+						switch (BIT(j)) {
+						case WIRELESS_11B:
+							RTW_PRINT_SEL(m, "%4s", "11B ");
+							break;
+						case WIRELESS_11G:
+							RTW_PRINT_SEL(m, "%4s", "11G ");
+							break;
+						case WIRELESS_11A:
+							RTW_PRINT_SEL(m, "%4s", "11A ");
+							break;
+						case WIRELESS_11_24N:
+							RTW_PRINT_SEL(m, "%7s", "11_24N ");
+							break;
+						case WIRELESS_11_5N:
+							RTW_PRINT_SEL(m, "%6s", "11_5N ");
+							break;
+						case WIRELESS_AUTO:
+							RTW_PRINT_SEL(m, "%5s", "AUTO ");
+							break;
+						case WIRELESS_11AC:
+							RTW_PRINT_SEL(m, "%5s", "11AC ");
+							break;
+						}
+					}
+				}
+				RTW_PRINT_SEL(m, "\n");
+
+				RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Bandwidth Mode");
+				switch (psta->bw_mode) {
+				case CHANNEL_WIDTH_20:
+					RTW_PRINT_SEL(m, "%s\n", "20MHz");
+					break;
+				case CHANNEL_WIDTH_40:
+					RTW_PRINT_SEL(m, "%s\n", "40MHz");
+					break;
+				case CHANNEL_WIDTH_80:
+					RTW_PRINT_SEL(m, "%s\n", "80MHz");
+					break;
+				case CHANNEL_WIDTH_160:
+					RTW_PRINT_SEL(m, "%s\n", "160MHz");
+					break;
+				case CHANNEL_WIDTH_80_80:
+					RTW_PRINT_SEL(m, "%s\n", "80MHz + 80MHz");
+					break;
+				}
+
+				RTW_PRINT_SEL(m, "%-*s = ", SpaceBtwnItemAndValue, "Privacy");
+				switch (psta->dot118021XPrivacy) {
+				case _NO_PRIVACY_:
+					RTW_PRINT_SEL(m, "%s\n", "NO PRIVACY");
+					break;
+				case _WEP40_:
+					RTW_PRINT_SEL(m, "%s\n", "WEP 40");
+					break;
+				case _TKIP_:
+					RTW_PRINT_SEL(m, "%s\n", "TKIP");
+					break;
+				case _TKIP_WTMIC_:
+					RTW_PRINT_SEL(m, "%s\n", "TKIP WTMIC");
+					break;
+				case _AES_:
+					RTW_PRINT_SEL(m, "%s\n", "AES");
+					break;
+				case _WEP104_:
+					RTW_PRINT_SEL(m, "%s\n", "WEP 104");
+					break;
+				case _WEP_WPA_MIXED_:
+					RTW_PRINT_SEL(m, "%s\n", "WEP/WPA Mixed");
+					break;
+				case _SMS4_:
+					RTW_PRINT_SEL(m, "%s\n", "SMS4");
+					break;
+#ifdef CONFIG_IEEE80211W
+				case _BIP_:
+					RTW_PRINT_SEL(m, "%s\n", "BIP");
+					break;
+#endif /* CONFIG_IEEE80211W */
+				}
+
+				RTW_PRINT_SEL(m, "%-*s = %d sec/%d sec\n", SpaceBtwnItemAndValue, "TPK Lifetime (Current/Expire)", psta->TPK_count, psta->TDLS_PeerKey_Lifetime);
+				RTW_PRINT_SEL(m, "%-*s = %llu\n", SpaceBtwnItemAndValue, "Tx Packets Over Direct Link", psta->sta_stats.tx_pkts);
+				RTW_PRINT_SEL(m, "%-*s = %llu\n", SpaceBtwnItemAndValue, "Rx Packets Over Direct Link", psta->sta_stats.rx_data_pkts);
+			}
+		}
+	}
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+	if (NumOfTdlsStaToShow == 0) {
+		RTW_PRINT_SEL(m, "============[TDLS Peer STA Info]============\n");
+		RTW_PRINT_SEL(m, "No TDLS direct link exists!\n");
+	}
+
+	return 0;
+}
+
+int proc_get_tdls_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct wlan_network *cur_network = &(pmlmepriv->cur_network);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	struct sta_info *psta;
+	int i = 0, j = 0;
+	unsigned long irqL;
+	_list	*plist, *phead;
+	u8 SpaceBtwnItemAndValue = 41;
+	u8 SpaceBtwnItemAndValueTmp = 0;
+	u8 NumOfTdlsStaToShow = 0;
+	bool FirstMatchFound = false;
+
+	if (hal_chk_wl_func(padapter, WL_FUNC_TDLS) == false) {
+		RTW_PRINT_SEL(m, "No tdls info can be shown since hal doesn't support tdls\n");
+		return 0;
+	}
+
+	proc_tdls_display_tdls_function_info(m);
+	proc_tdls_display_network_info(m);
+	proc_tdls_display_tdls_sta_info(m);
+
+	return 0;
+}
+#endif
+
+int proc_get_monitor(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if (WIFI_MONITOR_STATE == get_fwstate(pmlmepriv)) {
+		RTW_PRINT_SEL(m, "Monitor mode : Enable\n");
+
+		RTW_PRINT_SEL(m, "ch=%d, ch_offset=%d, bw=%d\n",
+			rtw_get_oper_ch(padapter), rtw_get_oper_choffset(padapter), rtw_get_oper_bw(padapter));
+	} else
+		RTW_PRINT_SEL(m, "Monitor mode : Disable\n");
+
+	return 0;
+}
+
+ssize_t proc_set_monitor(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	char tmp[32];
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	u8 target_chan, target_offset, target_bw;
+
+	if (count < 3) {
+		RTW_INFO("argument size is less than 3\n");
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int num = sscanf(tmp, "%hhu %hhu %hhu", &target_chan, &target_offset, &target_bw);
+
+		if (num != 3) {
+			RTW_INFO("invalid write_reg parameter!\n");
+			return count;
+		}
+
+		padapter->mlmeextpriv.cur_channel  = target_chan;
+		set_channel_bwmode(padapter, target_chan, target_offset, target_bw);
+	}
+
+	return count;
+}
+
+#include <hal_data.h>
+int proc_get_efuse_map(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	PHAL_DATA_TYPE pHalData = GET_HAL_DATA(padapter);
+	struct pwrctrl_priv *pwrctrlpriv  = adapter_to_pwrctl(padapter);
+	PEFUSE_HAL pEfuseHal = &pHalData->EfuseHal;
+	int i, j;
+	u8 ips_mode = IPS_NUM;
+	u16 mapLen;
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, false);
+	if (mapLen > EFUSE_MAX_MAP_LEN)
+		mapLen = EFUSE_MAX_MAP_LEN;
+
+	ips_mode = pwrctrlpriv->ips_mode;
+	rtw_pm_set_ips(padapter, IPS_NONE);
+
+	if (pHalData->efuse_file_status == EFUSE_FILE_LOADED) {
+		RTW_PRINT_SEL(m, "File eFuse Map loaded! file path:%s\nDriver eFuse Map From File\n", EFUSE_MAP_PATH);
+		if (pHalData->bautoload_fail_flag)
+			RTW_PRINT_SEL(m, "File Autoload fail!!!\n");
+	} else if (pHalData->efuse_file_status ==  EFUSE_FILE_FAILED) {
+		RTW_PRINT_SEL(m, "Open File eFuse Map Fail ! file path:%s\nDriver eFuse Map From Default\n", EFUSE_MAP_PATH);
+		if (pHalData->bautoload_fail_flag)
+			RTW_PRINT_SEL(m, "HW Autoload fail!!!\n");
+	} else {
+		RTW_PRINT_SEL(m, "Driver eFuse Map From HW\n");
+		if (pHalData->bautoload_fail_flag)
+			RTW_PRINT_SEL(m, "HW Autoload fail!!!\n");
+	}
+	for (i = 0; i < mapLen; i += 16) {
+		RTW_PRINT_SEL(m, "0x%02x\t", i);
+		for (j = 0; j < 8; j++)
+			RTW_PRINT_SEL(m, "%02X ", pHalData->efuse_eeprom_data[i + j]);
+		RTW_PRINT_SEL(m, "\t");
+		for (; j < 16; j++)
+			RTW_PRINT_SEL(m, "%02X ", pHalData->efuse_eeprom_data[i + j]);
+		RTW_PRINT_SEL(m, "\n");
+	}
+
+	if (rtw_efuse_map_read(padapter, 0, mapLen, pEfuseHal->fakeEfuseInitMap) == _FAIL) {
+		RTW_PRINT_SEL(m, "WARN - Read Realmap Failed\n");
+		return 0;
+	}
+
+	RTW_PRINT_SEL(m, "\n");
+	RTW_PRINT_SEL(m, "HW eFuse Map\n");
+	for (i = 0; i < mapLen; i += 16) {
+		RTW_PRINT_SEL(m, "0x%02x\t", i);
+		for (j = 0; j < 8; j++)
+			RTW_PRINT_SEL(m, "%02X ", pEfuseHal->fakeEfuseInitMap[i + j]);
+		RTW_PRINT_SEL(m, "\t");
+		for (; j < 16; j++)
+			RTW_PRINT_SEL(m, "%02X ", pEfuseHal->fakeEfuseInitMap[i + j]);
+		RTW_PRINT_SEL(m, "\n");
+	}
+
+	rtw_pm_set_ips(padapter, ips_mode);
+
+	return 0;
+}
+
+ssize_t proc_set_efuse_map(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	return count;
+}
+
+#ifdef CONFIG_IEEE80211W
+ssize_t proc_set_tx_sa_query(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
+	struct sta_info *psta;
+	_list	*plist, *phead;
+	unsigned long	 irqL;
+	char tmp[16];
+	u8	mac_addr[NUM_STA][ETH_ALEN];
+	u32 key_type;
+	u8 index;
+
+	if (count > 2) {
+		RTW_INFO("argument size is more than 2\n");
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%x", &key_type);
+
+		if (num !=  1) {
+			RTW_INFO("invalid read_reg parameter!\n");
+			return count;
+		}
+		RTW_INFO("0: set sa query request , key_type=%d\n", key_type);
+	}
+
+	if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
+	    && (check_fwstate(pmlmepriv, _FW_LINKED) == true) && padapter->securitypriv.binstallBIPkey == true) {
+		RTW_INFO("STA:"MAC_FMT"\n", MAC_ARG(get_my_bssid(&(pmlmeinfo->network))));
+		/* TX unicast sa_query to AP */
+		issue_action_SA_Query(padapter, get_my_bssid(&(pmlmeinfo->network)), 0, 0, (u8)key_type);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true && padapter->securitypriv.binstallBIPkey == true) {
+		/* TX unicast sa_query to every client STA */
+		_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+		for (index = 0; index < NUM_STA; index++) {
+			psta = NULL;
+
+			phead = &(pstapriv->sta_hash[index]);
+			plist = get_next(phead);
+
+			while ((rtw_end_of_queue_search(phead, plist)) == false) {
+				psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+				plist = get_next(plist);
+				memcpy(&mac_addr[psta->mac_id][0], psta->hwaddr, ETH_ALEN);
+			}
+		}
+		_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+		for (index = 0; index < macid_ctl->num && index < NUM_STA; index++) {
+			if (rtw_macid_is_used(macid_ctl, index) && !rtw_macid_is_bmc(macid_ctl, index)) {
+				if (memcmp(get_my_bssid(&(pmlmeinfo->network)), &mac_addr[index][0], ETH_ALEN)
+				    && !IS_MCAST(&mac_addr[index][0])) {
+					issue_action_SA_Query(padapter, &mac_addr[index][0], 0, 0, (u8)key_type);
+					RTW_INFO("STA[%u]:"MAC_FMT"\n", index , MAC_ARG(&mac_addr[index][0]));
+				}
+			}
+		}
+	}
+
+	return count;
+}
+
+int proc_get_tx_sa_query(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+
+	RTW_PRINT_SEL(m, "%s\n", __func__);
+	return 0;
+}
+
+ssize_t proc_set_tx_deauth(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
+	struct sta_info *psta;
+	_list	*plist, *phead;
+	unsigned long	 irqL;
+	char tmp[16];
+	u8	mac_addr[NUM_STA][ETH_ALEN];
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u32 key_type;
+	u8 index;
+
+
+	if (count > 2) {
+		RTW_INFO("argument size is more than 2\n");
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%x", &key_type);
+
+		if (num !=  1) {
+			RTW_INFO("invalid read_reg parameter!\n");
+			return count;
+		}
+		RTW_INFO("key_type=%d\n", key_type);
+	}
+	if (key_type < 0 || key_type > 4)
+		return count;
+
+	if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
+	    && (check_fwstate(pmlmepriv, _FW_LINKED) == true)) {
+		if (key_type == 3) /* key_type 3 only for AP mode */
+			return count;
+		/* TX unicast deauth to AP */
+		issue_deauth_11w(padapter, get_my_bssid(&(pmlmeinfo->network)), 0, (u8)key_type);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+
+		if (key_type == 3)
+			issue_deauth_11w(padapter, bc_addr, 0, IEEE80211W_RIGHT_KEY);
+
+		/* TX unicast deauth to every client STA */
+		_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+		for (index = 0; index < NUM_STA; index++) {
+			psta = NULL;
+
+			phead = &(pstapriv->sta_hash[index]);
+			plist = get_next(phead);
+
+			while ((rtw_end_of_queue_search(phead, plist)) == false) {
+				psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+				plist = get_next(plist);
+				memcpy(&mac_addr[psta->mac_id][0], psta->hwaddr, ETH_ALEN);
+			}
+		}
+		_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+		for (index = 0; index < macid_ctl->num && index < NUM_STA; index++) {
+			if (rtw_macid_is_used(macid_ctl, index) && !rtw_macid_is_bmc(macid_ctl, index)) {
+				if (memcmp(get_my_bssid(&(pmlmeinfo->network)), &mac_addr[index][0], ETH_ALEN)) {
+					if (key_type != 3)
+						issue_deauth_11w(padapter, &mac_addr[index][0], 0, (u8)key_type);
+
+					psta = rtw_get_stainfo(pstapriv, &mac_addr[index][0]);
+					if (psta && key_type != IEEE80211W_WRONG_KEY && key_type != IEEE80211W_NO_KEY) {
+						u8 updated = false;
+
+						_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+						if (list_empty(&psta->asoc_list) == false) {
+							list_del_init(&psta->asoc_list);
+							pstapriv->asoc_list_cnt--;
+							updated = ap_free_sta(padapter, psta, false, WLAN_REASON_PREV_AUTH_NOT_VALID, true);
+
+						}
+						_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+						associated_clients_update(padapter, updated, STA_INFO_UPDATE_ALL);
+					}
+
+					RTW_INFO("STA[%u]:"MAC_FMT"\n", index , MAC_ARG(&mac_addr[index][0]));
+				}
+			}
+		}
+	}
+
+	return count;
+}
+
+int proc_get_tx_deauth(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+
+	RTW_PRINT_SEL(m, "%s\n", __func__);
+	return 0;
+}
+
+ssize_t proc_set_tx_auth(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
+	struct sta_info *psta;
+	_list	*plist, *phead;
+	unsigned long	 irqL;
+	char tmp[16];
+	u8	mac_addr[NUM_STA][ETH_ALEN];
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u32 tx_auth;
+	u8 index;
+
+
+	if (count > 2) {
+		RTW_INFO("argument size is more than 2\n");
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%x", &tx_auth);
+
+		if (num !=  1) {
+			RTW_INFO("invalid read_reg parameter!\n");
+			return count;
+		}
+		RTW_INFO("1: setnd auth, 2: send assoc request. tx_auth=%d\n", tx_auth);
+	}
+
+	if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
+	    && (check_fwstate(pmlmepriv, _FW_LINKED) == true)) {
+		if (tx_auth == 1) {
+			/* TX unicast auth to AP */
+			issue_auth(padapter, NULL, 0);
+		} else if (tx_auth == 2) {
+			/* TX unicast auth to AP */
+			issue_assocreq(padapter);
+		}
+	}
+
+	return count;
+}
+
+int proc_get_tx_auth(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+
+	RTW_PRINT_SEL(m, "%s\n", __func__);
+	return 0;
+}
+#endif /* CONFIG_IEEE80211W */
+
+#ifdef CONFIG_MCC_MODE
+int proc_get_mcc_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+
+	dump_adapters_status(m, adapter_to_dvobj(adapter));
+	rtw_hal_dump_mcc_info(m, adapter_to_dvobj(adapter));
+	return 0;
+}
+
+int proc_get_mcc_policy_table(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *adapter = (_adapter *)rtw_netdev_priv(dev);
+
+	rtw_hal_dump_mcc_policy_table(m);
+	return 0;
+}
+
+ssize_t proc_set_mcc_policy_table(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[255];
+	s32 mcc_policy_table_idx;
+	u32 mcc_duration;
+	u32 mcc_tsf_sync_offset;
+	u32 mcc_start_time_offset;
+	u32 mcc_interval;
+	s32 mcc_guard_offset0;
+	s32 mcc_guard_offset1;
+
+	if (NULL == buffer) {
+		RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter));
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+		_adapter *iface = NULL;
+		u8 i = 0;
+		int num = sscanf(tmp, "%d %u %u %u %u %d %d"
+			, &mcc_policy_table_idx, &mcc_duration, &mcc_tsf_sync_offset, &mcc_start_time_offset
+			, &mcc_interval, &mcc_guard_offset0, &mcc_guard_offset1);
+
+		if (num < 7) {
+			RTW_INFO(FUNC_ADPT_FMT ": input parameters < 7\n", FUNC_ADPT_ARG(padapter));
+			return -EINVAL;
+		}
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			iface = dvobj->padapters[i];
+			if (!iface)
+				continue;
+			iface->registrypriv.rtw_mcc_policy_table_idx = mcc_policy_table_idx;
+			iface->registrypriv.rtw_mcc_duration = mcc_duration;
+			iface->registrypriv.rtw_mcc_tsf_sync_offset = mcc_tsf_sync_offset;
+			iface->registrypriv.rtw_mcc_start_time_offset = mcc_start_time_offset;
+			iface->registrypriv.rtw_mcc_interval = mcc_interval;
+			iface->registrypriv.rtw_mcc_guard_offset0 = mcc_guard_offset0;
+			iface->registrypriv.rtw_mcc_guard_offset1 = mcc_guard_offset1;
+		}
+
+		rtw_hal_mcc_update_switch_channel_policy_table(padapter);
+	}
+
+	return count;
+}
+
+ssize_t proc_set_mcc_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[255];
+	u32 en_mcc = 0;
+
+	if (NULL == buffer) {
+		RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter));
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+		_adapter *iface = NULL;
+		u8 i = 0;
+		int num = sscanf(tmp, "%u", &en_mcc);
+
+		if (num < 1) {
+			RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter));
+			return -EINVAL;
+		}
+
+		RTW_INFO("%s: en_mcc = %d\n", __func__, en_mcc);
+
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			iface = dvobj->padapters[i];
+			if (!iface)
+				continue;
+			iface->registrypriv.en_mcc = en_mcc;
+		}
+	}
+
+	return count;
+}
+
+ssize_t proc_set_mcc_single_tx_criteria(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[255];
+	u32 mcc_single_tx_criteria = 0;
+
+	if (NULL == buffer) {
+		RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter));
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+		_adapter *iface = NULL;
+		u8 i = 0;
+		int num = sscanf(tmp, "%u", &mcc_single_tx_criteria);
+
+		if (num < 1) {
+			RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter));
+			return -EINVAL;
+		}
+
+		RTW_INFO("%s: mcc_single_tx_criteria = %d\n", __func__, mcc_single_tx_criteria);
+
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			iface = dvobj->padapters[i];
+			if (!iface)
+				continue;
+			iface->registrypriv.rtw_mcc_single_tx_cri = mcc_single_tx_criteria;
+		}
+
+
+	}
+
+	return count;
+}
+
+
+ssize_t proc_set_mcc_ap_bw20_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[255];
+	u32 mcc_ap_bw20_target_tp = 0;
+
+	if (NULL == buffer) {
+		RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter));
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int num = sscanf(tmp, "%u", &mcc_ap_bw20_target_tp);
+
+		if (num < 1) {
+			RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter));
+			return -EINVAL;
+		}
+
+		RTW_INFO("%s: mcc_ap_bw20_target_tp = %d\n", __func__, mcc_ap_bw20_target_tp);
+
+		padapter->registrypriv.rtw_mcc_ap_bw20_target_tx_tp = mcc_ap_bw20_target_tp;
+
+
+	}
+
+	return count;
+}
+
+ssize_t proc_set_mcc_ap_bw40_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[255];
+	u32 mcc_ap_bw40_target_tp = 0;
+
+	if (NULL == buffer) {
+		RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter));
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int num = sscanf(tmp, "%u", &mcc_ap_bw40_target_tp);
+
+		if (num < 1) {
+			RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter));
+			return -EINVAL;
+		}
+
+		RTW_INFO("%s: mcc_ap_bw40_target_tp = %d\n", __func__, mcc_ap_bw40_target_tp);
+
+		padapter->registrypriv.rtw_mcc_ap_bw40_target_tx_tp = mcc_ap_bw40_target_tp;
+
+
+	}
+
+	return count;
+}
+
+ssize_t proc_set_mcc_ap_bw80_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[255];
+	u32 mcc_ap_bw80_target_tp = 0;
+
+	if (NULL == buffer) {
+		RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter));
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int num = sscanf(tmp, "%u", &mcc_ap_bw80_target_tp);
+
+		if (num < 1) {
+			RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter));
+			return -EINVAL;
+		}
+
+		RTW_INFO("%s: mcc_ap_bw80_target_tp = %d\n", __func__, mcc_ap_bw80_target_tp);
+
+		padapter->registrypriv.rtw_mcc_ap_bw80_target_tx_tp = mcc_ap_bw80_target_tp;
+
+
+	}
+
+	return count;
+}
+
+ssize_t proc_set_mcc_sta_bw20_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[255];
+	u32 mcc_sta_bw20_target_tp = 0;
+
+	if (NULL == buffer) {
+		RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter));
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int num = sscanf(tmp, "%u", &mcc_sta_bw20_target_tp);
+
+		if (num < 1) {
+			RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter));
+			return -EINVAL;
+		}
+
+		RTW_INFO("%s: mcc_sta_bw20_target_tp = %d\n", __func__, mcc_sta_bw20_target_tp);
+
+		padapter->registrypriv.rtw_mcc_sta_bw20_target_tx_tp = mcc_sta_bw20_target_tp;
+
+
+	}
+
+	return count;
+}
+
+ssize_t proc_set_mcc_sta_bw40_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[255];
+	u32 mcc_sta_bw40_target_tp = 0;
+
+	if (NULL == buffer) {
+		RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter));
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int num = sscanf(tmp, "%u", &mcc_sta_bw40_target_tp);
+
+		if (num < 1) {
+			RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter));
+			return -EINVAL;
+		}
+
+		RTW_INFO("%s: mcc_sta_bw40_target_tp = %d\n", __func__, mcc_sta_bw40_target_tp);
+
+		padapter->registrypriv.rtw_mcc_sta_bw40_target_tx_tp = mcc_sta_bw40_target_tp;
+
+
+	}
+
+	return count;
+}
+
+ssize_t proc_set_mcc_sta_bw80_target_tp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[255];
+	u32 mcc_sta_bw80_target_tp = 0;
+
+	if (NULL == buffer) {
+		RTW_INFO(FUNC_ADPT_FMT ": input buffer is NULL!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is 0!\n", FUNC_ADPT_ARG(padapter));
+		return -EFAULT;
+	}
+
+	if (count > sizeof(tmp)) {
+		RTW_INFO(FUNC_ADPT_FMT ": input length is too large\n", FUNC_ADPT_ARG(padapter));
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int num = sscanf(tmp, "%u", &mcc_sta_bw80_target_tp);
+
+		if (num < 1) {
+			RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter));
+			return -EINVAL;
+		}
+
+		RTW_INFO("%s: mcc_sta_bw80_target_tp = %d\n", __func__, mcc_sta_bw80_target_tp);
+
+		padapter->registrypriv.rtw_mcc_sta_bw80_target_tx_tp = mcc_sta_bw80_target_tp;
+
+
+	}
+
+	return count;
+}
+#endif /* CONFIG_MCC_MODE */
+
+int proc_get_ack_timeout(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	u8 ack_timeout_val, ack_timeout_val_cck;
+
+	ack_timeout_val = rtw_read8(padapter, REG_ACKTO);
+
+	RTW_PRINT_SEL(m, "Current ACK Timeout = %d us (0x%x).\n", ack_timeout_val, ack_timeout_val);
+
+	return 0;
+}
+
+ssize_t proc_set_ack_timeout(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 ack_timeout_ms, ack_timeout_ms_cck;
+
+	if (count > sizeof(tmp)) {
+		rtw_warn_on(1);
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, count)) {
+		int num = sscanf(tmp, "%u %u", &ack_timeout_ms, &ack_timeout_ms_cck);
+
+		if (num < 1) {
+			RTW_INFO(FUNC_ADPT_FMT ": input parameters < 1\n", FUNC_ADPT_ARG(padapter));
+			return -EINVAL;
+		}
+		/* This register sets the Ack time out value after Tx unicast packet. It is in units of us. */
+		rtw_write8(padapter, REG_ACKTO, (u8)ack_timeout_ms);
+
+		RTW_INFO("Set ACK Timeout to %d us.\n", ack_timeout_ms);
+	}
+
+	return count;
+}
+
+#endif /* CONFIG_PROC_DEBUG */
diff --git a/drivers/staging/rtl8188eu/core/rtw_efuse.c b/drivers/staging/rtl8188eu/core/rtw_efuse.c
new file mode 100644
index 000000000000..46ffa8d668e3
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_efuse.c
@@ -0,0 +1,1523 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_EFUSE_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#include "../hal/efuse_mask.h"
+
+/*------------------------Define local variable------------------------------*/
+u8	fakeEfuseBank = {0};
+u32	fakeEfuseUsedBytes = {0};
+u8	fakeEfuseContent[EFUSE_MAX_HW_SIZE] = {0};
+u8	fakeEfuseInitMap[EFUSE_MAX_MAP_LEN] = {0};
+u8	fakeEfuseModifiedMap[EFUSE_MAX_MAP_LEN] = {0};
+
+u32	BTEfuseUsedBytes = {0};
+u8	BTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+u8	BTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+u8	BTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+
+u32	fakeBTEfuseUsedBytes = {0};
+u8	fakeBTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+u8	fakeBTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+u8	fakeBTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+
+u8	maskfileBuffer[64];
+/*------------------------Define local variable------------------------------*/
+static bool rtw_file_efuse_IsMasked(PADAPTER pAdapter, u16 Offset)
+{
+	int r = Offset / 16;
+	int c = (Offset % 16) / 2;
+	int result = 0;
+
+	if (pAdapter->registrypriv.boffefusemask)
+		return false;
+
+	if (c < 4) /* Upper double word */
+		result = (maskfileBuffer[r] & (0x10 << c));
+	else
+		result = (maskfileBuffer[r] & (0x01 << (c - 4)));
+
+	return (result > 0) ? 0 : 1;
+}
+
+static bool efuse_IsMasked(PADAPTER pAdapter, u16 Offset)
+{
+	PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter);
+
+	if (pAdapter->registrypriv.boffefusemask)
+		return false;
+
+	if (IS_HARDWARE_TYPE_8188E(pAdapter))
+		return (IS_MASKED(8188E, _MUSB, Offset)) ? true : false;
+
+	return false;
+}
+
+void rtw_efuse_mask_array(PADAPTER pAdapter, u8 *pArray)
+{
+	PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter);
+
+	if (IS_HARDWARE_TYPE_8188E(pAdapter))
+		GET_MASK_ARRAY(8188E, _MUSB, pArray);
+}
+
+u16 rtw_get_efuse_mask_arraylen(PADAPTER pAdapter)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(pAdapter);
+
+	if (IS_HARDWARE_TYPE_8188E(pAdapter))
+		return GET_MASK_ARRAY_LEN(8188E, _MUSB);
+	return 0;
+}
+
+u8 rtw_efuse_mask_map_read(PADAPTER padapter, u16 addr, u16 cnts, u8 *data)
+{
+	u8	ret = _SUCCESS;
+	u16	mapLen = 0, i = 0;
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, false);
+
+	ret = rtw_efuse_map_read(padapter, addr, cnts , data);
+
+	if (padapter->registrypriv.boffefusemask == 0) {
+
+		for (i = 0; i < cnts; i++) {
+			if (padapter->registrypriv.bFileMaskEfuse == true) {
+				if (rtw_file_efuse_IsMasked(padapter, addr + i)) /*use file efuse mask.*/
+					data[i] = 0xff;
+			} else {
+				/*RTW_INFO(" %s , data[%d] = %x\n", __func__, i, data[i]);*/
+				if (efuse_IsMasked(padapter, addr + i)) {
+					data[i] = 0xff;
+					/*RTW_INFO(" %s ,mask data[%d] = %x\n", __func__, i, data[i]);*/
+				}
+			}
+		}
+
+	}
+	return ret;
+
+}
+
+
+/* ------------------------------------------------------------------------------ */
+#define REG_EFUSE_CTRL		0x0030
+#define EFUSE_CTRL			REG_EFUSE_CTRL		/* E-Fuse Control. */
+/* ------------------------------------------------------------------------------ */
+
+static void efuse_PreUpdateAction(
+	PADAPTER	pAdapter,
+	u32 *	BackupRegs)
+{
+}
+
+static void efuse_PostUpdateAction(
+	PADAPTER	pAdapter,
+	u32 *	BackupRegs)
+{
+}
+
+
+bool
+Efuse_Read1ByteFromFakeContent(
+		PADAPTER	pAdapter,
+		u16		Offset,
+	u8		*Value);
+bool
+Efuse_Read1ByteFromFakeContent(
+		PADAPTER	pAdapter,
+		u16		Offset,
+	u8		*Value)
+{
+	if (Offset >= EFUSE_MAX_HW_SIZE)
+		return false;
+	/* DbgPrint("Read fake content, offset = %d\n", Offset); */
+	if (fakeEfuseBank == 0)
+		*Value = fakeEfuseContent[Offset];
+	else
+		*Value = fakeBTEfuseContent[fakeEfuseBank - 1][Offset];
+	return true;
+}
+
+bool
+Efuse_Write1ByteToFakeContent(
+		PADAPTER	pAdapter,
+		u16		Offset,
+		u8		Value);
+bool
+Efuse_Write1ByteToFakeContent(
+		PADAPTER	pAdapter,
+		u16		Offset,
+		u8		Value)
+{
+	if (Offset >= EFUSE_MAX_HW_SIZE)
+		return false;
+	if (fakeEfuseBank == 0)
+		fakeEfuseContent[Offset] = Value;
+	else
+		fakeBTEfuseContent[fakeEfuseBank - 1][Offset] = Value;
+	return true;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	Efuse_PowerSwitch
+ *
+ * Overview:	When we want to enable write operation, we should change to
+ *				pwr on state. When we stop write, we should switch to 500k mode
+ *				and disable LDO 2.5V.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/17/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void
+Efuse_PowerSwitch(
+	PADAPTER	pAdapter,
+	u8		bWrite,
+	u8		PwrState)
+{
+	pAdapter->hal_func.EfusePowerSwitch(pAdapter, bWrite, PwrState);
+}
+
+void
+BTEfuse_PowerSwitch(
+	PADAPTER	pAdapter,
+	u8		bWrite,
+	u8		PwrState)
+{
+	if (pAdapter->hal_func.BTEfusePowerSwitch)
+		pAdapter->hal_func.BTEfusePowerSwitch(pAdapter, bWrite, PwrState);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	efuse_GetCurrentSize
+ *
+ * Overview:	Get current efuse size!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/16/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+u16
+Efuse_GetCurrentSize(
+	PADAPTER		pAdapter,
+	u8			efuseType,
+	bool		bPseudoTest)
+{
+	u16 ret = 0;
+
+	ret = pAdapter->hal_func.EfuseGetCurrentSize(pAdapter, efuseType, bPseudoTest);
+
+	return ret;
+}
+
+/*
+ *	Description:
+ *		Execute E-Fuse read byte operation.
+ *		Refered from SD1 Richard.
+ *
+ *	Assumption:
+ *		1. Boot from E-Fuse and successfully auto-load.
+ *		2. PASSIVE_LEVEL (USB interface)
+ *
+ *	Created by Roger, 2008.10.21.
+ *   */
+void
+ReadEFuseByte(
+	PADAPTER	Adapter,
+	u16			_offset,
+	u8			*pbuf,
+	bool	bPseudoTest)
+{
+	u32	value32;
+	u8	readbyte;
+	u16	retry;
+	/* u32 start=jiffies; */
+
+	if (bPseudoTest) {
+		Efuse_Read1ByteFromFakeContent(Adapter, _offset, pbuf);
+		return;
+	}
+	/* Write Address */
+	rtw_write8(Adapter, EFUSE_CTRL + 1, (_offset & 0xff));
+	readbyte = rtw_read8(Adapter, EFUSE_CTRL + 2);
+	rtw_write8(Adapter, EFUSE_CTRL + 2, ((_offset >> 8) & 0x03) | (readbyte & 0xfc));
+
+	/* Write bit 32 0 */
+	readbyte = rtw_read8(Adapter, EFUSE_CTRL + 3);
+	rtw_write8(Adapter, EFUSE_CTRL + 3, (readbyte & 0x7f));
+
+	/* Check bit 32 read-ready */
+	retry = 0;
+	value32 = rtw_read32(Adapter, EFUSE_CTRL);
+	/* while(!(((value32 >> 24) & 0xff) & 0x80)  && (retry<10)) */
+	while (!(((value32 >> 24) & 0xff) & 0x80)  && (retry < 10000)) {
+		value32 = rtw_read32(Adapter, EFUSE_CTRL);
+		retry++;
+	}
+
+	/* 20100205 Joseph: Add delay suggested by SD1 Victor. */
+	/* This fix the problem that Efuse read error in high temperature condition. */
+	/* Designer says that there shall be some delay after ready bit is set, or the */
+	/* result will always stay on last data we read. */
+	rtw_udelay_os(50);
+	value32 = rtw_read32(Adapter, EFUSE_CTRL);
+
+	*pbuf = (u8)(value32 & 0xff);
+	/* RTW_INFO("ReadEFuseByte _offset:%08u, in %d ms\n",_offset ,rtw_get_passing_time_ms(start)); */
+
+}
+
+/*
+ *	Description:
+ *		1. Execute E-Fuse read byte operation according as map offset and
+ *		    save to E-Fuse table.
+ *		2. Refered from SD1 Richard.
+ *
+ *	Assumption:
+ *		1. Boot from E-Fuse and successfully auto-load.
+ *		2. PASSIVE_LEVEL (USB interface)
+ *
+ *	Created by Roger, 2008.10.21.
+ *
+ *	2008/12/12 MH	1. Reorganize code flow and reserve bytes. and add description.
+ *					2. Add efuse utilization collect.
+ *	2008/12/22 MH	Read Efuse must check if we write section 1 data again!!! Sec1
+ *					write addr must be after sec5.
+ *   */
+
+void
+efuse_ReadEFuse(
+	PADAPTER	Adapter,
+	u8		efuseType,
+	u16		_offset,
+	u16		_size_byte,
+	u8	*pbuf,
+	bool	bPseudoTest
+);
+void
+efuse_ReadEFuse(
+	PADAPTER	Adapter,
+	u8		efuseType,
+	u16		_offset,
+	u16		_size_byte,
+	u8	*pbuf,
+	bool	bPseudoTest
+)
+{
+	Adapter->hal_func.ReadEFuse(Adapter, efuseType, _offset, _size_byte, pbuf, bPseudoTest);
+}
+
+void
+EFUSE_GetEfuseDefinition(
+		PADAPTER	pAdapter,
+		u8		efuseType,
+		u8		type,
+		void		*pOut,
+		bool		bPseudoTest
+)
+{
+	pAdapter->hal_func.EFUSEGetEfuseDefinition(pAdapter, efuseType, type, pOut, bPseudoTest);
+}
+
+
+/*  11/16/2008 MH Read one byte from real Efuse. */
+u8
+efuse_OneByteRead(
+	PADAPTER	pAdapter,
+	u16			addr,
+	u8			*data,
+	bool		bPseudoTest)
+{
+	u32	tmpidx = 0;
+	u8	bResult;
+	u8	readbyte;
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(pAdapter);
+
+	/* RTW_INFO("===> EFUSE_OneByteRead(), addr = %x\n", addr); */
+	/* RTW_INFO("===> EFUSE_OneByteRead() start, 0x34 = 0x%X\n", rtw_read32(pAdapter, EFUSE_TEST)); */
+
+	if (bPseudoTest) {
+		bResult = Efuse_Read1ByteFromFakeContent(pAdapter, addr, data);
+		return bResult;
+	}
+
+	if ((IS_VENDOR_8188E_I_CUT_SERIES(pAdapter)) ||
+	    (IS_CHIP_VENDOR_SMIC(pHalData->version_id))) {
+		/* <20130121, Kordan> For SMIC EFUSE specificatoin. */
+		/* 0x34[11]: SW force PGMEN input of efuse to high. (for the bank selected by 0x34[9:8])	 */
+		/* phy_set_mac_reg(pAdapter, 0x34, BIT11, 0); */
+		rtw_write16(pAdapter, 0x34, rtw_read16(pAdapter, 0x34) & (~BIT11));
+	}
+
+	/* -----------------e-fuse reg ctrl --------------------------------- */
+	/* address			 */
+	rtw_write8(pAdapter, EFUSE_CTRL + 1, (u8)(addr & 0xff));
+	rtw_write8(pAdapter, EFUSE_CTRL + 2, ((u8)((addr >> 8) & 0x03)) |
+		   (rtw_read8(pAdapter, EFUSE_CTRL + 2) & 0xFC));
+
+	/* rtw_write8(pAdapter, EFUSE_CTRL+3,  0x72); */ /* read cmd	 */
+	/* Write bit 32 0 */
+	readbyte = rtw_read8(pAdapter, EFUSE_CTRL + 3);
+	rtw_write8(pAdapter, EFUSE_CTRL + 3, (readbyte & 0x7f));
+
+	while (!(0x80 & rtw_read8(pAdapter, EFUSE_CTRL + 3)) && (tmpidx < 1000)) {
+		rtw_mdelay_os(1);
+		tmpidx++;
+	}
+	if (tmpidx < 100) {
+		*data = rtw_read8(pAdapter, EFUSE_CTRL);
+		bResult = true;
+	} else {
+		*data = 0xff;
+		bResult = false;
+		RTW_INFO("%s: [ERROR] addr=0x%x bResult=%d time out 1s !!!\n", __func__, addr, bResult);
+		RTW_INFO("%s: [ERROR] EFUSE_CTRL =0x%08x !!!\n", __func__, rtw_read32(pAdapter, EFUSE_CTRL));
+	}
+
+	return bResult;
+}
+
+/*  11/16/2008 MH Write one byte to reald Efuse. */
+u8
+efuse_OneByteWrite(
+	PADAPTER	pAdapter,
+	u16			addr,
+	u8			data,
+	bool		bPseudoTest)
+{
+	u8	tmpidx = 0;
+	u8	bResult = false;
+	u32 efuseValue = 0;
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(pAdapter);
+
+	/* RTW_INFO("===> EFUSE_OneByteWrite(), addr = %x data=%x\n", addr, data); */
+	/* RTW_INFO("===> EFUSE_OneByteWrite() start, 0x34 = 0x%X\n", rtw_read32(pAdapter, EFUSE_TEST)); */
+
+	if (bPseudoTest) {
+		bResult = Efuse_Write1ByteToFakeContent(pAdapter, addr, data);
+		return bResult;
+	}
+
+	Efuse_PowerSwitch(pAdapter, true, true);
+
+	/* -----------------e-fuse reg ctrl ---------------------------------	 */
+	/* address			 */
+
+
+	efuseValue = rtw_read32(pAdapter, EFUSE_CTRL);
+	efuseValue |= (BIT21 | BIT31);
+	efuseValue &= ~(0x3FFFF);
+	efuseValue |= ((addr << 8 | data) & 0x3FFFF);
+
+	/* <20130227, Kordan> 8192E MP chip A-cut had better not set 0x34[11] until B-Cut. */
+	if ((IS_VENDOR_8188E_I_CUT_SERIES(pAdapter)) ||
+	    (IS_CHIP_VENDOR_SMIC(pHalData->version_id))) {
+		/* <20130121, Kordan> For SMIC EFUSE specificatoin. */
+		/* 0x34[11]: SW force PGMEN input of efuse to high. (for the bank selected by 0x34[9:8]) */
+		/* phy_set_mac_reg(pAdapter, 0x34, BIT11, 1); */
+		rtw_write16(pAdapter, 0x34, rtw_read16(pAdapter, 0x34) | (BIT11));
+		rtw_write32(pAdapter, EFUSE_CTRL, 0x90600000 | ((addr << 8 | data)));
+	} else
+		rtw_write32(pAdapter, EFUSE_CTRL, efuseValue);
+
+	rtw_mdelay_os(1);
+
+	while ((0x80 &  rtw_read8(pAdapter, EFUSE_CTRL + 3)) && (tmpidx < 100)) {
+		rtw_mdelay_os(1);
+		tmpidx++;
+	}
+
+	if (tmpidx < 100)
+		bResult = true;
+	else {
+		bResult = false;
+		RTW_INFO("%s: [ERROR] addr=0x%x ,efuseValue=0x%x ,bResult=%d time out 1s !!!\n",
+			 __func__, addr, efuseValue, bResult);
+		RTW_INFO("%s: [ERROR] EFUSE_CTRL =0x%08x !!!\n", __func__, rtw_read32(pAdapter, EFUSE_CTRL));
+	}
+
+	/* disable Efuse program enable */
+	if ((IS_VENDOR_8188E_I_CUT_SERIES(pAdapter)) ||
+	    (IS_CHIP_VENDOR_SMIC(pHalData->version_id)))
+		phy_set_mac_reg(pAdapter, EFUSE_TEST, BIT(11), 0);
+
+	Efuse_PowerSwitch(pAdapter, true, false);
+
+	return bResult;
+}
+
+int
+Efuse_PgPacketRead(PADAPTER	pAdapter,
+		   u8			offset,
+		   u8			*data,
+		   bool		bPseudoTest)
+{
+	int	ret = 0;
+
+	ret =  pAdapter->hal_func.Efuse_PgPacketRead(pAdapter, offset, data, bPseudoTest);
+
+	return ret;
+}
+
+int
+Efuse_PgPacketWrite(PADAPTER	pAdapter,
+		    u8			offset,
+		    u8			word_en,
+		    u8			*data,
+		    bool		bPseudoTest)
+{
+	int ret;
+
+	ret =  pAdapter->hal_func.Efuse_PgPacketWrite(pAdapter, offset, word_en, data, bPseudoTest);
+
+	return ret;
+}
+
+
+static int
+Efuse_PgPacketWrite_BT(PADAPTER	pAdapter,
+		       u8			offset,
+		       u8			word_en,
+		       u8			*data,
+		       bool		bPseudoTest)
+{
+	int ret;
+
+	ret =  pAdapter->hal_func.Efuse_PgPacketWrite_BT(pAdapter, offset, word_en, data, bPseudoTest);
+
+	return ret;
+}
+
+
+u8
+Efuse_WordEnableDataWrite(PADAPTER	pAdapter,
+			  u16		efuse_addr,
+			  u8		word_en,
+			  u8		*data,
+			  bool		bPseudoTest)
+{
+	u8	ret = 0;
+
+	ret =  pAdapter->hal_func.Efuse_WordEnableDataWrite(pAdapter, efuse_addr, word_en, data, bPseudoTest);
+
+	return ret;
+}
+
+static u8 efuse_read8(PADAPTER padapter, u16 address, u8 *value)
+{
+	return efuse_OneByteRead(padapter, address, value, false);
+}
+
+static u8 efuse_write8(PADAPTER padapter, u16 address, u8 *value)
+{
+	return efuse_OneByteWrite(padapter, address, *value, false);
+}
+
+/*
+ * read/wirte raw efuse data
+ */
+u8 rtw_efuse_access(PADAPTER padapter, u8 bWrite, u16 start_addr, u16 cnts, u8 *data)
+{
+	int i = 0;
+	u16	real_content_len = 0, max_available_size = 0;
+	u8 res = _FAIL ;
+	u8(*rw8)(PADAPTER, u16, u8 *);
+	u32	backupRegs[4] = {0};
+
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_REAL_CONTENT_LEN, (void *)&real_content_len, false);
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (void *)&max_available_size, false);
+
+	if (start_addr > real_content_len)
+		return _FAIL;
+
+	if (bWrite) {
+		if ((start_addr + cnts) > max_available_size)
+			return _FAIL;
+		rw8 = &efuse_write8;
+	} else
+		rw8 = &efuse_read8;
+
+	efuse_PreUpdateAction(padapter, backupRegs);
+
+	Efuse_PowerSwitch(padapter, bWrite, true);
+
+	/* e-fuse one byte read / write */
+	for (i = 0; i < cnts; i++) {
+		if (start_addr >= real_content_len) {
+			res = _FAIL;
+			break;
+		}
+
+		res = rw8(padapter, start_addr++, data++);
+		if (_FAIL == res)
+			break;
+	}
+
+	Efuse_PowerSwitch(padapter, bWrite, false);
+
+	efuse_PostUpdateAction(padapter, backupRegs);
+
+	return res;
+}
+/* ------------------------------------------------------------------------------ */
+u16 efuse_GetMaxSize(PADAPTER padapter)
+{
+	u16	max_size;
+
+	max_size = 0;
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI , TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (void *)&max_size, false);
+	return max_size;
+}
+/* ------------------------------------------------------------------------------ */
+u8 efuse_GetCurrentSize(PADAPTER padapter, u16 *size)
+{
+	Efuse_PowerSwitch(padapter, false, true);
+	*size = Efuse_GetCurrentSize(padapter, EFUSE_WIFI, false);
+	Efuse_PowerSwitch(padapter, false, false);
+
+	return _SUCCESS;
+}
+/* ------------------------------------------------------------------------------ */
+u16 efuse_bt_GetMaxSize(PADAPTER padapter)
+{
+	u16	max_size;
+
+	max_size = 0;
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_BT , TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, (void *)&max_size, false);
+	return max_size;
+}
+
+u8 efuse_bt_GetCurrentSize(PADAPTER padapter, u16 *size)
+{
+	Efuse_PowerSwitch(padapter, false, true);
+	*size = Efuse_GetCurrentSize(padapter, EFUSE_BT, false);
+	Efuse_PowerSwitch(padapter, false, false);
+
+	return _SUCCESS;
+}
+
+u8 rtw_efuse_map_read(PADAPTER padapter, u16 addr, u16 cnts, u8 *data)
+{
+	u16	mapLen = 0;
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, false);
+
+	if ((addr + cnts) > mapLen)
+		return _FAIL;
+
+	Efuse_PowerSwitch(padapter, false, true);
+
+	efuse_ReadEFuse(padapter, EFUSE_WIFI, addr, cnts, data, false);
+
+	Efuse_PowerSwitch(padapter, false, false);
+
+	return _SUCCESS;
+}
+
+u8 rtw_BT_efuse_map_read(PADAPTER padapter, u16 addr, u16 cnts, u8 *data)
+{
+	u16	mapLen = 0;
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_BT, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, false);
+
+	if ((addr + cnts) > mapLen)
+		return _FAIL;
+
+	Efuse_PowerSwitch(padapter, false, true);
+
+	efuse_ReadEFuse(padapter, EFUSE_BT, addr, cnts, data, false);
+
+	Efuse_PowerSwitch(padapter, false, false);
+
+	return _SUCCESS;
+}
+
+/* ------------------------------------------------------------------------------ */
+u8 rtw_efuse_map_write(PADAPTER padapter, u16 addr, u16 cnts, u8 *data)
+{
+#define RT_ASSERT_RET(expr)												\
+	if (!(expr)) {															\
+		RTW_INFO("Assertion failed! %s at ......\n", #expr);							\
+		RTW_INFO("      ......%s,%s, line=%d\n",__FILE__, __func__, __LINE__);	\
+		return _FAIL;	\
+	}
+
+	u8	offset, word_en;
+	u8	*map;
+	u8	newdata[PGPKT_DATA_SIZE];
+	s32	i, j, idx, chk_total_byte;
+	u8	ret = _SUCCESS;
+	u16	mapLen = 0, startAddr = 0, efuse_max_available_len = 0;
+	u32	backupRegs[4] = {0};
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	PEFUSE_HAL	pEfuseHal = &pHalData->EfuseHal;
+
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, false);
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &efuse_max_available_len, false);
+
+	if ((addr + cnts) > mapLen)
+		return _FAIL;
+
+	RT_ASSERT_RET(PGPKT_DATA_SIZE == 8); /* have to be 8 byte alignment */
+	RT_ASSERT_RET((mapLen & 0x7) == 0); /* have to be PGPKT_DATA_SIZE alignment for memcpy */
+
+	map = rtw_zmalloc(mapLen);
+	if (map == NULL)
+		return _FAIL;
+
+	memset(map, 0xFF, mapLen);
+
+	ret = rtw_efuse_map_read(padapter, 0, mapLen, map);
+	if (ret == _FAIL)
+		goto exit;
+
+	if (padapter->registrypriv.boffefusemask == 0) {
+		for (i = 0; i < cnts; i++) {
+			if (padapter->registrypriv.bFileMaskEfuse == true) {
+				if (rtw_file_efuse_IsMasked(padapter, addr + i))	/*use file efuse mask. */
+					data[i] = map[addr + i];
+			} else {
+				if (efuse_IsMasked(padapter, addr + i))
+					data[i] = map[addr + i];
+			}
+			RTW_INFO("%s , data[%d] = %x, map[addr+i]= %x\n", __func__, i, data[i], map[addr + i]);
+		}
+	}
+	/*Efuse_PowerSwitch(padapter, true, true);*/
+
+	chk_total_byte = 0;
+	idx = 0;
+	offset = (addr >> 3);
+
+	while (idx < cnts) {
+		word_en = 0xF;
+		j = (addr + idx) & 0x7;
+		for (i = j; i < PGPKT_DATA_SIZE && idx < cnts; i++, idx++) {
+			if (data[idx] != map[addr + idx])
+				word_en &= ~BIT(i >> 1);
+		}
+
+		if (word_en != 0xF) {
+			chk_total_byte += Efuse_CalculateWordCnts(word_en) * 2;
+
+			if (offset >= EFUSE_MAX_SECTION_BASE) /* Over EFUSE_MAX_SECTION 16 for 2 ByteHeader */
+				chk_total_byte += 2;
+			else
+				chk_total_byte += 1;
+		}
+
+		offset++;
+	}
+
+	RTW_INFO("Total PG bytes Count = %d\n", chk_total_byte);
+	rtw_hal_get_hwreg(padapter, HW_VAR_EFUSE_BYTES, (u8 *)&startAddr);
+
+	if (startAddr == 0) {
+		startAddr = Efuse_GetCurrentSize(padapter, EFUSE_WIFI, false);
+		RTW_INFO("%s: Efuse_GetCurrentSize startAddr=%#X\n", __func__, startAddr);
+	}
+	RTW_DBG("%s: startAddr=%#X\n", __func__, startAddr);
+
+	if ((startAddr + chk_total_byte) >= efuse_max_available_len) {
+		RTW_INFO("%s: startAddr(0x%X) + PG data len %d >= efuse_max_available_len(0x%X)\n",
+			 __func__, startAddr, chk_total_byte, efuse_max_available_len);
+		ret = _FAIL;
+		goto exit;
+	}
+
+	efuse_PreUpdateAction(padapter, backupRegs);
+
+	idx = 0;
+	offset = (addr >> 3);
+	while (idx < cnts) {
+		word_en = 0xF;
+		j = (addr + idx) & 0x7;
+		memcpy(newdata, &map[offset << 3], PGPKT_DATA_SIZE);
+		for (i = j; i < PGPKT_DATA_SIZE && idx < cnts; i++, idx++) {
+			if (data[idx] != map[addr + idx]) {
+				word_en &= ~BIT(i >> 1);
+				newdata[i] = data[idx];
+			}
+		}
+
+		if (word_en != 0xF) {
+			ret = Efuse_PgPacketWrite(padapter, offset, word_en, newdata, false);
+			RTW_INFO("offset=%x\n", offset);
+			RTW_INFO("word_en=%x\n", word_en);
+
+			for (i = 0; i < PGPKT_DATA_SIZE; i++)
+				RTW_INFO("data=%x \t", newdata[i]);
+			if (ret == _FAIL)
+				break;
+		}
+
+		offset++;
+	}
+
+	/*Efuse_PowerSwitch(padapter, true, false);*/
+
+	efuse_PostUpdateAction(padapter, backupRegs);
+
+exit:
+
+	rtw_mfree(map, mapLen);
+
+	return ret;
+}
+
+
+u8 rtw_BT_efuse_map_write(PADAPTER padapter, u16 addr, u16 cnts, u8 *data)
+{
+#define RT_ASSERT_RET(expr)												\
+	if (!(expr)) {															\
+		RTW_INFO("Assertion failed! %s at ......\n", #expr);							\
+		RTW_INFO("      ......%s,%s, line=%d\n",__FILE__, __func__, __LINE__);	\
+		return _FAIL;	\
+	}
+
+	u8	offset, word_en;
+	u8	*map;
+	u8	newdata[PGPKT_DATA_SIZE];
+	s32	i = 0, j = 0, idx;
+	u8	ret = _SUCCESS;
+	u16	mapLen = 0;
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_BT, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, false);
+
+	if ((addr + cnts) > mapLen)
+		return _FAIL;
+
+	RT_ASSERT_RET(PGPKT_DATA_SIZE == 8); /* have to be 8 byte alignment */
+	RT_ASSERT_RET((mapLen & 0x7) == 0); /* have to be PGPKT_DATA_SIZE alignment for memcpy */
+
+	map = rtw_zmalloc(mapLen);
+	if (map == NULL)
+		return _FAIL;
+
+	ret = rtw_BT_efuse_map_read(padapter, 0, mapLen, map);
+	if (ret == _FAIL)
+		goto exit;
+	RTW_INFO("OFFSET\tVALUE(hex)\n");
+	for (i = 0; i < 1024; i += 16) { /* set 512 because the iwpriv's extra size have limit 0x7FF */
+		RTW_INFO("0x%03x\t", i);
+		for (j = 0; j < 8; j++)
+			RTW_INFO("%02X ", map[i + j]);
+		RTW_INFO("\t");
+		for (; j < 16; j++)
+			RTW_INFO("%02X ", map[i + j]);
+		RTW_INFO("\n");
+	}
+	RTW_INFO("\n");
+	Efuse_PowerSwitch(padapter, true, true);
+
+	idx = 0;
+	offset = (addr >> 3);
+	while (idx < cnts) {
+		word_en = 0xF;
+		j = (addr + idx) & 0x7;
+		memcpy(newdata, &map[offset << 3], PGPKT_DATA_SIZE);
+		for (i = j; i < PGPKT_DATA_SIZE && idx < cnts; i++, idx++) {
+			if (data[idx] != map[addr + idx]) {
+				word_en &= ~BIT(i >> 1);
+				newdata[i] = data[idx];
+			}
+		}
+
+		if (word_en != 0xF) {
+			RTW_INFO("offset=%x\n", offset);
+			RTW_INFO("word_en=%x\n", word_en);
+			RTW_INFO("%s: data=", __func__);
+			for (i = 0; i < PGPKT_DATA_SIZE; i++)
+				RTW_INFO("0x%02X ", newdata[i]);
+			RTW_INFO("\n");
+			ret = Efuse_PgPacketWrite_BT(padapter, offset, word_en, newdata, false);
+			if (ret == _FAIL)
+				break;
+		}
+
+		offset++;
+	}
+
+	Efuse_PowerSwitch(padapter, true, false);
+
+exit:
+
+	rtw_mfree(map, mapLen);
+
+	return ret;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	Efuse_ReadAllMap
+ *
+ * Overview:	Read All Efuse content
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/11/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void
+Efuse_ReadAllMap(
+		PADAPTER	pAdapter,
+		u8		efuseType,
+	u8		*Efuse,
+		bool		bPseudoTest);
+void
+Efuse_ReadAllMap(
+		PADAPTER	pAdapter,
+		u8		efuseType,
+	u8		*Efuse,
+		bool		bPseudoTest)
+{
+	u16	mapLen = 0;
+
+	Efuse_PowerSwitch(pAdapter, false, true);
+
+	EFUSE_GetEfuseDefinition(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, bPseudoTest);
+
+	efuse_ReadEFuse(pAdapter, efuseType, 0, mapLen, Efuse, bPseudoTest);
+
+	Efuse_PowerSwitch(pAdapter, false, false);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	efuse_ShadowRead1Byte
+ *			efuse_ShadowRead2Byte
+ *			efuse_ShadowRead4Byte
+ *
+ * Overview:	Read from efuse init map by one/two/four bytes !!!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/12/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+static void
+efuse_ShadowRead1Byte(
+	PADAPTER	pAdapter,
+	u16		Offset,
+	u8		*Value)
+{
+	PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter);
+
+	*Value = pHalData->efuse_eeprom_data[Offset];
+
+}	/* EFUSE_ShadowRead1Byte */
+
+/* ---------------Read Two Bytes */
+static void
+efuse_ShadowRead2Byte(
+	PADAPTER	pAdapter,
+	u16		Offset,
+	u16		*Value)
+{
+	PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter);
+
+	*Value = pHalData->efuse_eeprom_data[Offset];
+	*Value |= pHalData->efuse_eeprom_data[Offset + 1] << 8;
+
+}	/* EFUSE_ShadowRead2Byte */
+
+/* ---------------Read Four Bytes */
+static void
+efuse_ShadowRead4Byte(
+	PADAPTER	pAdapter,
+	u16		Offset,
+	u32		*Value)
+{
+	PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter);
+
+	*Value = pHalData->efuse_eeprom_data[Offset];
+	*Value |= pHalData->efuse_eeprom_data[Offset + 1] << 8;
+	*Value |= pHalData->efuse_eeprom_data[Offset + 2] << 16;
+	*Value |= pHalData->efuse_eeprom_data[Offset + 3] << 24;
+
+}	/* efuse_ShadowRead4Byte */
+
+
+/*-----------------------------------------------------------------------------
+ * Function:	efuse_ShadowWrite1Byte
+ *			efuse_ShadowWrite2Byte
+ *			efuse_ShadowWrite4Byte
+ *
+ * Overview:	Write efuse modify map by one/two/four byte.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/12/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+
+static void
+efuse_ShadowWrite1Byte(
+	PADAPTER	pAdapter,
+	u16		Offset,
+	u8		Value)
+{
+	PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter);
+
+	pHalData->efuse_eeprom_data[Offset] = Value;
+
+}	/* efuse_ShadowWrite1Byte */
+
+/* ---------------Write Two Bytes */
+static void
+efuse_ShadowWrite2Byte(
+	PADAPTER	pAdapter,
+	u16		Offset,
+	u16		Value)
+{
+
+	PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter);
+
+
+	pHalData->efuse_eeprom_data[Offset] = Value & 0x00FF;
+	pHalData->efuse_eeprom_data[Offset + 1] = Value >> 8;
+
+}	/* efuse_ShadowWrite1Byte */
+
+/* ---------------Write Four Bytes */
+static void
+efuse_ShadowWrite4Byte(
+	PADAPTER	pAdapter,
+	u16		Offset,
+	u32		Value)
+{
+	PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter);
+
+	pHalData->efuse_eeprom_data[Offset] = (u8)(Value & 0x000000FF);
+	pHalData->efuse_eeprom_data[Offset + 1] = (u8)((Value >> 8) & 0x0000FF);
+	pHalData->efuse_eeprom_data[Offset + 2] = (u8)((Value >> 16) & 0x00FF);
+	pHalData->efuse_eeprom_data[Offset + 3] = (u8)((Value >> 24) & 0xFF);
+
+}	/* efuse_ShadowWrite1Byte */
+
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_ShadowRead
+ *
+ * Overview:	Read from efuse init map !!!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/12/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void
+EFUSE_ShadowRead(
+		PADAPTER	pAdapter,
+		u8		Type,
+		u16		Offset,
+	u32		*Value)
+{
+	if (Type == 1)
+		efuse_ShadowRead1Byte(pAdapter, Offset, (u8 *)Value);
+	else if (Type == 2)
+		efuse_ShadowRead2Byte(pAdapter, Offset, (u16 *)Value);
+	else if (Type == 4)
+		efuse_ShadowRead4Byte(pAdapter, Offset, (u32 *)Value);
+
+}	/* EFUSE_ShadowRead */
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_ShadowWrite
+ *
+ * Overview:	Write efuse modify map for later update operation to use!!!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/12/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void
+EFUSE_ShadowWrite(
+	PADAPTER	pAdapter,
+	u8		Type,
+	u16		Offset,
+	u32		Value);
+void
+EFUSE_ShadowWrite(
+	PADAPTER	pAdapter,
+	u8		Type,
+	u16		Offset,
+	u32		Value)
+{
+#if (MP_DRIVER == 0)
+	return;
+#endif
+	if (pAdapter->registrypriv.mp_mode == 0)
+		return;
+
+
+	if (Type == 1)
+		efuse_ShadowWrite1Byte(pAdapter, Offset, (u8)Value);
+	else if (Type == 2)
+		efuse_ShadowWrite2Byte(pAdapter, Offset, (u16)Value);
+	else if (Type == 4)
+		efuse_ShadowWrite4Byte(pAdapter, Offset, (u32)Value);
+
+}	/* EFUSE_ShadowWrite */
+
+void
+Efuse_InitSomeVar(
+		PADAPTER	pAdapter
+);
+void
+Efuse_InitSomeVar(
+		PADAPTER	pAdapter
+)
+{
+	u8 i;
+
+	memset((void *)&fakeEfuseContent[0], 0xff, EFUSE_MAX_HW_SIZE);
+	memset((void *)&fakeEfuseInitMap[0], 0xff, EFUSE_MAX_MAP_LEN);
+	memset((void *)&fakeEfuseModifiedMap[0], 0xff, EFUSE_MAX_MAP_LEN);
+
+	for (i = 0; i < EFUSE_MAX_BT_BANK; i++)
+		memset((void *)&BTEfuseContent[i][0], 0xff, EFUSE_MAX_HW_SIZE);
+	memset((void *)&BTEfuseInitMap[0], 0xff, EFUSE_BT_MAX_MAP_LEN);
+	memset((void *)&BTEfuseModifiedMap[0], 0xff, EFUSE_BT_MAX_MAP_LEN);
+
+	for (i = 0; i < EFUSE_MAX_BT_BANK; i++)
+		memset((void *)&fakeBTEfuseContent[i][0], 0xff, EFUSE_MAX_HW_SIZE);
+	memset((void *)&fakeBTEfuseInitMap[0], 0xff, EFUSE_BT_MAX_MAP_LEN);
+	memset((void *)&fakeBTEfuseModifiedMap[0], 0xff, EFUSE_BT_MAX_MAP_LEN);
+}
+
+/*  11/16/2008 MH Add description. Get current efuse area enabled word!!. */
+u8
+Efuse_CalculateWordCnts(u8	word_en)
+{
+	u8 word_cnts = 0;
+	if (!(word_en & BIT(0)))
+		word_cnts++; /* 0 : write enable */
+	if (!(word_en & BIT(1)))
+		word_cnts++;
+	if (!(word_en & BIT(2)))
+		word_cnts++;
+	if (!(word_en & BIT(3)))
+		word_cnts++;
+	return word_cnts;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	efuse_WordEnableDataRead
+ *
+ * Overview:	Read allowed word in current efuse section data.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/16/2008	MHC		Create Version 0.
+ * 11/21/2008	MHC		Fix Write bug when we only enable late word.
+ *
+ *---------------------------------------------------------------------------*/
+void
+efuse_WordEnableDataRead(u8	word_en,
+			 u8	*sourdata,
+			 u8	*targetdata)
+{
+	if (!(word_en & BIT(0))) {
+		targetdata[0] = sourdata[0];
+		targetdata[1] = sourdata[1];
+	}
+	if (!(word_en & BIT(1))) {
+		targetdata[2] = sourdata[2];
+		targetdata[3] = sourdata[3];
+	}
+	if (!(word_en & BIT(2))) {
+		targetdata[4] = sourdata[4];
+		targetdata[5] = sourdata[5];
+	}
+	if (!(word_en & BIT(3))) {
+		targetdata[6] = sourdata[6];
+		targetdata[7] = sourdata[7];
+	}
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_ShadowMapUpdate
+ *
+ * Overview:	Transfer current EFUSE content to shadow init and modify map.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/13/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void EFUSE_ShadowMapUpdate(
+	PADAPTER	pAdapter,
+	u8		efuseType,
+	bool	bPseudoTest)
+{
+	PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter);
+	u16	mapLen = 0;
+	EFUSE_GetEfuseDefinition(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, bPseudoTest);
+
+	if (pHalData->bautoload_fail_flag == true)
+		memset(pHalData->efuse_eeprom_data, 0xFF, mapLen);
+	else {
+#ifdef CONFIG_ADAPTOR_INFO_CACHING_FILE
+		if (_SUCCESS != retriveAdaptorInfoFile(pAdapter->registrypriv.adaptor_info_caching_file_path, pHalData->efuse_eeprom_data)) {
+#endif
+
+			Efuse_ReadAllMap(pAdapter, efuseType, pHalData->efuse_eeprom_data, bPseudoTest);
+
+#ifdef CONFIG_ADAPTOR_INFO_CACHING_FILE
+			storeAdaptorInfoFile(pAdapter->registrypriv.adaptor_info_caching_file_path, pHalData->efuse_eeprom_data);
+		}
+#endif
+	}
+
+	rtw_dump_cur_efuse(pAdapter);
+} /* EFUSE_ShadowMapUpdate */
+
+const u8 _mac_hidden_max_bw_to_hal_bw_cap[MAC_HIDDEN_MAX_BW_NUM] = {
+	0,
+	0,
+	(BW_CAP_160M | BW_CAP_80M | BW_CAP_40M | BW_CAP_20M | BW_CAP_10M | BW_CAP_5M),
+	(BW_CAP_5M),
+	(BW_CAP_10M | BW_CAP_5M),
+	(BW_CAP_20M | BW_CAP_10M | BW_CAP_5M),
+	(BW_CAP_40M | BW_CAP_20M | BW_CAP_10M | BW_CAP_5M),
+	(BW_CAP_80M | BW_CAP_40M | BW_CAP_20M | BW_CAP_10M | BW_CAP_5M),
+};
+
+const u8 _mac_hidden_proto_to_hal_proto_cap[MAC_HIDDEN_PROTOCOL_NUM] = {
+	0,
+	0,
+	(PROTO_CAP_11N | PROTO_CAP_11G | PROTO_CAP_11B),
+	(PROTO_CAP_11AC | PROTO_CAP_11N | PROTO_CAP_11G | PROTO_CAP_11B),
+};
+
+u8 mac_hidden_wl_func_to_hal_wl_func(u8 func)
+{
+	u8 wl_func = 0;
+
+	if (func & BIT0)
+		wl_func |= WL_FUNC_MIRACAST;
+	if (func & BIT1)
+		wl_func |= WL_FUNC_P2P;
+	if (func & BIT2)
+		wl_func |= WL_FUNC_TDLS;
+	if (func & BIT3)
+		wl_func |= WL_FUNC_FTM;
+
+	return wl_func;
+}
+
+#ifdef CONFIG_ADAPTOR_INFO_CACHING_FILE
+
+int isAdaptorInfoFileValid(void)
+{
+	return true;
+}
+
+int storeAdaptorInfoFile(char *path, u8 *efuse_data)
+{
+	int ret = _SUCCESS;
+
+	if (path && efuse_data) {
+		ret = rtw_store_to_file(path, efuse_data, EEPROM_MAX_SIZE_512);
+		if (ret == EEPROM_MAX_SIZE)
+			ret = _SUCCESS;
+		else
+			ret = _FAIL;
+	} else {
+		RTW_INFO("%s NULL pointer\n", __func__);
+		ret =  _FAIL;
+	}
+	return ret;
+}
+
+int retriveAdaptorInfoFile(char *path, u8 *efuse_data)
+{
+	int ret = _SUCCESS;
+	mm_segment_t oldfs;
+	struct file *fp;
+
+	if (path && efuse_data) {
+
+		ret = rtw_retrieve_from_file(path, efuse_data, EEPROM_MAX_SIZE);
+
+		if (ret == EEPROM_MAX_SIZE)
+			ret = _SUCCESS;
+		else
+			ret = _FAIL;
+	} else {
+		RTW_INFO("%s NULL pointer\n", __func__);
+		ret = _FAIL;
+	}
+	return ret;
+}
+#endif /* CONFIG_ADAPTOR_INFO_CACHING_FILE */
+
+u8 rtw_efuse_file_read(PADAPTER padapter, u8 *filepatch, u8 *buf, u32 len)
+{
+	char *ptmpbuf = NULL, *ptr;
+	u8 val8;
+	u32 count, i, j;
+	int err;
+	u32 bufsize = 4096;
+
+	ptmpbuf = rtw_zmalloc(bufsize);
+	if (ptmpbuf == NULL)
+		return false;
+
+	count = rtw_retrieve_from_file(filepatch, ptmpbuf, bufsize);
+	if (count <= 100) {
+		rtw_mfree(ptmpbuf, bufsize);
+		RTW_ERR("%s, filepatch %s, size=%d, FAIL!!\n", __func__, filepatch, count);
+		return false;
+	}
+
+	i = 0;
+	j = 0;
+	ptr = ptmpbuf;
+	while ((j < len) && (i < count)) {
+		if (ptmpbuf[i] == '\0')
+			break;
+	
+		ptr = strpbrk(&ptmpbuf[i], " \t\n\r");
+		if (ptr) {
+			if (ptr == &ptmpbuf[i]) {
+				i++;
+				continue;
+			}
+
+			/* Add string terminating null */
+			*ptr = 0;
+		} else {
+			ptr = &ptmpbuf[count-1];
+		}
+
+		err = sscanf(&ptmpbuf[i], "%hhx", &val8);
+		if (err != 1) {
+			RTW_WARN("Something wrong to parse efuse file, string=%s\n", &ptmpbuf[i]);
+		} else {
+			buf[j] = val8;
+			RTW_DBG("i=%d, j=%d, 0x%02x\n", i, j, buf[j]);
+			j++;
+		}
+
+		i = ptr - ptmpbuf + 1;
+	}
+
+	rtw_mfree(ptmpbuf, bufsize);
+	RTW_INFO("%s, filepatch %s, size=%d, done\n", __func__, filepatch, count);
+	return true;
+}
+
+#ifdef CONFIG_EFUSE_CONFIG_FILE
+u32 rtw_read_efuse_from_file(const char *path, u8 *buf, int map_size)
+{
+	u32 i;
+	u8 c;
+	u8 temp[3];
+	u8 temp_i;
+	u8 end = false;
+	u32 ret = _FAIL;
+
+	u8 *file_data = NULL;
+	u32 file_size, read_size, pos = 0;
+	u8 *map = NULL;
+
+	if (rtw_is_file_readable_with_size(path, &file_size) != true) {
+		RTW_PRINT("%s %s is not readable\n", __func__, path);
+		goto exit;
+	}
+
+	file_data = vmalloc(file_size);
+	if (!file_data) {
+		RTW_ERR("%s vmalloc(%d) fail\n", __func__, file_size);
+		goto exit;
+	}
+
+	read_size = rtw_retrieve_from_file(path, file_data, file_size);
+	if (read_size == 0) {
+		RTW_ERR("%s read from %s fail\n", __func__, path);
+		goto exit;
+	}
+
+	map = vmalloc(map_size);
+	if (!map) {
+		RTW_ERR("%s vmalloc(%d) fail\n", __func__, map_size);
+		goto exit;
+	}
+	memset(map, 0xff, map_size);
+
+	temp[2] = 0; /* end of string '\0' */
+
+	for (i = 0 ; i < map_size ; i++) {
+		temp_i = 0;
+
+		while (1) {
+			if (pos >= read_size) {
+				end = true;
+				break;
+			}
+			c = file_data[pos++];
+
+			/* bypass spece or eol or null before first hex digit */
+			if (temp_i == 0 && (is_eol(c) == true || is_space(c) == true || is_null(c) == true))
+				continue;
+
+			if (IsHexDigit(c) == false) {
+				RTW_ERR("%s invalid 8-bit hex format for offset:0x%03x\n", __func__, i);
+				goto exit;
+			}
+
+			temp[temp_i++] = c;
+
+			if (temp_i == 2) {
+				/* parse value */
+				if (sscanf(temp, "%hhx", &map[i]) != 1) {
+					RTW_ERR("%s sscanf fail for offset:0x%03x\n", __func__, i);
+					goto exit;
+				}
+				break;
+			}
+		}
+
+		if (end == true) {
+			if (temp_i != 0) {
+				RTW_ERR("%s incomplete 8-bit hex format for offset:0x%03x\n", __func__, i);
+				goto exit;
+			}
+			break;
+		}
+	}
+
+	RTW_PRINT("efuse file:%s, 0x%03x byte content read\n", path, i);
+
+	memcpy(buf, map, map_size);
+
+	ret = _SUCCESS;
+
+exit:
+	if (file_data)
+		rtw_vmfree(file_data, file_size);
+	if (map)
+		rtw_vmfree(map, map_size);
+
+	return ret;
+}
+
+u32 rtw_read_macaddr_from_file(const char *path, u8 *buf)
+{
+	u32 i;
+	u8 temp[3];
+	u32 ret = _FAIL;
+
+	u8 file_data[17];
+	u32 read_size, pos = 0;
+	u8 addr[ETH_ALEN];
+
+	if (rtw_is_file_readable(path) != true) {
+		RTW_PRINT("%s %s is not readable\n", __func__, path);
+		goto exit;
+	}
+
+	read_size = rtw_retrieve_from_file(path, file_data, 17);
+	if (read_size != 17) {
+		RTW_ERR("%s read from %s fail\n", __func__, path);
+		goto exit;
+	}
+
+	temp[2] = 0; /* end of string '\0' */
+
+	for (i = 0 ; i < ETH_ALEN ; i++) {
+		if (IsHexDigit(file_data[i * 3]) == false || IsHexDigit(file_data[i * 3 + 1]) == false) {
+			RTW_ERR("%s invalid 8-bit hex format for address offset:%u\n", __func__, i);
+			goto exit;
+		}
+
+		if (i < ETH_ALEN - 1 && file_data[i * 3 + 2] != ':') {
+			RTW_ERR("%s invalid separator after address offset:%u\n", __func__, i);
+			goto exit;
+		}
+
+		temp[0] = file_data[i * 3];
+		temp[1] = file_data[i * 3 + 1];
+		if (sscanf(temp, "%hhx", &addr[i]) != 1) {
+			RTW_ERR("%s sscanf fail for address offset:0x%03x\n", __func__, i);
+			goto exit;
+		}
+	}
+
+	memcpy(buf, addr, ETH_ALEN);
+
+	RTW_PRINT("wifi_mac file: %s\n", path);
+#ifdef CONFIG_RTW_DEBUG
+	RTW_INFO(MAC_FMT"\n", MAC_ARG(buf));
+#endif
+
+	ret = _SUCCESS;
+
+exit:
+	return ret;
+}
+#endif /* CONFIG_EFUSE_CONFIG_FILE */
diff --git a/drivers/staging/rtl8188eu/core/rtw_ieee80211.c b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
new file mode 100644
index 000000000000..33636425c345
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
@@ -0,0 +1,2673 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _IEEE80211_C
+
+#ifdef CONFIG_PLATFORM_INTEL_BYT
+	#include <linux/fs.h>
+#endif
+#include <drv_types.h>
+#include <hal_data.h>
+
+u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 };
+u16 RTW_WPA_VERSION = 1;
+u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_CIPHER_SUITE_WEP40[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_CIPHER_SUITE_TKIP[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_WRAP[] = { 0x00, 0x50, 0xf2, 3 };
+u8 WPA_CIPHER_SUITE_CCMP[] = { 0x00, 0x50, 0xf2, 4 };
+u8 WPA_CIPHER_SUITE_WEP104[] = { 0x00, 0x50, 0xf2, 5 };
+
+u16 RSN_VERSION_BSD = 1;
+u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_NONE[] = { 0x00, 0x0f, 0xac, 0 };
+u8 RSN_CIPHER_SUITE_WEP40[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_CIPHER_SUITE_TKIP[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_WRAP[] = { 0x00, 0x0f, 0xac, 3 };
+u8 RSN_CIPHER_SUITE_CCMP[] = { 0x00, 0x0f, 0xac, 4 };
+u8 RSN_CIPHER_SUITE_WEP104[] = { 0x00, 0x0f, 0xac, 5 };
+/* -----------------------------------------------------------
+ * for adhoc-master to generate ie and provide supported-rate to fw
+ * ----------------------------------------------------------- */
+
+static u8	WIFI_CCKRATES[] = {
+	(IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
+	(IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
+	(IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
+	(IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)
+};
+
+static u8	WIFI_OFDMRATES[] = {
+	(IEEE80211_OFDM_RATE_6MB),
+	(IEEE80211_OFDM_RATE_9MB),
+	(IEEE80211_OFDM_RATE_12MB),
+	(IEEE80211_OFDM_RATE_18MB),
+	(IEEE80211_OFDM_RATE_24MB),
+	IEEE80211_OFDM_RATE_36MB,
+	IEEE80211_OFDM_RATE_48MB,
+	IEEE80211_OFDM_RATE_54MB
+};
+
+u8 mgn_rates_cck[4] = {MGN_1M, MGN_2M, MGN_5_5M, MGN_11M};
+u8 mgn_rates_ofdm[8] = {MGN_6M, MGN_9M, MGN_12M, MGN_18M, MGN_24M, MGN_36M, MGN_48M, MGN_54M};
+u8 mgn_rates_mcs0_7[8] = {MGN_MCS0, MGN_MCS1, MGN_MCS2, MGN_MCS3, MGN_MCS4, MGN_MCS5, MGN_MCS6, MGN_MCS7};
+u8 mgn_rates_mcs8_15[8] = {MGN_MCS8, MGN_MCS9, MGN_MCS10, MGN_MCS11, MGN_MCS12, MGN_MCS13, MGN_MCS14, MGN_MCS15};
+u8 mgn_rates_mcs16_23[8] = {MGN_MCS16, MGN_MCS17, MGN_MCS18, MGN_MCS19, MGN_MCS20, MGN_MCS21, MGN_MCS22, MGN_MCS23};
+u8 mgn_rates_mcs24_31[8] = {MGN_MCS24, MGN_MCS25, MGN_MCS26, MGN_MCS27, MGN_MCS28, MGN_MCS29, MGN_MCS30, MGN_MCS31};
+u8 mgn_rates_vht1ss[10] = {MGN_VHT1SS_MCS0, MGN_VHT1SS_MCS1, MGN_VHT1SS_MCS2, MGN_VHT1SS_MCS3, MGN_VHT1SS_MCS4
+	, MGN_VHT1SS_MCS5, MGN_VHT1SS_MCS6, MGN_VHT1SS_MCS7, MGN_VHT1SS_MCS8, MGN_VHT1SS_MCS9
+			  };
+u8 mgn_rates_vht2ss[10] = {MGN_VHT2SS_MCS0, MGN_VHT2SS_MCS1, MGN_VHT2SS_MCS2, MGN_VHT2SS_MCS3, MGN_VHT2SS_MCS4
+	, MGN_VHT2SS_MCS5, MGN_VHT2SS_MCS6, MGN_VHT2SS_MCS7, MGN_VHT2SS_MCS8, MGN_VHT2SS_MCS9
+			  };
+u8 mgn_rates_vht3ss[10] = {MGN_VHT3SS_MCS0, MGN_VHT3SS_MCS1, MGN_VHT3SS_MCS2, MGN_VHT3SS_MCS3, MGN_VHT3SS_MCS4
+	, MGN_VHT3SS_MCS5, MGN_VHT3SS_MCS6, MGN_VHT3SS_MCS7, MGN_VHT3SS_MCS8, MGN_VHT3SS_MCS9
+			  };
+u8 mgn_rates_vht4ss[10] = {MGN_VHT4SS_MCS0, MGN_VHT4SS_MCS1, MGN_VHT4SS_MCS2, MGN_VHT4SS_MCS3, MGN_VHT4SS_MCS4
+	, MGN_VHT4SS_MCS5, MGN_VHT4SS_MCS6, MGN_VHT4SS_MCS7, MGN_VHT4SS_MCS8, MGN_VHT4SS_MCS9
+			  };
+
+static const char *const _rate_section_str[] = {
+	"CCK",
+	"OFDM",
+	"HT_1SS",
+	"HT_2SS",
+	"HT_3SS",
+	"HT_4SS",
+	"VHT_1SS",
+	"VHT_2SS",
+	"VHT_3SS",
+	"VHT_4SS",
+	"RATE_SECTION_UNKNOWN",
+};
+
+const char *rate_section_str(u8 section)
+{
+	section = (section >= RATE_SECTION_NUM) ? RATE_SECTION_NUM : section;
+	return _rate_section_str[section];
+}
+
+struct rate_section_ent rates_by_sections[RATE_SECTION_NUM] = {
+	{RF_1TX, 4, mgn_rates_cck},
+	{RF_1TX, 8, mgn_rates_ofdm},
+	{RF_1TX, 8, mgn_rates_mcs0_7},
+	{RF_2TX, 8, mgn_rates_mcs8_15},
+	{RF_3TX, 8, mgn_rates_mcs16_23},
+	{RF_4TX, 8, mgn_rates_mcs24_31},
+	{RF_1TX, 10, mgn_rates_vht1ss},
+	{RF_2TX, 10, mgn_rates_vht2ss},
+	{RF_3TX, 10, mgn_rates_vht3ss},
+	{RF_4TX, 10, mgn_rates_vht4ss},
+};
+
+int rtw_get_bit_value_from_ieee_value(u8 val)
+{
+	unsigned char dot11_rate_table[] = {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 0}; /* last element must be zero!! */
+
+	int i = 0;
+	while (dot11_rate_table[i] != 0) {
+		if (dot11_rate_table[i] == val)
+			return BIT(i);
+		i++;
+	}
+	return 0;
+}
+
+uint	rtw_is_cckrates_included(u8 *rate)
+{
+	u32	i = 0;
+
+	while (rate[i] != 0) {
+		if ((((rate[i]) & 0x7f) == 2)	|| (((rate[i]) & 0x7f) == 4) ||
+		    (((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22))
+			return true;
+		i++;
+	}
+
+	return false;
+}
+
+uint	rtw_is_cckratesonly_included(u8 *rate)
+{
+	u32 i = 0;
+
+
+	while (rate[i] != 0) {
+		if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
+		    (((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22))
+			return false;
+
+		i++;
+	}
+
+	return true;
+
+}
+
+int rtw_check_network_type(unsigned char *rate, int ratelen, int channel)
+{
+	if (channel > 14) {
+		if ((rtw_is_cckrates_included(rate)))
+			return WIRELESS_INVALID;
+		else
+			return WIRELESS_11A;
+	} else { /* could be pure B, pure G, or B/G */
+		if ((rtw_is_cckratesonly_included(rate)))
+			return WIRELESS_11B;
+		else if ((rtw_is_cckrates_included(rate)))
+			return	WIRELESS_11BG;
+		else
+			return WIRELESS_11G;
+	}
+
+}
+
+u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source,
+		     unsigned int *frlen)
+{
+	memcpy((void *)pbuf, (void *)source, len);
+	*frlen = *frlen + len;
+	return pbuf + len;
+}
+
+/* rtw_set_ie will update frame length */
+u8 *rtw_set_ie
+(
+	u8 *pbuf,
+	sint index,
+	uint len,
+	u8 *source,
+	uint *frlen /* frame length */
+)
+{
+	*pbuf = (u8)index;
+
+	*(pbuf + 1) = (u8)len;
+
+	if (len > 0)
+		memcpy((void *)(pbuf + 2), (void *)source, len);
+
+	*frlen = *frlen + (len + 2);
+
+	return pbuf + len + 2;
+}
+
+inline u8 *rtw_set_ie_ch_switch(u8 *buf, u32 *buf_len, u8 ch_switch_mode,
+				u8 new_ch, u8 ch_switch_cnt)
+{
+	u8 ie_data[3];
+
+	ie_data[0] = ch_switch_mode;
+	ie_data[1] = new_ch;
+	ie_data[2] = ch_switch_cnt;
+	return rtw_set_ie(buf, WLAN_EID_CHANNEL_SWITCH,  3, ie_data, buf_len);
+}
+
+inline u8 secondary_ch_offset_to_hal_ch_offset(u8 ch_offset)
+{
+	if (ch_offset == SCN)
+		return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	else if (ch_offset == SCA)
+		return HAL_PRIME_CHNL_OFFSET_UPPER;
+	else if (ch_offset == SCB)
+		return HAL_PRIME_CHNL_OFFSET_LOWER;
+
+	return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+}
+
+inline u8 hal_ch_offset_to_secondary_ch_offset(u8 ch_offset)
+{
+	if (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
+		return SCN;
+	else if (ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
+		return SCB;
+	else if (ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
+		return SCA;
+
+	return SCN;
+}
+
+inline u8 *rtw_set_ie_secondary_ch_offset(u8 *buf, u32 *buf_len, u8 secondary_ch_offset)
+{
+	return rtw_set_ie(buf, WLAN_EID_SECONDARY_CHANNEL_OFFSET,  1, &secondary_ch_offset, buf_len);
+}
+
+inline u8 *rtw_set_ie_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl,
+		u8 flags, u16 reason, u16 precedence)
+{
+	u8 ie_data[6];
+
+	ie_data[0] = ttl;
+	ie_data[1] = flags;
+	RTW_PUT_LE16((u8 *)&ie_data[2], reason);
+	RTW_PUT_LE16((u8 *)&ie_data[4], precedence);
+
+	return rtw_set_ie(buf, 0x118,  6, ie_data, buf_len);
+}
+
+/*----------------------------------------------------------------------------
+index: the information element id index, limit is the limit for search
+-----------------------------------------------------------------------------*/
+u8 *rtw_get_ie(u8 *pbuf, sint index, sint *len, sint limit)
+{
+	sint tmp, i;
+	u8 *p;
+	if (limit < 1) {
+		return NULL;
+	}
+
+	p = pbuf;
+	i = 0;
+	*len = 0;
+	while (1) {
+		if (*p == index) {
+			*len = *(p + 1);
+			return p;
+		} else {
+			tmp = *(p + 1);
+			p += (tmp + 2);
+			i += (tmp + 2);
+		}
+		if (i >= limit)
+			break;
+	}
+	return NULL;
+}
+
+/**
+ * rtw_get_ie_ex - Search specific IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @eid: Element ID to match
+ * @oui: OUI to match
+ * @oui_len: OUI length
+ * @ie: If not NULL and the specific IE is found, the IE will be copied to the buf starting from the specific IE
+ * @ielen: If not NULL and the specific IE is found, will set to the length of the entire IE
+ *
+ * Returns: The address of the specific IE found, or NULL
+ */
+u8 *rtw_get_ie_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len, u8 *ie, uint *ielen)
+{
+	uint cnt;
+	u8 *target_ie = NULL;
+
+
+	if (ielen)
+		*ielen = 0;
+
+	if (!in_ie || in_len <= 0)
+		return target_ie;
+
+	cnt = 0;
+
+	while (cnt < in_len) {
+		if (eid == in_ie[cnt]
+		    && (!oui || !memcmp(&in_ie[cnt + 2], oui, oui_len))) {
+			target_ie = &in_ie[cnt];
+
+			if (ie)
+				memcpy(ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+
+			if (ielen)
+				*ielen = in_ie[cnt + 1] + 2;
+
+			break;
+		} else {
+			cnt += in_ie[cnt + 1] + 2; /* goto next	 */
+		}
+
+	}
+
+	return target_ie;
+}
+
+/**
+ * rtw_ies_remove_ie - Find matching IEs and remove
+ * @ies: Address of IEs to search
+ * @ies_len: Pointer of length of ies, will update to new length
+ * @offset: The offset to start scarch
+ * @eid: Element ID to match
+ * @oui: OUI to match
+ * @oui_len: OUI length
+ *
+ * Returns: _SUCCESS: ies is updated, _FAIL: not updated
+ */
+int rtw_ies_remove_ie(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len)
+{
+	int ret = _FAIL;
+	u8 *target_ie;
+	u32 target_ielen;
+	u8 *start;
+	uint search_len;
+
+	if (!ies || !ies_len || *ies_len <= offset)
+		goto exit;
+
+	start = ies + offset;
+	search_len = *ies_len - offset;
+
+	while (1) {
+		target_ie = rtw_get_ie_ex(start, search_len, eid, oui, oui_len, NULL, &target_ielen);
+		if (target_ie && target_ielen) {
+			u8 *remain_ies = target_ie + target_ielen;
+			uint remain_len = search_len - (remain_ies - start);
+
+			memmove(target_ie, remain_ies, remain_len);
+			*ies_len = *ies_len - target_ielen;
+			ret = _SUCCESS;
+
+			start = target_ie;
+			search_len = remain_len;
+		} else
+			break;
+	}
+exit:
+	return ret;
+}
+
+void rtw_set_supported_rate(u8 *SupportedRates, uint mode)
+{
+
+	memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+	switch (mode) {
+	case WIRELESS_11B:
+		memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+		break;
+
+	case WIRELESS_11G:
+	case WIRELESS_11A:
+	case WIRELESS_11_5N:
+	case WIRELESS_11A_5N: /* Todo: no basic rate for ofdm ? */
+	case WIRELESS_11_5AC:
+		memcpy(SupportedRates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
+		break;
+
+	case WIRELESS_11BG:
+	case WIRELESS_11G_24N:
+	case WIRELESS_11_24N:
+	case WIRELESS_11BG_24N:
+		memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
+		memcpy(SupportedRates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
+		break;
+
+	}
+}
+
+uint	rtw_get_rateset_len(u8	*rateset)
+{
+	uint i = 0;
+	while (1) {
+		if ((rateset[i]) == 0)
+			break;
+
+		if (i > 12)
+			break;
+
+		i++;
+	}
+	return i;
+}
+
+int rtw_generate_ie(struct registry_priv *pregistrypriv)
+{
+	u8	wireless_mode;
+	int	sz = 0, rateLen;
+	WLAN_BSSID_EX	*pdev_network = &pregistrypriv->dev_network;
+	u8	*ie = pdev_network->IEs;
+
+
+	/* timestamp will be inserted by hardware */
+	sz += 8;
+	ie += sz;
+
+	/* beacon interval : 2bytes */
+	*(__le16 *)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod); /* BCN_INTERVAL; */
+	sz += 2;
+	ie += 2;
+
+	/* capability info */
+	*(__le16 *)ie = 0;
+
+	*(__le16 *)ie |= cpu_to_le16(cap_IBSS);
+
+	if (pregistrypriv->preamble == PREAMBLE_SHORT)
+		*(__le16 *)ie |= cpu_to_le16(cap_ShortPremble);
+
+	if (pdev_network->Privacy)
+		*(__le16 *)ie |= cpu_to_le16(cap_Privacy);
+
+	sz += 2;
+	ie += 2;
+
+	/* SSID */
+	ie = rtw_set_ie(ie, _SSID_IE_, pdev_network->Ssid.SsidLength, pdev_network->Ssid.Ssid, &sz);
+
+	/* supported rates */
+	if (pregistrypriv->wireless_mode == WIRELESS_11ABGN) {
+		if (pdev_network->Configuration.DSConfig > 14)
+			wireless_mode = WIRELESS_11A_5N;
+		else
+			wireless_mode = WIRELESS_11BG_24N;
+	} else if (pregistrypriv->wireless_mode == WIRELESS_MODE_MAX) { /* WIRELESS_11ABGN | WIRELESS_11AC */
+		if (pdev_network->Configuration.DSConfig > 14)
+			wireless_mode = WIRELESS_11_5AC;
+		else
+			wireless_mode = WIRELESS_11BG_24N;
+	} else
+		wireless_mode = pregistrypriv->wireless_mode;
+
+	rtw_set_supported_rate(pdev_network->SupportedRates, wireless_mode) ;
+
+	rateLen = rtw_get_rateset_len(pdev_network->SupportedRates);
+
+	if (rateLen > 8) {
+		ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, pdev_network->SupportedRates, &sz);
+		/* ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */
+	} else
+		ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, pdev_network->SupportedRates, &sz);
+
+	/* DS parameter set */
+	ie = rtw_set_ie(ie, _DSSET_IE_, 1, (u8 *)&(pdev_network->Configuration.DSConfig), &sz);
+
+
+	/* IBSS Parameter Set */
+
+	ie = rtw_set_ie(ie, _IBSS_PARA_IE_, 2, (u8 *)&(pdev_network->Configuration.ATIMWindow), &sz);
+
+	if (rateLen > 8)
+		ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz);
+
+	/* HT Cap. */
+	if (((pregistrypriv->wireless_mode & WIRELESS_11_5N) || (pregistrypriv->wireless_mode & WIRELESS_11_24N))
+	    && (pregistrypriv->ht_enable)) {
+		/* todo: */
+	}
+
+	return sz;
+}
+
+unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
+{
+	int len;
+	u16 val16;
+	__le16 leval16;
+	unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
+	u8 *pbuf = pie;
+	int limit_new = limit;
+
+	while (1) {
+		pbuf = rtw_get_ie(pbuf, _WPA_IE_ID_, &len, limit_new);
+		if (pbuf) {
+			/* check if oui matches... */
+			if (memcmp((pbuf + 2), wpa_oui_type, sizeof(wpa_oui_type)))
+				goto check_next_ie;
+			/* check version... */
+			memcpy((u8 *)&leval16, (pbuf + 6), sizeof(val16));
+
+			val16 = le16_to_cpu(leval16);
+			if (val16 != 0x0001)
+				goto check_next_ie;
+
+			*wpa_ie_len = *(pbuf + 1);
+
+			return pbuf;
+
+		} else {
+			*wpa_ie_len = 0;
+			return NULL;
+		}
+
+check_next_ie:
+
+		limit_new = limit - (pbuf - pie) - 2 - len;
+
+		if (limit_new <= 0)
+			break;
+
+		pbuf += (2 + len);
+
+	}
+
+	*wpa_ie_len = 0;
+
+	return NULL;
+
+}
+
+unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit)
+{
+
+	return rtw_get_ie(pie, _WPA2_IE_ID_, rsn_ie_len, limit);
+
+}
+
+int rtw_get_wpa_cipher_suite(u8 *s)
+{
+	if (!memcmp(s, WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_NONE;
+	if (!memcmp(s, WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_WEP40;
+	if (!memcmp(s, WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_TKIP;
+	if (!memcmp(s, WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_CCMP;
+	if (!memcmp(s, WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN))
+		return WPA_CIPHER_WEP104;
+
+	return 0;
+}
+
+int rtw_get_wpa2_cipher_suite(u8 *s)
+{
+	if (!memcmp(s, RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_NONE;
+	if (!memcmp(s, RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_WEP40;
+	if (!memcmp(s, RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_TKIP;
+	if (!memcmp(s, RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_CCMP;
+	if (!memcmp(s, RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN))
+		return WPA_CIPHER_WEP104;
+
+	return 0;
+}
+
+
+int rtw_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+{
+	int i, ret = _SUCCESS;
+	int left, count;
+	u8 *pos;
+	u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
+
+	if (wpa_ie_len <= 0) {
+		/* No WPA IE - fail silently */
+		return _FAIL;
+	}
+
+
+	if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie + 1) != (u8)(wpa_ie_len - 2)) ||
+	    (memcmp(wpa_ie + 2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN)))
+		return _FAIL;
+
+	pos = wpa_ie;
+
+	pos += 8;
+	left = wpa_ie_len - 8;
+
+
+	/* group_cipher */
+	if (left >= WPA_SELECTOR_LEN) {
+
+		*group_cipher = rtw_get_wpa_cipher_suite(pos);
+
+		pos += WPA_SELECTOR_LEN;
+		left -= WPA_SELECTOR_LEN;
+
+	} else if (left > 0) {
+
+		return _FAIL;
+	}
+
+
+	/* pairwise_cipher */
+	if (left >= 2) {
+		/* count = le16_to_cpu(*(u16*)pos);	 */
+		count = RTW_GET_LE16(pos);
+		pos += 2;
+		left -= 2;
+
+		if (count == 0 || left < count * WPA_SELECTOR_LEN) {
+			return _FAIL;
+		}
+
+		for (i = 0; i < count; i++) {
+			*pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
+
+			pos += WPA_SELECTOR_LEN;
+			left -= WPA_SELECTOR_LEN;
+		}
+
+	} else if (left == 1) {
+		return _FAIL;
+	}
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (!memcmp(pos, SUITE_1X, 4)) {
+				*is_8021x = 1;
+			}
+		}
+	}
+
+	return ret;
+
+}
+
+int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+{
+	int i, ret = _SUCCESS;
+	int left, count;
+	u8 *pos;
+	u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01};
+
+	if (rsn_ie_len <= 0) {
+		/* No RSN IE - fail silently */
+		return _FAIL;
+	}
+
+
+	if ((*rsn_ie != _WPA2_IE_ID_) || (*(rsn_ie + 1) != (u8)(rsn_ie_len - 2)))
+		return _FAIL;
+
+	pos = rsn_ie;
+	pos += 4;
+	left = rsn_ie_len - 4;
+
+	/* group_cipher */
+	if (left >= RSN_SELECTOR_LEN) {
+
+		*group_cipher = rtw_get_wpa2_cipher_suite(pos);
+
+		pos += RSN_SELECTOR_LEN;
+		left -= RSN_SELECTOR_LEN;
+
+	} else if (left > 0) {
+		return _FAIL;
+	}
+
+	/* pairwise_cipher */
+	if (left >= 2) {
+		/* count = le16_to_cpu(*(u16*)pos); */
+		count = RTW_GET_LE16(pos);
+		pos += 2;
+		left -= 2;
+
+		if (count == 0 || left < count * RSN_SELECTOR_LEN) {
+			return _FAIL;
+		}
+
+		for (i = 0; i < count; i++) {
+			*pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
+
+			pos += RSN_SELECTOR_LEN;
+			left -= RSN_SELECTOR_LEN;
+		}
+
+	} else if (left == 1) {
+
+		return _FAIL;
+	}
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (!memcmp(pos, SUITE_1X, 4)) {
+				*is_8021x = 1;
+			}
+		}
+	}
+
+	return ret;
+
+}
+
+/* #ifdef CONFIG_WAPI_SUPPORT */
+int rtw_get_wapi_ie(u8 *in_ie, uint in_len, u8 *wapi_ie, u16 *wapi_len)
+{
+	int len = 0;
+	u8 authmode, i;
+	uint	cnt;
+	u8 wapi_oui1[4] = {0x0, 0x14, 0x72, 0x01};
+	u8 wapi_oui2[4] = {0x0, 0x14, 0x72, 0x02};
+
+
+	if (wapi_len)
+		*wapi_len = 0;
+
+	if (!in_ie || in_len <= 0)
+		return len;
+
+	cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
+
+	while (cnt < in_len) {
+		authmode = in_ie[cnt];
+
+		/* if(authmode==_WAPI_IE_) */
+		if (authmode == _WAPI_IE_ && (!memcmp(&in_ie[cnt + 6], wapi_oui1, 4) ||
+			!memcmp(&in_ie[cnt + 6], wapi_oui2, 4))) {
+			if (wapi_ie)
+				memcpy(wapi_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+
+			if (wapi_len)
+				*wapi_len = in_ie[cnt + 1] + 2;
+
+			cnt += in_ie[cnt + 1] + 2; /* get next */
+		} else {
+			cnt += in_ie[cnt + 1] + 2; /* get next */
+		}
+	}
+
+	if (wapi_len)
+		len = *wapi_len;
+
+
+	return len;
+
+}
+/* #endif */
+
+int rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie, u16 *wpa_len)
+{
+	u8 authmode, sec_idx, i;
+	u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
+	uint	cnt;
+
+
+	/* Search required WPA or WPA2 IE and copy to sec_ie[ ] */
+
+	cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
+
+	sec_idx = 0;
+
+	while (cnt < in_len) {
+		authmode = in_ie[cnt];
+
+		if ((authmode == _WPA_IE_ID_) && (!memcmp(&in_ie[cnt + 2], &wpa_oui[0], 4))) {
+
+			if (wpa_ie)
+				memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+
+			*wpa_len = in_ie[cnt + 1] + 2;
+			cnt += in_ie[cnt + 1] + 2; /* get next */
+		} else {
+			if (authmode == _WPA2_IE_ID_) {
+
+				if (rsn_ie)
+					memcpy(rsn_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+
+				*rsn_len = in_ie[cnt + 1] + 2;
+				cnt += in_ie[cnt + 1] + 2; /* get next */
+			} else {
+				cnt += in_ie[cnt + 1] + 2; /* get next */
+			}
+		}
+
+	}
+
+
+	return *rsn_len + *wpa_len;
+
+}
+
+u8 rtw_is_wps_ie(u8 *ie_ptr, uint *wps_ielen)
+{
+	u8 match = false;
+	u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+	if (ie_ptr == NULL)
+		return match;
+
+	eid = ie_ptr[0];
+
+	if ((eid == _WPA_IE_ID_) && (!memcmp(&ie_ptr[2], wps_oui, 4))) {
+		/* RTW_INFO("==> found WPS_IE.....\n"); */
+		*wps_ielen = ie_ptr[1] + 2;
+		match = true;
+	}
+	return match;
+}
+
+u8 *rtw_get_wps_ie_from_scan_queue(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen, u8 frame_type)
+{
+	u8	*wps = NULL;
+
+	RTW_INFO("[%s] frame_type = %d\n", __func__, frame_type);
+	switch (frame_type) {
+	case 1:
+	case 3: {
+		/*	Beacon or Probe Response */
+		wps = rtw_get_wps_ie(in_ie + _PROBERSP_IE_OFFSET_, in_len - _PROBERSP_IE_OFFSET_, wps_ie, wps_ielen);
+		break;
+	}
+	case 2: {
+		/*	Probe Request */
+		wps = rtw_get_wps_ie(in_ie + _PROBEREQ_IE_OFFSET_ , in_len - _PROBEREQ_IE_OFFSET_ , wps_ie, wps_ielen);
+		break;
+	}
+	}
+	return wps;
+}
+
+/**
+ * rtw_get_wps_ie - Search WPS IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie
+ * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE
+ *
+ * Returns: The address of the WPS IE found, or NULL
+ */
+u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
+{
+	uint cnt;
+	u8 *wpsie_ptr = NULL;
+	u8 eid, wps_oui[4] = {0x00, 0x50, 0xf2, 0x04};
+
+	if (wps_ielen)
+		*wps_ielen = 0;
+
+	if (!in_ie) {
+		rtw_warn_on(1);
+		return wpsie_ptr;
+	}
+
+	if (in_len <= 0)
+		return wpsie_ptr;
+
+	cnt = 0;
+
+	while (cnt + 1 + 4 < in_len) {
+		eid = in_ie[cnt];
+
+		if (cnt + 1 + 4 >= MAX_IE_SZ) {
+			rtw_warn_on(1);
+			return NULL;
+		}
+
+		if (eid == WLAN_EID_VENDOR_SPECIFIC && !memcmp(&in_ie[cnt + 2], wps_oui, 4)) {
+			wpsie_ptr = in_ie + cnt;
+
+			if (wps_ie)
+				memcpy(wps_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+
+			if (wps_ielen)
+				*wps_ielen = in_ie[cnt + 1] + 2;
+
+			break;
+		} else
+			cnt += in_ie[cnt + 1] + 2;
+
+	}
+
+	return wpsie_ptr;
+}
+
+/**
+ * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
+ * @wps_ie: Address of WPS IE to search
+ * @wps_ielen: Length limit from wps_ie
+ * @target_attr_id: The attribute ID of WPS attribute to search
+ * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
+ * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
+ *
+ * Returns: the address of the specific WPS attribute found, or NULL
+ */
+u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_attr, u32 *len_attr)
+{
+	u8 *attr_ptr = NULL;
+	u8 *target_attr_ptr = NULL;
+	u8 wps_oui[4] = {0x00, 0x50, 0xF2, 0x04};
+
+	if (len_attr)
+		*len_attr = 0;
+
+	if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) ||
+	    (memcmp(wps_ie + 2, wps_oui , 4)))
+		return attr_ptr;
+
+	/* 6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */
+	attr_ptr = wps_ie + 6; /* goto first attr */
+
+	while (attr_ptr - wps_ie < wps_ielen) {
+		/* 4 = 2(Attribute ID) + 2(Length) */
+		u16 attr_id = RTW_GET_BE16(attr_ptr);
+		u16 attr_data_len = RTW_GET_BE16(attr_ptr + 2);
+		u16 attr_len = attr_data_len + 4;
+
+		/* RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len); */
+		if (attr_id == target_attr_id) {
+			target_attr_ptr = attr_ptr;
+
+			if (buf_attr)
+				memcpy(buf_attr, attr_ptr, attr_len);
+
+			if (len_attr)
+				*len_attr = attr_len;
+
+			break;
+		} else {
+			attr_ptr += attr_len; /* goto next */
+		}
+
+	}
+
+	return target_attr_ptr;
+}
+
+/**
+ * rtw_get_wps_attr_content - Search a specific WPS attribute content from a given WPS IE
+ * @wps_ie: Address of WPS IE to search
+ * @wps_ielen: Length limit from wps_ie
+ * @target_attr_id: The attribute ID of WPS attribute to search
+ * @buf_content: If not NULL and the WPS attribute is found, WPS attribute content will be copied to the buf starting from buf_content
+ * @len_content: If not NULL and the WPS attribute is found, will set to the length of the WPS attribute content
+ *
+ * Returns: the address of the specific WPS attribute content found, or NULL
+ */
+u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_content, uint *len_content)
+{
+	u8 *attr_ptr;
+	u32 attr_len;
+
+	if (len_content)
+		*len_content = 0;
+
+	attr_ptr = rtw_get_wps_attr(wps_ie, wps_ielen, target_attr_id, NULL, &attr_len);
+
+	if (attr_ptr && attr_len) {
+		if (buf_content)
+			memcpy(buf_content, attr_ptr + 4, attr_len - 4);
+
+		if (len_content)
+			*len_content = attr_len - 4;
+
+		return attr_ptr + 4;
+	}
+
+	return NULL;
+}
+
+static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
+		struct rtw_ieee802_11_elems *elems,
+		int show_errors)
+{
+	unsigned int oui;
+
+	/* first 3 bytes in vendor specific information element are the IEEE
+	 * OUI of the vendor. The following byte is used a vendor specific
+	 * sub-type. */
+	if (elen < 4) {
+		if (show_errors) {
+			RTW_INFO("short vendor specific "
+				 "information element ignored (len=%lu)\n",
+				 (unsigned long) elen);
+		}
+		return -1;
+	}
+
+	oui = RTW_GET_BE24(pos);
+	switch (oui) {
+	case OUI_MICROSOFT:
+		/* Microsoft/Wi-Fi information elements are further typed and
+		 * subtyped */
+		switch (pos[3]) {
+		case 1:
+			/* Microsoft OUI (00:50:F2) with OUI Type 1:
+			 * real WPA information element */
+			elems->wpa_ie = pos;
+			elems->wpa_ie_len = elen;
+			break;
+		case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
+			if (elen < 5) {
+				RTW_DBG("short WME "
+					"information element ignored "
+					"(len=%lu)\n",
+					(unsigned long) elen);
+				return -1;
+			}
+			switch (pos[4]) {
+			case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
+			case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
+				elems->wme = pos;
+				elems->wme_len = elen;
+				break;
+			case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
+				elems->wme_tspec = pos;
+				elems->wme_tspec_len = elen;
+				break;
+			default:
+				RTW_DBG("unknown WME "
+					"information element ignored "
+					"(subtype=%d len=%lu)\n",
+					pos[4], (unsigned long) elen);
+				return -1;
+			}
+			break;
+		case 4:
+			/* Wi-Fi Protected Setup (WPS) IE */
+			elems->wps_ie = pos;
+			elems->wps_ie_len = elen;
+			break;
+		default:
+			RTW_DBG("Unknown Microsoft "
+				"information element ignored "
+				"(type=%d len=%lu)\n",
+				pos[3], (unsigned long) elen);
+			return -1;
+		}
+		break;
+
+	case OUI_BROADCOM:
+		switch (pos[3]) {
+		case VENDOR_HT_CAPAB_OUI_TYPE:
+			elems->vendor_ht_cap = pos;
+			elems->vendor_ht_cap_len = elen;
+			break;
+		default:
+			RTW_DBG("Unknown Broadcom "
+				"information element ignored "
+				"(type=%d len=%lu)\n",
+				pos[3], (unsigned long) elen);
+			return -1;
+		}
+		break;
+
+	default:
+		RTW_DBG("unknown vendor specific information "
+			"element ignored (vendor OUI %02x:%02x:%02x "
+			"len=%lu)\n",
+			pos[0], pos[1], pos[2], (unsigned long) elen);
+		return -1;
+	}
+
+	return 0;
+
+}
+
+/**
+ * ieee802_11_parse_elems - Parse information elements in management frames
+ * @start: Pointer to the start of IEs
+ * @len: Length of IE buffer in octets
+ * @elems: Data structure for parsed elements
+ * @show_errors: Whether to show parsing errors in debug log
+ * Returns: Parsing result
+ */
+ParseRes rtw_ieee802_11_parse_elems(u8 *start, uint len,
+				    struct rtw_ieee802_11_elems *elems,
+				    int show_errors)
+{
+	uint left = len;
+	u8 *pos = start;
+	int unknown = 0;
+
+	memset(elems, 0, sizeof(*elems));
+
+	while (left >= 2) {
+		u8 id, elen;
+
+		id = *pos++;
+		elen = *pos++;
+		left -= 2;
+
+		if (elen > left) {
+			if (show_errors) {
+				RTW_INFO("IEEE 802.11 element "
+					 "parse failed (id=%d elen=%d "
+					 "left=%lu)\n",
+					 id, elen, (unsigned long) left);
+			}
+			return ParseFailed;
+		}
+
+		switch (id) {
+		case WLAN_EID_SSID:
+			elems->ssid = pos;
+			elems->ssid_len = elen;
+			break;
+		case WLAN_EID_SUPP_RATES:
+			elems->supp_rates = pos;
+			elems->supp_rates_len = elen;
+			break;
+		case WLAN_EID_FH_PARAMS:
+			elems->fh_params = pos;
+			elems->fh_params_len = elen;
+			break;
+		case WLAN_EID_DS_PARAMS:
+			elems->ds_params = pos;
+			elems->ds_params_len = elen;
+			break;
+		case WLAN_EID_CF_PARAMS:
+			elems->cf_params = pos;
+			elems->cf_params_len = elen;
+			break;
+		case WLAN_EID_TIM:
+			elems->tim = pos;
+			elems->tim_len = elen;
+			break;
+		case WLAN_EID_IBSS_PARAMS:
+			elems->ibss_params = pos;
+			elems->ibss_params_len = elen;
+			break;
+		case WLAN_EID_CHALLENGE:
+			elems->challenge = pos;
+			elems->challenge_len = elen;
+			break;
+		case WLAN_EID_ERP_INFO:
+			elems->erp_info = pos;
+			elems->erp_info_len = elen;
+			break;
+		case WLAN_EID_EXT_SUPP_RATES:
+			elems->ext_supp_rates = pos;
+			elems->ext_supp_rates_len = elen;
+			break;
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (rtw_ieee802_11_parse_vendor_specific(pos, elen,
+					elems,
+					show_errors))
+				unknown++;
+			break;
+		case WLAN_EID_RSN:
+			elems->rsn_ie = pos;
+			elems->rsn_ie_len = elen;
+			break;
+		case WLAN_EID_PWR_CAPABILITY:
+			elems->power_cap = pos;
+			elems->power_cap_len = elen;
+			break;
+		case WLAN_EID_SUPPORTED_CHANNELS:
+			elems->supp_channels = pos;
+			elems->supp_channels_len = elen;
+			break;
+		case WLAN_EID_MOBILITY_DOMAIN:
+			elems->mdie = pos;
+			elems->mdie_len = elen;
+			break;
+		case WLAN_EID_FAST_BSS_TRANSITION:
+			elems->ftie = pos;
+			elems->ftie_len = elen;
+			break;
+		case WLAN_EID_TIMEOUT_INTERVAL:
+			elems->timeout_int = pos;
+			elems->timeout_int_len = elen;
+			break;
+		case WLAN_EID_HT_CAP:
+			elems->ht_capabilities = pos;
+			elems->ht_capabilities_len = elen;
+			break;
+		case WLAN_EID_HT_OPERATION:
+			elems->ht_operation = pos;
+			elems->ht_operation_len = elen;
+			break;
+		case WLAN_EID_VHT_CAPABILITY:
+			elems->vht_capabilities = pos;
+			elems->vht_capabilities_len = elen;
+			break;
+		case WLAN_EID_VHT_OPERATION:
+			elems->vht_operation = pos;
+			elems->vht_operation_len = elen;
+			break;
+		case WLAN_EID_VHT_OP_MODE_NOTIFY:
+			elems->vht_op_mode_notify = pos;
+			elems->vht_op_mode_notify_len = elen;
+			break;
+		default:
+			unknown++;
+			if (!show_errors)
+				break;
+			RTW_DBG("IEEE 802.11 element parse "
+				"ignored unknown element (id=%d elen=%d)\n",
+				id, elen);
+			break;
+		}
+
+		left -= elen;
+		pos += elen;
+	}
+
+	if (left)
+		return ParseFailed;
+
+	return unknown ? ParseUnknown : ParseOK;
+
+}
+
+static u8 key_char2num(u8 ch);
+static u8 key_char2num(u8 ch)
+{
+	if ((ch >= '0') && (ch <= '9'))
+		return ch - '0';
+	else if ((ch >= 'a') && (ch <= 'f'))
+		return ch - 'a' + 10;
+	else if ((ch >= 'A') && (ch <= 'F'))
+		return ch - 'A' + 10;
+	else
+		return 0xff;
+}
+
+u8 str_2char2num(u8 hch, u8 lch);
+u8 str_2char2num(u8 hch, u8 lch)
+{
+	return (key_char2num(hch) * 10) + key_char2num(lch);
+}
+
+u8 key_2char2num(u8 hch, u8 lch);
+u8 key_2char2num(u8 hch, u8 lch)
+{
+	return (key_char2num(hch) << 4) | key_char2num(lch);
+}
+
+void macstr2num(u8 *dst, u8 *src);
+void macstr2num(u8 *dst, u8 *src)
+{
+	int	jj, kk;
+	for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
+		dst[jj] = key_2char2num(src[kk], src[kk + 1]);
+}
+
+u8 convert_ip_addr(u8 hch, u8 mch, u8 lch)
+{
+	return (key_char2num(hch) * 100) + (key_char2num(mch) * 10) + key_char2num(lch);
+}
+
+#ifdef CONFIG_PLATFORM_INTEL_BYT
+#define MAC_ADDRESS_LEN 12
+
+int rtw_get_mac_addr_intel(unsigned char *buf)
+{
+	int ret = 0;
+	int i;
+	struct file *fp = NULL;
+	mm_segment_t oldfs;
+	unsigned char c_mac[MAC_ADDRESS_LEN];
+	char fname[] = "/config/wifi/mac.txt";
+	int jj, kk;
+
+	RTW_INFO("%s Enter\n", __func__);
+
+	ret = rtw_retrieve_from_file(fname, c_mac, MAC_ADDRESS_LEN);
+	if (ret < MAC_ADDRESS_LEN)
+		return -1;
+
+	for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 2)
+		buf[jj] = key_2char2num(c_mac[kk], c_mac[kk + 1]);
+
+	RTW_INFO("%s: read from file mac address: "MAC_FMT"\n",
+		 __func__, MAC_ARG(buf));
+
+	return 0;
+}
+#endif /* CONFIG_PLATFORM_INTEL_BYT */
+
+/*
+ * Description:
+ * rtw_check_invalid_mac_address:
+ * This is only used for checking mac address valid or not.
+ *
+ * Input:
+ * adapter: mac_address pointer.
+ * check_local_bit: check locally bit or not.
+ *
+ * Output:
+ * true: The mac address is invalid.
+ * false: The mac address is valid.
+ *
+ * Auther: Isaac.Li
+ */
+u8 rtw_check_invalid_mac_address(u8 *mac_addr, u8 check_local_bit)
+{
+	u8 null_mac_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
+	u8 multi_mac_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 res = false;
+
+	if (!memcmp(mac_addr, null_mac_addr, ETH_ALEN)) {
+		res = true;
+		goto func_exit;
+	}
+
+	if (!memcmp(mac_addr, multi_mac_addr, ETH_ALEN)) {
+		res = true;
+		goto func_exit;
+	}
+
+	if (mac_addr[0] & BIT0) {
+		res = true;
+		goto func_exit;
+	}
+
+	if (check_local_bit) {
+		if (mac_addr[0] & BIT1) {
+			res = true;
+			goto func_exit;
+		}
+	}
+
+func_exit:
+	return res;
+}
+
+/**
+ * rtw_macaddr_cfg - Decide the mac address used
+ * @out: buf to store mac address decided
+ * @hw_mac_addr: mac address from efuse/epprom
+ */
+void rtw_macaddr_cfg(u8 *out, const u8 *hw_mac_addr)
+{
+#define DEFAULT_RANDOM_MACADDR 1
+	u8 mac[ETH_ALEN];
+
+	if (out == NULL) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	/* Users specify the mac address */
+	if (rtw_initmac) {
+		int jj, kk;
+
+		for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
+			mac[jj] = key_2char2num(rtw_initmac[kk], rtw_initmac[kk + 1]);
+
+		goto err_chk;
+	}
+
+	/* platform specified */
+#ifdef CONFIG_PLATFORM_INTEL_BYT
+	if (rtw_get_mac_addr_intel(mac) == 0)
+		goto err_chk;
+#endif
+
+	/* Use the mac address stored in the Efuse */
+	if (hw_mac_addr) {
+		memcpy(mac, hw_mac_addr, ETH_ALEN);
+		goto err_chk;
+	}
+
+err_chk:
+	if (rtw_check_invalid_mac_address(mac, true)) {
+#if DEFAULT_RANDOM_MACADDR
+		RTW_ERR("invalid mac addr:"MAC_FMT", assign random MAC\n", MAC_ARG(mac));
+		*((u32 *)(&mac[2])) = rtw_random32();
+		mac[0] = 0x00;
+		mac[1] = 0xe0;
+		mac[2] = 0x4c;
+#else
+		RTW_ERR("invalid mac addr:"MAC_FMT", assign default one\n", MAC_ARG(mac));
+		mac[0] = 0x00;
+		mac[1] = 0xe0;
+		mac[2] = 0x4c;
+		mac[3] = 0x87;
+		mac[4] = 0x00;
+		mac[5] = 0x00;
+#endif
+	}
+
+	memcpy(out, mac, ETH_ALEN);
+	RTW_INFO("%s mac addr:"MAC_FMT"\n", __func__, MAC_ARG(out));
+}
+
+void dump_ht_cap_ie_content(void *sel, u8 *buf, u32 buf_len)
+{
+	if (buf_len != 26) {
+		RTW_PRINT_SEL(sel, "Invalid HT capability IE len:%d != %d\n", buf_len, 26);
+		return;
+	}
+
+	RTW_PRINT_SEL(sel, "HT Capabilities Info:%02x%02x\n", *(buf), *(buf + 1));
+	RTW_PRINT_SEL(sel, "A-MPDU Parameters:"HT_AMPDU_PARA_FMT"\n"
+		      , HT_AMPDU_PARA_ARG(HT_CAP_ELE_AMPDU_PARA(buf)));
+	RTW_PRINT_SEL(sel, "Supported MCS Set:"HT_SUP_MCS_SET_FMT"\n"
+		      , HT_SUP_MCS_SET_ARG(HT_CAP_ELE_SUP_MCS_SET(buf)));
+}
+
+static void dump_ht_cap_ie(void *sel, u8 *ie, u32 ie_len)
+{
+	u8 *pos = (u8 *)ie;
+	u16 id;
+	u16 len;
+
+	u8 *ht_cap_ie;
+	sint ht_cap_ielen;
+
+	ht_cap_ie = rtw_get_ie(ie, _HT_CAPABILITY_IE_, &ht_cap_ielen, ie_len);
+	if (!ie || ht_cap_ie != ie)
+		return;
+
+	dump_ht_cap_ie_content(sel, ht_cap_ie + 2, ht_cap_ielen);
+}
+
+void dump_ies(void *sel, u8 *buf, u32 buf_len)
+{
+	u8 *pos = (u8 *)buf;
+	u8 id, len;
+
+	while (pos - buf + 1 < buf_len) {
+		id = *pos;
+		len = *(pos + 1);
+
+		RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u\n", __func__, id, len);
+		dump_ht_cap_ie(sel, pos, len + 2);
+		dump_wps_ie(sel, pos, len + 2);
+#ifdef CONFIG_P2P
+		dump_p2p_ie(sel, pos, len + 2);
+#ifdef CONFIG_WFD
+		dump_wfd_ie(sel, pos, len + 2);
+#endif
+#endif
+
+		pos += (2 + len);
+	}
+}
+
+void dump_wps_ie(void *sel, u8 *ie, u32 ie_len)
+{
+	u8 *pos = (u8 *)ie;
+	u16 id;
+	u16 len;
+
+	u8 *wps_ie;
+	uint wps_ielen;
+
+	wps_ie = rtw_get_wps_ie(ie, ie_len, NULL, &wps_ielen);
+	if (wps_ie != ie || wps_ielen == 0)
+		return;
+
+	pos += 6;
+	while (pos - ie + 4 <= ie_len) {
+		id = RTW_GET_BE16(pos);
+		len = RTW_GET_BE16(pos + 2);
+
+		RTW_PRINT_SEL(sel, "%s ID:0x%04x, LEN:%u%s\n", __func__, id, len
+			, ((pos - ie + 4 + len) <= ie_len) ? "" : "(exceed ie_len)");
+
+		pos += (4 + len);
+	}
+}
+
+/**
+ * rtw_ies_get_chbw - get operation ch, bw, offset from IEs of BSS.
+ * @ies: pointer of the first tlv IE
+ * @ies_len: length of @ies
+ * @ch: pointer of ch, used as output
+ * @bw: pointer of bw, used as output
+ * @offset: pointer of offset, used as output
+ */
+void rtw_ies_get_chbw(u8 *ies, int ies_len, u8 *ch, u8 *bw, u8 *offset)
+{
+	u8 *p;
+	int	ie_len;
+
+	*ch = 0;
+	*bw = CHANNEL_WIDTH_20;
+	*offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	p = rtw_get_ie(ies, _DSSET_IE_, &ie_len, ies_len);
+	if (p && ie_len > 0)
+		*ch = *(p + 2);
+
+	{
+		u8 *ht_cap_ie, *ht_op_ie;
+		int ht_cap_ielen, ht_op_ielen;
+
+		ht_cap_ie = rtw_get_ie(ies, EID_HTCapability, &ht_cap_ielen, ies_len);
+		if (ht_cap_ie && ht_cap_ielen) {
+			if (GET_HT_CAP_ELE_CHL_WIDTH(ht_cap_ie + 2))
+				*bw = CHANNEL_WIDTH_40;
+		}
+
+		ht_op_ie = rtw_get_ie(ies, EID_HTInfo, &ht_op_ielen, ies_len);
+		if (ht_op_ie && ht_op_ielen) {
+			if (*ch == 0)
+				*ch = GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2);
+			else if (*ch != 0 && *ch != GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2)) {
+				RTW_INFO("%s ch inconsistent, DSSS:%u, HT primary:%u\n"
+					, __func__, *ch, GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2));
+			}
+
+			if (!GET_HT_OP_ELE_STA_CHL_WIDTH(ht_op_ie + 2))
+				*bw = CHANNEL_WIDTH_20;
+
+			if (*bw == CHANNEL_WIDTH_40) {
+				switch (GET_HT_OP_ELE_2ND_CHL_OFFSET(ht_op_ie + 2)) {
+				case SCA:
+					*offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+					break;
+				case SCB:
+					*offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+					break;
+				}
+			}
+		}
+	}
+}
+
+void rtw_bss_get_chbw(WLAN_BSSID_EX *bss, u8 *ch, u8 *bw, u8 *offset)
+{
+	rtw_ies_get_chbw(bss->IEs + sizeof(NDIS_802_11_FIXED_IEs)
+		, bss->IELength - sizeof(NDIS_802_11_FIXED_IEs)
+		, ch, bw, offset);
+
+	if (*ch == 0)
+		*ch = bss->Configuration.DSConfig;
+	else if (*ch != bss->Configuration.DSConfig) {
+		RTW_INFO("inconsistent ch - ies:%u bss->Configuration.DSConfig:%u\n"
+			 , *ch, bss->Configuration.DSConfig);
+		*ch = bss->Configuration.DSConfig;
+		rtw_warn_on(1);
+	}
+}
+
+/**
+ * rtw_is_chbw_grouped - test if the two ch settings can be grouped together
+ * @ch_a: ch of set a
+ * @bw_a: bw of set a
+ * @offset_a: offset of set a
+ * @ch_b: ch of set b
+ * @bw_b: bw of set b
+ * @offset_b: offset of set b
+ */
+bool rtw_is_chbw_grouped(u8 ch_a, u8 bw_a, u8 offset_a
+			 , u8 ch_b, u8 bw_b, u8 offset_b)
+{
+	bool is_grouped = false;
+
+	if (ch_a != ch_b) {
+		/* ch is different */
+		goto exit;
+	} else if ((bw_a == CHANNEL_WIDTH_40 || bw_a == CHANNEL_WIDTH_80)
+		   && (bw_b == CHANNEL_WIDTH_40 || bw_b == CHANNEL_WIDTH_80)
+		  ) {
+		if (offset_a != offset_b)
+			goto exit;
+	}
+
+	is_grouped = true;
+
+exit:
+	return is_grouped;
+}
+
+/**
+ * rtw_sync_chbw - obey g_ch, adjust g_bw, g_offset, bw, offset
+ * @req_ch: pointer of the request ch, may be modified further
+ * @req_bw: pointer of the request bw, may be modified further
+ * @req_offset: pointer of the request offset, may be modified further
+ * @g_ch: pointer of the ongoing group ch
+ * @g_bw: pointer of the ongoing group bw, may be modified further
+ * @g_offset: pointer of the ongoing group offset, may be modified further
+ */
+void rtw_sync_chbw(u8 *req_ch, u8 *req_bw, u8 *req_offset
+		   , u8 *g_ch, u8 *g_bw, u8 *g_offset)
+{
+
+	*req_ch = *g_ch;
+
+	if (*req_bw == CHANNEL_WIDTH_80 && *g_ch <= 14) {
+		/*2.4G ch, downgrade to 40Mhz */
+		*req_bw = CHANNEL_WIDTH_40;
+	}
+
+	switch (*req_bw) {
+	case CHANNEL_WIDTH_80:
+		if (*g_bw == CHANNEL_WIDTH_40 || *g_bw == CHANNEL_WIDTH_80)
+			*req_offset = *g_offset;
+		else if (*g_bw == CHANNEL_WIDTH_20)
+			*req_offset = rtw_get_offset_by_ch(*req_ch);
+
+		if (*req_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) {
+			RTW_ERR("%s req 80MHz BW without offset, down to 20MHz\n", __func__);
+			rtw_warn_on(1);
+			*req_bw = CHANNEL_WIDTH_20;
+		}
+		break;
+	case CHANNEL_WIDTH_40:
+		if (*g_bw == CHANNEL_WIDTH_40 || *g_bw == CHANNEL_WIDTH_80)
+			*req_offset = *g_offset;
+		else if (*g_bw == CHANNEL_WIDTH_20)
+			*req_offset = rtw_get_offset_by_ch(*req_ch);
+
+		if (*req_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) {
+			RTW_ERR("%s req 40MHz BW without offset, down to 20MHz\n", __func__);
+			rtw_warn_on(1);
+			*req_bw = CHANNEL_WIDTH_20;
+		}
+		break;
+	case CHANNEL_WIDTH_20:
+		*req_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+		break;
+	default:
+		RTW_ERR("%s req unsupported BW:%u\n", __func__, *req_bw);
+		rtw_warn_on(1);
+	}
+
+	if (*req_bw > *g_bw) {
+		*g_bw = *req_bw;
+		*g_offset = *req_offset;
+	}
+}
+
+/**
+ * rtw_get_p2p_merged_len - Get merged ie length from muitiple p2p ies.
+ * @in_ie: Pointer of the first p2p ie
+ * @in_len: Total len of muiltiple p2p ies
+ * Returns: Length of merged p2p ie length
+ */
+u32 rtw_get_p2p_merged_ies_len(u8 *in_ie, u32 in_len)
+{
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8 OUI[4] = { 0x50, 0x6f, 0x9a, 0x09 };
+	int i = 0;
+	int j = 0, len = 0;
+
+	while (i < in_len) {
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(in_ie + i);
+
+		if (pIE->ElementID == _VENDOR_SPECIFIC_IE_ && !memcmp(pIE->data, OUI, 4)) {
+			len += pIE->Length - 4; /* 4 is P2P OUI length, don't count it in this loop */
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+	return len + 4;	/* Append P2P OUI length at last. */
+}
+
+/**
+ * rtw_p2p_merge_ies - Merge muitiple p2p ies into one
+ * @in_ie: Pointer of the first p2p ie
+ * @in_len: Total len of muiltiple p2p ies
+ * @merge_ie: Pointer of merged ie
+ * Returns: Length of merged p2p ie
+ */
+int rtw_p2p_merge_ies(u8 *in_ie, u32 in_len, u8 *merge_ie)
+{
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8 len = 0;
+	u8 OUI[4] = { 0x50, 0x6f, 0x9a, 0x09 };
+	u8 ELOUI[6] = { 0xDD, 0x00, 0x50, 0x6f, 0x9a, 0x09 };	/* EID;Len;OUI, Len would copy at the end of function */
+	int i = 0;
+
+	if (merge_ie != NULL) {
+		/* Set first P2P OUI */
+		memcpy(merge_ie, ELOUI, 6);
+		merge_ie += 6;
+
+		while (i < in_len) {
+			pIE = (PNDIS_802_11_VARIABLE_IEs)(in_ie + i);
+
+			/* Take out the rest of P2P OUIs */
+			if (pIE->ElementID == _VENDOR_SPECIFIC_IE_ && !memcmp(pIE->data, OUI, 4)) {
+				memcpy(merge_ie, pIE->data + 4, pIE->Length - 4);
+				len += pIE->Length - 4;
+				merge_ie += pIE->Length - 4;
+			}
+
+			i += (pIE->Length + 2);
+		}
+
+		return len + 4;	/* 4 is for P2P OUI */
+
+	}
+
+	return 0;
+}
+
+void dump_p2p_ie(void *sel, u8 *ie, u32 ie_len)
+{
+	u8 *pos = (u8 *)ie;
+	u8 id;
+	u16 len;
+
+	u8 *p2p_ie;
+	uint p2p_ielen;
+
+	p2p_ie = rtw_get_p2p_ie(ie, ie_len, NULL, &p2p_ielen);
+	if (p2p_ie != ie || p2p_ielen == 0)
+		return;
+
+	pos += 6;
+	while (pos - ie + 3 <= ie_len) {
+		id = *pos;
+		len = RTW_GET_LE16(pos + 1);
+
+		RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u%s\n", __func__, id, len
+			, ((pos - ie + 3 + len) <= ie_len) ? "" : "(exceed ie_len)");
+
+		pos += (3 + len);
+	}
+}
+
+/**
+ * rtw_get_p2p_ie - Search P2P IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @p2p_ie: If not NULL and P2P IE is found, P2P IE will be copied to the buf starting from p2p_ie
+ * @p2p_ielen: If not NULL and P2P IE is found, will set to the length of the entire P2P IE
+ *
+ * Returns: The address of the P2P IE found, or NULL
+ */
+u8 *rtw_get_p2p_ie(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen)
+{
+	uint cnt;
+	u8 *p2p_ie_ptr = NULL;
+	u8 eid, p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09};
+
+	if (p2p_ielen)
+		*p2p_ielen = 0;
+
+	if (!in_ie || in_len < 0) {
+		rtw_warn_on(1);
+		return p2p_ie_ptr;
+	}
+
+	if (in_len <= 0)
+		return p2p_ie_ptr;
+
+	cnt = 0;
+
+	while (cnt + 1 + 4 < in_len) {
+		eid = in_ie[cnt];
+
+		if (cnt + 1 + 4 >= MAX_IE_SZ) {
+			rtw_warn_on(1);
+			return NULL;
+		}
+
+		if (eid == WLAN_EID_VENDOR_SPECIFIC && !memcmp(&in_ie[cnt + 2], p2p_oui, 4)) {
+			p2p_ie_ptr = in_ie + cnt;
+
+			if (p2p_ie)
+				memcpy(p2p_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+
+			if (p2p_ielen)
+				*p2p_ielen = in_ie[cnt + 1] + 2;
+
+			break;
+		} else
+			cnt += in_ie[cnt + 1] + 2;
+
+	}
+
+	return p2p_ie_ptr;
+}
+
+/**
+ * rtw_get_p2p_attr - Search a specific P2P attribute from a given P2P IE
+ * @p2p_ie: Address of P2P IE to search
+ * @p2p_ielen: Length limit from p2p_ie
+ * @target_attr_id: The attribute ID of P2P attribute to search
+ * @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will be copied to the buf starting from buf_attr
+ * @len_attr: If not NULL and the P2P attribute is found, will set to the length of the entire P2P attribute
+ *
+ * Returns: the address of the specific WPS attribute found, or NULL
+ */
+u8 *rtw_get_p2p_attr(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id , u8 *buf_attr, u32 *len_attr)
+{
+	u8 *attr_ptr = NULL;
+	u8 *target_attr_ptr = NULL;
+	u8 p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09};
+
+	if (len_attr)
+		*len_attr = 0;
+
+	if (!p2p_ie
+	    || p2p_ielen <= 6
+	    || (p2p_ie[0] != WLAN_EID_VENDOR_SPECIFIC)
+	    || (memcmp(p2p_ie + 2, p2p_oui, 4)))
+		return attr_ptr;
+
+	/* 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */
+	attr_ptr = p2p_ie + 6; /* goto first attr */
+
+	while ((attr_ptr - p2p_ie + 3) <= p2p_ielen) {
+		/* 3 = 1(Attribute ID) + 2(Length) */
+		u8 attr_id = *attr_ptr;
+		u16 attr_data_len = RTW_GET_LE16(attr_ptr + 1);
+		u16 attr_len = attr_data_len + 3;
+
+		if (0)
+			RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len);
+
+		if ((attr_ptr - p2p_ie + attr_len) > p2p_ielen)
+			break;
+
+		if (attr_id == target_attr_id) {
+			target_attr_ptr = attr_ptr;
+
+			if (buf_attr)
+				memcpy(buf_attr, attr_ptr, attr_len);
+
+			if (len_attr)
+				*len_attr = attr_len;
+
+			break;
+		} else
+			attr_ptr += attr_len;
+	}
+
+	return target_attr_ptr;
+}
+
+/**
+ * rtw_get_p2p_attr_content - Search a specific P2P attribute content from a given P2P IE
+ * @p2p_ie: Address of P2P IE to search
+ * @p2p_ielen: Length limit from p2p_ie
+ * @target_attr_id: The attribute ID of P2P attribute to search
+ * @buf_content: If not NULL and the P2P attribute is found, P2P attribute content will be copied to the buf starting from buf_content
+ * @len_content: If not NULL and the P2P attribute is found, will set to the length of the P2P attribute content
+ *
+ * Returns: the address of the specific P2P attribute content found, or NULL
+ */
+u8 *rtw_get_p2p_attr_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id , u8 *buf_content, uint *len_content)
+{
+	u8 *attr_ptr;
+	u32 attr_len;
+
+	if (len_content)
+		*len_content = 0;
+
+	attr_ptr = rtw_get_p2p_attr(p2p_ie, p2p_ielen, target_attr_id, NULL, &attr_len);
+
+	if (attr_ptr && attr_len) {
+		if (buf_content)
+			memcpy(buf_content, attr_ptr + 3, attr_len - 3);
+
+		if (len_content)
+			*len_content = attr_len - 3;
+
+		return attr_ptr + 3;
+	}
+
+	return NULL;
+}
+
+u32 rtw_set_p2p_attr_content(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr)
+{
+	u32 a_len;
+
+	*pbuf = attr_id;
+
+	/* *(u16*)(pbuf + 1) = cpu_to_le16(attr_len); */
+	RTW_PUT_LE16(pbuf + 1, attr_len);
+
+	if (pdata_attr)
+		memcpy(pbuf + 3, pdata_attr, attr_len);
+
+	a_len = attr_len + 3;
+
+	return a_len;
+}
+
+uint rtw_del_p2p_ie(u8 *ies, uint ies_len_ori, const char *msg)
+{
+#define DBG_DEL_P2P_IE 0
+
+	u8 *target_ie;
+	u32 target_ie_len;
+	uint ies_len = ies_len_ori;
+	int index = 0;
+
+	while (1) {
+		target_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &target_ie_len);
+		if (target_ie && target_ie_len) {
+			u8 *next_ie = target_ie + target_ie_len;
+			uint remain_len = ies_len - (next_ie - ies);
+
+			if (DBG_DEL_P2P_IE && msg) {
+				RTW_INFO("%s %d before\n", __func__, index);
+				dump_ies(RTW_DBGDUMP, ies, ies_len);
+
+				RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
+				RTW_INFO("target_ie:%p, target_ie_len:%u\n", target_ie, target_ie_len);
+				RTW_INFO("next_ie:%p, remain_len:%u\n", next_ie, remain_len);
+			}
+
+			memmove(target_ie, next_ie, remain_len);
+			memset(target_ie + remain_len, 0, target_ie_len);
+			ies_len -= target_ie_len;
+
+			if (DBG_DEL_P2P_IE && msg) {
+				RTW_INFO("%s %d after\n", __func__, index);
+				dump_ies(RTW_DBGDUMP, ies, ies_len);
+			}
+
+			index++;
+		} else
+			break;
+	}
+
+	return ies_len;
+}
+
+uint rtw_del_p2p_attr(u8 *ie, uint ielen_ori, u8 attr_id)
+{
+#define DBG_DEL_P2P_ATTR 0
+
+	u8 *target_attr;
+	u32 target_attr_len;
+	uint ielen = ielen_ori;
+	int index = 0;
+
+	while (1) {
+		target_attr = rtw_get_p2p_attr(ie, ielen, attr_id, NULL, &target_attr_len);
+		if (target_attr && target_attr_len) {
+			u8 *next_attr = target_attr + target_attr_len;
+			uint remain_len = ielen - (next_attr - ie);
+
+			if (DBG_DEL_P2P_ATTR) {
+				RTW_INFO("%s %d before\n", __func__, index);
+				dump_ies(RTW_DBGDUMP, ie, ielen);
+
+				RTW_INFO("ie:%p, ielen:%u\n", ie, ielen);
+				RTW_INFO("target_attr:%p, target_attr_len:%u\n", target_attr, target_attr_len);
+				RTW_INFO("next_attr:%p, remain_len:%u\n", next_attr, remain_len);
+			}
+
+			memmove(target_attr, next_attr, remain_len);
+			memset(target_attr + remain_len, 0, target_attr_len);
+			*(ie + 1) -= target_attr_len;
+			ielen -= target_attr_len;
+
+			if (DBG_DEL_P2P_ATTR) {
+				RTW_INFO("%s %d after\n", __func__, index);
+				dump_ies(RTW_DBGDUMP, ie, ielen);
+			}
+
+			index++;
+		} else
+			break;
+	}
+
+	return ielen;
+}
+
+inline u8 *rtw_bss_ex_get_p2p_ie(WLAN_BSSID_EX *bss_ex, u8 *p2p_ie, uint *p2p_ielen)
+{
+	return rtw_get_p2p_ie(BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex), p2p_ie, p2p_ielen);
+}
+
+void rtw_bss_ex_del_p2p_ie(WLAN_BSSID_EX *bss_ex)
+{
+#define DBG_BSS_EX_DEL_P2P_IE 0
+
+	u8 *ies = BSS_EX_TLV_IES(bss_ex);
+	uint ies_len_ori = BSS_EX_TLV_IES_LEN(bss_ex);
+	uint ies_len;
+
+	ies_len = rtw_del_p2p_ie(ies, ies_len_ori, DBG_BSS_EX_DEL_P2P_IE ? __func__ : NULL);
+	bss_ex->IELength -= ies_len_ori - ies_len;
+}
+
+void rtw_bss_ex_del_p2p_attr(WLAN_BSSID_EX *bss_ex, u8 attr_id)
+{
+#define DBG_BSS_EX_DEL_P2P_ATTR 0
+
+	u8 *ies = BSS_EX_TLV_IES(bss_ex);
+	uint ies_len = BSS_EX_TLV_IES_LEN(bss_ex);
+
+	u8 *ie;
+	uint ie_len, ie_len_ori;
+
+	int index = 0;
+
+	while (1) {
+		ie = rtw_get_p2p_ie(ies, ies_len, NULL, &ie_len_ori);
+		if (ie) {
+			u8 *next_ie_ori = ie + ie_len_ori;
+			uint remain_len = bss_ex->IELength - (next_ie_ori - bss_ex->IEs);
+			u8 has_target_attr = 0;
+
+			if (DBG_BSS_EX_DEL_P2P_ATTR) {
+				if (rtw_get_p2p_attr(ie, ie_len_ori, attr_id, NULL, NULL)) {
+					RTW_INFO("%s %d before\n", __func__, index);
+					dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
+
+					RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
+					RTW_INFO("ie:%p, ie_len_ori:%u\n", ie, ie_len_ori);
+					RTW_INFO("next_ie_ori:%p, remain_len:%u\n", next_ie_ori, remain_len);
+					has_target_attr = 1;
+				}
+			}
+
+			ie_len = rtw_del_p2p_attr(ie, ie_len_ori, attr_id);
+			if (ie_len != ie_len_ori) {
+				u8 *next_ie = ie + ie_len;
+
+				memmove(next_ie, next_ie_ori, remain_len);
+				memset(next_ie + remain_len, 0, ie_len_ori - ie_len);
+				bss_ex->IELength -= ie_len_ori - ie_len;
+
+				ies = next_ie;
+			} else
+				ies = next_ie_ori;
+
+			if (DBG_BSS_EX_DEL_P2P_ATTR) {
+				if (has_target_attr) {
+					RTW_INFO("%s %d after\n", __func__, index);
+					dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
+				}
+			}
+
+			ies_len = remain_len;
+
+			index++;
+		} else
+			break;
+	}
+}
+
+void dump_wfd_ie(void *sel, u8 *ie, u32 ie_len)
+{
+	u8 *pos = (u8 *)ie;
+	u8 id;
+	u16 len;
+
+	u8 *wfd_ie;
+	uint wfd_ielen;
+
+	wfd_ie = rtw_get_wfd_ie(ie, ie_len, NULL, &wfd_ielen);
+	if (wfd_ie != ie || wfd_ielen == 0)
+		return;
+
+	pos += 6;
+	while (pos - ie + 3 <= ie_len) {
+		id = *pos;
+		len = RTW_GET_BE16(pos + 1);
+
+		RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u%s\n", __func__, id, len
+			, ((pos - ie + 3 + len) <= ie_len) ? "" : "(exceed ie_len)");
+
+		pos += (3 + len);
+	}
+}
+
+/**
+ * rtw_get_wfd_ie - Search WFD IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @wfd_ie: If not NULL and WFD IE is found, WFD IE will be copied to the buf starting from wfd_ie
+ * @wfd_ielen: If not NULL and WFD IE is found, will set to the length of the entire WFD IE
+ *
+ * Returns: The address of the P2P IE found, or NULL
+ */
+u8 *rtw_get_wfd_ie(u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen)
+{
+	uint cnt;
+	u8 *wfd_ie_ptr = NULL;
+	u8 eid, wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A};
+
+	if (wfd_ielen)
+		*wfd_ielen = 0;
+
+	if (!in_ie || in_len < 0) {
+		rtw_warn_on(1);
+		return wfd_ie_ptr;
+	}
+
+	if (in_len <= 0)
+		return wfd_ie_ptr;
+
+	cnt = 0;
+
+	while (cnt + 1 + 4 < in_len) {
+		eid = in_ie[cnt];
+
+		if (cnt + 1 + 4 >= MAX_IE_SZ) {
+			rtw_warn_on(1);
+			return NULL;
+		}
+
+		if (eid == WLAN_EID_VENDOR_SPECIFIC && !memcmp(&in_ie[cnt + 2], wfd_oui, 4)) {
+			wfd_ie_ptr = in_ie + cnt;
+
+			if (wfd_ie)
+				memcpy(wfd_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
+
+			if (wfd_ielen)
+				*wfd_ielen = in_ie[cnt + 1] + 2;
+
+			break;
+		} else
+			cnt += in_ie[cnt + 1] + 2;
+
+	}
+
+	return wfd_ie_ptr;
+}
+
+/**
+ * rtw_get_wfd_attr - Search a specific WFD attribute from a given WFD IE
+ * @wfd_ie: Address of WFD IE to search
+ * @wfd_ielen: Length limit from wfd_ie
+ * @target_attr_id: The attribute ID of WFD attribute to search
+ * @buf_attr: If not NULL and the WFD attribute is found, WFD attribute will be copied to the buf starting from buf_attr
+ * @len_attr: If not NULL and the WFD attribute is found, will set to the length of the entire WFD attribute
+ *
+ * Returns: the address of the specific WPS attribute found, or NULL
+ */
+u8 *rtw_get_wfd_attr(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id, u8 *buf_attr, u32 *len_attr)
+{
+	u8 *attr_ptr = NULL;
+	u8 *target_attr_ptr = NULL;
+	u8 wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A};
+
+	if (len_attr)
+		*len_attr = 0;
+
+	if (!wfd_ie
+	    || wfd_ielen <= 6
+	    || (wfd_ie[0] != WLAN_EID_VENDOR_SPECIFIC)
+	    || (memcmp(wfd_ie + 2, wfd_oui, 4)))
+		return attr_ptr;
+
+	/* 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */
+	attr_ptr = wfd_ie + 6; /* goto first attr */
+
+	while ((attr_ptr - wfd_ie + 3) <= wfd_ielen) {
+		/* 3 = 1(Attribute ID) + 2(Length) */
+		u8 attr_id = *attr_ptr;
+		u16 attr_data_len = RTW_GET_BE16(attr_ptr + 1);
+		u16 attr_len = attr_data_len + 3;
+
+		if (0)
+			RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len);
+
+		if ((attr_ptr - wfd_ie + attr_len) > wfd_ielen)
+			break;
+
+		if (attr_id == target_attr_id) {
+			target_attr_ptr = attr_ptr;
+
+			if (buf_attr)
+				memcpy(buf_attr, attr_ptr, attr_len);
+
+			if (len_attr)
+				*len_attr = attr_len;
+
+			break;
+		} else
+			attr_ptr += attr_len;
+	}
+
+	return target_attr_ptr;
+}
+
+/**
+ * rtw_get_wfd_attr_content - Search a specific WFD attribute content from a given WFD IE
+ * @wfd_ie: Address of WFD IE to search
+ * @wfd_ielen: Length limit from wfd_ie
+ * @target_attr_id: The attribute ID of WFD attribute to search
+ * @buf_content: If not NULL and the WFD attribute is found, WFD attribute content will be copied to the buf starting from buf_content
+ * @len_content: If not NULL and the WFD attribute is found, will set to the length of the WFD attribute content
+ *
+ * Returns: the address of the specific WFD attribute content found, or NULL
+ */
+u8 *rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id, u8 *buf_content, uint *len_content)
+{
+	u8 *attr_ptr;
+	u32 attr_len;
+
+	if (len_content)
+		*len_content = 0;
+
+	attr_ptr = rtw_get_wfd_attr(wfd_ie, wfd_ielen, target_attr_id, NULL, &attr_len);
+
+	if (attr_ptr && attr_len) {
+		if (buf_content)
+			memcpy(buf_content, attr_ptr + 3, attr_len - 3);
+
+		if (len_content)
+			*len_content = attr_len - 3;
+
+		return attr_ptr + 3;
+	}
+
+	return NULL;
+}
+
+uint rtw_del_wfd_ie(u8 *ies, uint ies_len_ori, const char *msg)
+{
+#define DBG_DEL_WFD_IE 0
+
+	u8 *target_ie;
+	u32 target_ie_len;
+	uint ies_len = ies_len_ori;
+	int index = 0;
+
+	while (1) {
+		target_ie = rtw_get_wfd_ie(ies, ies_len, NULL, &target_ie_len);
+		if (target_ie && target_ie_len) {
+			u8 *next_ie = target_ie + target_ie_len;
+			uint remain_len = ies_len - (next_ie - ies);
+
+			if (DBG_DEL_WFD_IE && msg) {
+				RTW_INFO("%s %d before\n", __func__, index);
+				dump_ies(RTW_DBGDUMP, ies, ies_len);
+
+				RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
+				RTW_INFO("target_ie:%p, target_ie_len:%u\n", target_ie, target_ie_len);
+				RTW_INFO("next_ie:%p, remain_len:%u\n", next_ie, remain_len);
+			}
+
+			memmove(target_ie, next_ie, remain_len);
+			memset(target_ie + remain_len, 0, target_ie_len);
+			ies_len -= target_ie_len;
+
+			if (DBG_DEL_WFD_IE && msg) {
+				RTW_INFO("%s %d after\n", __func__, index);
+				dump_ies(RTW_DBGDUMP, ies, ies_len);
+			}
+
+			index++;
+		} else
+			break;
+	}
+
+	return ies_len;
+}
+
+uint rtw_del_wfd_attr(u8 *ie, uint ielen_ori, u8 attr_id)
+{
+#define DBG_DEL_WFD_ATTR 0
+
+	u8 *target_attr;
+	u32 target_attr_len;
+	uint ielen = ielen_ori;
+	int index = 0;
+
+	while (1) {
+		target_attr = rtw_get_wfd_attr(ie, ielen, attr_id, NULL, &target_attr_len);
+		if (target_attr && target_attr_len) {
+			u8 *next_attr = target_attr + target_attr_len;
+			uint remain_len = ielen - (next_attr - ie);
+
+			if (DBG_DEL_WFD_ATTR) {
+				RTW_INFO("%s %d before\n", __func__, index);
+				dump_ies(RTW_DBGDUMP, ie, ielen);
+
+				RTW_INFO("ie:%p, ielen:%u\n", ie, ielen);
+				RTW_INFO("target_attr:%p, target_attr_len:%u\n", target_attr, target_attr_len);
+				RTW_INFO("next_attr:%p, remain_len:%u\n", next_attr, remain_len);
+			}
+
+			memmove(target_attr, next_attr, remain_len);
+			memset(target_attr + remain_len, 0, target_attr_len);
+			*(ie + 1) -= target_attr_len;
+			ielen -= target_attr_len;
+
+			if (DBG_DEL_WFD_ATTR) {
+				RTW_INFO("%s %d after\n", __func__, index);
+				dump_ies(RTW_DBGDUMP, ie, ielen);
+			}
+
+			index++;
+		} else
+			break;
+	}
+
+	return ielen;
+}
+
+inline u8 *rtw_bss_ex_get_wfd_ie(WLAN_BSSID_EX *bss_ex, u8 *wfd_ie, uint *wfd_ielen)
+{
+	return rtw_get_wfd_ie(BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex), wfd_ie, wfd_ielen);
+}
+
+void rtw_bss_ex_del_wfd_ie(WLAN_BSSID_EX *bss_ex)
+{
+#define DBG_BSS_EX_DEL_WFD_IE 0
+	u8 *ies = BSS_EX_TLV_IES(bss_ex);
+	uint ies_len_ori = BSS_EX_TLV_IES_LEN(bss_ex);
+	uint ies_len;
+
+	ies_len = rtw_del_wfd_ie(ies, ies_len_ori, DBG_BSS_EX_DEL_WFD_IE ? __func__ : NULL);
+	bss_ex->IELength -= ies_len_ori - ies_len;
+}
+
+void rtw_bss_ex_del_wfd_attr(WLAN_BSSID_EX *bss_ex, u8 attr_id)
+{
+#define DBG_BSS_EX_DEL_WFD_ATTR 0
+
+	u8 *ies = BSS_EX_TLV_IES(bss_ex);
+	uint ies_len = BSS_EX_TLV_IES_LEN(bss_ex);
+
+	u8 *ie;
+	uint ie_len, ie_len_ori;
+
+	int index = 0;
+
+	while (1) {
+		ie = rtw_get_wfd_ie(ies, ies_len, NULL, &ie_len_ori);
+		if (ie) {
+			u8 *next_ie_ori = ie + ie_len_ori;
+			uint remain_len = bss_ex->IELength - (next_ie_ori - bss_ex->IEs);
+			u8 has_target_attr = 0;
+
+			if (DBG_BSS_EX_DEL_WFD_ATTR) {
+				if (rtw_get_wfd_attr(ie, ie_len_ori, attr_id, NULL, NULL)) {
+					RTW_INFO("%s %d before\n", __func__, index);
+					dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
+
+					RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
+					RTW_INFO("ie:%p, ie_len_ori:%u\n", ie, ie_len_ori);
+					RTW_INFO("next_ie_ori:%p, remain_len:%u\n", next_ie_ori, remain_len);
+					has_target_attr = 1;
+				}
+			}
+
+			ie_len = rtw_del_wfd_attr(ie, ie_len_ori, attr_id);
+			if (ie_len != ie_len_ori) {
+				u8 *next_ie = ie + ie_len;
+
+				memmove(next_ie, next_ie_ori, remain_len);
+				memset(next_ie + remain_len, 0, ie_len_ori - ie_len);
+				bss_ex->IELength -= ie_len_ori - ie_len;
+
+				ies = next_ie;
+			} else
+				ies = next_ie_ori;
+
+			if (DBG_BSS_EX_DEL_WFD_ATTR) {
+				if (has_target_attr) {
+					RTW_INFO("%s %d after\n", __func__, index);
+					dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
+				}
+			}
+
+			ies_len = remain_len;
+
+			index++;
+		} else
+			break;
+	}
+}
+
+/* Baron adds to avoid FreeBSD warning */
+int ieee80211_is_empty_essid(const char *essid, int essid_len)
+{
+	/* Single white space is for Linksys APs */
+	if (essid_len == 1 && essid[0] == ' ')
+		return 1;
+
+	/* Otherwise, if the entire essid is 0, we assume it is hidden */
+	while (essid_len) {
+		essid_len--;
+		if (essid[essid_len] != '\0')
+			return 0;
+	}
+
+	return 1;
+}
+
+int ieee80211_get_hdrlen(u16 fc)
+{
+	int hdrlen = 24;
+
+	switch (WLAN_FC_GET_TYPE(fc)) {
+	case RTW_IEEE80211_FTYPE_DATA:
+		if (fc & RTW_IEEE80211_STYPE_QOS_DATA)
+			hdrlen += 2;
+		if ((fc & RTW_IEEE80211_FCTL_FROMDS) && (fc & RTW_IEEE80211_FCTL_TODS))
+			hdrlen += 6; /* Addr4 */
+		break;
+	case RTW_IEEE80211_FTYPE_CTL:
+		switch (WLAN_FC_GET_STYPE(fc)) {
+		case RTW_IEEE80211_STYPE_CTS:
+		case RTW_IEEE80211_STYPE_ACK:
+			hdrlen = 10;
+			break;
+		default:
+			hdrlen = 16;
+			break;
+		}
+		break;
+	}
+
+	return hdrlen;
+}
+
+static int rtw_get_cipher_info(struct wlan_network *pnetwork)
+{
+	u32 wpa_ielen;
+	unsigned char *pbuf;
+	int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
+	int ret = _FAIL;
+	pbuf = rtw_get_wpa_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength - 12);
+
+	if (pbuf && (wpa_ielen > 0)) {
+		if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen + 2, &group_cipher, &pairwise_cipher, &is8021x)) {
+
+			pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+			pnetwork->BcnInfo.group_cipher = group_cipher;
+			pnetwork->BcnInfo.is_8021x = is8021x;
+			ret = _SUCCESS;
+		}
+	} else {
+
+		pbuf = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength - 12);
+
+		if (pbuf && (wpa_ielen > 0)) {
+			if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen + 2, &group_cipher, &pairwise_cipher, &is8021x)) {
+				pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+				pnetwork->BcnInfo.group_cipher = group_cipher;
+				pnetwork->BcnInfo.is_8021x = is8021x;
+				ret = _SUCCESS;
+			}
+		}
+	}
+
+	return ret;
+}
+
+void rtw_get_bcn_info(struct wlan_network *pnetwork)
+{
+	unsigned short cap = 0;
+	__le16 le_cap;
+	u8 bencrypt = 0;
+	/* u8 wpa_ie[255],rsn_ie[255]; */
+	u16 wpa_len = 0, rsn_len = 0;
+	struct HT_info_element *pht_info = NULL;
+	struct rtw_ieee80211_ht_cap *pht_cap = NULL;
+	unsigned int		len;
+	unsigned char		*p;
+
+	memcpy((u8 *)&le_cap, rtw_get_capability_from_ie(pnetwork->network.IEs), 2);
+	cap = le16_to_cpu(le_cap);
+	if (cap & WLAN_CAPABILITY_PRIVACY) {
+		bencrypt = 1;
+		pnetwork->network.Privacy = 1;
+	} else
+		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
+	rtw_get_sec_ie(pnetwork->network.IEs , pnetwork->network.IELength, NULL, &rsn_len, NULL, &wpa_len);
+
+	if (rsn_len > 0)
+		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+	else if (wpa_len > 0)
+		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA;
+	else {
+		if (bencrypt)
+			pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
+	}
+	rtw_get_cipher_info(pnetwork);
+
+	/* get bwmode and ch_offset */
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+		pht_cap = (struct rtw_ieee80211_ht_cap *)(p + 2);
+		pnetwork->BcnInfo.ht_cap_info = le16_to_cpu(pht_cap->cap_info);
+	} else
+		pnetwork->BcnInfo.ht_cap_info = 0;
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+		pht_info = (struct HT_info_element *)(p + 2);
+		pnetwork->BcnInfo.ht_info_infos_0 = pht_info->infos[0];
+	} else
+		pnetwork->BcnInfo.ht_info_infos_0 = 0;
+}
+
+u8	rtw_ht_mcsset_to_nss(u8 *supp_mcs_set)
+{
+	u8 nss = 1;
+
+	if (supp_mcs_set[3])
+		nss = 4;
+	else if (supp_mcs_set[2])
+		nss = 3;
+	else if (supp_mcs_set[1])
+		nss = 2;
+	else if (supp_mcs_set[0])
+		nss = 1;
+	else
+		RTW_INFO("%s,%d, warning! supp_mcs_set is zero\n", __func__, __LINE__);
+	/* RTW_INFO("%s HT: %dSS\n", __func__, nss); */
+	return nss;
+}
+
+u32	rtw_ht_mcs_set_to_bitmap(u8 *mcs_set, u8 nss)
+{
+	u8 i;
+	u32 bitmap = 0;
+
+	for (i = 0; i < nss; i++)
+		bitmap |= mcs_set[i] << (i * 8);
+
+	RTW_INFO("ht_mcs_set=%02x %02x %02x %02x, nss=%u, bitmap=%08x\n"
+		, mcs_set[0], mcs_set[1], mcs_set[2], mcs_set[3], nss, bitmap);
+
+	return bitmap;
+}
+
+/* show MCS rate, unit: 100Kbps */
+u16 rtw_mcs_rate(u8 rf_type, u8 bw_40MHz, u8 short_GI, unsigned char *MCS_rate)
+{
+	u16 max_rate = 0;
+
+	if (MCS_rate[3]) {
+		if (MCS_rate[3] & BIT(7))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 6000 : 5400) : ((short_GI) ? 2889 : 2600);
+		else if (MCS_rate[3] & BIT(6))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 5400 : 4860) : ((short_GI) ? 2600 : 2340);
+		else if (MCS_rate[3] & BIT(5))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 4800 : 4320) : ((short_GI) ? 2311 : 2080);
+		else if (MCS_rate[3] & BIT(4))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 3600 : 3240) : ((short_GI) ? 1733 : 1560);
+		else if (MCS_rate[3] & BIT(3))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 2400 : 2160) : ((short_GI) ? 1156 : 1040);
+		else if (MCS_rate[3] & BIT(2))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780);
+		else if (MCS_rate[3] & BIT(1))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520);
+		else if (MCS_rate[3] & BIT(0))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260);
+	} else if (MCS_rate[2]) {
+		if (MCS_rate[2] & BIT(7))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 4500 : 4050) : ((short_GI) ? 2167 : 1950);
+		else if (MCS_rate[2] & BIT(6))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 4050 : 3645) : ((short_GI) ? 1950 : 1750);
+		else if (MCS_rate[2] & BIT(5))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 3600 : 3240) : ((short_GI) ? 1733 : 1560);
+		else if (MCS_rate[2] & BIT(4))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 2700 : 2430) : ((short_GI) ? 1300 : 1170);
+		else if (MCS_rate[2] & BIT(3))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780);
+		else if (MCS_rate[2] & BIT(2))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 1350 : 1215) : ((short_GI) ? 650 : 585);
+		else if (MCS_rate[2] & BIT(1))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390);
+		else if (MCS_rate[2] & BIT(0))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 450 : 405) : ((short_GI) ? 217 : 195);
+	} else if (MCS_rate[1]) {
+		if (MCS_rate[1] & BIT(7))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 3000 : 2700) : ((short_GI) ? 1444 : 1300);
+		else if (MCS_rate[1] & BIT(6))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 2700 : 2430) : ((short_GI) ? 1300 : 1170);
+		else if (MCS_rate[1] & BIT(5))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 2400 : 2160) : ((short_GI) ? 1156 : 1040);
+		else if (MCS_rate[1] & BIT(4))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780);
+		else if (MCS_rate[1] & BIT(3))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520);
+		else if (MCS_rate[1] & BIT(2))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390);
+		else if (MCS_rate[1] & BIT(1))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260);
+		else if (MCS_rate[1] & BIT(0))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 300 : 270) : ((short_GI) ? 144 : 130);
+	} else {
+		if (MCS_rate[0] & BIT(7))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 1500 : 1350) : ((short_GI) ? 722 : 650);
+		else if (MCS_rate[0] & BIT(6))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 1350 : 1215) : ((short_GI) ? 650 : 585);
+		else if (MCS_rate[0] & BIT(5))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520);
+		else if (MCS_rate[0] & BIT(4))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390);
+		else if (MCS_rate[0] & BIT(3))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260);
+		else if (MCS_rate[0] & BIT(2))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 450 : 405) : ((short_GI) ? 217 : 195);
+		else if (MCS_rate[0] & BIT(1))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 300 : 270) : ((short_GI) ? 144 : 130);
+		else if (MCS_rate[0] & BIT(0))
+			max_rate = (bw_40MHz) ? ((short_GI) ? 150 : 135) : ((short_GI) ? 72 : 65);
+	}
+
+	return max_rate;
+}
+
+int rtw_action_frame_parse(const u8 *frame, u32 frame_len, u8 *category, u8 *action)
+{
+	const u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u16 fc;
+	u8 c;
+	u8 a = ACT_PUBLIC_MAX;
+
+	fc = le16_to_cpu(((struct rtw_ieee80211_hdr_3addr *)frame)->frame_ctl);
+
+	if ((fc & (RTW_IEEE80211_FCTL_FTYPE | RTW_IEEE80211_FCTL_STYPE))
+	    != (RTW_IEEE80211_FTYPE_MGMT | RTW_IEEE80211_STYPE_ACTION)
+	   )
+		return false;
+
+	c = frame_body[0];
+
+	switch (c) {
+	case RTW_WLAN_CATEGORY_P2P: /* vendor-specific */
+		break;
+	default:
+		a = frame_body[1];
+	}
+
+	if (category)
+		*category = c;
+	if (action)
+		*action = a;
+
+	return true;
+}
+
+static const char *_action_public_str[] = {
+	"ACT_PUB_BSSCOEXIST",
+	"ACT_PUB_DSE_ENABLE",
+	"ACT_PUB_DSE_DEENABLE",
+	"ACT_PUB_DSE_REG_LOCATION",
+	"ACT_PUB_EXT_CHL_SWITCH",
+	"ACT_PUB_DSE_MSR_REQ",
+	"ACT_PUB_DSE_MSR_RPRT",
+	"ACT_PUB_MP",
+	"ACT_PUB_DSE_PWR_CONSTRAINT",
+	"ACT_PUB_VENDOR",
+	"ACT_PUB_GAS_INITIAL_REQ",
+	"ACT_PUB_GAS_INITIAL_RSP",
+	"ACT_PUB_GAS_COMEBACK_REQ",
+	"ACT_PUB_GAS_COMEBACK_RSP",
+	"ACT_PUB_TDLS_DISCOVERY_RSP",
+	"ACT_PUB_LOCATION_TRACK",
+	"ACT_PUB_RSVD",
+};
+
+const char *action_public_str(u8 action)
+{
+	action = (action >= ACT_PUBLIC_MAX) ? ACT_PUBLIC_MAX : action;
+	return _action_public_str[action];
+}
+
diff --git a/drivers/staging/rtl8188eu/core/rtw_io.c b/drivers/staging/rtl8188eu/core/rtw_io.c
new file mode 100644
index 000000000000..d6639cec3bb8
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_io.c
@@ -0,0 +1,473 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+/*
+
+The purpose of rtw_io.c
+
+a. provides the API
+
+b. provides the protocol engine
+
+c. provides the software interface between caller and the hardware interface
+
+
+Compiler Flag Option:
+
+2. CONFIG_USB_HCI:
+   a. USE_ASYNC_IRP: Both sync/async operations are provided.
+Only sync read/rtw_write_mem operations are provided.
+
+jackson@xxxxxxxxxxxxxx
+
+*/
+
+#define _RTW_IO_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+	#define rtw_le16_to_cpu(val)		le16_to_cpu(val)
+	#define rtw_le32_to_cpu(val)		le32_to_cpu(val)
+	#define rtw_cpu_to_le16(val)		cpu_to_le16(val)
+	#define rtw_cpu_to_le32(val)		cpu_to_le32(val)
+
+u8 _rtw_read8(_adapter *adapter, u32 addr)
+{
+	u8 r_val;
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u8(*_read8)(struct intf_hdl *pintfhdl, u32 addr);
+	_read8 = pintfhdl->io_ops._read8;
+
+	r_val = _read8(pintfhdl, addr);
+	return r_val;
+}
+
+u16 _rtw_read16(_adapter *adapter, u32 addr)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u16(*_read16)(struct intf_hdl *pintfhdl, u32 addr);
+	_read16 = pintfhdl->io_ops._read16;
+
+	return _read16(pintfhdl, addr);
+}
+
+u32 _rtw_read32(_adapter *adapter, u32 addr)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u32(*_read32)(struct intf_hdl *pintfhdl, u32 addr);
+	_read32 = pintfhdl->io_ops._read32;
+
+	return _read32(pintfhdl, addr);
+}
+
+int _rtw_write8(_adapter *adapter, u32 addr, u8 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write8)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+	int ret;
+	_write8 = pintfhdl->io_ops._write8;
+
+	ret = _write8(pintfhdl, addr, val);
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write16(_adapter *adapter, u32 addr, u16 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write16)(struct intf_hdl *pintfhdl, u32 addr, __le16 val);
+	int ret;
+	__le16 outval;
+
+	_write16 = pintfhdl->io_ops._write16;
+
+	outval = rtw_cpu_to_le16(val);
+	ret = _write16(pintfhdl, addr, outval);
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write32(_adapter *adapter, u32 addr, u32 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write32)(struct intf_hdl *pintfhdl, u32 addr, __le32 val);
+	int ret;
+	__le32 outval;
+
+	_write32 = pintfhdl->io_ops._write32;
+
+	outval = rtw_cpu_to_le32(val);
+	ret = _write32(pintfhdl, addr, outval);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+int _rtw_writeN(_adapter *adapter, u32 addr , u32 length , u8 *pdata)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl	*pintfhdl = (struct intf_hdl *)(&(pio_priv->intf));
+	int (*_writeN)(struct intf_hdl *pintfhdl, u32 addr, u32 length, u8 *pdata);
+	int ret;
+	_writeN = pintfhdl->io_ops._writeN;
+
+	ret = _writeN(pintfhdl, addr, length, pdata);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+int _rtw_write8_async(_adapter *adapter, u32 addr, u8 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write8_async)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+	int ret;
+	_write8_async = pintfhdl->io_ops._write8_async;
+
+	ret = _write8_async(pintfhdl, addr, val);
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write16_async(_adapter *adapter, u32 addr, u16 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write16_async)(struct intf_hdl *pintfhdl, u32 addr, __le16 val);
+	int ret;
+	__le16 outval;
+
+	_write16_async = pintfhdl->io_ops._write16_async;
+	outval = rtw_cpu_to_le16(val);
+	ret = _write16_async(pintfhdl, addr, outval);
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write32_async(_adapter *adapter, u32 addr, u32 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write32_async)(struct intf_hdl *pintfhdl, u32 addr, __le32 val);
+	int ret;
+	__le32 outval;
+
+	_write32_async = pintfhdl->io_ops._write32_async;
+	outval = rtw_cpu_to_le32(val);
+	ret = _write32_async(pintfhdl, addr, outval);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+void _rtw_read_mem(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	void (*_read_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+
+
+	if (RTW_CANNOT_RUN(adapter)) {
+		return;
+	}
+
+	_read_mem = pintfhdl->io_ops._read_mem;
+
+	_read_mem(pintfhdl, addr, cnt, pmem);
+
+
+}
+
+void _rtw_write_mem(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	void (*_write_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+
+
+	_write_mem = pintfhdl->io_ops._write_mem;
+
+	_write_mem(pintfhdl, addr, cnt, pmem);
+
+
+}
+
+void _rtw_read_port(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	u32(*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+
+
+	if (RTW_CANNOT_RUN(adapter)) {
+		return;
+	}
+
+	_read_port = pintfhdl->io_ops._read_port;
+
+	_read_port(pintfhdl, addr, cnt, pmem);
+
+
+}
+
+void _rtw_read_port_cancel(_adapter *adapter)
+{
+	void (*_read_port_cancel)(struct intf_hdl *pintfhdl);
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+
+	_read_port_cancel = pintfhdl->io_ops._read_port_cancel;
+
+	RTW_DISABLE_FUNC(adapter, DF_RX_BIT);
+
+	if (_read_port_cancel)
+		_read_port_cancel(pintfhdl);
+}
+
+u32 _rtw_write_port(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	u32(*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u32 ret = _SUCCESS;
+
+
+	_write_port = pintfhdl->io_ops._write_port;
+
+	ret = _write_port(pintfhdl, addr, cnt, pmem);
+
+
+	return ret;
+}
+
+u32 _rtw_write_port_and_wait(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem, int timeout_ms)
+{
+	int ret = _SUCCESS;
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)pmem;
+	struct submit_ctx sctx;
+
+	rtw_sctx_init(&sctx, timeout_ms);
+	pxmitbuf->sctx = &sctx;
+
+	ret = _rtw_write_port(adapter, addr, cnt, pmem);
+
+	if (ret == _SUCCESS)
+		ret = rtw_sctx_wait(&sctx, __func__);
+
+	return ret;
+}
+
+void _rtw_write_port_cancel(_adapter *adapter)
+{
+	void (*_write_port_cancel)(struct intf_hdl *pintfhdl);
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+
+	_write_port_cancel = pintfhdl->io_ops._write_port_cancel;
+
+	RTW_DISABLE_FUNC(adapter, DF_TX_BIT);
+
+	if (_write_port_cancel)
+		_write_port_cancel(pintfhdl);
+}
+int rtw_init_io_priv(_adapter *padapter, void (*set_intf_ops)(_adapter *padapter, struct _io_ops *pops))
+{
+	struct io_priv	*piopriv = &padapter->iopriv;
+	struct intf_hdl *pintf = &piopriv->intf;
+
+	if (set_intf_ops == NULL)
+		return _FAIL;
+
+	piopriv->padapter = padapter;
+	pintf->padapter = padapter;
+	pintf->pintf_dev = adapter_to_dvobj(padapter);
+
+	set_intf_ops(padapter, &pintf->io_ops);
+
+	return _SUCCESS;
+}
+
+/*
+* Increase and check if the continual_io_error of this @param dvobjprive is larger than MAX_CONTINUAL_IO_ERR
+* @return true:
+* @return false:
+*/
+int rtw_inc_and_chk_continual_io_error(struct dvobj_priv *dvobj)
+{
+	int ret = false;
+	int value;
+
+	value = ATOMIC_INC_RETURN(&dvobj->continual_io_error);
+	if (value > MAX_CONTINUAL_IO_ERR) {
+		RTW_INFO("[dvobj:%p][ERROR] continual_io_error:%d > %d\n", dvobj, value, MAX_CONTINUAL_IO_ERR);
+		ret = true;
+	} else {
+		/* RTW_INFO("[dvobj:%p] continual_io_error:%d\n", dvobj, value); */
+	}
+	return ret;
+}
+
+/*
+* Set the continual_io_error of this @param dvobjprive to 0
+*/
+void rtw_reset_continual_io_error(struct dvobj_priv *dvobj)
+{
+	ATOMIC_SET(&dvobj->continual_io_error, 0);
+}
+
+#ifdef DBG_IO
+
+u32 read_sniff_ranges[][2] = {
+	/* {0x520, 0x523}, */
+};
+
+u32 write_sniff_ranges[][2] = {
+	/* {0x520, 0x523}, */
+	/* {0x4c, 0x4c}, */
+};
+
+int read_sniff_num = sizeof(read_sniff_ranges) / sizeof(u32) / 2;
+int write_sniff_num = sizeof(write_sniff_ranges) / sizeof(u32) / 2;
+
+bool match_read_sniff_ranges(u32 addr, u16 len)
+{
+	int i;
+	for (i = 0; i < read_sniff_num; i++) {
+		if (addr + len > read_sniff_ranges[i][0] && addr <= read_sniff_ranges[i][1])
+			return true;
+	}
+
+	return false;
+}
+
+bool match_write_sniff_ranges(u32 addr, u16 len)
+{
+	int i;
+	for (i = 0; i < write_sniff_num; i++) {
+		if (addr + len > write_sniff_ranges[i][0] && addr <= write_sniff_ranges[i][1])
+			return true;
+	}
+
+	return false;
+}
+
+struct rf_sniff_ent {
+	u8 path;
+	u16 reg;
+	u32 mask;
+};
+
+struct rf_sniff_ent rf_read_sniff_ranges[] = {
+	/* example for all path addr 0x55 with all RF Reg mask */
+	/* {MAX_RF_PATH, 0x55, bRFRegOffsetMask}, */
+};
+
+struct rf_sniff_ent rf_write_sniff_ranges[] = {
+	/* example for all path addr 0x55 with all RF Reg mask */
+	/* {MAX_RF_PATH, 0x55, bRFRegOffsetMask}, */
+};
+
+int rf_read_sniff_num = sizeof(rf_read_sniff_ranges) / sizeof(struct rf_sniff_ent);
+int rf_write_sniff_num = sizeof(rf_write_sniff_ranges) / sizeof(struct rf_sniff_ent);
+
+bool match_rf_read_sniff_ranges(u8 path, u32 addr, u32 mask)
+{
+	int i;
+
+	for (i = 0; i < rf_read_sniff_num; i++) {
+		if (rf_read_sniff_ranges[i].path == MAX_RF_PATH || rf_read_sniff_ranges[i].path == path)
+			if (addr == rf_read_sniff_ranges[i].reg && (mask & rf_read_sniff_ranges[i].mask))
+				return true;
+	}
+
+	return false;
+}
+
+bool match_rf_write_sniff_ranges(u8 path, u32 addr, u32 mask)
+{
+	int i;
+
+	for (i = 0; i < rf_write_sniff_num; i++) {
+		if (rf_write_sniff_ranges[i].path == MAX_RF_PATH || rf_write_sniff_ranges[i].path == path)
+			if (addr == rf_write_sniff_ranges[i].reg && (mask & rf_write_sniff_ranges[i].mask))
+				return true;
+	}
+
+	return false;
+}
+
+u8 dbg_rtw_read8(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u8 val = _rtw_read8(adapter, addr);
+
+	if (match_read_sniff_ranges(addr, 1))
+		RTW_INFO("DBG_IO %s:%d rtw_read8(0x%04x) return 0x%02x\n", caller, line, addr, val);
+
+	return val;
+}
+
+u16 dbg_rtw_read16(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u16 val = _rtw_read16(adapter, addr);
+
+	if (match_read_sniff_ranges(addr, 2))
+		RTW_INFO("DBG_IO %s:%d rtw_read16(0x%04x) return 0x%04x\n", caller, line, addr, val);
+
+	return val;
+}
+
+u32 dbg_rtw_read32(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u32 val = _rtw_read32(adapter, addr);
+
+	if (match_read_sniff_ranges(addr, 4))
+		RTW_INFO("DBG_IO %s:%d rtw_read32(0x%04x) return 0x%08x\n", caller, line, addr, val);
+
+	return val;
+}
+
+int dbg_rtw_write8(_adapter *adapter, u32 addr, u8 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 1))
+		RTW_INFO("DBG_IO %s:%d rtw_write8(0x%04x, 0x%02x)\n", caller, line, addr, val);
+
+	return _rtw_write8(adapter, addr, val);
+}
+int dbg_rtw_write16(_adapter *adapter, u32 addr, u16 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 2))
+		RTW_INFO("DBG_IO %s:%d rtw_write16(0x%04x, 0x%04x)\n", caller, line, addr, val);
+
+	return _rtw_write16(adapter, addr, val);
+}
+int dbg_rtw_write32(_adapter *adapter, u32 addr, u32 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 4))
+		RTW_INFO("DBG_IO %s:%d rtw_write32(0x%04x, 0x%08x)\n", caller, line, addr, val);
+
+	return _rtw_write32(adapter, addr, val);
+}
+int dbg_rtw_writeN(_adapter *adapter, u32 addr , u32 length , u8 *data, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, length))
+		RTW_INFO("DBG_IO %s:%d rtw_writeN(0x%04x, %u)\n", caller, line, addr, length);
+
+	return _rtw_writeN(adapter, addr, length, data);
+}
+
+#endif
diff --git a/drivers/staging/rtl8188eu/core/rtw_ioctl_query.c b/drivers/staging/rtl8188eu/core/rtw_ioctl_query.c
new file mode 100644
index 000000000000..c8a8de75e396
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_ioctl_query.c
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_IOCTL_QUERY_C_
+
+
diff --git a/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
new file mode 100644
index 000000000000..9170b8cfc19c
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
@@ -0,0 +1,1035 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_IOCTL_SET_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+#include <rtw_mlme.h>
+
+#define IS_MAC_ADDRESS_BROADCAST(addr) \
+	(\
+	 ((addr[0] == 0xff) && (addr[1] == 0xff) && \
+	  (addr[2] == 0xff) && (addr[3] == 0xff) && \
+	  (addr[4] == 0xff) && (addr[5] == 0xff)) ? true : false \
+	)
+
+u8 rtw_validate_bssid(u8 *bssid)
+{
+	u8 ret = true;
+
+	if (is_zero_mac_addr(bssid)
+	    || is_broadcast_mac_addr(bssid)
+	    || is_multicast_mac_addr(bssid)
+	   )
+		ret = false;
+
+	return ret;
+}
+
+u8 rtw_validate_ssid(NDIS_802_11_SSID *ssid)
+{
+	u8	 i;
+	u8	ret = true;
+
+	if (ssid->SsidLength > 32) {
+		ret = false;
+		goto exit;
+	}
+
+#ifdef CONFIG_VALIDATE_SSID
+	for (i = 0; i < ssid->SsidLength; i++) {
+		/* wifi, printable ascii code must be supported */
+		if (!((ssid->Ssid[i] >= 0x20) && (ssid->Ssid[i] <= 0x7e))) {
+			ret = false;
+			break;
+		}
+	}
+#endif /* CONFIG_VALIDATE_SSID */
+
+exit:
+
+	return ret;
+}
+
+u8 rtw_do_join(_adapter *padapter);
+u8 rtw_do_join(_adapter *padapter)
+{
+	unsigned long	irqL;
+	_list	*plist, *phead;
+	u8 *pibss = NULL;
+	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	_queue	*queue	= &(pmlmepriv->scanned_queue);
+	u8 ret = _SUCCESS;
+
+	_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+	phead = get_list_head(queue);
+	plist = get_next(phead);
+
+	pmlmepriv->cur_network.join_res = -2;
+
+	set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+	pmlmepriv->pscanned = plist;
+
+	pmlmepriv->to_join = true;
+
+	if (_rtw_queue_empty(queue) ) {
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+		/* when set_ssid/set_bssid for rtw_do_join(), but scanning queue is empty */
+		/* we try to issue sitesurvey firstly	 */
+
+		if (pmlmepriv->LinkDetectInfo.bBusyTraffic == false
+		    || rtw_to_roam(padapter) > 0
+		   ) {
+			/* submit site_survey_cmd */
+			ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0);
+			if (_SUCCESS != ret) {
+				pmlmepriv->to_join = false;
+			}
+		} else {
+			pmlmepriv->to_join = false;
+			ret = _FAIL;
+		}
+
+		goto exit;
+	} else {
+		int select_ret;
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+		select_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+		if (select_ret == _SUCCESS) {
+			pmlmepriv->to_join = false;
+			_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+		} else {
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ) {
+				/* submit createbss_cmd to change to a ADHOC_MASTER */
+
+				/* pmlmepriv->lock has been acquired by caller... */
+				WLAN_BSSID_EX    *pdev_network = &(padapter->registrypriv.dev_network);
+
+				/*pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;*/
+				init_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+
+				pibss = padapter->registrypriv.dev_network.MacAddress;
+
+				memset(&pdev_network->Ssid, 0, sizeof(NDIS_802_11_SSID));
+				memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(NDIS_802_11_SSID));
+
+				rtw_update_registrypriv_dev_network(padapter);
+
+				rtw_generate_random_ibss(pibss);
+
+				if (rtw_create_ibss_cmd(padapter, 0) != _SUCCESS) {
+					ret =  false;
+					goto exit;
+				}
+
+				pmlmepriv->to_join = false;
+
+			} else {
+				/* can't associate ; reset under-linking			 */
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+				/* when set_ssid/set_bssid for rtw_do_join(), but there are no desired bss in scanning queue */
+				/* we try to issue sitesurvey firstly			 */
+				if (pmlmepriv->LinkDetectInfo.bBusyTraffic == false
+				    || rtw_to_roam(padapter) > 0
+				   ) {
+					/* RTW_INFO("rtw_do_join() when   no desired bss in scanning queue\n"); */
+					ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0);
+					if (_SUCCESS != ret) {
+						pmlmepriv->to_join = false;
+					}
+				} else {
+					ret = _FAIL;
+					pmlmepriv->to_join = false;
+				}
+			}
+
+		}
+
+	}
+
+exit:
+
+	return ret;
+}
+
+u8 rtw_set_802_11_bssid(_adapter *padapter, u8 *bssid)
+{
+	unsigned long irqL;
+	u8 status = _SUCCESS;
+
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	RTW_PRINT("set bssid:%pM\n", bssid);
+
+	if ((bssid[0] == 0x00 && bssid[1] == 0x00 && bssid[2] == 0x00 && bssid[3] == 0x00 && bssid[4] == 0x00 && bssid[5] == 0x00) ||
+	    (bssid[0] == 0xFF && bssid[1] == 0xFF && bssid[2] == 0xFF && bssid[3] == 0xFF && bssid[4] == 0xFF && bssid[5] == 0xFF)) {
+		status = _FAIL;
+		goto exit;
+	}
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	RTW_INFO("Set BSSID under fw_state=0x%08x\n", get_fwstate(pmlmepriv));
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) )
+		goto handle_tkip_countermeasure;
+	else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) )
+		goto release_mlme_lock;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE) ) {
+
+		if (!memcmp(&pmlmepriv->cur_network.network.MacAddress, bssid, ETH_ALEN) ) {
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false)
+				goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */
+		} else {
+
+			rtw_disassoc_cmd(padapter, 0, true);
+
+			if (check_fwstate(pmlmepriv, _FW_LINKED) )
+				rtw_indicate_disconnect(padapter, 0, false);
+
+			rtw_free_assoc_resources(padapter, 1);
+
+			if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) )) {
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+		}
+	}
+
+handle_tkip_countermeasure:
+	if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) {
+		status = _FAIL;
+		goto release_mlme_lock;
+	}
+
+	memset(&pmlmepriv->assoc_ssid, 0, sizeof(NDIS_802_11_SSID));
+	memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN);
+	pmlmepriv->assoc_by_bssid = true;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) )
+		pmlmepriv->to_join = true;
+	else
+		status = rtw_do_join(padapter);
+
+release_mlme_lock:
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+exit:
+
+	return status;
+}
+
+u8 rtw_set_802_11_ssid(_adapter *padapter, NDIS_802_11_SSID *ssid)
+{
+	unsigned long irqL;
+	u8 status = _SUCCESS;
+	u32 cur_time = 0;
+
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *pnetwork = &pmlmepriv->cur_network;
+
+	RTW_PRINT("set ssid [%s] fw_state=0x%08x\n",
+		  ssid->Ssid, get_fwstate(pmlmepriv));
+
+	if (!rtw_is_hw_init_completed(padapter)) {
+		status = _FAIL;
+		goto exit;
+	}
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	RTW_INFO("Set SSID under fw_state=0x%08x\n", get_fwstate(pmlmepriv));
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) )
+		goto handle_tkip_countermeasure;
+	else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) )
+		goto release_mlme_lock;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE) ) {
+
+		if ((pmlmepriv->assoc_ssid.SsidLength == ssid->SsidLength) &&
+		    (!memcmp(&pmlmepriv->assoc_ssid.Ssid, ssid->Ssid, ssid->SsidLength) )) {
+			if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false)) {
+
+				if (rtw_is_same_ibss(padapter, pnetwork) == false) {
+					/* if in WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE, create bss or rejoin again */
+					rtw_disassoc_cmd(padapter, 0, true);
+
+					if (check_fwstate(pmlmepriv, _FW_LINKED) )
+						rtw_indicate_disconnect(padapter, 0, false);
+
+					rtw_free_assoc_resources(padapter, 1);
+
+					if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ) {
+						_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+						set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+					}
+				} else {
+					goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */
+				}
+			}
+#ifdef CONFIG_LPS
+			else
+				rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_JOINBSS, 1);
+#endif
+		} else {
+
+			rtw_disassoc_cmd(padapter, 0, true);
+
+			if (check_fwstate(pmlmepriv, _FW_LINKED) )
+				rtw_indicate_disconnect(padapter, 0, false);
+
+			rtw_free_assoc_resources(padapter, 1);
+
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ) {
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+		}
+	}
+
+handle_tkip_countermeasure:
+	if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) {
+		status = _FAIL;
+		goto release_mlme_lock;
+	}
+
+	if (rtw_validate_ssid(ssid) == false) {
+		status = _FAIL;
+		goto release_mlme_lock;
+	}
+
+	memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(NDIS_802_11_SSID));
+	pmlmepriv->assoc_by_bssid = false;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) )
+		pmlmepriv->to_join = true;
+	else
+		status = rtw_do_join(padapter);
+
+release_mlme_lock:
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+exit:
+
+	return status;
+
+}
+
+u8 rtw_set_802_11_connect(_adapter *padapter, u8 *bssid, NDIS_802_11_SSID *ssid)
+{
+	unsigned long irqL;
+	u8 status = _SUCCESS;
+	u32 cur_time = 0;
+	bool bssid_valid = true;
+	bool ssid_valid = true;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (!ssid || rtw_validate_ssid(ssid) == false)
+		ssid_valid = false;
+
+	if (!bssid || rtw_validate_bssid(bssid) == false)
+		bssid_valid = false;
+
+	if (ssid_valid == false && bssid_valid == false) {
+		RTW_INFO(FUNC_ADPT_FMT" ssid:%p, ssid_valid:%d, bssid:%p, bssid_valid:%d\n",
+			FUNC_ADPT_ARG(padapter), ssid, ssid_valid, bssid, bssid_valid);
+		status = _FAIL;
+		goto exit;
+	}
+
+	if (!rtw_is_hw_init_completed(padapter)) {
+		status = _FAIL;
+		goto exit;
+	}
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	RTW_PRINT(FUNC_ADPT_FMT"  fw_state=0x%08x\n",
+		  FUNC_ADPT_ARG(padapter), get_fwstate(pmlmepriv));
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) )
+		goto handle_tkip_countermeasure;
+	else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) )
+		goto release_mlme_lock;
+
+handle_tkip_countermeasure:
+	if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) {
+		status = _FAIL;
+		goto release_mlme_lock;
+	}
+
+	if (ssid && ssid_valid)
+		memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(NDIS_802_11_SSID));
+	else
+		memset(&pmlmepriv->assoc_ssid, 0, sizeof(NDIS_802_11_SSID));
+
+	if (bssid && bssid_valid) {
+		memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN);
+		pmlmepriv->assoc_by_bssid = true;
+	} else
+		pmlmepriv->assoc_by_bssid = false;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) )
+		pmlmepriv->to_join = true;
+	else
+		status = rtw_do_join(padapter);
+
+release_mlme_lock:
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+exit:
+
+	return status;
+}
+
+u8 rtw_set_802_11_infrastructure_mode(_adapter *padapter,
+			      NDIS_802_11_NETWORK_INFRASTRUCTURE networktype)
+{
+	unsigned long irqL;
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct	wlan_network	*cur_network = &pmlmepriv->cur_network;
+	NDIS_802_11_NETWORK_INFRASTRUCTURE *pold_state = &(cur_network->network.InfrastructureMode);
+	u8 ap2sta_mode = false;
+
+	if (*pold_state != networktype) {
+		/* RTW_INFO("change mode, old_mode=%d, new_mode=%d, fw_state=0x%x\n", *pold_state, networktype, get_fwstate(pmlmepriv)); */
+
+		if (*pold_state == Ndis802_11APMode) {
+			/* change to other mode from Ndis802_11APMode			 */
+			cur_network->join_res = -1;
+			ap2sta_mode = true;
+#ifdef CONFIG_NATIVEAP_MLME
+			stop_ap_mode(padapter);
+#endif
+		}
+
+		_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+		if ((check_fwstate(pmlmepriv, _FW_LINKED) ) || (*pold_state == Ndis802_11IBSS))
+			rtw_disassoc_cmd(padapter, 0, true);
+
+		if ((check_fwstate(pmlmepriv, _FW_LINKED) ) ||
+		    (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ))
+			rtw_free_assoc_resources(padapter, 1);
+
+		if ((*pold_state == Ndis802_11Infrastructure) || (*pold_state == Ndis802_11IBSS)) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED) ) {
+				rtw_indicate_disconnect(padapter, 0, false); /*will clr Linked_state; before this function, we must have checked whether issue dis-assoc_cmd or not*/
+			}
+		}
+
+		*pold_state = networktype;
+
+		_clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE);
+
+		switch (networktype) {
+		case Ndis802_11IBSS:
+			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			break;
+
+		case Ndis802_11Infrastructure:
+			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+
+			if (ap2sta_mode)
+				rtw_init_bcmc_stainfo(padapter);
+			break;
+
+		case Ndis802_11APMode:
+			set_fwstate(pmlmepriv, WIFI_AP_STATE);
+#ifdef CONFIG_NATIVEAP_MLME
+			start_ap_mode(padapter);
+			/* rtw_indicate_connect(padapter); */
+#endif
+
+			break;
+
+		case Ndis802_11AutoUnknown:
+		case Ndis802_11InfrastructureMax:
+			break;
+		case Ndis802_11Monitor:
+			set_fwstate(pmlmepriv, WIFI_MONITOR_STATE);
+			break;
+		}
+
+		/* SecClearAllKeys(adapter); */
+
+		_exit_critical_bh(&pmlmepriv->lock, &irqL);
+	}
+
+	return true;
+}
+
+u8 rtw_set_802_11_disassociate(_adapter *padapter)
+{
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) ) {
+
+		rtw_disassoc_cmd(padapter, 0, true);
+		rtw_indicate_disconnect(padapter, 0, false);
+		/* modify for CONFIG_IEEE80211W, none 11w can use it */
+		rtw_free_assoc_resources_cmd(padapter);
+		if (_FAIL == rtw_pwr_wakeup(padapter))
+			RTW_INFO("%s(): rtw_pwr_wakeup fail !!!\n", __func__);
+	}
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+	return true;
+}
+
+u8 rtw_set_802_11_bssid_list_scan(_adapter *padapter, NDIS_802_11_SSID *pssid, int ssid_max_num, struct rtw_ieee80211_channel *ch, int ch_num)
+{
+	unsigned long	irqL;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8	res = true;
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+	res = rtw_sitesurvey_cmd(padapter, pssid, ssid_max_num, ch, ch_num);
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+	return res;
+}
+
+u8 rtw_set_802_11_authentication_mode(_adapter *padapter, NDIS_802_11_AUTHENTICATION_MODE authmode)
+{
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	int res;
+	u8 ret;
+
+	psecuritypriv->ndisauthtype = authmode;
+
+	if (psecuritypriv->ndisauthtype > 3)
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+#ifdef CONFIG_WAPI_SUPPORT
+	if (psecuritypriv->ndisauthtype == 6)
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_WAPI;
+#endif
+
+	res = rtw_set_auth(padapter, psecuritypriv);
+
+	if (res == _SUCCESS)
+		ret = true;
+	else
+		ret = false;
+
+	return ret;
+}
+
+u8 rtw_set_802_11_add_wep(_adapter *padapter, NDIS_802_11_WEP *wep)
+{
+
+	u8		bdefaultkey;
+	u8		btransmitkey;
+	sint		keyid, res;
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	u8		ret = _SUCCESS;
+
+	bdefaultkey = (wep->KeyIndex & 0x40000000) > 0 ? false : true; /* for ??? */
+	btransmitkey = (wep->KeyIndex & 0x80000000) > 0 ? true  : false;	/* for ??? */
+	keyid = wep->KeyIndex & 0x3fffffff;
+
+	if (keyid >= 4) {
+		ret = false;
+		goto exit;
+	}
+
+	switch (wep->KeyLength) {
+	case 5:
+		psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
+		break;
+	case 13:
+		psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
+		break;
+	default:
+		psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		break;
+	}
+
+	memcpy(&(psecuritypriv->dot11DefKey[keyid].skey[0]), &(wep->KeyMaterial), wep->KeyLength);
+
+	psecuritypriv->dot11DefKeylen[keyid] = wep->KeyLength;
+
+	psecuritypriv->dot11PrivacyKeyIndex = keyid;
+
+	res = rtw_set_key(padapter, psecuritypriv, keyid, 1, true);
+
+	if (res == _FAIL)
+		ret = false;
+exit:
+
+	return ret;
+
+}
+
+u8 rtw_set_802_11_remove_wep(_adapter *padapter, u32 keyindex)
+{
+
+	u8 ret = _SUCCESS;
+
+	if (keyindex >= 0x80000000 || padapter == NULL) {
+
+		ret = false;
+		goto exit;
+
+	} else {
+		int res;
+		struct security_priv *psecuritypriv = &(padapter->securitypriv);
+		if (keyindex < 4) {
+
+			memset(&psecuritypriv->dot11DefKey[keyindex], 0, 16);
+
+			res = rtw_set_key(padapter, psecuritypriv, keyindex, 0, true);
+
+			psecuritypriv->dot11DefKeylen[keyindex] = 0;
+
+			if (res == _FAIL)
+				ret = _FAIL;
+
+		} else
+			ret = _FAIL;
+
+	}
+
+exit:
+
+	return ret;
+
+}
+
+u8 rtw_set_802_11_add_key(_adapter *padapter, NDIS_802_11_KEY *key)
+{
+
+	uint	encryptionalgo;
+	u8 *pbssid;
+	struct sta_info *stainfo;
+	u8	bgroup = false;
+	u8	bgrouptkey = false;/* can be remove later */
+	u8	ret = _SUCCESS;
+
+	if (((key->KeyIndex & 0x80000000) == 0) && ((key->KeyIndex & 0x40000000) > 0)) {
+
+		/* It is invalid to clear bit 31 and set bit 30. If the miniport driver encounters this combination, */
+		/* it must fail the request and return NDIS_STATUS_INVALID_DATA. */
+		ret = _FAIL;
+		goto exit;
+	}
+
+	if (key->KeyIndex & 0x40000000) {
+		/* Pairwise key */
+
+		pbssid = get_bssid(&padapter->mlmepriv);
+		stainfo = rtw_get_stainfo(&padapter->stapriv, pbssid);
+
+		if ((stainfo != NULL) && (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)) {
+			encryptionalgo = stainfo->dot118021XPrivacy;
+		} else {
+			encryptionalgo = padapter->securitypriv.dot11PrivacyAlgrthm;
+		}
+
+		if (key->KeyIndex & 0x000000FF) {
+			/* The key index is specified in the lower 8 bits by values of zero to 255. */
+			/* The key index should be set to zero for a Pairwise key, and the driver should fail with */
+			/* NDIS_STATUS_INVALID_DATA if the lower 8 bits is not zero */
+			ret = _FAIL;
+			goto exit;
+		}
+
+		/* check BSSID */
+		if (IS_MAC_ADDRESS_BROADCAST(key->BSSID) ) {
+
+			ret = false;
+			goto exit;
+		}
+
+		/* Check key length for TKIP. */
+		/* if(encryptionAlgorithm == RT_ENC_TKIP_ENCRYPTION && key->KeyLength != 32) */
+		if ((encryptionalgo == _TKIP_) && (key->KeyLength != 32)) {
+			ret = _FAIL;
+			goto exit;
+
+		}
+
+		/* Check key length for AES. */
+		if ((encryptionalgo == _AES_) && (key->KeyLength != 16)) {
+			/* For our supplicant, EAPPkt9x.vxd, cannot differentiate TKIP and AES case. */
+			if (key->KeyLength == 32)
+				key->KeyLength = 16;
+			else {
+				ret = _FAIL;
+				goto exit;
+			}
+		}
+
+		/* Check key length for WEP. For NDTEST, 2005.01.27, by rcnjko. -> modify checking condition*/
+		if (((encryptionalgo == _WEP40_) && (key->KeyLength != 5)) || ((encryptionalgo == _WEP104_) && (key->KeyLength != 13))) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		bgroup = false;
+
+		/* Check the pairwise key. Added by Annie, 2005-07-06. */
+
+	} else {
+		/* Group key - KeyIndex(BIT30==0) */
+
+		/* when add wep key through add key and didn't assigned encryption type before */
+		if ((padapter->securitypriv.ndisauthtype <= 3) && (padapter->securitypriv.dot118021XGrpPrivacy == 0)) {
+
+			switch (key->KeyLength) {
+			case 5:
+				padapter->securitypriv.dot11PrivacyAlgrthm = _WEP40_;
+				break;
+			case 13:
+				padapter->securitypriv.dot11PrivacyAlgrthm = _WEP104_;
+				break;
+			default:
+				padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+				break;
+			}
+
+			encryptionalgo = padapter->securitypriv.dot11PrivacyAlgrthm;
+
+		} else {
+			encryptionalgo = padapter->securitypriv.dot118021XGrpPrivacy;
+
+		}
+
+		if ((check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE) ) && (IS_MAC_ADDRESS_BROADCAST(key->BSSID) == false)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		/* Check key length for TKIP */
+		if ((encryptionalgo == _TKIP_) && (key->KeyLength != 32)) {
+
+			ret = _FAIL;
+			goto exit;
+
+		} else if (encryptionalgo == _AES_ && (key->KeyLength != 16 && key->KeyLength != 32)) {
+
+			/* Check key length for AES */
+			/* For NDTEST, we allow keylen=32 in this case. 2005.01.27, by rcnjko. */
+			ret = _FAIL;
+			goto exit;
+		}
+
+		/* Change the key length for EAPPkt9x.vxd. Added by Annie, 2005-11-03. */
+		if ((encryptionalgo ==  _AES_) && (key->KeyLength == 32)) {
+			key->KeyLength = 16;
+		}
+
+		if (key->KeyIndex & 0x8000000) /* error ??? 0x8000_0000 */
+			bgrouptkey = true;
+
+		if ((check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE) ) && (check_fwstate(&padapter->mlmepriv, _FW_LINKED) == true))
+			bgrouptkey = true;
+
+		bgroup = true;
+
+	}
+
+	/* If WEP encryption algorithm, just call rtw_set_802_11_add_wep(). */
+	if ((padapter->securitypriv.dot11AuthAlgrthm != dot11AuthAlgrthm_8021X) && (encryptionalgo == _WEP40_  || encryptionalgo == _WEP104_)) {
+		u8 ret;
+		u32 keyindex;
+		u32 len = FIELD_OFFSET(NDIS_802_11_KEY, KeyMaterial) + key->KeyLength;
+		NDIS_802_11_WEP *wep = &padapter->securitypriv.ndiswep;
+
+		wep->Length = len;
+		keyindex = key->KeyIndex & 0x7fffffff;
+		wep->KeyIndex = keyindex ;
+		wep->KeyLength = key->KeyLength;
+
+		memcpy(wep->KeyMaterial, key->KeyMaterial, key->KeyLength);
+		memcpy(&(padapter->securitypriv.dot11DefKey[keyindex].skey[0]), key->KeyMaterial, key->KeyLength);
+
+		padapter->securitypriv.dot11DefKeylen[keyindex] = key->KeyLength;
+		padapter->securitypriv.dot11PrivacyKeyIndex = keyindex;
+
+		ret = rtw_set_802_11_add_wep(padapter, wep);
+
+		goto exit;
+
+	}
+
+	if (key->KeyIndex & 0x20000000) {
+		/* SetRSC */
+		if (bgroup ) {
+			NDIS_802_11_KEY_RSC keysrc = key->KeyRSC & 0x00FFFFFFFFFFFFULL;
+			memcpy(&padapter->securitypriv.dot11Grprxpn, &keysrc, 8);
+		} else {
+			NDIS_802_11_KEY_RSC keysrc = key->KeyRSC & 0x00FFFFFFFFFFFFULL;
+			memcpy(&padapter->securitypriv.dot11Grptxpn, &keysrc, 8);
+		}
+
+	}
+
+	/* Indicate this key idx is used for TX */
+	/* Save the key in KeyMaterial */
+	if (bgroup ) { /* Group transmit key */
+		int res;
+
+		if (bgrouptkey )
+			padapter->securitypriv.dot118021XGrpKeyid = (u8)key->KeyIndex;
+
+		if ((key->KeyIndex & 0x3) == 0) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		memset(&padapter->securitypriv.dot118021XGrpKey[(u8)((key->KeyIndex) & 0x03)], 0, 16);
+		memset(&padapter->securitypriv.dot118021XGrptxmickey[(u8)((key->KeyIndex) & 0x03)], 0, 16);
+		memset(&padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)], 0, 16);
+
+		if ((key->KeyIndex & 0x10000000)) {
+			memcpy(&padapter->securitypriv.dot118021XGrptxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 16, 8);
+			memcpy(&padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 24, 8);
+
+		} else {
+			memcpy(&padapter->securitypriv.dot118021XGrptxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 24, 8);
+			memcpy(&padapter->securitypriv.dot118021XGrprxmickey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial + 16, 8);
+
+		}
+
+		/* set group key by index */
+		memcpy(&padapter->securitypriv.dot118021XGrpKey[(u8)((key->KeyIndex) & 0x03)], key->KeyMaterial, key->KeyLength);
+
+		key->KeyIndex = key->KeyIndex & 0x03;
+
+		padapter->securitypriv.binstallGrpkey = true;
+
+		padapter->securitypriv.bcheck_grpkey = false;
+
+		res = rtw_set_key(padapter, &padapter->securitypriv, key->KeyIndex, 1, true);
+
+		if (res == _FAIL)
+			ret = _FAIL;
+
+		goto exit;
+
+	} else { /* Pairwise Key */
+		u8 res;
+
+		pbssid = get_bssid(&padapter->mlmepriv);
+		stainfo = rtw_get_stainfo(&padapter->stapriv , pbssid);
+
+		if (stainfo != NULL) {
+			memset(&stainfo->dot118021x_UncstKey, 0, 16); /* clear keybuffer */
+
+			memcpy(&stainfo->dot118021x_UncstKey, key->KeyMaterial, 16);
+
+			if (encryptionalgo == _TKIP_) {
+				padapter->securitypriv.busetkipkey = false;
+
+				/* _set_timer(&padapter->securitypriv.tkip_timer, 50); */
+
+				/* if TKIP, save the Receive/Transmit MIC key in KeyMaterial[128-255] */
+				if ((key->KeyIndex & 0x10000000)) {
+					memcpy(&stainfo->dot11tkiptxmickey, key->KeyMaterial + 16, 8);
+					memcpy(&stainfo->dot11tkiprxmickey, key->KeyMaterial + 24, 8);
+
+				} else {
+					memcpy(&stainfo->dot11tkiptxmickey, key->KeyMaterial + 24, 8);
+					memcpy(&stainfo->dot11tkiprxmickey, key->KeyMaterial + 16, 8);
+
+				}
+
+			} else if (encryptionalgo == _AES_) {
+
+			}
+
+			/* Set key to CAM through H2C command */
+
+			res = rtw_setstakey_cmd(padapter, stainfo, UNICAST_KEY, true);
+
+			if (res == false)
+				ret = _FAIL;
+		}
+	}
+
+exit:
+
+	return ret;
+}
+
+u8 rtw_set_802_11_remove_key(_adapter	*padapter, NDIS_802_11_REMOVE_KEY *key)
+{
+
+	uint				encryptionalgo;
+	u8 *pbssid;
+	struct sta_info *stainfo;
+	u8	bgroup = (key->KeyIndex & 0x4000000) > 0 ? false : true;
+	u8	keyIndex = (u8)key->KeyIndex & 0x03;
+	u8	ret = _SUCCESS;
+
+	if ((key->KeyIndex & 0xbffffffc) > 0) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	if (bgroup ) {
+		encryptionalgo = padapter->securitypriv.dot118021XGrpPrivacy;
+		/* clear group key by index */
+		/* NdisZeroMemory(Adapter->MgntInfo.SecurityInfo.KeyBuf[keyIndex], MAX_WEP_KEY_LEN); */
+		/* Adapter->MgntInfo.SecurityInfo.KeyLen[keyIndex] = 0; */
+
+		memset(&padapter->securitypriv.dot118021XGrpKey[keyIndex], 0, 16);
+
+		/* ! \todo Send a H2C Command to Firmware for removing this Key in CAM Entry. */
+
+	} else {
+
+		pbssid = get_bssid(&padapter->mlmepriv);
+		stainfo = rtw_get_stainfo(&padapter->stapriv , pbssid);
+		if (stainfo != NULL) {
+			encryptionalgo = stainfo->dot118021XPrivacy;
+
+			/* clear key by BSSID */
+			memset(&stainfo->dot118021x_UncstKey, 0, 16);
+
+			/* ! \todo Send a H2C Command to Firmware for disable this Key in CAM Entry. */
+
+		} else {
+			ret = _FAIL;
+			goto exit;
+		}
+	}
+
+exit:
+
+	return true;
+
+}
+
+/*
+* rtw_get_cur_max_rate -
+* @adapter: pointer to _adapter structure
+*
+* Return 0 or 100Kbps
+*/
+u16 rtw_get_cur_max_rate(_adapter *adapter)
+{
+	int	i = 0;
+	u16	rate = 0, max_rate = 0;
+	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	WLAN_BSSID_EX	*pcur_bss = &pmlmepriv->cur_network.network;
+	struct sta_info *psta = NULL;
+	u8	short_GI = 0;
+	u8	rf_type = 0;
+
+#ifdef CONFIG_MP_INCLUDED
+	if (adapter->registrypriv.mp_mode == 1) {
+		if (check_fwstate(pmlmepriv, WIFI_MP_STATE) )
+			return 0;
+	}
+#endif
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED) != true)
+	    && (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) != true))
+		return 0;
+
+	psta = rtw_get_stainfo(&adapter->stapriv, get_bssid(pmlmepriv));
+	if (psta == NULL)
+		return 0;
+
+	short_GI = query_ra_short_GI(psta, psta->bw_mode);
+
+	if (is_supported_ht(psta->wireless_mode)) {
+		rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+		max_rate = rtw_mcs_rate(rf_type
+			, (psta->bw_mode == CHANNEL_WIDTH_40) ? 1 : 0
+			, short_GI
+			, psta->htpriv.ht_cap.supp_mcs_set
+		);
+	} else {
+		while ((pcur_bss->SupportedRates[i] != 0) && (pcur_bss->SupportedRates[i] != 0xFF)) {
+			rate = pcur_bss->SupportedRates[i] & 0x7F;
+			if (rate > max_rate)
+				max_rate = rate;
+			i++;
+		}
+
+		max_rate = max_rate * 10 / 2;
+	}
+
+	return max_rate;
+}
+
+/*
+* rtw_set_scan_mode -
+* @adapter: pointer to _adapter structure
+* @scan_mode:
+*
+* Return _SUCCESS or _FAIL
+*/
+int rtw_set_scan_mode(_adapter *adapter, RT_SCAN_TYPE scan_mode)
+{
+	if (scan_mode != SCAN_ACTIVE && scan_mode != SCAN_PASSIVE)
+		return _FAIL;
+
+	adapter->mlmepriv.scan_mode = scan_mode;
+
+	return _SUCCESS;
+}
+
+/*
+* rtw_set_channel_plan -
+* @adapter: pointer to _adapter structure
+* @channel_plan:
+*
+* Return _SUCCESS or _FAIL
+*/
+int rtw_set_channel_plan(_adapter *adapter, u8 channel_plan)
+{
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	/* handle by cmd_thread to sync with scan operation */
+	return rtw_set_chplan_cmd(adapter, RTW_CMDF_WAIT_ACK, channel_plan, 1);
+}
+
+/*
+* rtw_set_country -
+* @adapter: pointer to _adapter structure
+* @country_code: string of country code
+*
+* Return _SUCCESS or _FAIL
+*/
+int rtw_set_country(_adapter *adapter, const char *country_code)
+{
+#ifdef CONFIG_RTW_IOCTL_SET_COUNTRY
+	return rtw_set_country_cmd(adapter, RTW_CMDF_WAIT_ACK, country_code, 1);
+#else
+	return _FAIL;
+#endif
+}
+
+/*
+* rtw_set_band -
+* @adapter: pointer to _adapter structure
+* @band: band to set
+*
+* Return _SUCCESS or _FAIL
+*/
+int rtw_set_band(_adapter *adapter, u8 band)
+{
+	if (rtw_band_valid(band)) {
+		RTW_INFO(FUNC_ADPT_FMT" band:%d\n", FUNC_ADPT_ARG(adapter), band);
+		adapter->setband = band;
+		return _SUCCESS;
+	}
+
+	RTW_PRINT(FUNC_ADPT_FMT" band:%d fail\n", FUNC_ADPT_ARG(adapter), band);
+	return _FAIL;
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_iol.c b/drivers/staging/rtl8188eu/core/rtw_iol.c
new file mode 100644
index 000000000000..44ab64cf7a88
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_iol.c
@@ -0,0 +1,355 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+
+#include <drv_types.h>
+
+#ifdef CONFIG_IOL
+struct xmit_frame	*rtw_IOL_accquire_xmit_frame(ADAPTER *adapter)
+{
+	struct xmit_frame	*xmit_frame;
+	struct xmit_buf	*xmitbuf;
+	struct pkt_attrib	*pattrib;
+	struct xmit_priv	*pxmitpriv = &(adapter->xmitpriv);
+
+	xmit_frame = rtw_alloc_xmitframe(pxmitpriv);
+	if (xmit_frame == NULL) {
+		RTW_INFO("%s rtw_alloc_xmitframe return null\n", __func__);
+		goto exit;
+	}
+
+	xmitbuf = rtw_alloc_xmitbuf(pxmitpriv);
+	if (xmitbuf == NULL) {
+		RTW_INFO("%s rtw_alloc_xmitbuf return null\n", __func__);
+		rtw_free_xmitframe(pxmitpriv, xmit_frame);
+		xmit_frame = NULL;
+		goto exit;
+	}
+
+	xmit_frame->frame_tag = MGNT_FRAMETAG;
+	xmit_frame->pxmitbuf = xmitbuf;
+	xmit_frame->buf_addr = xmitbuf->pbuf;
+	xmitbuf->priv_data = xmit_frame;
+
+	pattrib = &xmit_frame->attrib;
+	update_mgntframe_attrib(adapter, pattrib);
+	pattrib->qsel = QSLT_BEACON;/* Beacon	 */
+	pattrib->subtype = WIFI_BEACON;
+	pattrib->pktlen = pattrib->last_txcmdsz = 0;
+
+exit:
+	return xmit_frame;
+}
+
+int rtw_IOL_append_cmds(struct xmit_frame *xmit_frame, u8 *IOL_cmds, u32 cmd_len)
+{
+	struct pkt_attrib	*pattrib = &xmit_frame->attrib;
+	u16 buf_offset;
+	u32 ori_len;
+
+	buf_offset = TXDESC_OFFSET;
+	ori_len = buf_offset + pattrib->pktlen;
+
+	/* check if the io_buf can accommodate new cmds */
+	if (ori_len + cmd_len + 8 > MAX_XMITBUF_SZ) {
+		RTW_INFO("%s %u is large than MAX_XMITBUF_SZ:%u, can't accommodate new cmds\n", __func__
+			 , ori_len + cmd_len + 8, MAX_XMITBUF_SZ);
+		return _FAIL;
+	}
+
+	memcpy(xmit_frame->buf_addr + buf_offset + pattrib->pktlen, IOL_cmds, cmd_len);
+	pattrib->pktlen += cmd_len;
+	pattrib->last_txcmdsz += cmd_len;
+
+	/* RTW_INFO("%s ori:%u + cmd_len:%u = %u\n", __func__, ori_len, cmd_len, buf_offset+pattrib->pktlen); */
+
+	return _SUCCESS;
+}
+
+bool rtw_IOL_applied(ADAPTER *adapter)
+{
+	if (1 == adapter->registrypriv.fw_iol)
+		return true;
+
+	if ((2 == adapter->registrypriv.fw_iol) && (IS_FULL_SPEED_USB(adapter)))
+		return true;
+
+	return false;
+}
+
+int rtw_IOL_exec_cmds_sync(ADAPTER *adapter, struct xmit_frame *xmit_frame, u32 max_wating_ms, u32 bndy_cnt)
+{
+	return rtw_hal_iol_cmd(adapter, xmit_frame, max_wating_ms, bndy_cnt);
+}
+
+#ifdef CONFIG_IOL_NEW_GENERATION
+int rtw_IOL_append_LLT_cmd(struct xmit_frame *xmit_frame, u8 page_boundary)
+{
+	return _SUCCESS;
+}
+int _rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value, u8 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_WB_REG, 0x0, 0x0, 0x0};
+
+	/* RTW_PUT_LE16((u8*)&cmd.address, addr);	 */
+	/* RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	 */
+	cmd.address = cpu_to_le16(addr);
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0xFF) {
+		cmd.length = 12;
+		/* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	 */
+		cmd.mask = cpu_to_le32(mask);
+	}
+
+	/* RTW_INFO("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __func__, addr,value,mask); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+
+}
+int _rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value, u16 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_WW_REG, 0x0, 0x0, 0x0};
+
+	/* RTW_PUT_LE16((u8*)&cmd.address, addr);	 */
+	/* RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	 */
+	cmd.address = cpu_to_le16(addr);
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0xFFFF) {
+		cmd.length = 12;
+		/* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	 */
+		cmd.mask =  cpu_to_le32(mask);
+	}
+
+	/* RTW_INFO("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __func__, addr,value,mask); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+
+}
+int _rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value, u32 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_WD_REG, 0x0, 0x0, 0x0};
+
+	/* RTW_PUT_LE16((u8*)&cmd.address, addr);	 */
+	/* RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	 */
+	cmd.address = cpu_to_le16(addr);
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0xFFFFFFFF) {
+		cmd.length = 12;
+		/* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	 */
+		cmd.mask =  cpu_to_le32(mask);
+	}
+
+	/* RTW_INFO("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FU2NCTION__, addr,value,mask); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+
+}
+
+int _rtw_IOL_append_WRF_cmd(struct xmit_frame *xmit_frame, u8 rf_path, u16 addr, u32 value, u32 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_W_RF, 0x0, 0x0, 0x0};
+
+	/* RTW_PUT_LE16((u8*)&cmd.address, addr);	 */
+	/* RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	 */
+	cmd.address = cpu_to_le16((rf_path << 8) | ((addr) & 0xFF));
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0x000FFFFF) {
+		cmd.length = 12;
+		/* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	 */
+		cmd.mask =  cpu_to_le32(mask);
+	}
+
+	/* RTW_INFO("%s rf_path:0x%02x addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FU2NCTION__,rf_path, addr,value,mask); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+
+}
+
+
+
+int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us)
+{
+	struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0};
+	/* RTW_PUT_LE16((u8*)&cmd.address, us);	 */
+	cmd.address = cpu_to_le16(us);
+
+	/* RTW_INFO("%s %u\n", __func__, us); */
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+}
+
+int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms)
+{
+	struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0};
+
+	/* RTW_PUT_LE16((u8*)&cmd.address, ms);	 */
+	cmd.address = cpu_to_le16(ms);
+
+	/* RTW_INFO("%s %u\n", __func__, ms); */
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+}
+int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame)
+{
+	struct ioreg_cfg cmd = {4, IOREG_CMD_END, cpu_to_le16(0xFFFF), cpu_to_le32(0xFF), 0x0};
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+
+}
+
+u8 rtw_IOL_cmd_boundary_handle(struct xmit_frame *pxmit_frame)
+{
+	u8 is_cmd_bndy = false;
+	if (((pxmit_frame->attrib.pktlen + 32) % 256) + 8 >= 256) {
+		rtw_IOL_append_END_cmd(pxmit_frame);
+		pxmit_frame->attrib.pktlen = ((((pxmit_frame->attrib.pktlen + 32) / 256) + 1) * 256);
+
+		/* RTW_INFO("==> %s, pktlen(%d)\n",__func__,pxmit_frame->attrib.pktlen); */
+		pxmit_frame->attrib.last_txcmdsz = pxmit_frame->attrib.pktlen;
+		is_cmd_bndy = true;
+	}
+	return is_cmd_bndy;
+}
+
+void rtw_IOL_cmd_buf_dump(ADAPTER *Adapter, int buf_len, u8 *pbuf)
+{
+	int i;
+	int j = 1;
+
+	RTW_INFO("###### %s ######\n", __func__);
+	for (i = 0; i < buf_len; i++) {
+		RTW_INFO("%02x-", *(pbuf + i));
+
+		if (j % 32 == 0)
+			RTW_INFO("\n");
+		j++;
+	}
+	RTW_INFO("\n");
+	RTW_INFO("============= ioreg_cmd len = %d ===============\n", buf_len);
+}
+
+
+#else /* CONFIG_IOL_NEW_GENERATION */
+int rtw_IOL_append_LLT_cmd(struct xmit_frame *xmit_frame, u8 page_boundary)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_LLT, 0x0, 0x0};
+
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)page_boundary);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+int _rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_WB_REG, 0x0, 0x0};
+
+	RTW_PUT_BE16((u8 *)&cmd.address, (u16)addr);
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)value);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+int _rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_WW_REG, 0x0, 0x0};
+
+	RTW_PUT_BE16((u8 *)&cmd.address, (u16)addr);
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)value);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+int _rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_WD_REG, 0x0, 0x0};
+	u8 *pos = (u8 *)&cmd;
+
+	RTW_PUT_BE16((u8 *)&cmd.address, (u16)addr);
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)value);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+#ifdef DBG_IO
+int dbg_rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 1))
+		RTW_INFO("DBG_IO %s:%d IOL_WB(0x%04x, 0x%02x)\n", caller, line, addr, value);
+
+	return _rtw_IOL_append_WB_cmd(xmit_frame, addr, value);
+}
+
+int dbg_rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 2))
+		RTW_INFO("DBG_IO %s:%d IOL_WW(0x%04x, 0x%04x)\n", caller, line, addr, value);
+
+	return _rtw_IOL_append_WW_cmd(xmit_frame, addr, value);
+}
+
+int dbg_rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 4))
+		RTW_INFO("DBG_IO %s:%d IOL_WD(0x%04x, 0x%08x)\n", caller, line, addr, value);
+
+	return _rtw_IOL_append_WD_cmd(xmit_frame, addr, value);
+}
+#endif
+
+int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_DELAY_US, 0x0, 0x0};
+
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)us);
+
+	/* RTW_INFO("%s %u\n", __func__, us); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_DELAY_MS, 0x0, 0x0};
+
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)ms);
+
+	/* RTW_INFO("%s %u\n", __func__, ms); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame)
+{
+	IOL_CMD end_cmd = {0x0, IOL_CMD_END, 0x0, 0x0};
+
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&end_cmd, 8);
+
+}
+
+int rtw_IOL_exec_cmd_array_sync(PADAPTER adapter, u8 *IOL_cmds, u32 cmd_num, u32 max_wating_ms)
+{
+	struct xmit_frame	*xmit_frame;
+
+	xmit_frame = rtw_IOL_accquire_xmit_frame(adapter);
+	if (xmit_frame == NULL)
+		return _FAIL;
+
+	if (rtw_IOL_append_cmds(xmit_frame, IOL_cmds, cmd_num << 3) == _FAIL)
+		return _FAIL;
+
+	return rtw_IOL_exec_cmds_sync(adapter, xmit_frame, max_wating_ms, 0);
+}
+
+int rtw_IOL_exec_empty_cmds_sync(ADAPTER *adapter, u32 max_wating_ms)
+{
+	IOL_CMD end_cmd = {0x0, IOL_CMD_END, 0x0, 0x0};
+	return rtw_IOL_exec_cmd_array_sync(adapter, (u8 *)&end_cmd, 1, max_wating_ms);
+}
+#endif /* CONFIG_IOL_NEW_GENERATION */
+
+
+
+
+#endif /* CONFIG_IOL */
diff --git a/drivers/staging/rtl8188eu/core/rtw_led.c b/drivers/staging/rtl8188eu/core/rtw_led.c
new file mode 100644
index 000000000000..6422dc57853a
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_led.c
@@ -0,0 +1,1705 @@
+/******************************************************************************
+ *
+ * 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 */
+/*  */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+void BlinkTimerCallback(struct timer_list *t)
+#else
+void BlinkTimerCallback(void *data)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+	struct LED_871x *pLed = from_timer(pLed, t, BlinkTimer);
+#else
+	struct LED_871x *pLed = (struct LED_871x *)data;
+#endif
+	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);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 15, 0)
+	timer_setup(&pLed->BlinkTimer, BlinkTimerCallback, 0);
+#else
+	_init_timer(&(pLed->BlinkTimer), padapter->pnetdev, BlinkTimerCallback, pLed);
+#endif
+	_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 LedControl8188eu(struct adapter *padapter, enum LED_CTL_MODE LedAction)
+{
+	struct led_priv *ledpriv = &(padapter->ledpriv);
+	struct registry_priv *registry_par;
+
+       if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped) ||
+	   (!padapter->hw_init_completed))
+		return;
+
+	if (!ledpriv->bRegUseLed)
+		return;
+
+	registry_par = &padapter->registrypriv;
+	if (!registry_par->led_enable)
+		return;
+
+	if ((padapter->pwrctrlpriv.rf_pwrstate != rf_on &&
+	     padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) &&
+	    (LedAction == LED_CTL_TX || LedAction == LED_CTL_RX ||
+	     LedAction == LED_CTL_SITE_SURVEY ||
+	     LedAction == LED_CTL_LINK ||
+	     LedAction == LED_CTL_NO_LINK ||
+	     LedAction == LED_CTL_POWER_ON))
+		return;
+
+	switch (ledpriv->LedStrategy) {
+	case SW_LED_MODE0:
+		break;
+	case SW_LED_MODE1:
+		SwLedControlMode1(padapter, LedAction);
+		break;
+	case SW_LED_MODE2:
+		SwLedControlMode2(padapter, LedAction);
+		break;
+	case SW_LED_MODE3:
+		SwLedControlMode3(padapter, LedAction);
+		break;
+	case SW_LED_MODE4:
+		SwLedControlMode4(padapter, LedAction);
+		break;
+	case SW_LED_MODE5:
+		SwLedControlMode5(padapter, LedAction);
+		break;
+	case SW_LED_MODE6:
+		SwLedControlMode6(padapter, LedAction);
+		break;
+	default:
+		break;
+	}
+
+	RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
+		 ("LedStrategy:%d, LedAction %d\n",
+		 ledpriv->LedStrategy, LedAction));
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_mi.c b/drivers/staging/rtl8188eu/core/rtw_mi.c
new file mode 100644
index 000000000000..327b59aebdb5
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_mi.c
@@ -0,0 +1,1198 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_MI_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+void rtw_mi_update_union_chan_inf(_adapter *adapter, u8 ch, u8 offset , u8 bw)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct mi_state *iface_state = &dvobj->iface_state;
+
+	iface_state->union_ch = ch;
+	iface_state->union_bw = bw;
+	iface_state->union_offset = offset;
+}
+
+/* Find union about ch, bw, ch_offset of all linked/linking interfaces */
+static int _rtw_mi_get_ch_setting_union(_adapter *adapter, u8 *ch, u8 *bw, u8 *offset, bool include_self)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	_adapter *iface;
+	struct mlme_ext_priv *mlmeext;
+	int i;
+	u8 ch_ret = 0;
+	u8 bw_ret = CHANNEL_WIDTH_20;
+	u8 offset_ret = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	int num = 0;
+
+	if (ch)
+		*ch = 0;
+	if (bw)
+		*bw = CHANNEL_WIDTH_20;
+	if (offset)
+		*offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		mlmeext = &iface->mlmeextpriv;
+
+		if (!check_fwstate(&iface->mlmepriv, _FW_LINKED | _FW_UNDER_LINKING))
+			continue;
+
+		if (check_fwstate(&iface->mlmepriv, WIFI_OP_CH_SWITCHING))
+			continue;
+
+		if (include_self == false && adapter == iface)
+			continue;
+
+		if (num == 0) {
+			ch_ret = mlmeext->cur_channel;
+			bw_ret = mlmeext->cur_bwmode;
+			offset_ret = mlmeext->cur_ch_offset;
+			num++;
+			continue;
+		}
+
+		if (ch_ret != mlmeext->cur_channel) {
+			num = 0;
+			break;
+		}
+
+		if (bw_ret < mlmeext->cur_bwmode) {
+			bw_ret = mlmeext->cur_bwmode;
+			offset_ret = mlmeext->cur_ch_offset;
+		} else if (bw_ret == mlmeext->cur_bwmode && offset_ret != mlmeext->cur_ch_offset) {
+			num = 0;
+			break;
+		}
+
+		num++;
+	}
+
+	if (num) {
+		if (ch)
+			*ch = ch_ret;
+		if (bw)
+			*bw = bw_ret;
+		if (offset)
+			*offset = offset_ret;
+	}
+
+	return num;
+}
+
+inline int rtw_mi_get_ch_setting_union(_adapter *adapter, u8 *ch, u8 *bw, u8 *offset)
+{
+	return _rtw_mi_get_ch_setting_union(adapter, ch, bw, offset, 1);
+}
+
+inline int rtw_mi_get_ch_setting_union_no_self(_adapter *adapter, u8 *ch, u8 *bw, u8 *offset)
+{
+	return _rtw_mi_get_ch_setting_union(adapter, ch, bw, offset, 0);
+}
+
+/* For now, not return union_ch/bw/offset */
+static void _rtw_mi_status(_adapter *adapter, struct mi_state *mstate, bool include_self)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	_adapter *iface;
+	int i;
+
+	memset(mstate, 0, sizeof(struct mi_state));
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+
+		if (include_self == false && iface == adapter)
+			continue;
+
+		if (check_fwstate(&iface->mlmepriv, WIFI_STATION_STATE) == true) {
+			MSTATE_STA_NUM(mstate)++;
+			if (check_fwstate(&iface->mlmepriv, _FW_LINKED) == true)
+				MSTATE_STA_LD_NUM(mstate)++;
+
+			if (check_fwstate(&iface->mlmepriv, _FW_UNDER_LINKING) == true)
+				MSTATE_STA_LG_NUM(mstate)++;
+
+		} else if (check_fwstate(&iface->mlmepriv, WIFI_AP_STATE) == true
+			&& check_fwstate(&iface->mlmepriv, _FW_LINKED) == true
+		) {
+			MSTATE_AP_NUM(mstate)++;
+			if (iface->stapriv.asoc_sta_count > 2)
+				MSTATE_AP_LD_NUM(mstate)++;
+
+		} else if (check_fwstate(&iface->mlmepriv, WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE) == true
+			&& check_fwstate(&iface->mlmepriv, _FW_LINKED) == true
+		) {
+			MSTATE_ADHOC_NUM(mstate)++;
+			if (iface->stapriv.asoc_sta_count > 2)
+				MSTATE_ADHOC_LD_NUM(mstate)++;
+		}
+
+		if (check_fwstate(&iface->mlmepriv, WIFI_UNDER_WPS) == true)
+			MSTATE_WPS_NUM(mstate)++;
+
+#ifdef CONFIG_IOCTL_CFG80211
+		if (rtw_cfg80211_get_is_mgmt_tx(iface))
+			MSTATE_MGMT_TX_NUM(mstate)++;
+		#ifdef CONFIG_P2P
+		if (rtw_cfg80211_get_is_roch(iface) == true)
+			MSTATE_ROCH_NUM(mstate)++;
+		#endif
+#endif /* CONFIG_IOCTL_CFG80211 */
+
+	}
+}
+
+inline void rtw_mi_status(_adapter *adapter, struct mi_state *mstate)
+{
+	return _rtw_mi_status(adapter, mstate, 1);
+}
+inline void rtw_mi_status_no_self(_adapter *adapter, struct mi_state *mstate)
+{
+	return _rtw_mi_status(adapter, mstate, 0);
+}
+void dump_mi_status(void *sel, struct dvobj_priv *dvobj)
+{
+	RTW_PRINT_SEL(sel, "== dvobj-iface_state ==\n");
+	RTW_PRINT_SEL(sel, "sta_num:%d\n", DEV_STA_NUM(dvobj));
+	RTW_PRINT_SEL(sel, "linking_sta_num:%d\n", DEV_STA_LG_NUM(dvobj));
+	RTW_PRINT_SEL(sel, "linked_sta_num:%d\n", DEV_STA_LD_NUM(dvobj));
+	RTW_PRINT_SEL(sel, "ap_num:%d\n", DEV_AP_NUM(dvobj));
+	RTW_PRINT_SEL(sel, "linked_ap_num:%d\n", DEV_AP_LD_NUM(dvobj));
+	RTW_PRINT_SEL(sel, "adhoc_num:%d\n", DEV_ADHOC_NUM(dvobj));
+	RTW_PRINT_SEL(sel, "linked_adhoc_num:%d\n", DEV_ADHOC_LD_NUM(dvobj));
+#ifdef CONFIG_P2P
+	RTW_PRINT_SEL(sel, "p2p_device_num:%d\n", rtw_mi_stay_in_p2p_mode(dvobj->padapters[IFACE_ID0]));
+#endif
+#if defined(CONFIG_IOCTL_CFG80211)
+	#if defined(CONFIG_P2P)
+	RTW_PRINT_SEL(sel, "roch_num:%d\n", DEV_ROCH_NUM(dvobj));
+	#endif
+	RTW_PRINT_SEL(sel, "mgmt_tx_num:%d\n", DEV_MGMT_TX_NUM(dvobj));
+#endif
+	RTW_PRINT_SEL(sel, "under_wps_num:%d\n", DEV_WPS_NUM(dvobj));
+	RTW_PRINT_SEL(sel, "union_ch:%d\n", DEV_U_CH(dvobj));
+	RTW_PRINT_SEL(sel, "union_bw:%d\n", DEV_U_BW(dvobj));
+	RTW_PRINT_SEL(sel, "union_offset:%d\n", DEV_U_OFFSET(dvobj));
+	RTW_PRINT_SEL(sel, "================\n\n");
+}
+
+void dump_dvobj_mi_status(void *sel, const char *fun_name, _adapter *adapter)
+{
+	RTW_INFO("\n[ %s ] call %s\n", fun_name, __func__);
+	dump_mi_status(sel, adapter_to_dvobj(adapter));
+}
+
+inline void rtw_mi_update_iface_status(struct mlme_priv *pmlmepriv, sint state)
+{
+	_adapter *adapter = container_of(pmlmepriv, _adapter, mlmepriv);
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct mi_state *iface_state = &dvobj->iface_state;
+	struct mi_state tmp_mstate;
+	u8 i;
+	u8 u_ch, u_offset, u_bw;
+	_adapter *iface;
+
+	if (state == WIFI_MONITOR_STATE
+		|| state == WIFI_SITE_MONITOR
+		|| state == 0xFFFFFFFF
+	)
+		return;
+
+	if (0)
+		RTW_INFO("%s => will change or clean state to 0x%08x\n", __func__, state);
+
+	rtw_mi_status(adapter, &tmp_mstate);
+	memcpy(iface_state, &tmp_mstate, sizeof(struct mi_state));
+
+	if (rtw_mi_get_ch_setting_union(adapter, &u_ch, &u_bw, &u_offset))
+		rtw_mi_update_union_chan_inf(adapter , u_ch, u_offset , u_bw);
+	else {
+		if (0) {
+			dump_adapters_status(RTW_DBGDUMP , dvobj);
+			RTW_INFO("%s-[ERROR] cannot get union channel\n", __func__);
+			rtw_warn_on(1);
+		}
+	}
+
+#ifdef DBG_IFACE_STATUS
+	DBG_IFACE_STATUS_DUMP(adapter);
+#endif
+}
+u8 rtw_mi_check_status(_adapter *adapter, u8 type)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct mi_state *iface_state = &dvobj->iface_state;
+	u8 ret = false;
+
+#ifdef DBG_IFACE_STATUS
+	DBG_IFACE_STATUS_DUMP(adapter);
+	RTW_INFO("%s-"ADPT_FMT" check type:%d\n", __func__, ADPT_ARG(adapter), type);
+#endif
+
+	switch (type) {
+	case MI_LINKED:
+		if (MSTATE_STA_LD_NUM(iface_state) || MSTATE_AP_NUM(iface_state) || MSTATE_ADHOC_NUM(iface_state)) /*check_fwstate(&iface->mlmepriv, _FW_LINKED)*/
+			ret = true;
+		break;
+	case MI_ASSOC:
+		if (MSTATE_STA_LD_NUM(iface_state) || MSTATE_AP_LD_NUM(iface_state) || MSTATE_ADHOC_LD_NUM(iface_state))
+			ret = true;
+		break;
+	case MI_UNDER_WPS:
+		if (MSTATE_WPS_NUM(iface_state))
+			ret = true;
+		break;
+
+	case MI_AP_MODE:
+		if (MSTATE_AP_NUM(iface_state))
+			ret = true;
+		break;
+	case MI_AP_ASSOC:
+		if (MSTATE_AP_LD_NUM(iface_state))
+			ret = true;
+		break;
+
+	case MI_ADHOC:
+		if (MSTATE_ADHOC_NUM(iface_state))
+			ret = true;
+		break;
+	case MI_ADHOC_ASSOC:
+		if (MSTATE_ADHOC_LD_NUM(iface_state))
+			ret = true;
+		break;
+
+	case MI_STA_NOLINK: /* this is misleading, but not used now */
+		if (MSTATE_STA_NUM(iface_state) && (!(MSTATE_STA_LD_NUM(iface_state) || MSTATE_STA_LG_NUM(iface_state))))
+			ret = true;
+		break;
+	case MI_STA_LINKED:
+		if (MSTATE_STA_LD_NUM(iface_state))
+			ret = true;
+		break;
+	case MI_STA_LINKING:
+		if (MSTATE_STA_LG_NUM(iface_state))
+			ret = true;
+		break;
+
+	default:
+		break;
+	}
+	return ret;
+}
+
+u8 rtw_mi_mp_mode_check(_adapter *padapter)
+{
+#ifdef CONFIG_MP_INCLUDED
+#ifdef CONFIG_CONCURRENT_MODE
+	int i;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	_adapter *iface = NULL;
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+
+		if ((iface) && (iface->registrypriv.mp_mode == 1))
+			return true;
+	}
+#else
+	if (padapter->registrypriv.mp_mode == 1)
+		return true;
+#endif
+#endif /* CONFIG_MP_INCLUDED */
+	return false;
+}
+
+/*
+* return value : 0 is failed or have not interface meet condition
+* return value : !0 is success or interface numbers which meet condition
+* return value of ops_func must be true or false
+*/
+static u8 _rtw_mi_process(_adapter *padapter, bool exclude_self,
+		  void *data, u8(*ops_func)(_adapter *padapter, void *data))
+{
+	int i;
+	_adapter *iface;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+	u8 ret = 0;
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		if ((iface) && rtw_is_adapter_up(iface)) {
+
+			if ((exclude_self) && (iface == padapter))
+				continue;
+
+			if (ops_func)
+				if (ops_func(iface, data))
+					ret++;
+		}
+	}
+	return ret;
+}
+static u8 _rtw_mi_netif_stop_queue(_adapter *padapter, void *data)
+{
+	bool carrier_off = *(bool *)data;
+	struct net_device *pnetdev = padapter->pnetdev;
+
+	if (carrier_off)
+		netif_carrier_off(pnetdev);
+	rtw_netif_stop_queue(pnetdev);
+	return true;
+}
+u8 rtw_mi_netif_stop_queue(_adapter *padapter, bool carrier_off)
+{
+	bool in_data = carrier_off;
+
+	return _rtw_mi_process(padapter, false, &in_data, _rtw_mi_netif_stop_queue);
+}
+u8 rtw_mi_buddy_netif_stop_queue(_adapter *padapter, bool carrier_off)
+{
+	bool in_data = carrier_off;
+
+	return _rtw_mi_process(padapter, true, &in_data, _rtw_mi_netif_stop_queue);
+}
+
+static u8 _rtw_mi_netif_wake_queue(_adapter *padapter, void *data)
+{
+	struct net_device *pnetdev = padapter->pnetdev;
+
+	if (pnetdev)
+		rtw_netif_wake_queue(pnetdev);
+	return true;
+}
+u8 rtw_mi_netif_wake_queue(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_netif_wake_queue);
+}
+u8 rtw_mi_buddy_netif_wake_queue(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, true, NULL, _rtw_mi_netif_wake_queue);
+}
+
+static u8 _rtw_mi_netif_carrier_on(_adapter *padapter, void *data)
+{
+	struct net_device *pnetdev = padapter->pnetdev;
+
+	if (pnetdev)
+		rtw_netif_carrier_on(pnetdev);
+	return true;
+}
+u8 rtw_mi_netif_carrier_on(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_netif_carrier_on);
+}
+u8 rtw_mi_buddy_netif_carrier_on(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, true, NULL, _rtw_mi_netif_carrier_on);
+}
+
+static u8 _rtw_mi_scan_abort(_adapter *adapter, void *data)
+{
+	bool bwait = *(bool *)data;
+
+	if (bwait)
+		rtw_scan_abort(adapter);
+	else
+		rtw_scan_abort_no_wait(adapter);
+
+	return true;
+}
+void rtw_mi_scan_abort(_adapter *adapter, bool bwait)
+{
+	bool in_data = bwait;
+
+	_rtw_mi_process(adapter, false, &in_data, _rtw_mi_scan_abort);
+
+}
+void rtw_mi_buddy_scan_abort(_adapter *adapter, bool bwait)
+{
+	bool in_data = bwait;
+
+	_rtw_mi_process(adapter, true, &in_data, _rtw_mi_scan_abort);
+}
+
+static u8 _rtw_mi_start_drv_threads(_adapter *adapter, void *data)
+{
+	rtw_start_drv_threads(adapter);
+	return true;
+}
+void rtw_mi_start_drv_threads(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, false, NULL, _rtw_mi_start_drv_threads);
+}
+void rtw_mi_buddy_start_drv_threads(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, true, NULL, _rtw_mi_start_drv_threads);
+}
+
+static u8 _rtw_mi_stop_drv_threads(_adapter *adapter, void *data)
+{
+	rtw_stop_drv_threads(adapter);
+	return true;
+}
+void rtw_mi_stop_drv_threads(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, false, NULL, _rtw_mi_stop_drv_threads);
+}
+void rtw_mi_buddy_stop_drv_threads(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, true, NULL, _rtw_mi_stop_drv_threads);
+}
+
+static u8 _rtw_mi_cancel_all_timer(_adapter *adapter, void *data)
+{
+	rtw_cancel_all_timer(adapter);
+	return true;
+}
+void rtw_mi_cancel_all_timer(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, false, NULL, _rtw_mi_cancel_all_timer);
+}
+void rtw_mi_buddy_cancel_all_timer(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, true, NULL, _rtw_mi_cancel_all_timer);
+}
+
+static u8 _rtw_mi_reset_drv_sw(_adapter *adapter, void *data)
+{
+	rtw_reset_drv_sw(adapter);
+	return true;
+}
+void rtw_mi_reset_drv_sw(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, false, NULL, _rtw_mi_reset_drv_sw);
+}
+void rtw_mi_buddy_reset_drv_sw(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, true, NULL, _rtw_mi_reset_drv_sw);
+}
+
+static u8 _rtw_mi_intf_start(_adapter *adapter, void *data)
+{
+	rtw_intf_start(adapter);
+	return true;
+}
+void rtw_mi_intf_start(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, false, NULL, _rtw_mi_intf_start);
+}
+void rtw_mi_buddy_intf_start(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, true, NULL, _rtw_mi_intf_start);
+}
+
+static u8 _rtw_mi_intf_stop(_adapter *adapter, void *data)
+{
+	rtw_intf_stop(adapter);
+	return true;
+}
+void rtw_mi_intf_stop(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, false, NULL, _rtw_mi_intf_stop);
+}
+void rtw_mi_buddy_intf_stop(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, true, NULL, _rtw_mi_intf_stop);
+}
+
+static u8 _rtw_mi_suspend_free_assoc_resource(_adapter *padapter, void *data)
+{
+	return rtw_suspend_free_assoc_resource(padapter);
+}
+void rtw_mi_suspend_free_assoc_resource(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, false, NULL, _rtw_mi_suspend_free_assoc_resource);
+}
+void rtw_mi_buddy_suspend_free_assoc_resource(_adapter *adapter)
+{
+	_rtw_mi_process(adapter, true, NULL, _rtw_mi_suspend_free_assoc_resource);
+}
+
+static u8 _rtw_mi_is_scan_deny(_adapter *adapter, void *data)
+{
+	return rtw_is_scan_deny(adapter);
+}
+
+u8 rtw_mi_is_scan_deny(_adapter *adapter)
+{
+	return _rtw_mi_process(adapter, false, NULL, _rtw_mi_is_scan_deny);
+
+}
+u8 rtw_mi_buddy_is_scan_deny(_adapter *adapter)
+{
+	return _rtw_mi_process(adapter, true, NULL, _rtw_mi_is_scan_deny);
+}
+
+#ifdef CONFIG_SET_SCAN_DENY_TIMER
+static u8 _rtw_mi_set_scan_deny(_adapter *adapter, void *data)
+{
+	u32 ms = *(u32 *)data;
+
+	rtw_set_scan_deny(adapter, ms);
+	return true;
+}
+void rtw_mi_set_scan_deny(_adapter *adapter, u32 ms)
+{
+	u32 in_data = ms;
+
+	_rtw_mi_process(adapter, false, &in_data, _rtw_mi_set_scan_deny);
+}
+void rtw_mi_buddy_set_scan_deny(_adapter *adapter, u32 ms)
+{
+	u32 in_data = ms;
+
+	_rtw_mi_process(adapter, true, &in_data, _rtw_mi_set_scan_deny);
+}
+#endif
+
+struct nulldata_param {
+	unsigned char *da;
+	unsigned int power_mode;
+	int try_cnt;
+	int wait_ms;
+};
+
+static u8 _rtw_mi_issue_nulldata(_adapter *padapter, void *data)
+{
+	struct nulldata_param *pnulldata_param = (struct nulldata_param *)data;
+
+	if (is_client_associated_to_ap(padapter) == true) {
+		/* TODO: TDLS peers */
+		issue_nulldata(padapter, pnulldata_param->da, pnulldata_param->power_mode, pnulldata_param->try_cnt, pnulldata_param->wait_ms);
+		return true;
+	}
+	return false;
+}
+
+u8 rtw_mi_issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
+{
+	struct nulldata_param nparam;
+
+	nparam.da = da;
+	nparam.power_mode = power_mode;/*0 or 1*/
+	nparam.try_cnt = try_cnt;
+	nparam.wait_ms = wait_ms;
+
+	return _rtw_mi_process(padapter, false, &nparam, _rtw_mi_issue_nulldata);
+}
+u8 rtw_mi_buddy_issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
+{
+	struct nulldata_param nparam;
+
+	nparam.da = da;
+	nparam.power_mode = power_mode;
+	nparam.try_cnt = try_cnt;
+	nparam.wait_ms = wait_ms;
+
+	return _rtw_mi_process(padapter, true, &nparam, _rtw_mi_issue_nulldata);
+}
+
+static u8 _rtw_mi_beacon_update(_adapter *padapter, void *data)
+{
+	struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
+
+	if (mlmeext_msr(mlmeext) == WIFI_FW_AP_STATE
+	    && check_fwstate(&padapter->mlmepriv, _FW_LINKED) == true) {
+		RTW_INFO(ADPT_FMT"-WIFI_FW_AP_STATE - update_beacon\n", ADPT_ARG(padapter));
+		update_beacon(padapter, 0, NULL, true);
+	}
+	return true;
+}
+
+void rtw_mi_beacon_update(_adapter *padapter)
+{
+	_rtw_mi_process(padapter, false, NULL, _rtw_mi_beacon_update);
+}
+
+void rtw_mi_buddy_beacon_update(_adapter *padapter)
+{
+	_rtw_mi_process(padapter, true, NULL, _rtw_mi_beacon_update);
+}
+
+static u8 _rtw_mi_hal_dump_macaddr(_adapter *padapter, void *data)
+{
+	u8 mac_addr[ETH_ALEN] = {0};
+
+	rtw_hal_get_macaddr_port(padapter, mac_addr);
+	RTW_INFO(ADPT_FMT"MAC Address ="MAC_FMT"\n", ADPT_ARG(padapter), MAC_ARG(mac_addr));
+	return true;
+}
+void rtw_mi_hal_dump_macaddr(_adapter *padapter)
+{
+	_rtw_mi_process(padapter, false, NULL, _rtw_mi_hal_dump_macaddr);
+}
+void rtw_mi_buddy_hal_dump_macaddr(_adapter *padapter)
+{
+	_rtw_mi_process(padapter, true, NULL, _rtw_mi_hal_dump_macaddr);
+}
+
+static u8 _rtw_mi_busy_traffic_check(_adapter *padapter, void *data)
+{
+	u32 passtime;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	bool check_sc_interval = *(bool *)data;
+
+	if (pmlmepriv->LinkDetectInfo.bBusyTraffic == true) {
+		if (check_sc_interval) {
+			/* Miracast can't do AP scan*/
+			passtime = rtw_get_passing_time_ms(pmlmepriv->lastscantime);
+			pmlmepriv->lastscantime = jiffies;
+			if (passtime > BUSY_TRAFFIC_SCAN_DENY_PERIOD) {
+				RTW_INFO(ADPT_FMT" bBusyTraffic == true\n", ADPT_ARG(padapter));
+				return true;
+			}
+		} else
+			return true;
+	}
+
+	return false;
+}
+
+u8 rtw_mi_busy_traffic_check(_adapter *padapter, bool check_sc_interval)
+{
+	bool in_data = check_sc_interval;
+
+	return _rtw_mi_process(padapter, false, &in_data, _rtw_mi_busy_traffic_check);
+}
+u8 rtw_mi_buddy_busy_traffic_check(_adapter *padapter, bool check_sc_interval)
+{
+	bool in_data = check_sc_interval;
+
+	return _rtw_mi_process(padapter, true, &in_data, _rtw_mi_busy_traffic_check);
+}
+static u8 _rtw_mi_check_mlmeinfo_state(_adapter *padapter, void *data)
+{
+	u32 state = *(u32 *)data;
+	struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
+
+	/*if (mlmeext_msr(mlmeext) == state)*/
+	if (check_mlmeinfo_state(mlmeext, state))
+		return true;
+	else
+		return false;
+}
+
+u8 rtw_mi_check_mlmeinfo_state(_adapter *padapter, u32 state)
+{
+	u32 in_data = state;
+
+	return _rtw_mi_process(padapter, false, &in_data, _rtw_mi_check_mlmeinfo_state);
+}
+
+u8 rtw_mi_buddy_check_mlmeinfo_state(_adapter *padapter, u32 state)
+{
+	u32 in_data = state;
+
+	return _rtw_mi_process(padapter, true, &in_data, _rtw_mi_check_mlmeinfo_state);
+}
+
+/*#define DBG_DUMP_FW_STATE*/
+#ifdef DBG_DUMP_FW_STATE
+static void rtw_dbg_dump_fwstate(_adapter *padapter, sint state)
+{
+	u8 buf[32] = {0};
+
+	if (state & WIFI_FW_NULL_STATE) {
+		memset(buf, 0, 32);
+		sprintf(buf, "WIFI_FW_NULL_STATE");
+		RTW_INFO(FUNC_ADPT_FMT"fwstate-%s\n", FUNC_ADPT_ARG(padapter), buf);
+	}
+
+	if (state & _FW_LINKED) {
+		memset(buf, 0, 32);
+		sprintf(buf, "_FW_LINKED");
+		RTW_INFO(FUNC_ADPT_FMT"fwstate-%s\n", FUNC_ADPT_ARG(padapter), buf);
+	}
+
+	if (state & _FW_UNDER_LINKING) {
+		memset(buf, 0, 32);
+		sprintf(buf, "_FW_UNDER_LINKING");
+		RTW_INFO(FUNC_ADPT_FMT"fwstate-%s\n", FUNC_ADPT_ARG(padapter), buf);
+	}
+
+	if (state & _FW_UNDER_SURVEY) {
+		memset(buf, 0, 32);
+		sprintf(buf, "_FW_UNDER_SURVEY");
+		RTW_INFO(FUNC_ADPT_FMT"fwstate-%s\n", FUNC_ADPT_ARG(padapter), buf);
+	}
+}
+#endif
+
+static u8 _rtw_mi_check_fwstate(_adapter *padapter, void *data)
+{
+	u8 ret = false;
+
+	sint state = *(sint *)data;
+
+	if ((state == WIFI_FW_NULL_STATE) &&
+	    (padapter->mlmepriv.fw_state == WIFI_FW_NULL_STATE))
+		ret = true;
+	else if (check_fwstate(&padapter->mlmepriv, state))
+		ret = true;
+#ifdef DBG_DUMP_FW_STATE
+	if (ret)
+		rtw_dbg_dump_fwstate(padapter, state);
+#endif
+	return ret;
+}
+u8 rtw_mi_check_fwstate(_adapter *padapter, sint state)
+{
+	sint in_data = state;
+
+	return _rtw_mi_process(padapter, false, &in_data, _rtw_mi_check_fwstate);
+}
+u8 rtw_mi_buddy_check_fwstate(_adapter *padapter, sint state)
+{
+	sint in_data = state;
+
+	return _rtw_mi_process(padapter, true, &in_data, _rtw_mi_check_fwstate);
+}
+
+static u8 _rtw_mi_traffic_statistics(_adapter *padapter , void *data)
+{
+	struct dvobj_priv	*pdvobjpriv = adapter_to_dvobj(padapter);
+
+	/* Tx */
+	pdvobjpriv->traffic_stat.tx_bytes += padapter->xmitpriv.tx_bytes;
+	pdvobjpriv->traffic_stat.tx_pkts += padapter->xmitpriv.tx_pkts;
+	pdvobjpriv->traffic_stat.tx_drop += padapter->xmitpriv.tx_drop;
+
+	/* Rx */
+	pdvobjpriv->traffic_stat.rx_bytes += padapter->recvpriv.rx_bytes;
+	pdvobjpriv->traffic_stat.rx_pkts += padapter->recvpriv.rx_pkts;
+	pdvobjpriv->traffic_stat.rx_drop += padapter->recvpriv.rx_drop;
+	return true;
+}
+u8 rtw_mi_traffic_statistics(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_traffic_statistics);
+}
+
+static u8 _rtw_mi_check_miracast_enabled(_adapter *padapter , void *data)
+{
+	return is_miracast_enabled(padapter);
+}
+u8 rtw_mi_check_miracast_enabled(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_check_miracast_enabled);
+}
+
+#ifdef CONFIG_XMIT_THREAD_MODE
+static u8 _rtw_mi_check_pending_xmitbuf(_adapter *padapter , void *data)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	return check_pending_xmitbuf(pxmitpriv);
+}
+u8 rtw_mi_check_pending_xmitbuf(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_check_pending_xmitbuf);
+}
+u8 rtw_mi_buddy_check_pending_xmitbuf(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, true, NULL, _rtw_mi_check_pending_xmitbuf);
+}
+#endif
+
+static void _rtw_mi_adapter_reset(_adapter *padapter , u8 exclude_self)
+{
+	int i;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		if (dvobj->padapters[i]) {
+			if ((exclude_self) && (dvobj->padapters[i] == padapter))
+				continue;
+			dvobj->padapters[i] = NULL;
+		}
+	}
+}
+
+void rtw_mi_adapter_reset(_adapter *padapter)
+{
+	_rtw_mi_adapter_reset(padapter, false);
+}
+
+void rtw_mi_buddy_adapter_reset(_adapter *padapter)
+{
+	_rtw_mi_adapter_reset(padapter, true);
+}
+
+static u8 _rtw_mi_dynamic_check_timer_handler(_adapter *adapter, void *data)
+{
+	rtw_iface_dynamic_check_timer_handler(adapter);
+	return true;
+}
+u8 rtw_mi_dynamic_check_timer_handler(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_dynamic_check_timer_handler);
+}
+u8 rtw_mi_buddy_dynamic_check_timer_handler(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, true, NULL, _rtw_mi_dynamic_check_timer_handler);
+}
+
+static u8 _rtw_mi_dev_unload(_adapter *adapter, void *data)
+{
+	rtw_dev_unload(adapter);
+	return true;
+}
+u8 rtw_mi_dev_unload(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_dev_unload);
+}
+u8 rtw_mi_buddy_dev_unload(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, true, NULL, _rtw_mi_dev_unload);
+}
+
+static u8 _rtw_mi_dynamic_chk_wk_hdl(_adapter *adapter, void *data)
+{
+	rtw_iface_dynamic_chk_wk_hdl(adapter);
+	return true;
+}
+u8 rtw_mi_dynamic_chk_wk_hdl(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_dynamic_chk_wk_hdl);
+}
+u8 rtw_mi_buddy_dynamic_chk_wk_hdl(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, true, NULL, _rtw_mi_dynamic_chk_wk_hdl);
+}
+
+static u8 _rtw_mi_os_xmit_schedule(_adapter *adapter, void *data)
+{
+	rtw_os_xmit_schedule(adapter);
+	return true;
+}
+u8 rtw_mi_os_xmit_schedule(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_os_xmit_schedule);
+}
+u8 rtw_mi_buddy_os_xmit_schedule(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, true, NULL, _rtw_mi_os_xmit_schedule);
+}
+
+static u8 _rtw_mi_report_survey_event(_adapter *adapter, void *data)
+{
+	union recv_frame *precv_frame = (union recv_frame *)data;
+
+	report_survey_event(adapter, precv_frame);
+	return true;
+}
+u8 rtw_mi_report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
+{
+	return _rtw_mi_process(padapter, false, precv_frame, _rtw_mi_report_survey_event);
+}
+u8 rtw_mi_buddy_report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
+{
+	return _rtw_mi_process(padapter, true, precv_frame, _rtw_mi_report_survey_event);
+}
+
+static u8 _rtw_mi_sreset_adapter_hdl(_adapter *adapter, void *data)
+{
+	u8 bstart = *(u8 *)data;
+
+	if (bstart)
+		sreset_start_adapter(adapter);
+	else
+		sreset_stop_adapter(adapter);
+	return true;
+}
+u8 rtw_mi_sreset_adapter_hdl(_adapter *padapter, u8 bstart)
+{
+	u8 in_data = bstart;
+
+	return _rtw_mi_process(padapter, false, &in_data, _rtw_mi_sreset_adapter_hdl);
+}
+u8 rtw_mi_buddy_sreset_adapter_hdl(_adapter *padapter, u8 bstart)
+{
+	u8 in_data = bstart;
+
+	return _rtw_mi_process(padapter, true, &in_data, _rtw_mi_sreset_adapter_hdl);
+}
+static u8 _rtw_mi_tx_beacon_hdl(_adapter *adapter, void *data)
+{
+	if (check_fwstate(&adapter->mlmepriv, WIFI_AP_STATE) == true
+	    && check_fwstate(&adapter->mlmepriv, WIFI_ASOC_STATE) == true
+	   ) {
+		adapter->mlmepriv.update_bcn = true;
+#ifndef CONFIG_INTERRUPT_BASED_TXBCN
+		tx_beacon_hdl(adapter, NULL);
+#endif
+	}
+	return true;
+}
+u8 rtw_mi_tx_beacon_hdl(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_tx_beacon_hdl);
+}
+u8 rtw_mi_buddy_tx_beacon_hdl(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, true, NULL, _rtw_mi_sreset_adapter_hdl);
+}
+
+static u8 _rtw_mi_set_tx_beacon_cmd(_adapter *adapter, void *data)
+{
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		if (pmlmepriv->update_bcn == true)
+			set_tx_beacon_cmd(adapter);
+	}
+	return true;
+}
+u8 rtw_mi_set_tx_beacon_cmd(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_set_tx_beacon_cmd);
+}
+u8 rtw_mi_buddy_set_tx_beacon_cmd(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, true, NULL, _rtw_mi_set_tx_beacon_cmd);
+}
+
+#ifdef CONFIG_P2P
+static u8 _rtw_mi_p2p_chk_state(_adapter *adapter, void *data)
+{
+	struct wifidirect_info *pwdinfo = &(adapter->wdinfo);
+	enum P2P_STATE state = *(enum P2P_STATE *)data;
+
+	return rtw_p2p_chk_state(pwdinfo, state);
+}
+u8 rtw_mi_p2p_chk_state(_adapter *padapter, enum P2P_STATE p2p_state)
+{
+	u8 in_data = p2p_state;
+
+	return _rtw_mi_process(padapter, false, &in_data, _rtw_mi_p2p_chk_state);
+}
+u8 rtw_mi_buddy_p2p_chk_state(_adapter *padapter, enum P2P_STATE p2p_state)
+{
+	u8 in_data  = p2p_state;
+
+	return _rtw_mi_process(padapter, true, &in_data, _rtw_mi_p2p_chk_state);
+}
+static u8 _rtw_mi_stay_in_p2p_mode(_adapter *adapter, void *data)
+{
+	struct wifidirect_info *pwdinfo = &(adapter->wdinfo);
+
+	if (rtw_p2p_role(pwdinfo) != P2P_ROLE_DISABLE)
+		return true;
+	return false;
+}
+u8 rtw_mi_stay_in_p2p_mode(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, false, NULL, _rtw_mi_stay_in_p2p_mode);
+}
+u8 rtw_mi_buddy_stay_in_p2p_mode(_adapter *padapter)
+{
+	return _rtw_mi_process(padapter, true, NULL, _rtw_mi_stay_in_p2p_mode);
+}
+#endif /*CONFIG_P2P*/
+
+_adapter *rtw_get_iface_by_id(_adapter *padapter, u8 iface_id)
+{
+	_adapter *iface = NULL;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+	if ((padapter == NULL) || (iface_id >= CONFIG_IFACE_NUMBER)) {
+		rtw_warn_on(1);
+		return iface;
+	}
+
+	return  dvobj->padapters[iface_id];
+}
+
+_adapter *rtw_get_iface_by_macddr(_adapter *padapter, u8 *mac_addr)
+{
+	int i;
+	_adapter *iface = NULL;
+	u8 bmatch = false;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		if ((iface) && (!memcmp(mac_addr, adapter_mac_addr(iface), ETH_ALEN))) {
+			bmatch = true;
+			break;
+		}
+	}
+	if (bmatch)
+		return iface;
+	else
+		return NULL;
+}
+
+_adapter *rtw_get_iface_by_hwport(_adapter *padapter, u8 hw_port)
+{
+	int i;
+	_adapter *iface = NULL;
+	u8 bmatch = false;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		if ((iface) && (hw_port == iface->hw_port)) {
+			bmatch = true;
+			break;
+		}
+	}
+	if (bmatch)
+		return iface;
+	else
+		return NULL;
+}
+
+/*#define CONFIG_SKB_ALLOCATED*/
+#define DBG_SKB_PROCESS
+#ifdef DBG_SKB_PROCESS
+static void rtw_dbg_skb_process(_adapter *padapter, union recv_frame *precvframe, union recv_frame *pcloneframe)
+{
+	_pkt *pkt_copy, *pkt_org;
+
+	pkt_org = precvframe->u.hdr.pkt;
+	pkt_copy = pcloneframe->u.hdr.pkt;
+	/*
+		RTW_INFO("%s ===== ORG SKB =====\n", __func__);
+		RTW_INFO(" SKB head(%p)\n", pkt_org->head);
+		RTW_INFO(" SKB data(%p)\n", pkt_org->data);
+		RTW_INFO(" SKB tail(%p)\n", pkt_org->tail);
+		RTW_INFO(" SKB end(%p)\n", pkt_org->end);
+
+		RTW_INFO(" recv frame head(%p)\n", precvframe->u.hdr.rx_head);
+		RTW_INFO(" recv frame data(%p)\n", precvframe->u.hdr.rx_data);
+		RTW_INFO(" recv frame tail(%p)\n", precvframe->u.hdr.rx_tail);
+		RTW_INFO(" recv frame end(%p)\n", precvframe->u.hdr.rx_end);
+
+		RTW_INFO("%s ===== COPY SKB =====\n", __func__);
+		RTW_INFO(" SKB head(%p)\n", pkt_copy->head);
+		RTW_INFO(" SKB data(%p)\n", pkt_copy->data);
+		RTW_INFO(" SKB tail(%p)\n", pkt_copy->tail);
+		RTW_INFO(" SKB end(%p)\n", pkt_copy->end);
+
+		RTW_INFO(" recv frame head(%p)\n", pcloneframe->u.hdr.rx_head);
+		RTW_INFO(" recv frame data(%p)\n", pcloneframe->u.hdr.rx_data);
+		RTW_INFO(" recv frame tail(%p)\n", pcloneframe->u.hdr.rx_tail);
+		RTW_INFO(" recv frame end(%p)\n", pcloneframe->u.hdr.rx_end);
+	*/
+	/*
+		RTW_INFO("%s => recv_frame adapter(%p,%p)\n", __func__, precvframe->u.hdr.adapter, pcloneframe->u.hdr.adapter);
+		RTW_INFO("%s => recv_frame dev(%p,%p)\n", __func__, pkt_org->dev , pkt_copy->dev);
+		RTW_INFO("%s => recv_frame len(%d,%d)\n", __func__, precvframe->u.hdr.len, pcloneframe->u.hdr.len);
+	*/
+	if (precvframe->u.hdr.len != pcloneframe->u.hdr.len)
+		RTW_INFO("%s [WARN]  recv_frame length(%d:%d) compare failed\n", __func__, precvframe->u.hdr.len, pcloneframe->u.hdr.len);
+
+	if (!memcmp(&precvframe->u.hdr.attrib, &pcloneframe->u.hdr.attrib, sizeof(struct rx_pkt_attrib)) == false)
+		RTW_INFO("%s [WARN]  recv_frame attrib compare failed\n", __func__);
+
+	if (!memcmp(precvframe->u.hdr.rx_data, pcloneframe->u.hdr.rx_data, precvframe->u.hdr.len) == false)
+		RTW_INFO("%s [WARN]  recv_frame rx_data compare failed\n", __func__);
+
+}
+#endif
+
+static s32 _rtw_mi_buddy_clone_bcmc_packet(_adapter *adapter, union recv_frame *precvframe, u8 *pphy_status, union recv_frame *pcloneframe)
+{
+	s32 ret = _SUCCESS;
+	u8 *pbuf = precvframe->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = NULL;
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(adapter);
+
+	if (pcloneframe) {
+		pcloneframe->u.hdr.adapter = adapter;
+
+		INIT_LIST_HEAD(&pcloneframe->u.hdr.list);
+		pcloneframe->u.hdr.precvbuf = NULL;	/*can't access the precvbuf for new arch.*/
+		pcloneframe->u.hdr.len = 0;
+
+		memcpy(&pcloneframe->u.hdr.attrib, &precvframe->u.hdr.attrib, sizeof(struct rx_pkt_attrib));
+
+		pattrib = &pcloneframe->u.hdr.attrib;
+#ifdef CONFIG_SKB_ALLOCATED
+		if (rtw_os_alloc_recvframe(adapter, pcloneframe, pbuf, NULL) == _SUCCESS)
+#else
+		if (rtw_os_recvframe_duplicate_skb(adapter, pcloneframe, precvframe->u.hdr.pkt) == _SUCCESS)
+#endif
+		{
+#ifdef CONFIG_SKB_ALLOCATED
+			recvframe_put(pcloneframe, pattrib->pkt_len);
+#endif
+
+#ifdef DBG_SKB_PROCESS
+			rtw_dbg_skb_process(adapter, precvframe, pcloneframe);
+#endif
+
+			if (pattrib->physt && pphy_status)
+				rx_query_phy_status(pcloneframe, pphy_status);
+
+			ret = rtw_recv_entry(pcloneframe);
+		} else {
+			ret = -1;
+			RTW_INFO("%s()-%d: rtw_os_alloc_recvframe() failed!\n", __func__, __LINE__);
+		}
+
+	}
+	return ret;
+}
+
+void rtw_mi_buddy_clone_bcmc_packet(_adapter *padapter, union recv_frame *precvframe, u8 *pphy_status)
+{
+	int i;
+	s32 ret = _SUCCESS;
+	_adapter *iface = NULL;
+	union recv_frame *pcloneframe = NULL;
+	struct recv_priv *precvpriv = &padapter->recvpriv;/*primary_padapter*/
+	_queue *pfree_recv_queue = &precvpriv->free_recv_queue;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		if (!iface || iface == padapter)
+			continue;
+		if (rtw_is_adapter_up(iface) == false || iface->registered == 0)
+			continue;
+
+		pcloneframe = rtw_alloc_recvframe(pfree_recv_queue);
+		if (pcloneframe) {
+			ret = _rtw_mi_buddy_clone_bcmc_packet(iface, precvframe, pphy_status, pcloneframe);
+			if (_SUCCESS != ret) {
+				if (ret == -1)
+					rtw_free_recvframe(pcloneframe, pfree_recv_queue);
+				/*RTW_INFO(ADPT_FMT"-clone BC/MC frame failed\n", ADPT_ARG(iface));*/
+			}
+		}
+	}
+
+}
+
+void rtw_mi_update_ap_bmc_camid(_adapter *padapter, u8 camid_a, u8 camid_b)
+{
+#ifdef CONFIG_CONCURRENT_MODE
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
+
+	int i;
+	_adapter *iface = NULL;
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		if (!iface)
+			continue;
+
+		if (macid_ctl->iface_bmc[iface->iface_id] != INVALID_SEC_MAC_CAM_ID) {
+			if (macid_ctl->iface_bmc[iface->iface_id] == camid_a)
+				macid_ctl->iface_bmc[iface->iface_id] = camid_b;
+			else if (macid_ctl->iface_bmc[iface->iface_id] == camid_b)
+				macid_ctl->iface_bmc[iface->iface_id] = camid_a;
+			iface->securitypriv.dot118021x_bmc_cam_id  = macid_ctl->iface_bmc[iface->iface_id];
+		}
+	}
+#endif
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme.c b/drivers/staging/rtl8188eu/core/rtw_mlme.c
new file mode 100644
index 000000000000..00eedc7c6aab
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme.c
@@ -0,0 +1,4401 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_MLME_C_
+
+#include <hal_data.h>
+#include <usb_osintf.h>
+
+extern void indicate_wx_scan_complete_event(_adapter *padapter);
+extern u8 rtw_do_join(_adapter *padapter);
+
+
+static sint _rtw_init_mlme_priv(_adapter *padapter)
+{
+	sint	i;
+	u8	*pbuf;
+	struct wlan_network	*pnetwork;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	sint	res = _SUCCESS;
+
+	/*ht_priv*/
+	pmlmepriv->htpriv.ampdu_enable = false;/*set to disabled*/
+
+	pmlmepriv->nic_hdl = (u8 *)padapter;
+
+	pmlmepriv->pscanned = NULL;
+	/*pmlmepriv->fw_state = WIFI_STATION_STATE; */ /*Must sync with rtw_wdev_alloc()*/
+	/*init_fwstate(pmlmepriv, WIFI_STATION_STATE);*/
+	init_fwstate(pmlmepriv, WIFI_NULL_STATE);/*assigned interface role(STA/AP) must after execute set_opmode*/
+
+	/* wdev->iftype = NL80211_IFTYPE_STATION*/
+	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) */
+
+	spin_lock_init(&(pmlmepriv->lock));
+	_rtw_init_queue(&(pmlmepriv->free_bss_pool));
+	_rtw_init_queue(&(pmlmepriv->scanned_queue));
+
+	set_scanned_network_val(pmlmepriv, 0);
+
+	memset(&pmlmepriv->assoc_ssid, 0, sizeof(NDIS_802_11_SSID));
+
+	pbuf = rtw_zvmalloc(MAX_BSS_CNT * (sizeof(struct wlan_network)));
+
+	if (pbuf == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	pmlmepriv->free_bss_buf = pbuf;
+
+	pnetwork = (struct wlan_network *)pbuf;
+
+	for (i = 0; i < MAX_BSS_CNT; i++) {
+		INIT_LIST_HEAD(&(pnetwork->list));
+
+		list_add_tail(&(pnetwork->list), &(pmlmepriv->free_bss_pool.queue));
+
+		pnetwork++;
+	}
+
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+
+	rtw_clear_scan_deny(padapter);
+#ifdef CONFIG_ARP_KEEP_ALIVE
+	pmlmepriv->bGetGateway = 0;
+	pmlmepriv->GetGatewayTryCnt = 0;
+#endif
+
+#ifdef CONFIG_LAYER2_ROAMING
+#define RTW_ROAM_SCAN_RESULT_EXP_MS (5*1000)
+#define RTW_ROAM_RSSI_DIFF_TH 10
+#define RTW_ROAM_SCAN_INTERVAL_MS (10*1000)
+#define RTW_ROAM_RSSI_THRESHOLD 70
+
+	pmlmepriv->roam_flags = 0
+				| RTW_ROAM_ON_EXPIRED
+#ifdef CONFIG_LAYER2_ROAMING_RESUME
+				| RTW_ROAM_ON_RESUME
+#endif
+#ifdef CONFIG_LAYER2_ROAMING_ACTIVE
+				| RTW_ROAM_ACTIVE
+#endif
+				;
+
+	pmlmepriv->roam_scanr_exp_ms = RTW_ROAM_SCAN_RESULT_EXP_MS;
+	pmlmepriv->roam_rssi_diff_th = RTW_ROAM_RSSI_DIFF_TH;
+	pmlmepriv->roam_scan_int_ms = RTW_ROAM_SCAN_INTERVAL_MS;
+	pmlmepriv->roam_rssi_threshold = RTW_ROAM_RSSI_THRESHOLD;
+#endif /* CONFIG_LAYER2_ROAMING */
+
+#ifdef CONFIG_RTW_80211R
+	memset(&pmlmepriv->ftpriv, 0, sizeof(ft_priv));
+	pmlmepriv->ftpriv.ft_flags = 0
+		| RTW_FT_STA_SUPPORTED
+		| RTW_FT_STA_OVER_DS_SUPPORTED
+		;
+#endif
+	rtw_init_mlme_timer(padapter);
+
+exit:
+
+
+	return res;
+}
+
+static void rtw_mfree_mlme_priv_lock(struct mlme_priv *pmlmepriv)
+{
+}
+
+static void rtw_free_mlme_ie_data(u8 **ppie, u32 *plen)
+{
+	if (*ppie) {
+		rtw_mfree(*ppie, *plen);
+		*plen = 0;
+		*ppie = NULL;
+	}
+}
+
+void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
+{
+#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
+	rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_beacon_ie, &pmlmepriv->wps_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_req_ie, &pmlmepriv->wps_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_resp_ie, &pmlmepriv->wps_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_assoc_resp_ie, &pmlmepriv->wps_assoc_resp_ie_len);
+
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_beacon_ie, &pmlmepriv->p2p_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_req_ie, &pmlmepriv->p2p_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_resp_ie, &pmlmepriv->p2p_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_go_probe_resp_ie, &pmlmepriv->p2p_go_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_req_ie, &pmlmepriv->p2p_assoc_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_resp_ie, &pmlmepriv->p2p_assoc_resp_ie_len);
+#endif
+
+#if defined(CONFIG_WFD) && defined(CONFIG_IOCTL_CFG80211)
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_beacon_ie, &pmlmepriv->wfd_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_req_ie, &pmlmepriv->wfd_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_resp_ie, &pmlmepriv->wfd_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_go_probe_resp_ie, &pmlmepriv->wfd_go_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_assoc_req_ie, &pmlmepriv->wfd_assoc_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_assoc_resp_ie, &pmlmepriv->wfd_assoc_resp_ie_len);
+#endif
+
+#ifdef CONFIG_RTW_80211R
+	rtw_free_mlme_ie_data(&pmlmepriv->auth_rsp, &pmlmepriv->auth_rsp_len);
+#endif
+}
+
+#if defined(CONFIG_WFD) && defined(CONFIG_IOCTL_CFG80211)
+int rtw_mlme_update_wfd_ie_data(struct mlme_priv *mlme, u8 type, u8 *ie, u32 ie_len)
+{
+	_adapter *adapter = mlme_to_adapter(mlme);
+	struct wifi_display_info *wfd_info = &adapter->wfd_info;
+	u8 clear = 0;
+	u8 **t_ie = NULL;
+	u32 *t_ie_len = NULL;
+	int ret = _FAIL;
+
+	if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
+		goto success;
+
+	if (wfd_info->wfd_enable)
+		goto success; /* WFD IE is build by self */
+
+	if (!ie && !ie_len)
+		clear = 1;
+	else if (!ie || !ie_len) {
+		RTW_PRINT(FUNC_ADPT_FMT" type:%u, ie:%p, ie_len:%u"
+			  , FUNC_ADPT_ARG(adapter), type, ie, ie_len);
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	switch (type) {
+	case MLME_BEACON_IE:
+		t_ie = &mlme->wfd_beacon_ie;
+		t_ie_len = &mlme->wfd_beacon_ie_len;
+		break;
+	case MLME_PROBE_REQ_IE:
+		t_ie = &mlme->wfd_probe_req_ie;
+		t_ie_len = &mlme->wfd_probe_req_ie_len;
+		break;
+	case MLME_PROBE_RESP_IE:
+		t_ie = &mlme->wfd_probe_resp_ie;
+		t_ie_len = &mlme->wfd_probe_resp_ie_len;
+		break;
+	case MLME_GO_PROBE_RESP_IE:
+		t_ie = &mlme->wfd_go_probe_resp_ie;
+		t_ie_len = &mlme->wfd_go_probe_resp_ie_len;
+		break;
+	case MLME_ASSOC_REQ_IE:
+		t_ie = &mlme->wfd_assoc_req_ie;
+		t_ie_len = &mlme->wfd_assoc_req_ie_len;
+		break;
+	case MLME_ASSOC_RESP_IE:
+		t_ie = &mlme->wfd_assoc_resp_ie;
+		t_ie_len = &mlme->wfd_assoc_resp_ie_len;
+		break;
+	default:
+		RTW_PRINT(FUNC_ADPT_FMT" unsupported type:%u"
+			  , FUNC_ADPT_ARG(adapter), type);
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	if (*t_ie) {
+		u32 free_len = *t_ie_len;
+		*t_ie_len = 0;
+		rtw_mfree(*t_ie, free_len);
+		*t_ie = NULL;
+	}
+
+	if (!clear) {
+		*t_ie = rtw_malloc(ie_len);
+		if (*t_ie == NULL) {
+			RTW_ERR(FUNC_ADPT_FMT" type:%u, rtw_malloc() fail\n"
+				, FUNC_ADPT_ARG(adapter), type);
+			goto exit;
+		}
+		memcpy(*t_ie, ie, ie_len);
+		*t_ie_len = ie_len;
+	}
+
+	if (*t_ie && *t_ie_len) {
+		u8 *attr_content;
+		u32 attr_contentlen = 0;
+
+		attr_content = rtw_get_wfd_attr_content(*t_ie, *t_ie_len, WFD_ATTR_DEVICE_INFO, NULL, &attr_contentlen);
+		if (attr_content && attr_contentlen) {
+			if (RTW_GET_BE16(attr_content + 2) != wfd_info->rtsp_ctrlport) {
+				wfd_info->rtsp_ctrlport = RTW_GET_BE16(attr_content + 2);
+				RTW_INFO(FUNC_ADPT_FMT" type:%u, RTSP CTRL port = %u\n"
+					, FUNC_ADPT_ARG(adapter), type, wfd_info->rtsp_ctrlport);
+			}
+		}
+	}
+
+success:
+	ret = _SUCCESS;
+
+exit:
+	return ret;
+}
+#endif /* defined(CONFIG_WFD) && defined(CONFIG_IOCTL_CFG80211) */
+
+static void _rtw_free_mlme_priv(struct mlme_priv *pmlmepriv)
+{
+	if (NULL == pmlmepriv) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+	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));
+	}
+exit:
+	return;
+}
+
+static sint	_rtw_enqueue_network(_queue *queue, struct wlan_network *pnetwork)
+{
+	unsigned long irqL;
+
+
+	if (pnetwork == NULL)
+		goto exit;
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	list_add_tail(&pnetwork->list, &queue->queue);
+
+	_exit_critical_bh(&queue->lock, &irqL);
+
+exit:
+
+
+	return _SUCCESS;
+}
+
+/*
+struct	wlan_network *_rtw_dequeue_network(_queue *queue)
+{
+	unsigned long irqL;
+
+	struct wlan_network *pnetwork;
+
+
+	_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);
+
+		list_del_init(&(pnetwork->list));
+	}
+
+	_exit_critical_bh(&queue->lock, &irqL);
+
+
+	return pnetwork;
+}
+*/
+
+struct	wlan_network *_rtw_alloc_network(struct	mlme_priv *pmlmepriv) /* (_queue *free_queue) */
+{
+	unsigned long	irqL;
+	struct	wlan_network	*pnetwork;
+	_queue *free_queue = &pmlmepriv->free_bss_pool;
+	_list *plist = NULL;
+
+
+	_enter_critical_bh(&free_queue->lock, &irqL);
+
+	if (_rtw_queue_empty(free_queue)) {
+		pnetwork = NULL;
+		goto exit;
+	}
+	plist = get_next(&(free_queue->queue));
+
+	pnetwork = LIST_CONTAINOR(plist , struct wlan_network, list);
+
+	list_del_init(&pnetwork->list);
+
+	pnetwork->network_type = 0;
+	pnetwork->fixed = false;
+	pnetwork->last_scanned = jiffies;
+	pnetwork->aid = 0;
+	pnetwork->join_res = 0;
+
+	pmlmepriv->num_of_scanned++;
+
+exit:
+	_exit_critical_bh(&free_queue->lock, &irqL);
+
+
+	return pnetwork;
+}
+
+void _rtw_free_network(struct	mlme_priv *pmlmepriv , struct wlan_network *pnetwork, u8 isfreeall)
+{
+	u32 delta_time;
+	u32 lifetime = SCANQUEUE_LIFETIME;
+	unsigned long irqL;
+	_queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+
+	if (pnetwork == NULL)
+		goto exit;
+
+	if (pnetwork->fixed)
+		goto exit;
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) ||
+	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)))
+		lifetime = 1;
+
+	if (!isfreeall) {
+		delta_time = (u32) rtw_get_passing_time_ms(pnetwork->last_scanned);
+		if (delta_time < lifetime) /* unit:msec */
+			goto exit;
+	}
+
+	_enter_critical_bh(&free_queue->lock, &irqL);
+
+	list_del_init(&(pnetwork->list));
+
+	list_add_tail(&(pnetwork->list), &(free_queue->queue));
+
+	pmlmepriv->num_of_scanned--;
+
+
+	/* RTW_INFO("_rtw_free_network:SSID=%s\n", pnetwork->network.Ssid.Ssid); */
+
+	_exit_critical_bh(&free_queue->lock, &irqL);
+
+exit:
+	return;
+}
+
+void _rtw_free_network_nolock(struct	mlme_priv *pmlmepriv, struct wlan_network *pnetwork)
+{
+
+	_queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+
+	if (pnetwork == NULL)
+		goto exit;
+
+	if (pnetwork->fixed)
+		goto exit;
+
+	/* _enter_critical(&free_queue->lock, &irqL); */
+
+	list_del_init(&(pnetwork->list));
+
+	list_add_tail(&(pnetwork->list), get_list_head(free_queue));
+
+	pmlmepriv->num_of_scanned--;
+
+	/* _exit_critical(&free_queue->lock, &irqL); */
+
+exit:
+	return;
+}
+
+
+/*
+	return the wlan_network with the matching addr
+
+	Shall be calle under atomic context... to avoid possible racing condition...
+*/
+struct wlan_network *_rtw_find_network(_queue *scanned_queue, u8 *addr)
+{
+
+	/* unsigned long irqL; */
+	_list	*phead, *plist;
+	struct	wlan_network *pnetwork = NULL;
+	u8 zero_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
+
+
+	if (!memcmp(zero_addr, addr, ETH_ALEN)) {
+		pnetwork = NULL;
+		goto exit;
+	}
+
+	/* _enter_critical_bh(&scanned_queue->lock, &irqL); */
+
+	phead = get_list_head(scanned_queue);
+	plist = get_next(phead);
+
+	while (plist != phead) {
+		pnetwork = LIST_CONTAINOR(plist, struct wlan_network , list);
+
+		if (!memcmp(addr, pnetwork->network.MacAddress, ETH_ALEN))
+			break;
+
+		plist = get_next(plist);
+	}
+
+	if (plist == phead)
+		pnetwork = NULL;
+
+	/* _exit_critical_bh(&scanned_queue->lock, &irqL); */
+
+exit:
+
+
+	return pnetwork;
+
+}
+
+
+void _rtw_free_network_queue(_adapter *padapter, u8 isfreeall)
+{
+	unsigned long irqL;
+	_list *phead, *plist;
+	struct wlan_network *pnetwork;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	_queue *scanned_queue = &pmlmepriv->scanned_queue;
+
+
+
+	_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);
+
+
+}
+
+
+
+
+sint rtw_if_up(_adapter *padapter)
+{
+
+	sint res;
+
+	if (RTW_CANNOT_RUN(padapter) ||
+	    (check_fwstate(&padapter->mlmepriv, _FW_LINKED) == false)) {
+		res = false;
+	} else
+		res =  true;
+
+	return res;
+}
+
+
+void rtw_generate_random_ibss(u8 *pibss)
+{
+	*((u32 *)(&pibss[2])) = rtw_random32();
+	pibss[0] = 0x02; /* in ad-hoc mode local bit must set to 1 */
+	pibss[1] = 0x11;
+	pibss[2] = 0x87;
+}
+
+u8 *rtw_get_capability_from_ie(u8 *ie)
+{
+	return ie + 8 + 2;
+}
+
+
+u16 rtw_get_capability(WLAN_BSSID_EX *bss)
+{
+	__le16	val;
+
+	memcpy((u8 *)&val, rtw_get_capability_from_ie(bss->IEs), 2);
+	return le16_to_cpu(val);
+}
+
+u8 *rtw_get_timestampe_from_ie(u8 *ie)
+{
+	return ie + 0;
+}
+
+u8 *rtw_get_beacon_interval_from_ie(u8 *ie)
+{
+	return ie + 8;
+}
+
+
+int	rtw_init_mlme_priv(_adapter *padapter) /* (struct	mlme_priv *pmlmepriv) */
+{
+	int	res;
+	res = _rtw_init_mlme_priv(padapter);/* (pmlmepriv); */
+	return res;
+}
+
+void rtw_free_mlme_priv(struct mlme_priv *pmlmepriv)
+{
+	_rtw_free_mlme_priv(pmlmepriv);
+}
+
+int	rtw_enqueue_network(_queue *queue, struct wlan_network *pnetwork);
+int	rtw_enqueue_network(_queue *queue, struct wlan_network *pnetwork)
+{
+	int	res;
+	res = _rtw_enqueue_network(queue, pnetwork);
+	return res;
+}
+
+/*
+static struct	wlan_network *rtw_dequeue_network(_queue *queue)
+{
+	struct wlan_network *pnetwork;
+	pnetwork = _rtw_dequeue_network(queue);
+	return pnetwork;
+}
+*/
+
+struct	wlan_network *rtw_alloc_network(struct	mlme_priv *pmlmepriv);
+struct	wlan_network *rtw_alloc_network(struct	mlme_priv *pmlmepriv) /* (_queue	*free_queue) */
+{
+	struct	wlan_network	*pnetwork;
+	pnetwork = _rtw_alloc_network(pmlmepriv);
+	return pnetwork;
+}
+
+void rtw_free_network(struct mlme_priv *pmlmepriv, struct	wlan_network *pnetwork, u8 is_freeall);
+void rtw_free_network(struct mlme_priv *pmlmepriv, struct	wlan_network *pnetwork, u8 is_freeall)/* (struct	wlan_network *pnetwork, _queue	*free_queue) */
+{
+	_rtw_free_network(pmlmepriv, pnetwork, is_freeall);
+}
+
+void rtw_free_network_nolock(_adapter *padapter, struct wlan_network *pnetwork);
+void rtw_free_network_nolock(_adapter *padapter, struct wlan_network *pnetwork)
+{
+	_rtw_free_network_nolock(&(padapter->mlmepriv), pnetwork);
+#ifdef CONFIG_IOCTL_CFG80211
+	rtw_cfg80211_unlink_bss(padapter, pnetwork);
+#endif /* CONFIG_IOCTL_CFG80211 */
+}
+
+
+void rtw_free_network_queue(_adapter *dev, u8 isfreeall)
+{
+	_rtw_free_network_queue(dev, isfreeall);
+}
+
+/*
+	return the wlan_network with the matching addr
+
+	Shall be calle under atomic context... to avoid possible racing condition...
+*/
+struct	wlan_network *rtw_find_network(_queue *scanned_queue, u8 *addr)
+{
+	struct	wlan_network *pnetwork = _rtw_find_network(scanned_queue, addr);
+
+	return pnetwork;
+}
+
+int rtw_is_same_ibss(_adapter *adapter, struct wlan_network *pnetwork)
+{
+	int ret = true;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+
+	if ((psecuritypriv->dot11PrivacyAlgrthm != _NO_PRIVACY_) &&
+	    (pnetwork->network.Privacy == 0))
+		ret = false;
+	else if ((psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_) &&
+		 (pnetwork->network.Privacy == 1))
+		ret = false;
+	else
+		ret = true;
+
+	return ret;
+
+}
+
+inline int is_same_ess(WLAN_BSSID_EX *a, WLAN_BSSID_EX *b)
+{
+	return (a->Ssid.SsidLength == b->Ssid.SsidLength)
+	       &&  !memcmp(a->Ssid.Ssid, b->Ssid.Ssid, a->Ssid.SsidLength);
+}
+
+int is_same_network(WLAN_BSSID_EX *src, WLAN_BSSID_EX *dst, u8 feature)
+{
+	u16 s_cap, d_cap;
+	__le16 le_tmp;
+
+	if (rtw_bug_check(dst, src, &s_cap, &d_cap) == false)
+		return false;
+
+	memcpy((u8 *)&le_tmp, rtw_get_capability_from_ie(src->IEs), 2);
+	s_cap = le16_to_cpu(le_tmp);
+	memcpy((u8 *)&le_tmp, rtw_get_capability_from_ie(dst->IEs), 2);
+	d_cap = le16_to_cpu(le_tmp);
+
+#ifdef CONFIG_P2P
+	if ((feature == 1) && /* 1: P2P supported */
+	    (!memcmp(src->MacAddress, dst->MacAddress, ETH_ALEN))
+	   )
+		return true;
+#endif
+
+	return ((src->Ssid.SsidLength == dst->Ssid.SsidLength) &&
+		/*	(src->Configuration.DSConfig == dst->Configuration.DSConfig) && */
+		((!memcmp(src->MacAddress, dst->MacAddress, ETH_ALEN))) &&
+		((!memcmp(src->Ssid.Ssid, dst->Ssid.Ssid, src->Ssid.SsidLength))) &&
+		((s_cap & WLAN_CAPABILITY_IBSS) ==
+		 (d_cap & WLAN_CAPABILITY_IBSS)) &&
+		((s_cap & WLAN_CAPABILITY_BSS) ==
+		 (d_cap & WLAN_CAPABILITY_BSS)));
+}
+
+struct wlan_network *_rtw_find_same_network(_queue *scanned_queue, struct wlan_network *network)
+{
+	_list *phead, *plist;
+	struct wlan_network *found = NULL;
+
+	phead = get_list_head(scanned_queue);
+	plist = get_next(phead);
+
+	while (plist != phead) {
+		found = LIST_CONTAINOR(plist, struct wlan_network , list);
+
+		if (is_same_network(&network->network, &found->network, 0))
+			break;
+
+		plist = get_next(plist);
+	}
+
+	if (plist == phead)
+		found = NULL;
+exit:
+	return found;
+}
+
+struct wlan_network *rtw_find_same_network(_queue *scanned_queue, struct wlan_network *network)
+{
+	unsigned long irqL;
+	struct wlan_network *found = NULL;
+
+	if (scanned_queue == NULL || network == NULL)
+		goto exit;
+
+	_enter_critical_bh(&scanned_queue->lock, &irqL);
+	found = _rtw_find_same_network(scanned_queue, network);
+	_exit_critical_bh(&scanned_queue->lock, &irqL);
+
+exit:
+	return found;
+}
+
+struct	wlan_network	*rtw_get_oldest_wlan_network(_queue *scanned_queue)
+{
+	_list	*plist, *phead;
+
+
+	struct	wlan_network	*pwlan = NULL;
+	struct	wlan_network	*oldest = NULL;
+	phead = get_list_head(scanned_queue);
+
+	plist = get_next(phead);
+
+	while (1) {
+
+		if (rtw_end_of_queue_search(phead, plist))
+			break;
+
+		pwlan = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		if (pwlan->fixed != true) {
+			if (oldest == NULL || time_after(oldest->last_scanned, pwlan->last_scanned))
+				oldest = pwlan;
+		}
+
+		plist = get_next(plist);
+	}
+	return oldest;
+
+}
+
+void update_network(WLAN_BSSID_EX *dst, WLAN_BSSID_EX *src,
+		    _adapter *padapter, bool update_ie)
+{
+	u8 ss_ori = dst->PhyInfo.SignalStrength;
+	u8 sq_ori = dst->PhyInfo.SignalQuality;
+	long rssi_ori = dst->Rssi;
+
+	u8 ss_smp = src->PhyInfo.SignalStrength;
+	u8 sq_smp = src->PhyInfo.SignalQuality;
+	long rssi_smp = src->Rssi;
+
+	u8 ss_final;
+	u8 sq_final;
+	long rssi_final;
+
+
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	rtw_hal_antdiv_rssi_compared(padapter, dst, src); /* this will update src.Rssi, need consider again */
+#endif
+
+#if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) && 1
+	if (strcmp(dst->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
+		RTW_INFO(FUNC_ADPT_FMT" %s("MAC_FMT", ch%u) ss_ori:%3u, sq_ori:%3u, rssi_ori:%3ld, ss_smp:%3u, sq_smp:%3u, rssi_smp:%3ld\n"
+			 , FUNC_ADPT_ARG(padapter)
+			, src->Ssid.Ssid, MAC_ARG(src->MacAddress), src->Configuration.DSConfig
+			 , ss_ori, sq_ori, rssi_ori
+			 , ss_smp, sq_smp, rssi_smp
+			);
+	}
+#endif
+
+	/* The rule below is 1/5 for sample value, 4/5 for history value */
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) && is_same_network(&(padapter->mlmepriv.cur_network.network), src, 0)) {
+		/* 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) {
+		dst->Reserved[0] = src->Reserved[0];
+		dst->Reserved[1] = src->Reserved[1];
+		memcpy((u8 *)dst, (u8 *)src, get_WLAN_BSSID_EX_sz(src));
+	}
+
+	dst->PhyInfo.SignalStrength = ss_final;
+	dst->PhyInfo.SignalQuality = sq_final;
+	dst->Rssi = rssi_final;
+
+#if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) && 1
+	if (strcmp(dst->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
+		RTW_INFO(FUNC_ADPT_FMT" %s("MAC_FMT"), SignalStrength:%u, SignalQuality:%u, RawRSSI:%ld\n"
+			 , FUNC_ADPT_ARG(padapter)
+			, dst->Ssid.Ssid, MAC_ARG(dst->MacAddress), dst->PhyInfo.SignalStrength, dst->PhyInfo.SignalQuality, dst->Rssi);
+	}
+#endif
+}
+
+static void update_current_network(_adapter *adapter, WLAN_BSSID_EX *pnetwork)
+{
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+
+
+	rtw_bug_check(&(pmlmepriv->cur_network.network),
+		      &(pmlmepriv->cur_network.network),
+		      &(pmlmepriv->cur_network.network),
+		      &(pmlmepriv->cur_network.network));
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED)) && (is_same_network(&(pmlmepriv->cur_network.network), pnetwork, 0))) {
+		update_network(&(pmlmepriv->cur_network.network), pnetwork, adapter, true);
+		rtw_update_protection(adapter, (pmlmepriv->cur_network.network.IEs) + sizeof(NDIS_802_11_FIXED_IEs),
+				      pmlmepriv->cur_network.network.IELength);
+	}
+
+
+}
+
+
+/*
+
+Caller must hold pmlmepriv->lock first.
+
+
+*/
+void rtw_update_scanned_network(_adapter *adapter, WLAN_BSSID_EX *target)
+{
+	unsigned long irqL;
+	_list	*plist, *phead;
+	u32	bssid_ex_sz;
+	struct mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(adapter->mlmeextpriv);
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &(adapter->wdinfo);
+#endif /* CONFIG_P2P */
+	_queue	*queue	= &(pmlmepriv->scanned_queue);
+	struct wlan_network	*pnetwork = NULL;
+	struct wlan_network	*oldest = NULL;
+	int target_find = 0;
+	u8 feature = 0;
+
+
+	_enter_critical_bh(&queue->lock, &irqL);
+	phead = get_list_head(queue);
+	plist = get_next(phead);
+
+#ifdef CONFIG_P2P
+	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		feature = 1; /* p2p enable */
+#endif
+
+	while (1) {
+		if (rtw_end_of_queue_search(phead, plist))
+			break;
+
+		pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		rtw_bug_check(pnetwork, pnetwork, pnetwork, pnetwork);
+
+#ifdef CONFIG_P2P
+		if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
+		    (!memcmp(pnetwork->network.MacAddress, target->MacAddress, ETH_ALEN))) {
+			target_find = 1;
+			break;
+		}
+#endif
+
+		if (is_same_network(&(pnetwork->network), target, feature)) {
+			target_find = 1;
+			break;
+		}
+
+		if (rtw_roam_flags(adapter)) {
+			/* TODO: don't  select netowrk in the same ess as oldest if it's new enough*/
+		}
+
+		if (oldest == NULL || 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 (!target_find) {
+		if (_rtw_queue_empty(&(pmlmepriv->free_bss_pool))) {
+			/* If there are no more slots, expire the oldest */
+			/* list_del_init(&oldest->list); */
+			pnetwork = oldest;
+			if (pnetwork == NULL) {
+				goto exit;
+			}
+#ifdef CONFIG_ANTENNA_DIVERSITY
+			rtw_hal_get_odm_var(adapter, HAL_ODM_ANTDIV_SELECT, &(target->PhyInfo.Optimum_antenna), NULL);
+#endif
+			memcpy(&(pnetwork->network), target,  get_WLAN_BSSID_EX_sz(target));
+			/* pnetwork->last_scanned = jiffies; */
+			/* variable initialize */
+			pnetwork->fixed = false;
+			pnetwork->last_scanned = jiffies;
+
+			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) {
+				goto exit;
+			}
+
+			bssid_ex_sz = get_WLAN_BSSID_EX_sz(target);
+			target->Length = bssid_ex_sz;
+#ifdef CONFIG_ANTENNA_DIVERSITY
+			rtw_hal_get_odm_var(adapter, HAL_ODM_ANTDIV_SELECT, &(target->PhyInfo.Optimum_antenna), NULL);
+#endif
+			memcpy(&(pnetwork->network), target, bssid_ex_sz);
+
+			pnetwork->last_scanned = jiffies;
+
+			/* bss info not receving from the right channel */
+			if (pnetwork->network.PhyInfo.SignalQuality == 101)
+				pnetwork->network.PhyInfo.SignalQuality = 0;
+
+			list_add_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 = jiffies;
+
+		/* 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;
+
+		/* probe resp(3) > beacon(1) > probe req(2) */
+		if ((target->Reserved[0] != 2) &&
+		    (target->Reserved[0] >= pnetwork->network.Reserved[0])
+		   )
+			update_ie = true;
+		else
+			update_ie = false;
+
+		update_network(&(pnetwork->network), target, adapter, update_ie);
+	}
+
+exit:
+	_exit_critical_bh(&queue->lock, &irqL);
+
+}
+
+void rtw_add_network(_adapter *adapter, WLAN_BSSID_EX *pnetwork);
+void rtw_add_network(_adapter *adapter, WLAN_BSSID_EX *pnetwork)
+{
+	unsigned long irqL;
+	struct	mlme_priv	*pmlmepriv = &(((_adapter *)adapter)->mlmepriv);
+	/* _queue	*queue	= &(pmlmepriv->scanned_queue); */
+
+
+	/* _enter_critical_bh(&queue->lock, &irqL); */
+
+#if defined(CONFIG_P2P) && defined(CONFIG_P2P_REMOVE_GROUP_INFO)
+	if (adapter->registrypriv.wifi_spec == 0)
+		rtw_bss_ex_del_p2p_attr(pnetwork, P2P_ATTR_GROUP_INFO);
+#endif
+
+	if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
+		rtw_bss_ex_del_wfd_ie(pnetwork);
+
+	update_current_network(adapter, pnetwork);
+
+	rtw_update_scanned_network(adapter, pnetwork);
+
+	/* _exit_critical_bh(&queue->lock, &irqL); */
+
+}
+
+/* select the desired network based on the capability of the (i)bss.
+ * check items: (1) security
+ *			   (2) network_type
+ *			   (3) WMM
+ *			   (4) HT
+ * (5) others */
+int rtw_is_desired_network(_adapter *adapter, struct wlan_network *pnetwork);
+int rtw_is_desired_network(_adapter *adapter, struct wlan_network *pnetwork)
+{
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u32 desired_encmode;
+	u32 privacy;
+
+	/* u8 wps_ie[512]; */
+	uint wps_ielen;
+
+	int bselected = true;
+
+	desired_encmode = psecuritypriv->ndisencryptstatus;
+	privacy = pnetwork->network.Privacy;
+
+	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+		if (rtw_get_wps_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, pnetwork->network.IELength - _FIXED_IE_LENGTH_, NULL, &wps_ielen) != NULL)
+			return true;
+		else
+			return false;
+	}
+	if (adapter->registrypriv.wifi_spec == 1) { /* for  correct flow of 8021X  to do.... */
+		u8 *p = NULL;
+		uint ie_len = 0;
+
+		if ((desired_encmode == Ndis802_11EncryptionDisabled) && (privacy != 0))
+			bselected = false;
+
+		if (psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPA2PSK) {
+			p = rtw_get_ie(pnetwork->network.IEs + _BEACON_IE_OFFSET_, _RSN_IE_2_, &ie_len, (pnetwork->network.IELength - _BEACON_IE_OFFSET_));
+			if (p && ie_len > 0)
+				bselected = true;
+			else
+				bselected = false;
+		}
+	}
+
+
+	if ((desired_encmode != Ndis802_11EncryptionDisabled) && (privacy == 0)) {
+		RTW_INFO("desired_encmode: %d, privacy: %d\n", desired_encmode, privacy);
+		bselected = false;
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+		if (pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode)
+			bselected = false;
+	}
+
+
+	return bselected;
+}
+
+/* TODO: Perry : For Power Management */
+void rtw_atimdone_event_callback(_adapter	*adapter , u8 *pbuf)
+{
+
+	return;
+}
+
+
+void rtw_survey_event_callback(_adapter	*adapter, u8 *pbuf)
+{
+	unsigned long  irqL;
+	u32 len;
+	WLAN_BSSID_EX *pnetwork;
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+
+
+	pnetwork = (WLAN_BSSID_EX *)pbuf;
+
+	len = get_WLAN_BSSID_EX_sz(pnetwork);
+	if (len > (sizeof(WLAN_BSSID_EX))) {
+		return;
+	}
+
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	/* update IBSS_network 's timestamp */
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))) {
+		if (!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);
+
+
+	return;
+}
+
+void rtw_surveydone_event_callback(_adapter	*adapter, u8 *pbuf)
+{
+	unsigned long  irqL;
+	u8 timer_cancelled;
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+#ifdef CONFIG_RTW_80211R
+	struct mlme_ext_priv	*pmlmeext = &adapter->mlmeextpriv;
+#endif
+
+#ifdef CONFIG_MLME_EXT
+	mlmeext_surveydone_event_callback(adapter);
+#endif
+
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+	if (pmlmepriv->wps_probe_req_ie) {
+		u32 free_len = pmlmepriv->wps_probe_req_ie_len;
+		pmlmepriv->wps_probe_req_ie_len = 0;
+		rtw_mfree(pmlmepriv->wps_probe_req_ie, free_len);
+		pmlmepriv->wps_probe_req_ie = NULL;
+	}
+
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == false) {
+		RTW_INFO(FUNC_ADPT_FMT" fw_state:0x%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv));
+		/* rtw_warn_on(1); */
+	}
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+	_cancel_timer(&pmlmepriv->scan_to_timer, &timer_cancelled);
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+	rtw_set_signal_stat_timer(&adapter->recvpriv);
+#endif
+
+	if (pmlmepriv->to_join) {
+		if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED) == false) {
+				set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+				if (rtw_select_and_join_from_scanned_queue(pmlmepriv) == _SUCCESS)
+					_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+				else {
+					WLAN_BSSID_EX    *pdev_network = &(adapter->registrypriv.dev_network);
+					u8 *pibss = adapter->registrypriv.dev_network.MacAddress;
+
+					/* pmlmepriv->fw_state ^= _FW_UNDER_SURVEY; */ /* because don't set assoc_timer */
+					_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+
+					memset(&pdev_network->Ssid, 0, sizeof(NDIS_802_11_SSID));
+					memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(NDIS_802_11_SSID));
+
+					rtw_update_registrypriv_dev_network(adapter);
+					rtw_generate_random_ibss(pibss);
+
+					/*pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;*/
+					init_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+
+					if (rtw_create_ibss_cmd(adapter, 0) != _SUCCESS)
+						RTW_ERR("rtw_create_ibss_cmd 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 {
+				RTW_INFO("try_to_join, but select scanning queue fail, to_roam:%d\n", rtw_to_roam(adapter));
+
+				if (rtw_to_roam(adapter) != 0) {
+					if (rtw_dec_to_roam(adapter) == 0
+					    || _SUCCESS != rtw_sitesurvey_cmd(adapter, &pmlmepriv->assoc_ssid, 1, NULL, 0)
+					   ) {
+						rtw_set_to_roam(adapter, 0);
+#ifdef CONFIG_INTEL_WIDI
+						if (adapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING) {
+							memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN);
+							intel_widi_wk_cmd(adapter, INTEL_WIDI_LISTEN_WK, NULL, 0);
+							RTW_INFO("change to widi listen\n");
+						}
+#endif /* CONFIG_INTEL_WIDI */
+						rtw_free_assoc_resources(adapter, 1);
+						rtw_indicate_disconnect(adapter, 0, false);
+					} else
+						pmlmepriv->to_join = true;
+				} else
+					rtw_indicate_disconnect(adapter, 0, false);
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+			}
+		}
+	} else {
+		if (rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE)) {
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)
+			    && check_fwstate(pmlmepriv, _FW_LINKED)) {
+				if (rtw_select_roaming_candidate(pmlmepriv) == _SUCCESS) {
+#ifdef CONFIG_RTW_80211R
+					if (rtw_chk_ft_flags(adapter, RTW_FT_OVER_DS_SUPPORTED)) {
+						start_clnt_ft_action(adapter, (u8 *)pmlmepriv->roam_network->network.MacAddress);
+					} else {
+						/*wait a little time to retrieve packets buffered in the current ap while scan*/
+						_set_timer(&pmlmeext->ft_roam_timer, 30);
+					}
+#else
+					receive_disconnect(adapter, pmlmepriv->cur_network.network.MacAddress
+						, WLAN_REASON_ACTIVE_ROAM, false);
+#endif
+				}
+			}
+		}
+	}
+
+	/* RTW_INFO("scan complete in %dms\n",rtw_get_passing_time_ms(pmlmepriv->scan_start_time)); */
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+#ifdef CONFIG_P2P_PS
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		p2p_ps_wk_cmd(adapter, P2P_PS_SCAN_DONE, 0);
+#endif /* CONFIG_P2P_PS */
+
+	rtw_mi_os_xmit_schedule(adapter);
+
+#ifdef CONFIG_DRVEXT_MODULE_WSC
+	drvext_surveydone_callback(&adapter->drvextpriv);
+#endif
+
+#ifdef DBG_CONFIG_ERROR_DETECT
+	{
+		struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+		if (pmlmeext->sitesurvey_res.bss_cnt == 0) {
+			/* rtw_hal_sreset_reset(adapter); */
+		}
+	}
+#endif
+
+#ifdef CONFIG_IOCTL_CFG80211
+	rtw_cfg80211_surveydone_event_callback(adapter);
+#endif /* CONFIG_IOCTL_CFG80211 */
+
+	rtw_indicate_scan_done(adapter, false);
+
+#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_IOCTL_CFG80211)
+	rtw_cfg80211_indicate_scan_done_for_buddy(adapter, false);
+#endif
+
+}
+
+void rtw_dummy_event_callback(_adapter *adapter , u8 *pbuf)
+{
+
+}
+
+void rtw_fwdbg_event_callback(_adapter *adapter , u8 *pbuf)
+{
+
+}
+
+static void free_scanqueue(struct	mlme_priv *pmlmepriv)
+{
+	unsigned long irqL, irqL0;
+	_queue *free_queue = &pmlmepriv->free_bss_pool;
+	_queue *scan_queue = &pmlmepriv->scanned_queue;
+	_list	*plist, *phead, *ptemp;
+
+
+	_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);
+		list_del_init(plist);
+		list_add_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);
+
+}
+
+static void rtw_reset_rx_info(struct debug_priv *pdbgpriv)
+{
+	pdbgpriv->dbg_rx_ampdu_drop_count = 0;
+	pdbgpriv->dbg_rx_ampdu_forced_indicate_count = 0;
+	pdbgpriv->dbg_rx_ampdu_loss_count = 0;
+	pdbgpriv->dbg_rx_dup_mgt_frame_drop_count = 0;
+	pdbgpriv->dbg_rx_ampdu_window_shift_cnt = 0;
+}
+
+/*
+*rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock
+*/
+void rtw_free_assoc_resources(_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;
+	struct dvobj_priv *psdpriv = adapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &adapter->tdlsinfo;
+#endif /* CONFIG_TDLS */
+
+
+	RTW_INFO("%s-"ADPT_FMT" tgt_network MacAddress=" MAC_FMT"ssid=%s\n",
+		__func__, ADPT_ARG(adapter), MAC_ARG(tgt_network->network.MacAddress), tgt_network->network.Ssid.Ssid);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		struct sta_info *psta;
+
+		psta = rtw_get_stainfo(&adapter->stapriv, tgt_network->network.MacAddress);
+
+#ifdef CONFIG_TDLS
+		if (ptdlsinfo->link_established) {
+			rtw_tdls_cmd(adapter, NULL, TDLS_RS_RCR);
+			rtw_reset_tdls_info(adapter);
+			rtw_free_all_stainfo(adapter);
+			/* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */
+		} else
+#endif /* CONFIG_TDLS */
+		{
+			/* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */
+			rtw_free_stainfo(adapter,  psta);
+		}
+
+		/* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */
+
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE)) {
+		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_same_network(&pmlmepriv->scanned_queue, tgt_network);
+	if ((pwlan)  && (!check_fwstate(pmlmepriv, WIFI_UNDER_WPS))) {
+		pwlan->fixed = false;
+
+		RTW_INFO("free disconnecting network of scanned_queue\n");
+		rtw_free_network_nolock(adapter, pwlan);
+#ifdef CONFIG_P2P
+		if (!rtw_p2p_chk_state(&adapter->wdinfo, P2P_STATE_NONE)) {
+			rtw_mi_set_scan_deny(adapter, 2000);
+			/* rtw_clear_scan_deny(adapter);			 */
+		}
+#endif /* CONFIG_P2P */
+	} else {
+		if (pwlan == NULL)
+			RTW_INFO("free disconnecting network of scanned_queue failed due to pwlan== NULL\n\n");
+		if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS))
+			RTW_INFO("donot free disconnecting network of scanned_queue when WIFI_UNDER_WPS\n\n");
+	}
+
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) && (adapter->stapriv.asoc_sta_count == 1))
+	    /*||check_fwstate(pmlmepriv, WIFI_STATION_STATE)*/) {
+		if (pwlan)
+			rtw_free_network_nolock(adapter, pwlan);
+	}
+
+	if (lock_scanned_queue)
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+	adapter->securitypriv.key_mask = 0;
+
+	rtw_reset_rx_info(pdbgpriv);
+}
+
+/*
+*rtw_indicate_connect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_connect(_adapter *padapter)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+
+
+
+	pmlmepriv->to_join = false;
+
+	if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+
+		set_fwstate(pmlmepriv, _FW_LINKED);
+
+		rtw_led_control(padapter, LED_CTL_LINK);
+
+
+#ifdef CONFIG_DRVEXT_MODULE
+		if (padapter->drvextpriv.enable_wpa)
+			indicate_l2_connect(padapter);
+		else
+#endif
+		{
+			rtw_os_indicate_connect(padapter);
+		}
+
+	}
+
+	rtw_set_to_roam(padapter, 0);
+#ifdef CONFIG_INTEL_WIDI
+	if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING) {
+		memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN);
+		intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_WK, NULL, 0);
+		RTW_INFO("change to widi listen\n");
+	}
+#endif /* CONFIG_INTEL_WIDI */
+	if (!check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE))
+		rtw_mi_set_scan_deny(padapter, 3000);
+
+
+}
+
+
+/*
+*rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_disconnect(_adapter *padapter, u16 reason, u8 locally_generated)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX	*cur_network = &(pmlmeinfo->network);
+	struct sta_info *psta;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *wps_ie = NULL;
+	uint wpsie_len = 0;
+
+
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING | WIFI_UNDER_WPS);
+
+	/* force to clear cur_network_scanned's SELECTED REGISTRAR */
+	if (pmlmepriv->cur_network_scanned) {
+		WLAN_BSSID_EX	*current_joined_bss = &(pmlmepriv->cur_network_scanned->network);
+		if (current_joined_bss) {
+			wps_ie = rtw_get_wps_ie(current_joined_bss->IEs + _FIXED_IE_LENGTH_,
+				current_joined_bss->IELength - _FIXED_IE_LENGTH_, NULL, &wpsie_len);
+			if (wps_ie && wpsie_len > 0) {
+				u8 *attr = NULL;
+				u32 attr_len;
+				attr = rtw_get_wps_attr(wps_ie, wpsie_len, WPS_ATTR_SELECTED_REGISTRAR,
+							NULL, &attr_len);
+				if (attr)
+					*(attr + 4) = 0;
+			}
+		}
+	}
+	/* RTW_INFO("clear wps when %s\n", __func__); */
+
+	if (rtw_to_roam(padapter) > 0)
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+
+#ifdef CONFIG_WAPI_SUPPORT
+	psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		rtw_wapi_return_one_sta_info(padapter, psta->hwaddr);
+	else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+		 check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
+		rtw_wapi_return_all_sta_info(padapter);
+#endif
+
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED)
+	    || (rtw_to_roam(padapter) <= 0)
+	   ) {
+
+		rtw_os_indicate_disconnect(padapter, reason, locally_generated);
+
+		/* set ips_deny_time to avoid enter IPS before LPS leave */
+		rtw_set_ips_deny(padapter, 3000);
+
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+
+		rtw_led_control(padapter, LED_CTL_NO_LINK);
+
+		rtw_clear_scan_deny(padapter);
+	}
+
+#ifdef CONFIG_P2P_PS
+	p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+#endif /* CONFIG_P2P_PS */
+
+#ifdef CONFIG_LPS
+	rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 1);
+#endif
+
+#ifdef CONFIG_BEAMFORMING
+	beamforming_wk_cmd(padapter, BEAMFORMING_CTRL_LEAVE, cur_network->MacAddress, ETH_ALEN, 1);
+#endif /*CONFIG_BEAMFORMING*/
+
+}
+
+inline void rtw_indicate_scan_done(_adapter *padapter, bool aborted)
+{
+	RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	rtw_os_indicate_scan_done(padapter, aborted);
+
+#ifdef CONFIG_IPS
+	if (is_primary_adapter(padapter)
+	    && (false == adapter_to_pwrctl(padapter)->bInSuspend)
+	    && (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE | WIFI_UNDER_LINKING) == false)) {
+		struct pwrctrl_priv *pwrpriv;
+
+		pwrpriv = adapter_to_pwrctl(padapter);
+		rtw_set_ips_deny(padapter, 0);
+#ifdef CONFIG_IPS_CHECK_IN_WD
+		_set_timer(&adapter_to_dvobj(padapter)->dynamic_chk_timer, 1);
+#else /* !CONFIG_IPS_CHECK_IN_WD */
+		_rtw_set_pwr_state_check_timer(pwrpriv, 1);
+#endif /* !CONFIG_IPS_CHECK_IN_WD */
+	}
+#endif /* CONFIG_IPS */
+}
+
+static u32 _rtw_wait_scan_done(_adapter *adapter, u8 abort, u32 timeout_ms)
+{
+	u32 start;
+	u32 pass_ms;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv);
+
+	start = jiffies;
+
+	pmlmeext->scan_abort = abort;
+
+	while (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)
+	       && rtw_get_passing_time_ms(start) <= timeout_ms) {
+
+		if (RTW_CANNOT_RUN(adapter))
+			break;
+
+		RTW_INFO(FUNC_NDEV_FMT"fw_state=_FW_UNDER_SURVEY!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+		rtw_msleep_os(20);
+	}
+
+	if (abort) {
+		if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+			if (!RTW_CANNOT_RUN(adapter))
+				RTW_INFO(FUNC_NDEV_FMT"waiting for scan_abort time out!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+#ifdef CONFIG_PLATFORM_MSTAR
+			/*_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);*/
+			set_survey_timer(pmlmeext, 0);
+			mlme_set_scan_to_timer(pmlmepriv, 50);
+#endif
+			rtw_indicate_scan_done(adapter, true);
+		}
+	}
+
+	pmlmeext->scan_abort = false;
+	pass_ms = rtw_get_passing_time_ms(start);
+
+	return pass_ms;
+
+}
+
+void rtw_scan_wait_completed(_adapter *adapter)
+{
+	u32 scan_to = SCANNING_TIMEOUT;
+
+#ifdef CONFIG_SCAN_BACKOP
+	if (is_supported_5g(adapter->registrypriv.wireless_mode)
+	    && IsSupported24G(adapter->registrypriv.wireless_mode)) /*dual band*/
+		scan_to = CONC_SCANNING_TIMEOUT_DUAL_BAND;
+	else /*single band*/
+		scan_to = CONC_SCANNING_TIMEOUT_SINGLE_BAND;
+#endif /* CONFIG_SCAN_BACKOP */
+
+	_rtw_wait_scan_done(adapter, false, scan_to);
+}
+
+u32 rtw_scan_abort_timeout(_adapter *adapter, u32 timeout_ms)
+{
+	return _rtw_wait_scan_done(adapter, true, timeout_ms);
+}
+
+void rtw_scan_abort_no_wait(_adapter *adapter)
+{
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+		pmlmeext->scan_abort = true;
+}
+
+void rtw_scan_abort(_adapter *adapter)
+{
+	rtw_scan_abort_timeout(adapter, 200);
+}
+
+static struct sta_info *rtw_joinbss_update_stainfo(_adapter *padapter, struct wlan_network *pnetwork)
+{
+	int i;
+	struct sta_info *bmc_sta, *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	psta = rtw_get_stainfo(pstapriv, pnetwork->network.MacAddress);
+	if (psta == NULL)
+		psta = rtw_alloc_stainfo(pstapriv, pnetwork->network.MacAddress);
+
+	if (psta) { /* update ptarget_sta */
+		RTW_INFO("%s\n", __func__);
+
+		psta->aid  = pnetwork->join_res;
+
+		update_sta_info(padapter, psta);
+
+		/* update station supportRate */
+		psta->bssratelen = rtw_get_rateset_len(pnetwork->network.SupportedRates);
+		memcpy(psta->bssrateset, pnetwork->network.SupportedRates, psta->bssratelen);
+		rtw_hal_update_sta_rate_mask(padapter, psta);
+
+		psta->wireless_mode = pmlmeext->cur_wireless_mode;
+		psta->raid = rtw_hal_networktype_to_raid(padapter, psta);
+
+
+		/* sta mode */
+		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
+
+		/* security related */
+#ifdef CONFIG_RTW_80211R
+		if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && (psta->ft_pairwise_key_installed == false)) {
+#else
+		if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) {
+#endif
+			padapter->securitypriv.binstallGrpkey = false;
+			padapter->securitypriv.busetkipkey = false;
+			padapter->securitypriv.bgrpkey_handshake = false;
+
+			psta->ieee8021x_blocked = true;
+			psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+
+			memset((u8 *)&psta->dot118021x_UncstKey, 0, sizeof(union Keytype));
+
+			memset((u8 *)&psta->dot11tkiprxmickey, 0, sizeof(union Keytype));
+			memset((u8 *)&psta->dot11tkiptxmickey, 0, sizeof(union Keytype));
+
+			memset((u8 *)&psta->dot11txpn, 0, sizeof(union pn48));
+			psta->dot11txpn.val = psta->dot11txpn.val + 1;
+#ifdef CONFIG_IEEE80211W
+			memset((u8 *)&psta->dot11wtxpn, 0, sizeof(union pn48));
+#endif /* CONFIG_IEEE80211W */
+			memset((u8 *)&psta->dot11rxpn, 0, sizeof(union pn48));
+		}
+
+		/*	Commented by Albert 2012/07/21 */
+		/*	When doing the WPS, the wps_ie_len won't equal to 0 */
+		/*	And the Wi-Fi driver shouldn't allow the data packet to be tramsmitted. */
+		if (padapter->securitypriv.wps_ie_len != 0) {
+			psta->ieee8021x_blocked = true;
+			padapter->securitypriv.wps_ie_len = 0;
+		}
+
+
+		/* for A-MPDU Rx reordering buffer control for bmc_sta & sta_info */
+		/* if A-MPDU Rx is enabled, reseting  rx_ordering_ctrl wstart_b(indicate_seq) to default value=0xffff */
+		/* todo: check if AP can send A-MPDU packets */
+		for (i = 0; i < 16 ; i++) {
+			/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+			preorder_ctrl->enable = false;
+			preorder_ctrl->indicate_seq = 0xffff;
+#ifdef DBG_RX_SEQ
+			RTW_INFO("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__,
+				 preorder_ctrl->indicate_seq);
+#endif
+			preorder_ctrl->wend_b = 0xffff;
+			preorder_ctrl->wsize_b = 64;/* max_ampdu_sz; */ /* ex. 32(kbytes) -> wsize_b=32 */
+			preorder_ctrl->ampdu_size = RX_AMPDU_SIZE_INVALID;
+		}
+
+
+		bmc_sta = rtw_get_bcmc_stainfo(padapter);
+		if (bmc_sta) {
+			for (i = 0; i < 16 ; i++) {
+				/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+				preorder_ctrl = &bmc_sta->recvreorder_ctrl[i];
+				preorder_ctrl->enable = false;
+				preorder_ctrl->indicate_seq = 0xffff;
+#ifdef DBG_RX_SEQ
+				RTW_INFO("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__,
+					 preorder_ctrl->indicate_seq);
+#endif
+				preorder_ctrl->wend_b = 0xffff;
+				preorder_ctrl->wsize_b = 64;/* max_ampdu_sz; */ /* ex. 32(kbytes) -> wsize_b=32 */
+				preorder_ctrl->ampdu_size = RX_AMPDU_SIZE_INVALID;
+			}
+		}
+	}
+
+	return psta;
+
+}
+
+/* pnetwork : returns from rtw_joinbss_event_callback
+ * ptarget_wlan: found from scanned_queue */
+static void rtw_joinbss_update_network(_adapter *padapter, struct wlan_network *ptarget_wlan, struct wlan_network  *pnetwork)
+{
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct wlan_network  *cur_network = &(pmlmepriv->cur_network);
+
+	RTW_INFO("%s\n", __func__);
+
+
+
+	/* 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;
+
+
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+#endif
+	padapter->recvpriv.signal_strength = ptarget_wlan->network.PhyInfo.SignalStrength;
+	padapter->recvpriv.signal_qual = ptarget_wlan->network.PhyInfo.SignalQuality;
+	/* the ptarget_wlan->network.Rssi is raw data, we use ptarget_wlan->network.PhyInfo.SignalStrength instead (has scaled) */
+	padapter->recvpriv.rssi = translate_percentage_to_dbm(ptarget_wlan->network.PhyInfo.SignalStrength);
+#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1
+	RTW_INFO(FUNC_ADPT_FMT" signal_strength:%3u, rssi:%3d, signal_qual:%3u"
+		 "\n"
+		 , FUNC_ADPT_ARG(padapter)
+		 , padapter->recvpriv.signal_strength
+		 , padapter->recvpriv.rssi
+		 , padapter->recvpriv.signal_qual
+		);
+#endif
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+#endif
+
+	/* update fw_state */ /* will clr _FW_UNDER_LINKING here indirectly */
+
+	switch (pnetwork->network.InfrastructureMode) {
+	case Ndis802_11Infrastructure:
+
+		if (pmlmepriv->fw_state & WIFI_UNDER_WPS)
+			/*pmlmepriv->fw_state = WIFI_STATION_STATE|WIFI_UNDER_WPS;*/
+			init_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_UNDER_WPS);
+		else
+			/*pmlmepriv->fw_state = WIFI_STATION_STATE;*/
+			init_fwstate(pmlmepriv, WIFI_STATION_STATE);
+		break;
+	case Ndis802_11IBSS:
+		/*pmlmepriv->fw_state = WIFI_ADHOC_STATE;*/
+		init_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+		break;
+	default:
+		/*pmlmepriv->fw_state = WIFI_NULL_STATE;*/
+		init_fwstate(pmlmepriv, WIFI_NULL_STATE);
+		break;
+	}
+
+	rtw_update_protection(padapter, (cur_network->network.IEs) + sizeof(NDIS_802_11_FIXED_IEs),
+			      (cur_network->network.IELength));
+
+	rtw_update_ht_cap(padapter, cur_network->network.IEs, cur_network->network.IELength, (u8) cur_network->network.Configuration.DSConfig);
+}
+
+/* Notes: the fucntion could be > passive_level (the same context as Rx tasklet)
+ * pnetwork : returns from rtw_joinbss_event_callback
+ * ptarget_wlan: found from scanned_queue
+ * if join_res > 0, for (fw_state==WIFI_STATION_STATE), we check if  "ptarget_sta" & "ptarget_wlan" exist.
+ * if join_res > 0, for (fw_state==WIFI_ADHOC_STATE), we only check if "ptarget_wlan" exist.
+ * if join_res > 0, update "cur_network->network" from "pnetwork->network" if (ptarget_wlan !=NULL).
+ */
+/* #define REJOIN */
+void rtw_joinbss_event_prehandle(_adapter *adapter, u8 *pbuf)
+{
+	unsigned long irqL, irqL2;
+	static u8 retry = 0;
+	u8 timer_cancelled;
+	struct sta_info *ptarget_sta = NULL, *pcur_sta = NULL;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+	struct wlan_network	*pnetwork	= (struct wlan_network *)pbuf;
+	struct wlan_network	*cur_network = &(pmlmepriv->cur_network);
+	struct wlan_network	*pcur_wlan = NULL, *ptarget_wlan = NULL;
+	unsigned int		the_same_macaddr = false;
+
+	rtw_get_encrypt_decrypt_from_registrypriv(adapter);
+
+	the_same_macaddr = !memcmp(pnetwork->network.MacAddress, cur_network->network.MacAddress, ETH_ALEN);
+
+	pnetwork->network.Length = get_WLAN_BSSID_EX_sz(&pnetwork->network);
+	if (pnetwork->network.Length > sizeof(WLAN_BSSID_EX))
+		return;
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+	pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
+	pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
+
+
+	if (pnetwork->join_res > 0) {
+		_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+		retry = 0;
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+			/* s1. find ptarget_wlan */
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				if (the_same_macaddr)
+					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)) {
+						if (ptarget_wlan)
+							ptarget_wlan->fixed = true;
+					}
+				}
+
+			} else {
+				ptarget_wlan = _rtw_find_same_network(&pmlmepriv->scanned_queue, pnetwork);
+				if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+					if (ptarget_wlan)
+						ptarget_wlan->fixed = true;
+				}
+			}
+
+			/* s2. update cur_network */
+			if (ptarget_wlan)
+				rtw_joinbss_update_network(adapter, ptarget_wlan, pnetwork);
+			else {
+				RTW_PRINT("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)) {
+				ptarget_sta = rtw_joinbss_update_stainfo(adapter, pnetwork);
+				if (ptarget_sta == NULL) {
+					RTW_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 (MLME_IS_STA(adapter) || MLME_IS_ADHOC(adapter)) {
+				pmlmepriv->cur_network_scanned = ptarget_wlan;
+				rtw_indicate_connect(adapter);
+			}
+
+			/* s5. Cancle assoc_timer					 */
+			_cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
+
+
+		} else {
+			_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+			goto ignore_joinbss_callback;
+		}
+
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+	} else if (pnetwork->join_res == -4) {
+		rtw_reset_securitypriv(adapter);
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+
+		/* rtw_free_assoc_resources(adapter, 1); */
+
+		if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING))) {
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+		}
+
+	} else { /* if join_res < 0 (join fails), then try again */
+
+#ifdef REJOIN
+		res = _FAIL;
+		if (retry < 2) {
+			res = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+		}
+
+		if (res == _SUCCESS) {
+			/* extend time of assoc_timer */
+			_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+			retry++;
+		} else if (res == 2) { /* there is no need to wait for join */
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+			rtw_indicate_connect(adapter);
+		} else {
+#endif
+
+			_set_timer(&pmlmepriv->assoc_timer, 1);
+			/* rtw_free_assoc_resources(adapter, 1); */
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+#ifdef REJOIN
+			retry = 0;
+		}
+#endif
+	}
+
+ignore_joinbss_callback:
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+}
+
+void rtw_joinbss_event_callback(_adapter *adapter, u8 *pbuf)
+{
+	struct wlan_network	*pnetwork	= (struct wlan_network *)pbuf;
+
+
+	mlmeext_joinbss_event_callback(adapter, pnetwork->join_res);
+
+	rtw_mi_os_xmit_schedule(adapter);
+
+}
+
+void rtw_sta_media_status_rpt(_adapter *adapter, struct sta_info *sta, bool connected)
+{
+	struct macid_ctl_t *macid_ctl = &adapter->dvobj->macid_ctl;
+	bool miracast_enabled = 0;
+	bool miracast_sink = 0;
+	u8 role = H2C_MSR_ROLE_RSVD;
+
+	if (sta == NULL) {
+		RTW_PRINT(FUNC_ADPT_FMT" sta is NULL\n"
+			  , FUNC_ADPT_ARG(adapter));
+		rtw_warn_on(1);
+		return;
+	}
+
+	if (sta->mac_id >= macid_ctl->num) {
+		RTW_PRINT(FUNC_ADPT_FMT" invalid macid:%u\n"
+			  , FUNC_ADPT_ARG(adapter), sta->mac_id);
+		rtw_warn_on(1);
+		return;
+	}
+
+	if (!rtw_macid_is_used(macid_ctl, sta->mac_id)) {
+		RTW_PRINT(FUNC_ADPT_FMT" macid:%u not is used, set connected to 0\n"
+			  , FUNC_ADPT_ARG(adapter), sta->mac_id);
+		connected = 0;
+		rtw_warn_on(1);
+	}
+
+	if (connected && !rtw_macid_is_bmc(macid_ctl, sta->mac_id)) {
+		miracast_enabled = STA_OP_WFD_MODE(sta) != 0 && is_miracast_enabled(adapter);
+		miracast_sink = miracast_enabled && (STA_OP_WFD_MODE(sta) & MIRACAST_SINK);
+
+#ifdef CONFIG_TDLS
+		if (sta->tdls_sta_state & TDLS_LINKED_STATE)
+			role = H2C_MSR_ROLE_TDLS;
+		else
+#endif
+			if (MLME_IS_STA(adapter)) {
+				if (MLME_IS_GC(adapter))
+					role = H2C_MSR_ROLE_GO;
+				else
+					role = H2C_MSR_ROLE_AP;
+			} else if (MLME_IS_AP(adapter)) {
+				if (MLME_IS_GO(adapter))
+					role = H2C_MSR_ROLE_GC;
+				else
+					role = H2C_MSR_ROLE_STA;
+			} else if (MLME_IS_ADHOC(adapter) || MLME_IS_ADHOC_MASTER(adapter))
+				role = H2C_MSR_ROLE_ADHOC;
+
+#ifdef CONFIG_WFD
+		if (role == H2C_MSR_ROLE_GC
+		    || role == H2C_MSR_ROLE_GO
+		    || role == H2C_MSR_ROLE_TDLS
+		   ) {
+			if (adapter->wfd_info.rtsp_ctrlport
+			    || adapter->wfd_info.tdls_rtsp_ctrlport
+			    || adapter->wfd_info.peer_rtsp_ctrlport)
+				rtw_wfd_st_switch(sta, 1);
+		}
+#endif
+	}
+
+	rtw_hal_set_FwMediaStatusRpt_single_cmd(adapter
+						, connected
+						, miracast_enabled
+						, miracast_sink
+						, role
+						, sta->mac_id
+					       );
+}
+
+u8 rtw_sta_media_status_rpt_cmd(_adapter *adapter, struct sta_info *sta, bool connected)
+{
+	struct cmd_priv	*cmdpriv = &adapter->cmdpriv;
+	struct cmd_obj *cmdobj;
+	struct drvextra_cmd_parm *cmd_parm;
+	struct sta_media_status_rpt_cmd_parm *rpt_parm;
+	u8	res = _SUCCESS;
+
+	cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (cmdobj == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (cmd_parm == NULL) {
+		rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	rpt_parm = (struct sta_media_status_rpt_cmd_parm *)rtw_zmalloc(sizeof(struct sta_media_status_rpt_cmd_parm));
+	if (rpt_parm == NULL) {
+		rtw_mfree((u8 *)cmdobj, sizeof(struct cmd_obj));
+		rtw_mfree((u8 *)cmd_parm, sizeof(struct drvextra_cmd_parm));
+		res = _FAIL;
+		goto exit;
+	}
+
+	rpt_parm->sta = sta;
+	rpt_parm->connected = connected;
+
+	cmd_parm->ec_id = STA_MSTATUS_RPT_WK_CID;
+	cmd_parm->type = 0;
+	cmd_parm->size = sizeof(struct sta_media_status_rpt_cmd_parm);
+	cmd_parm->pbuf = (u8 *)rpt_parm;
+	init_h2fwcmd_w_parm_no_rsp(cmdobj, cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(cmdpriv, cmdobj);
+
+exit:
+	return res;
+}
+
+inline void rtw_sta_media_status_rpt_cmd_hdl(_adapter *adapter, struct sta_media_status_rpt_cmd_parm *parm)
+{
+	rtw_sta_media_status_rpt(adapter, parm->sta, parm->connected);
+}
+
+void rtw_stassoc_event_callback(_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;
+
+
+#if CONFIG_RTW_MACADDR_ACL
+	if (rtw_access_ctrl(adapter, pstassoc->macaddr) == false)
+		return;
+#endif
+
+#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+		if (psta) {
+			u8 *passoc_req = NULL;
+			u32 assoc_req_len = 0;
+
+			rtw_sta_media_status_rpt(adapter, psta, 1);
+
+#ifndef CONFIG_AUTO_AP_MODE
+
+			ap_sta_info_defer_update(adapter, psta);
+
+			/* report to upper layer */
+			RTW_INFO("indicate_sta_assoc_event to upper layer - hostapd\n");
+#ifdef CONFIG_IOCTL_CFG80211
+			_enter_critical_bh(&psta->lock, &irqL);
+			if (psta->passoc_req && psta->assoc_req_len > 0) {
+				passoc_req = rtw_zmalloc(psta->assoc_req_len);
+				if (passoc_req) {
+					assoc_req_len = psta->assoc_req_len;
+					memcpy(passoc_req, psta->passoc_req, assoc_req_len);
+
+					rtw_mfree(psta->passoc_req , psta->assoc_req_len);
+					psta->passoc_req = NULL;
+					psta->assoc_req_len = 0;
+				}
+			}
+			_exit_critical_bh(&psta->lock, &irqL);
+
+			if (passoc_req && assoc_req_len > 0) {
+				rtw_cfg80211_indicate_sta_assoc(adapter, passoc_req, assoc_req_len);
+
+				rtw_mfree(passoc_req, assoc_req_len);
+			}
+#else /* !CONFIG_IOCTL_CFG80211	 */
+			rtw_indicate_sta_assoc_event(adapter, psta);
+#endif /* !CONFIG_IOCTL_CFG80211 */
+#endif /* !CONFIG_AUTO_AP_MODE */
+
+#ifdef CONFIG_BEAMFORMING
+			beamforming_wk_cmd(adapter, BEAMFORMING_CTRL_ENTER, (u8 *)psta, sizeof(struct sta_info), 0);
+#endif/*CONFIG_BEAMFORMING*/
+			if (is_wep_enc(adapter->securitypriv.dot11PrivacyAlgrthm))
+				rtw_ap_wep_pk_setting(adapter, psta);
+		}
+		goto exit;
+	}
+#endif /* defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */
+
+	/* for AD-HOC mode */
+	psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+	if (psta == NULL) {
+		RTW_ERR(FUNC_ADPT_FMT" get no sta_info with "MAC_FMT"\n"
+			, FUNC_ADPT_ARG(adapter), MAC_ARG(pstassoc->macaddr));
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	rtw_hal_set_odm_var(adapter, HAL_ODM_STA_INFO, psta, true);
+
+	rtw_sta_media_status_rpt(adapter, psta, 1);
+
+	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);
+			pmlmepriv->cur_network_scanned = ptarget_wlan;
+			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:
+	return;
+}
+
+#ifdef CONFIG_IEEE80211W
+void rtw_sta_timeout_event_callback(_adapter *adapter, u8 *pbuf)
+{
+	unsigned long irqL;
+	struct sta_info *psta;
+	struct stadel_event *pstadel = (struct stadel_event *)pbuf;
+	struct sta_priv *pstapriv = &adapter->stapriv;
+
+
+	psta = rtw_get_stainfo(&adapter->stapriv, pstadel->macaddr);
+
+	if (psta) {
+		u8 updated = false;
+
+		_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+		if (list_empty(&psta->asoc_list) == false) {
+			list_del_init(&psta->asoc_list);
+			pstapriv->asoc_list_cnt--;
+			updated = ap_free_sta(adapter, psta, true, WLAN_REASON_PREV_AUTH_NOT_VALID, true);
+		}
+		_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+		associated_clients_update(adapter, updated, STA_INFO_UPDATE_ALL);
+	}
+
+
+
+}
+#endif /* CONFIG_IEEE80211W */
+
+#ifdef CONFIG_RTW_80211R
+void rtw_update_ft_stainfo(_adapter *padapter, WLAN_BSSID_EX *pnetwork)
+{
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct sta_info		*psta = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, pnetwork->MacAddress);
+	if (psta == NULL)
+		psta = rtw_alloc_stainfo(pstapriv, pnetwork->MacAddress);
+
+	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;
+		psta->dot11txpn.val = psta->dot11txpn.val + 1;
+
+		memset((u8 *)&psta->dot118021x_UncstKey, 0, sizeof(union Keytype));
+		memset((u8 *)&psta->dot11tkiprxmickey, 0, sizeof(union Keytype));
+		memset((u8 *)&psta->dot11tkiptxmickey, 0, sizeof(union Keytype));
+		memset((u8 *)&psta->dot11txpn, 0, sizeof(union pn48));
+#ifdef CONFIG_IEEE80211W
+		memset((u8 *)&psta->dot11wtxpn, 0, sizeof(union pn48));
+#endif
+		memset((u8 *)&psta->dot11rxpn, 0, sizeof(union pn48));
+	}
+
+}
+
+void rtw_ft_reassoc_event_callback(_adapter *padapter, u8 *pbuf)
+{
+	struct mlme_priv		*pmlmepriv = &(padapter->mlmepriv);
+	struct stassoc_event	*pstassoc = (struct stassoc_event *)pbuf;
+	ft_priv	*pftpriv = &pmlmepriv->ftpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network));
+	struct cfg80211_ft_event_params ft_evt_parms;
+	unsigned long irqL;
+
+	memset(&ft_evt_parms, 0, sizeof(ft_evt_parms));
+	rtw_update_ft_stainfo(padapter, pnetwork);
+	ft_evt_parms.ies_len = pftpriv->ft_event.ies_len;
+	ft_evt_parms.ies =  rtw_zmalloc(ft_evt_parms.ies_len);
+	if (ft_evt_parms.ies)
+		memcpy((void *)ft_evt_parms.ies, pftpriv->ft_event.ies, ft_evt_parms.ies_len);
+	 else
+		goto err_2;
+
+	ft_evt_parms.target_ap = rtw_zmalloc(ETH_ALEN);
+	if (ft_evt_parms.target_ap)
+		memcpy((void *)ft_evt_parms.target_ap, pstassoc->macaddr, ETH_ALEN);
+	else
+		goto err_1;
+
+	ft_evt_parms.ric_ies = pftpriv->ft_event.ric_ies;
+	ft_evt_parms.ric_ies_len = pftpriv->ft_event.ric_ies_len;
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+	rtw_set_ft_status(padapter, RTW_FT_AUTHENTICATED_STA);
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+	rtw_cfg80211_ft_event(padapter, &ft_evt_parms);
+	RTW_INFO("%s: to "MAC_FMT"\n", __func__, MAC_ARG(ft_evt_parms.target_ap));
+
+	rtw_mfree((u8 *)pftpriv->ft_event.target_ap, ETH_ALEN);
+err_1:
+	rtw_mfree((u8 *)ft_evt_parms.ies, ft_evt_parms.ies_len);
+err_2:
+	return;
+}
+#endif
+
+static void rtw_sta_mstatus_disc_rpt(_adapter *adapter, u8 mac_id)
+{
+	struct macid_ctl_t *macid_ctl = &adapter->dvobj->macid_ctl;
+
+	RTW_INFO("%s "ADPT_FMT" - mac_id=%d\n", __func__, ADPT_ARG(adapter), mac_id);
+
+	if (mac_id >= 0 && mac_id < macid_ctl->num) {
+		rtw_hal_set_FwMediaStatusRpt_single_cmd(adapter, 0, 0, 0, 0, mac_id);
+		/*
+		 * For safety, prevent from keeping macid sleep.
+		 * If we can sure all power mode enter/leave are paired,
+		 * this check can be removed.
+		 * Lucas@20131113
+		 */
+		/* wakeup macid after disconnect. */
+		/*if (MLME_IS_STA(adapter))*/
+		rtw_hal_macid_wakeup(adapter, mac_id);
+	} else {
+		RTW_PRINT(FUNC_ADPT_FMT" invalid macid:%u\n"
+			  , FUNC_ADPT_ARG(adapter), mac_id);
+		rtw_warn_on(1);
+	}
+}
+void rtw_sta_mstatus_report(_adapter *adapter)
+{
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+	struct sta_info *psta = NULL;
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		psta = rtw_get_stainfo(&adapter->stapriv, tgt_network->network.MacAddress);
+		if (psta)
+			rtw_sta_mstatus_disc_rpt(adapter, psta->mac_id);
+		else {
+			RTW_INFO("%s "ADPT_FMT" - mac_addr: "MAC_FMT" psta == NULL\n", __func__, ADPT_ARG(adapter), MAC_ARG(tgt_network->network.MacAddress));
+			rtw_warn_on(1);
+		}
+	}
+}
+
+void rtw_stadel_event_callback(_adapter *adapter, u8 *pbuf)
+{
+	unsigned long irqL, irqL2;
+
+	struct sta_info *psta;
+	struct wlan_network *pwlan = NULL;
+	WLAN_BSSID_EX    *pdev_network = NULL;
+	u8 *pibss = NULL;
+	struct	mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
+	struct	stadel_event *pstadel	= (struct stadel_event *)pbuf;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
+	struct mlme_ext_priv	*pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+
+	RTW_INFO("%s(mac_id=%d)=" MAC_FMT "\n", __func__, pstadel->mac_id, MAC_ARG(pstadel->macaddr));
+	rtw_sta_mstatus_disc_rpt(adapter, pstadel->mac_id);
+
+	psta = rtw_get_stainfo(&adapter->stapriv, pstadel->macaddr);
+
+	if (psta == NULL) {
+		RTW_INFO("%s(mac_id=%d)=" MAC_FMT " psta == NULL\n", __func__, pstadel->mac_id, MAC_ARG(pstadel->macaddr));
+		/*rtw_warn_on(1);*/
+	}
+
+	if (psta)
+		rtw_wfd_st_switch(psta, 0);
+
+	/* if(check_fwstate(pmlmepriv, WIFI_AP_STATE)) */
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
+#ifdef CONFIG_IOCTL_CFG80211
+#ifdef COMPAT_KERNEL_RELEASE
+
+#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 37)) || defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)
+		rtw_cfg80211_indicate_sta_disassoc(adapter, pstadel->macaddr, *(u16 *)pstadel->rsvd);
+#endif /* (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 37)) || defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER) */
+#endif /* CONFIG_IOCTL_CFG80211 */
+
+		return;
+	}
+
+	mlmeext_sta_del_event_callback(adapter);
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL2);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		u16 reason = *((unsigned short *)(pstadel->rsvd));
+		bool roam = false;
+		struct wlan_network *roam_target = NULL;
+
+#ifdef CONFIG_LAYER2_ROAMING
+#ifdef CONFIG_RTW_80211R
+		if (reason == WLAN_REASON_EXPIRATION_CHK && rtw_chk_roam_flags(adapter, RTW_ROAM_ON_EXPIRED))
+			pmlmepriv->ftpriv.ft_roam_on_expired = true;
+		else
+			pmlmepriv->ftpriv.ft_roam_on_expired = false;
+#endif
+		if (adapter->registrypriv.wifi_spec == 1)
+			roam = false;
+		else if (reason == WLAN_REASON_EXPIRATION_CHK && rtw_chk_roam_flags(adapter, RTW_ROAM_ON_EXPIRED))
+			roam = true;
+		else if (reason == WLAN_REASON_ACTIVE_ROAM && rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE)) {
+			roam = true;
+			roam_target = pmlmepriv->roam_network;
+		}
+#ifdef CONFIG_INTEL_WIDI
+		else if (adapter->mlmepriv.widi_state == INTEL_WIDI_STATE_CONNECTED)
+			roam = true;
+#endif /* CONFIG_INTEL_WIDI */
+
+		if (roam) {
+			if (rtw_to_roam(adapter) > 0)
+				rtw_dec_to_roam(adapter); /* this stadel_event is caused by roaming, decrease to_roam */
+			else if (rtw_to_roam(adapter) == 0)
+				rtw_set_to_roam(adapter, adapter->registrypriv.max_roaming_times);
+		} else
+			rtw_set_to_roam(adapter, 0);
+#endif /* CONFIG_LAYER2_ROAMING */
+
+		rtw_free_uc_swdec_pending_queue(adapter);
+
+		rtw_free_assoc_resources(adapter, 1);
+		rtw_free_mlme_priv_ie_data(pmlmepriv);
+
+		_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)  && (!check_fwstate(pmlmepriv, WIFI_UNDER_WPS))) {
+			pwlan->fixed = false;
+			rtw_free_network_nolock(adapter, pwlan);
+		}
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+		rtw_indicate_disconnect(adapter, *(u16 *)pstadel->rsvd, pstadel->locally_generated);
+#ifdef CONFIG_INTEL_WIDI
+		if (!rtw_to_roam(adapter))
+			process_intel_widi_disconnect(adapter, 1);
+#endif /* CONFIG_INTEL_WIDI */
+
+		_rtw_roaming(adapter, roam_target);
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+
+		/* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */
+		rtw_free_stainfo(adapter,  psta);
+		/* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */
+
+		if (adapter->stapriv.asoc_sta_count == 1) { /* a sta + bc/mc_stainfo (not Ibss_stainfo) */
+			/* rtw_indicate_disconnect(adapter); */ /* removed@20091105 */
+			_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+			/* free old ibss network */
+			/* pwlan = rtw_find_network(&pmlmepriv->scanned_queue, pstadel->macaddr); */
+			pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+			if (pwlan) {
+				pwlan->fixed = false;
+				rtw_free_network_nolock(adapter, 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));
+
+			memset(&pdev_network->Ssid, 0, sizeof(NDIS_802_11_SSID));
+			memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(NDIS_802_11_SSID));
+
+			rtw_update_registrypriv_dev_network(adapter);
+
+			rtw_generate_random_ibss(pibss);
+
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+				set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+
+			if (rtw_create_ibss_cmd(adapter, 0) != _SUCCESS)
+				RTW_ERR("rtw_create_ibss_cmd FAIL\n");
+
+		}
+
+	}
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL2);
+
+
+}
+
+
+void rtw_cpwm_event_callback(PADAPTER padapter, u8 *pbuf)
+{
+#ifdef CONFIG_LPS_LCLK
+	struct reportpwrstate_parm *preportpwrstate;
+#endif
+
+
+#ifdef CONFIG_LPS_LCLK
+	preportpwrstate = (struct reportpwrstate_parm *)pbuf;
+	preportpwrstate->state |= (u8)(adapter_to_pwrctl(padapter)->cpwm_tog + 0x80);
+	cpwm_int_hdl(padapter, preportpwrstate);
+#endif
+
+
+}
+
+
+void rtw_wmm_event_callback(PADAPTER padapter, u8 *pbuf)
+{
+
+	WMMOnAssocRsp(padapter);
+
+
+}
+
+/*
+* _rtw_join_timeout_handler - Timeout/faliure handler for CMD JoinBss
+* @adapter: pointer to _adapter structure
+*/
+void _rtw_join_timeout_handler(_adapter *adapter)
+{
+	unsigned long irqL;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	RTW_INFO("%s, fw_state=%x\n", __func__, get_fwstate(pmlmepriv));
+
+	if (RTW_CANNOT_RUN(adapter))
+		return;
+
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+#ifdef CONFIG_LAYER2_ROAMING
+	if (rtw_to_roam(adapter) > 0) { /* join timeout caused by roaming */
+		while (1) {
+			rtw_dec_to_roam(adapter);
+			if (rtw_to_roam(adapter) != 0) { /* try another */
+				int do_join_r;
+				RTW_INFO("%s try another roaming\n", __func__);
+				do_join_r = rtw_do_join(adapter);
+				if (_SUCCESS != do_join_r) {
+					RTW_INFO("%s roaming do_join return %d\n", __func__ , do_join_r);
+					continue;
+				}
+				break;
+			} else {
+#ifdef CONFIG_INTEL_WIDI
+				if (adapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING) {
+					memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN);
+					intel_widi_wk_cmd(adapter, INTEL_WIDI_LISTEN_WK, NULL, 0);
+					RTW_INFO("change to widi listen\n");
+				}
+#endif /* CONFIG_INTEL_WIDI */
+				RTW_INFO("%s We've try roaming but fail\n", __func__);
+#ifdef CONFIG_RTW_80211R
+				rtw_clr_ft_flags(adapter, RTW_FT_SUPPORTED|RTW_FT_OVER_DS_SUPPORTED);
+				rtw_reset_ft_status(adapter);
+#endif
+				rtw_indicate_disconnect(adapter, 0, false);
+				break;
+			}
+		}
+
+	} else
+#endif
+	{
+		rtw_indicate_disconnect(adapter, 0, false);
+		free_scanqueue(pmlmepriv);/* ??? */
+
+#ifdef CONFIG_IOCTL_CFG80211
+		/* indicate disconnect for the case that join_timeout and check_fwstate != FW_LINKED */
+		rtw_cfg80211_indicate_disconnect(adapter, 0, false);
+#endif /* CONFIG_IOCTL_CFG80211 */
+
+	}
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+
+#ifdef CONFIG_DRVEXT_MODULE_WSC
+	drvext_assoc_fail_indicate(&adapter->drvextpriv);
+#endif
+
+
+
+}
+
+/*
+* rtw_scan_timeout_handler - Timeout/Faliure handler for CMD SiteSurvey
+* @adapter: pointer to _adapter structure
+*/
+void rtw_scan_timeout_handler(_adapter *adapter)
+{
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	RTW_INFO(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);
+
+#ifdef CONFIG_IOCTL_CFG80211
+	rtw_cfg80211_surveydone_event_callback(adapter);
+#endif /* CONFIG_IOCTL_CFG80211 */
+
+	rtw_indicate_scan_done(adapter, true);
+
+#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_IOCTL_CFG80211)
+	rtw_cfg80211_indicate_scan_done_for_buddy(adapter, true);
+#endif
+}
+
+void rtw_mlme_reset_auto_scan_int(_adapter *adapter, u8 *reason)
+{
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 u_ch;
+	u32 interval_ms = 0xffffffff; /* 0xffffffff: special value to make min() works well, also means no auto scan */
+
+	*reason = RTW_AUTO_SCAN_REASON_UNSPECIFIED;
+	rtw_mi_get_ch_setting_union(adapter, &u_ch, NULL, NULL);
+
+	if (hal_chk_bw_cap(adapter, BW_CAP_40M)
+	    && is_client_associated_to_ap(adapter)
+	    && u_ch >= 1 && u_ch <= 14
+	    && adapter->registrypriv.wifi_spec
+	    /* TODO: AP Connected is 40MHz capability? */
+	   ) {
+		interval_ms = rtw_min(interval_ms, 60 * 1000);
+		*reason |= RTW_AUTO_SCAN_REASON_2040_BSS;
+	}
+
+exit:
+	if (interval_ms == 0xffffffff)
+		interval_ms = 0;
+
+	rtw_mlme_set_auto_scan_int(adapter, interval_ms);
+	return;
+}
+
+void rtw_drv_scan_by_self(_adapter *padapter, u8 reason)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct rtw_ieee80211_channel ch_for_2040_bss[14] = {
+		{1, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{2, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{3, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{4, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{5, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{6, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{7, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{8, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{9, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{10, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{11, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{12, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{13, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+		{14, RTW_IEEE80211_CHAN_PASSIVE_SCAN},
+	};
+	struct rtw_ieee80211_channel *ch_sel = NULL;
+	int ch_num = 0;
+
+	if (rtw_is_scan_deny(padapter))
+		goto exit;
+
+	if (!rtw_is_adapter_up(padapter))
+		goto exit;
+
+	if (rtw_mi_busy_traffic_check(padapter, false)) {
+#ifdef CONFIG_LAYER2_ROAMING
+		if (rtw_chk_roam_flags(padapter, RTW_ROAM_ACTIVE) && pmlmepriv->need_to_roam) {
+			RTW_INFO("need to roam, don't care BusyTraffic\n");
+		} else
+#endif
+		{
+			RTW_INFO(FUNC_ADPT_FMT" exit BusyTraffic\n", FUNC_ADPT_ARG(padapter));
+			goto exit;
+		}
+	}
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) && check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+		RTW_INFO(FUNC_ADPT_FMT" WIFI_AP_STATE && WIFI_UNDER_WPS\n", FUNC_ADPT_ARG(padapter));
+		goto exit;
+	}
+	if (check_fwstate(pmlmepriv, (_FW_UNDER_SURVEY | _FW_UNDER_LINKING))) {
+		RTW_INFO(FUNC_ADPT_FMT" _FW_UNDER_SURVEY|_FW_UNDER_LINKING\n", FUNC_ADPT_ARG(padapter));
+		goto exit;
+	}
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (rtw_mi_buddy_check_fwstate(padapter, (_FW_UNDER_SURVEY | _FW_UNDER_LINKING | WIFI_UNDER_WPS))) {
+		RTW_INFO(FUNC_ADPT_FMT", but buddy_intf is under scanning or linking or wps_phase\n", FUNC_ADPT_ARG(padapter));
+		goto exit;
+	}
+#endif
+
+	RTW_INFO(FUNC_ADPT_FMT" reason:0x%02x\n", FUNC_ADPT_ARG(padapter), reason);
+
+	/* only for 20/40 BSS */
+	if (reason == RTW_AUTO_SCAN_REASON_2040_BSS) {
+		ch_sel = ch_for_2040_bss;
+		ch_num = 14;
+	}
+
+	rtw_set_802_11_bssid_list_scan(padapter, NULL, 0, ch_sel, ch_num);
+exit:
+	return;
+}
+
+static void rtw_auto_scan_handler(_adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8 reason = RTW_AUTO_SCAN_REASON_UNSPECIFIED;
+
+	rtw_mlme_reset_auto_scan_int(padapter, &reason);
+
+#ifdef CONFIG_P2P
+	if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE))
+		goto exit;
+#endif
+
+#ifdef CONFIG_TDLS
+	if (padapter->tdlsinfo.link_established)
+		goto exit;
+#endif
+
+	if (pmlmepriv->auto_scan_int_ms == 0
+	    || rtw_get_passing_time_ms(pmlmepriv->scan_start_time) < pmlmepriv->auto_scan_int_ms)
+		goto exit;
+
+	rtw_drv_scan_by_self(padapter, reason);
+
+exit:
+	return;
+}
+static u8 is_drv_in_lps(_adapter *adapter)
+{
+	u8 is_in_lps = false;
+
+	#ifdef CONFIG_LPS_LCLK_WD_TIMER /* to avoid leaving lps 32k frequently*/
+	if ((adapter_to_pwrctl(adapter)->bFwCurrentInPSMode)
+	#ifdef CONFIG_BT_COEXIST
+		&& (rtw_btcoex_IsBtControlLps(adapter) == false)
+	#endif
+		)
+		is_in_lps = true;
+	#endif /* CONFIG_LPS_LCLK_WD_TIMER*/
+	return is_in_lps;
+}
+void rtw_iface_dynamic_check_timer_handler(_adapter *adapter)
+{
+#ifdef CONFIG_AP_MODE
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+#endif /* CONFIG_AP_MODE */
+
+	if (adapter->net_closed)
+		return;
+	#ifdef CONFIG_LPS_LCLK_WD_TIMER /* to avoid leaving lps 32k frequently*/
+	if (is_drv_in_lps(adapter)) {
+		u8 bEnterPS;
+
+		linked_status_chk(adapter, 1);
+
+		bEnterPS = traffic_status_watchdog(adapter, 1);
+		if (bEnterPS) {
+			/* rtw_lps_ctrl_wk_cmd(adapter, LPS_CTRL_ENTER, 1); */
+			rtw_hal_dm_watchdog_in_lps(adapter);
+		} else {
+			/* call rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1) in traffic_status_watchdog() */
+		}
+	}
+	#endif /* CONFIG_LPS_LCLK_WD_TIMER	*/
+
+	/* auto site survey */
+	rtw_auto_scan_handler(adapter);
+
+#ifndef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+#ifdef CONFIG_AP_MODE
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		expire_timeout_chk(adapter);
+#endif
+#endif /* !CONFIG_ACTIVE_KEEP_ALIVE_CHECK */
+
+#ifdef CONFIG_BR_EXT
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35))
+	rcu_read_lock();
+#endif /* (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35)) */
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+	if (adapter->pnetdev->br_port
+#else	/* (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) */
+	if (rcu_dereference(adapter->pnetdev->rx_handler_data)
+#endif /* (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) */
+		&& (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE))) {
+		/* 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_handler() is called every 2 seconds */
+		if (adapter->pppoe_connection_in_progress > 0)
+			adapter->pppoe_connection_in_progress--;
+	}
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35))
+	rcu_read_unlock();
+#endif /* (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 35)) */
+
+#endif /* CONFIG_BR_EXT */
+
+}
+
+/*#define DBG_TRAFFIC_STATISTIC*/
+static void collect_traffic_statistics(_adapter *padapter)
+{
+	struct dvobj_priv	*pdvobjpriv = adapter_to_dvobj(padapter);
+
+	/*memset(&pdvobjpriv->traffic_stat, 0, sizeof(struct rtw_traffic_statistics));*/
+
+	/* Tx bytes reset*/
+	pdvobjpriv->traffic_stat.tx_bytes = 0;
+	pdvobjpriv->traffic_stat.tx_pkts = 0;
+	pdvobjpriv->traffic_stat.tx_drop = 0;
+
+	/* Rx bytes reset*/
+	pdvobjpriv->traffic_stat.rx_bytes = 0;
+	pdvobjpriv->traffic_stat.rx_pkts = 0;
+	pdvobjpriv->traffic_stat.rx_drop = 0;
+
+	rtw_mi_traffic_statistics(padapter);
+
+	/* Calculate throughput in last interval */
+	pdvobjpriv->traffic_stat.cur_tx_bytes = pdvobjpriv->traffic_stat.tx_bytes - pdvobjpriv->traffic_stat.last_tx_bytes;
+	pdvobjpriv->traffic_stat.cur_rx_bytes = pdvobjpriv->traffic_stat.rx_bytes - pdvobjpriv->traffic_stat.last_rx_bytes;
+	pdvobjpriv->traffic_stat.last_tx_bytes = pdvobjpriv->traffic_stat.tx_bytes;
+	pdvobjpriv->traffic_stat.last_rx_bytes = pdvobjpriv->traffic_stat.rx_bytes;
+
+	pdvobjpriv->traffic_stat.cur_tx_tp = (u32)(pdvobjpriv->traffic_stat.cur_tx_bytes * 8 / 2 / 1024 / 1024);
+	pdvobjpriv->traffic_stat.cur_rx_tp = (u32)(pdvobjpriv->traffic_stat.cur_rx_bytes * 8 / 2 / 1024 / 1024);
+
+	#ifdef DBG_TRAFFIC_STATISTIC
+	RTW_INFO("\n========================\n");
+	RTW_INFO("cur_tx_bytes:%lld\n", pdvobjpriv->traffic_stat.cur_tx_bytes);
+	RTW_INFO("cur_rx_bytes:%lld\n", pdvobjpriv->traffic_stat.cur_rx_bytes);
+
+	RTW_INFO("last_tx_bytes:%lld\n", pdvobjpriv->traffic_stat.last_tx_bytes);
+	RTW_INFO("last_rx_bytes:%lld\n", pdvobjpriv->traffic_stat.last_rx_bytes);
+
+	RTW_INFO("cur_tx_tp:%d\n", pdvobjpriv->traffic_stat.cur_tx_tp);
+	RTW_INFO("cur_rx_tp:%d\n", pdvobjpriv->traffic_stat.cur_rx_tp);
+	#endif
+}
+
+void rtw_dynamic_check_timer_handler(_adapter *adapter)
+{
+	if (!adapter)
+		return;
+
+	if (!rtw_is_hw_init_completed(adapter))
+		return;
+
+	if (RTW_CANNOT_RUN(adapter))
+		return;
+
+	collect_traffic_statistics(adapter);
+
+	rtw_mi_dynamic_check_timer_handler(adapter);
+
+	if (!is_drv_in_lps(adapter))
+		rtw_dynamic_chk_wk_cmd(adapter);
+}
+
+
+#ifdef CONFIG_SET_SCAN_DENY_TIMER
+inline bool rtw_is_scan_deny(_adapter *adapter)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+	return (ATOMIC_READ(&mlmepriv->set_scan_deny) != 0) ? true : false;
+}
+
+inline void rtw_clear_scan_deny(_adapter *adapter)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+	ATOMIC_SET(&mlmepriv->set_scan_deny, 0);
+	if (0)
+		RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
+}
+
+void rtw_set_scan_deny_timer_hdl(_adapter *adapter)
+{
+	rtw_clear_scan_deny(adapter);
+}
+void rtw_set_scan_deny(_adapter *adapter, u32 ms)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+	if (0)
+		RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
+	ATOMIC_SET(&mlmepriv->set_scan_deny, 1);
+	_set_timer(&mlmepriv->set_scan_deny_timer, ms);
+}
+#endif
+
+#ifdef CONFIG_LAYER2_ROAMING
+/*
+* Select a new roaming candidate from the original @param candidate and @param competitor
+* @return true: candidate is updated
+* @return false: candidate is not updated
+*/
+static int rtw_check_roaming_candidate(struct mlme_priv *mlme
+	, struct wlan_network **candidate, struct wlan_network *competitor)
+{
+	int updated = false;
+	_adapter *adapter = container_of(mlme, _adapter, mlmepriv);
+#ifdef CONFIG_RTW_80211R
+	ft_priv *pftpriv = &mlme->ftpriv;
+	u32 mdie_len = 0;
+	u8 *ptmp = NULL;
+#endif
+
+
+	if (is_same_ess(&competitor->network, &mlme->cur_network.network) == false)
+		goto exit;
+
+	if (rtw_is_desired_network(adapter, competitor) == false)
+		goto exit;
+
+#ifdef CONFIG_LAYER2_ROAMING
+	if (mlme->need_to_roam == false)
+		goto exit;
+#endif
+
+#ifdef CONFIG_RTW_80211R
+	if (rtw_chk_ft_flags(adapter, RTW_FT_SUPPORTED)) {
+		ptmp = rtw_get_ie(&competitor->network.IEs[12], _MDIE_, &mdie_len, competitor->network.IELength-12);
+		if (ptmp) {
+			if (memcmp(&pftpriv->mdid, ptmp+2, 2))
+				goto exit;
+
+			/*The candidate don't support over-the-DS*/
+			if (rtw_chk_ft_flags(adapter, RTW_FT_STA_OVER_DS_SUPPORTED)) {
+				if ((rtw_chk_ft_flags(adapter, RTW_FT_OVER_DS_SUPPORTED) && !(*(ptmp+4) & 0x01)) ||
+					(!rtw_chk_ft_flags(adapter, RTW_FT_OVER_DS_SUPPORTED) && (*(ptmp+4) & 0x01))) {
+					RTW_INFO("FT: ignore the candidate(" MAC_FMT ") for over-the-DS\n", MAC_ARG(competitor->network.MacAddress));
+					rtw_clr_ft_flags(adapter, RTW_FT_OVER_DS_SUPPORTED);
+					goto exit;
+				}
+			}
+		} else
+			goto exit;
+	}
+#endif
+
+	RTW_INFO("roam candidate:%s %s("MAC_FMT", ch%3u) rssi:%d, age:%5d\n",
+		 (competitor == mlme->cur_network_scanned) ? "*" : " " ,
+		 competitor->network.Ssid.Ssid,
+		 MAC_ARG(competitor->network.MacAddress),
+		 competitor->network.Configuration.DSConfig,
+		 (int)competitor->network.Rssi,
+		 rtw_get_passing_time_ms(competitor->last_scanned)
+		);
+
+	/* got specific addr to roam */
+	if (!is_zero_mac_addr(mlme->roam_tgt_addr)) {
+		if (!memcmp(mlme->roam_tgt_addr, competitor->network.MacAddress, ETH_ALEN))
+			goto update;
+		else
+			goto exit;
+	}
+	if (rtw_get_passing_time_ms((u32)competitor->last_scanned) >= mlme->roam_scanr_exp_ms)
+		goto exit;
+
+	if (competitor->network.Rssi - mlme->cur_network_scanned->network.Rssi < mlme->roam_rssi_diff_th)
+		goto exit;
+
+	if (*candidate != NULL && (*candidate)->network.Rssi >= competitor->network.Rssi)
+		goto exit;
+update:
+	*candidate = competitor;
+	updated = true;
+
+exit:
+	return updated;
+}
+
+int rtw_select_roaming_candidate(struct mlme_priv *mlme)
+{
+	unsigned long	irqL;
+	int ret = _FAIL;
+	_list	*phead;
+	_adapter *adapter;
+	_queue	*queue	= &(mlme->scanned_queue);
+	struct	wlan_network	*pnetwork = NULL;
+	struct	wlan_network	*candidate = NULL;
+	u8		bSupportAntDiv = false;
+
+
+	if (mlme->cur_network_scanned == NULL) {
+		rtw_warn_on(1);
+		return ret;
+	}
+
+	_enter_critical_bh(&(mlme->scanned_queue.lock), &irqL);
+	phead = get_list_head(queue);
+	adapter = (_adapter *)mlme->nic_hdl;
+
+	mlme->pscanned = get_next(phead);
+
+	while (!rtw_end_of_queue_search(phead, mlme->pscanned)) {
+
+		pnetwork = LIST_CONTAINOR(mlme->pscanned, struct wlan_network, list);
+		if (pnetwork == NULL) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		mlme->pscanned = get_next(mlme->pscanned);
+
+		if (0)
+			RTW_INFO("%s("MAC_FMT", ch%u) rssi:%d\n"
+				 , pnetwork->network.Ssid.Ssid
+				 , MAC_ARG(pnetwork->network.MacAddress)
+				 , pnetwork->network.Configuration.DSConfig
+				 , (int)pnetwork->network.Rssi);
+
+		rtw_check_roaming_candidate(mlme, &candidate, pnetwork);
+
+	}
+
+	if (candidate == NULL) {
+		RTW_INFO("%s: return _FAIL(candidate == NULL)\n", __func__);
+		ret = _FAIL;
+		goto exit;
+	} else {
+		RTW_INFO("%s: candidate: %s("MAC_FMT", ch:%u)\n", __func__,
+			candidate->network.Ssid.Ssid, MAC_ARG(candidate->network.MacAddress),
+			 candidate->network.Configuration.DSConfig);
+
+		mlme->roam_network = candidate;
+
+		if (!memcmp(candidate->network.MacAddress, mlme->roam_tgt_addr, ETH_ALEN))
+			memset(mlme->roam_tgt_addr, 0, ETH_ALEN);
+	}
+
+	ret = _SUCCESS;
+exit:
+	_exit_critical_bh(&(mlme->scanned_queue.lock), &irqL);
+
+	return ret;
+}
+#endif /* CONFIG_LAYER2_ROAMING */
+
+/*
+* 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 *mlme
+	    , struct wlan_network **candidate, struct wlan_network *competitor)
+{
+	int updated = false;
+	_adapter *adapter = container_of(mlme, _adapter, mlmepriv);
+
+
+	/* check bssid, if needed */
+	if (mlme->assoc_by_bssid) {
+		if (!memcmp(competitor->network.MacAddress, mlme->assoc_bssid, ETH_ALEN) == false)
+			goto exit;
+	}
+
+	/* check ssid, if needed */
+	if (mlme->assoc_ssid.Ssid[0] && mlme->assoc_ssid.SsidLength) {
+		if (competitor->network.Ssid.SsidLength != mlme->assoc_ssid.SsidLength
+		    || !memcmp(competitor->network.Ssid.Ssid, mlme->assoc_ssid.Ssid, mlme->assoc_ssid.SsidLength) == false
+		   )
+			goto exit;
+	}
+
+	if (rtw_is_desired_network(adapter, competitor)  == false)
+		goto exit;
+
+#ifdef CONFIG_LAYER2_ROAMING
+	if (rtw_to_roam(adapter) > 0) {
+		if (rtw_get_passing_time_ms((u32)competitor->last_scanned) >= mlme->roam_scanr_exp_ms
+		    || is_same_ess(&competitor->network, &mlme->cur_network.network) == false
+		   )
+			goto exit;
+	}
+#endif
+
+	if (*candidate == NULL || (*candidate)->network.Rssi < competitor->network.Rssi) {
+		*candidate = competitor;
+		updated = true;
+	}
+
+	if (updated) {
+		RTW_INFO("[by_bssid:%u][assoc_ssid:%s][to_roam:%u] "
+			 "new candidate: %s("MAC_FMT", ch%u) rssi:%d\n",
+			 mlme->assoc_by_bssid,
+			 mlme->assoc_ssid.Ssid,
+			 rtw_to_roam(adapter),
+			 (*candidate)->network.Ssid.Ssid,
+			 MAC_ARG((*candidate)->network.MacAddress),
+			 (*candidate)->network.Configuration.DSConfig,
+			 (int)(*candidate)->network.Rssi
+			);
+	}
+
+exit:
+	return updated;
+}
+
+/*
+Calling context:
+The caller of the sub-routine will be in critical section...
+
+The caller must hold the following spinlock
+
+pmlmepriv->lock
+
+
+*/
+
+int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv)
+{
+	unsigned long	irqL;
+	int ret;
+	_list	*phead;
+	_adapter *adapter;
+	_queue	*queue	= &(pmlmepriv->scanned_queue);
+	struct	wlan_network	*pnetwork = NULL;
+	struct	wlan_network	*candidate = NULL;
+	u8		bSupportAntDiv = false;
+
+
+	adapter = (_adapter *)pmlmepriv->nic_hdl;
+
+	_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+#ifdef CONFIG_LAYER2_ROAMING
+	if (pmlmepriv->roam_network) {
+		candidate = pmlmepriv->roam_network;
+		pmlmepriv->roam_network = NULL;
+		goto candidate_exist;
+	}
+#endif
+
+	phead = get_list_head(queue);
+	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) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		pmlmepriv->pscanned = get_next(pmlmepriv->pscanned);
+
+		if (0)
+			RTW_INFO("%s("MAC_FMT", ch%u) rssi:%d\n"
+				 , pnetwork->network.Ssid.Ssid
+				 , MAC_ARG(pnetwork->network.MacAddress)
+				 , pnetwork->network.Configuration.DSConfig
+				 , (int)pnetwork->network.Rssi);
+
+		rtw_check_join_candidate(pmlmepriv, &candidate, pnetwork);
+
+	}
+
+	if (candidate == NULL) {
+		RTW_INFO("%s: return _FAIL(candidate == NULL)\n", __func__);
+#ifdef CONFIG_WOWLAN
+		_clr_fwstate_(pmlmepriv, _FW_LINKED | _FW_UNDER_LINKING);
+#endif
+		ret = _FAIL;
+		goto exit;
+	} else {
+		RTW_INFO("%s: candidate: %s("MAC_FMT", ch:%u)\n", __func__,
+			candidate->network.Ssid.Ssid, MAC_ARG(candidate->network.MacAddress),
+			 candidate->network.Configuration.DSConfig);
+		goto candidate_exist;
+	}
+
+candidate_exist:
+
+	/* check for situation of  _FW_LINKED */
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		RTW_INFO("%s: _FW_LINKED while ask_for_joinbss!!!\n", __func__);
+
+		rtw_disassoc_cmd(adapter, 0, true);
+		rtw_indicate_disconnect(adapter, 0, false);
+		rtw_free_assoc_resources(adapter, 0);
+	}
+
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	rtw_hal_get_def_var(adapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &(bSupportAntDiv));
+	if (bSupportAntDiv) {
+		u8 CurrentAntenna;
+		rtw_hal_get_odm_var(adapter, HAL_ODM_ANTDIV_SELECT, &(CurrentAntenna), NULL);
+		RTW_INFO("#### Opt_Ant_(%s) , cur_Ant(%s)\n",
+			(MAIN_ANT == candidate->network.PhyInfo.Optimum_antenna) ? "MAIN_ANT" : "AUX_ANT",
+			 (MAIN_ANT == CurrentAntenna) ? "MAIN_ANT" : "AUX_ANT"
+			);
+	}
+#endif
+	set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+	ret = rtw_joinbss_cmd(adapter, candidate);
+
+exit:
+	_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+
+	return ret;
+}
+
+sint rtw_set_auth(_adapter *adapter, struct security_priv *psecuritypriv)
+{
+	struct	cmd_obj *pcmd;
+	struct	setauth_parm *psetauthparm;
+	struct	cmd_priv	*pcmdpriv = &(adapter->cmdpriv);
+	sint		res = _SUCCESS;
+
+
+	pcmd = (struct	cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
+	if (pcmd == NULL) {
+		res = _FAIL; /* try again */
+		goto exit;
+	}
+
+	psetauthparm = (struct setauth_parm *)rtw_zmalloc(sizeof(struct setauth_parm));
+	if (psetauthparm == NULL) {
+		rtw_mfree((unsigned char *)pcmd, sizeof(struct	cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	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;
+
+
+	INIT_LIST_HEAD(&pcmd->list);
+
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+
+
+	return res;
+
+}
+
+
+sint rtw_set_key(_adapter *adapter, struct security_priv *psecuritypriv, sint keyid, u8 set_tx, bool enqueue)
+{
+	u8	keylen;
+	struct cmd_obj		*pcmd;
+	struct setkey_parm	*psetkeyparm;
+	struct cmd_priv		*pcmdpriv = &(adapter->cmdpriv);
+	struct mlme_priv		*pmlmepriv = &(adapter->mlmepriv);
+	sint	res = _SUCCESS;
+
+
+	psetkeyparm = (struct setkey_parm *)rtw_zmalloc(sizeof(struct setkey_parm));
+	if (psetkeyparm == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	memset(psetkeyparm, 0, sizeof(struct setkey_parm));
+
+	if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) {
+		psetkeyparm->algorithm = (unsigned char)psecuritypriv->dot118021XGrpPrivacy;
+	} else {
+		psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm;
+
+	}
+	psetkeyparm->keyid = (u8)keyid;/* 0~3 */
+	psetkeyparm->set_tx = set_tx;
+	if (is_wep_enc(psetkeyparm->algorithm))
+		adapter->securitypriv.key_mask |= BIT(psetkeyparm->keyid);
+
+	RTW_INFO("==> rtw_set_key algorithm(%x),keyid(%x),key_mask(%x)\n", psetkeyparm->algorithm, psetkeyparm->keyid, adapter->securitypriv.key_mask);
+
+	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:
+		res = _FAIL;
+		rtw_mfree((unsigned char *)psetkeyparm, sizeof(struct setkey_parm));
+		goto exit;
+	}
+
+
+	if (enqueue) {
+		pcmd = (struct	cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
+		if (pcmd == NULL) {
+			rtw_mfree((unsigned char *)psetkeyparm, sizeof(struct setkey_parm));
+			res = _FAIL; /* try again */
+			goto exit;
+		}
+
+		pcmd->cmdcode = _SetKey_CMD_;
+		pcmd->parmbuf = (u8 *)psetkeyparm;
+		pcmd->cmdsz = (sizeof(struct setkey_parm));
+		pcmd->rsp = NULL;
+		pcmd->rspsz = 0;
+
+		INIT_LIST_HEAD(&pcmd->list);
+
+		/* sema_init(&(pcmd->cmd_sem), 0); */
+
+		res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+	} else {
+		setkey_hdl(adapter, (u8 *)psetkeyparm);
+		rtw_mfree((u8 *) psetkeyparm, sizeof(struct setkey_parm));
+	}
+exit:
+	return res;
+
+}
+
+
+/* adjust IEs for rtw_joinbss_cmd in WMM */
+int rtw_restruct_wmm_ie(_adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len, uint initial_out_len)
+{
+	unsigned	int ielength = 0;
+	unsigned int i, j;
+
+	i = 12; /* after the fixed IE */
+	while (i < in_len) {
+		ielength = initial_out_len;
+
+		if (in_ie[i] == 0xDD && in_ie[i + 2] == 0x00 && in_ie[i + 3] == 0x50  && in_ie[i + 4] == 0xF2 && in_ie[i + 5] == 0x02 && i + 5 < in_len) { /* WMM element ID and OUI */
+			/* Append WMM IE to the last index of out_ie */
+			for (j = i; j < i + 9; j++) {
+				out_ie[ielength] = in_ie[j];
+				ielength++;
+			}
+			out_ie[initial_out_len + 1] = 0x07;
+			out_ie[initial_out_len + 6] = 0x00;
+			out_ie[initial_out_len + 8] = 0x00;
+
+			break;
+		}
+
+		i += (in_ie[i + 1] + 2); /* to the next IE element */
+	}
+
+	return ielength;
+
+}
+
+
+/*
+ * Ported from 8185: IsInPreAuthKeyList(). (Renamed from SecIsInPreAuthKeyList(), 2006-10-13.)
+ * Added by Annie, 2006-05-07.
+ *
+ * Search by BSSID,
+ * Return Value:
+ *		-1		:if there is no pre-auth key in the  table
+ *		>=0		:if there is pre-auth key, and   return the entry id
+ *
+ *   */
+
+static int SecIsInPMKIDList(_adapter *Adapter, u8 *bssid)
+{
+	struct security_priv *psecuritypriv = &Adapter->securitypriv;
+	int i = 0;
+
+	do {
+		if ((psecuritypriv->PMKIDList[i].bUsed) &&
+		    (!memcmp(psecuritypriv->PMKIDList[i].Bssid, bssid, ETH_ALEN)))
+			break;
+		else {
+			i++;
+			/* continue; */
+		}
+
+	} while (i < NUM_PMKID_CACHE);
+
+	if (i == NUM_PMKID_CACHE) {
+		i = -1;/* Could not find. */
+	} else {
+		/* There is one Pre-Authentication Key for the specific BSSID. */
+	}
+
+	return i;
+
+}
+
+/*
+ * Check the RSN IE length
+ * If the RSN IE length <= 20, the RSN IE didn't include the PMKID information
+ * 0-11th element in the array are the fixed IE
+ * 12th element in the array is the IE
+ * 13th element in the array is the IE length
+ *   */
+
+static int rtw_append_pmkid(_adapter *adapter, int iEntry, u8 *ie, uint ie_len)
+{
+	struct security_priv *sec = &adapter->securitypriv;
+
+	if (ie[13] > 20) {
+		int i;
+		u16 pmkid_cnt = RTW_GET_LE16(ie + 14 + 20);
+		if (pmkid_cnt == 1 && !memcmp(ie + 14 + 20 + 2, &sec->PMKIDList[iEntry].PMKID, 16)) {
+			RTW_INFO(FUNC_ADPT_FMT" has carried the same PMKID:"KEY_FMT"\n"
+				, FUNC_ADPT_ARG(adapter), KEY_ARG(&sec->PMKIDList[iEntry].PMKID));
+			goto exit;
+		}
+
+		RTW_INFO(FUNC_ADPT_FMT" remove original PMKID, count:%u\n"
+			 , FUNC_ADPT_ARG(adapter), pmkid_cnt);
+
+		for (i = 0; i < pmkid_cnt; i++)
+			RTW_INFO("    "KEY_FMT"\n", KEY_ARG(ie + 14 + 20 + 2 + i * 16));
+
+		ie_len -= 2 + pmkid_cnt * 16;
+		ie[13] = 20;
+	}
+
+	if (ie[13] <= 20) {
+		/* The RSN IE didn't include the PMK ID, append the PMK information */
+
+		RTW_INFO(FUNC_ADPT_FMT" append PMKID:"KEY_FMT"\n"
+			, FUNC_ADPT_ARG(adapter), KEY_ARG(&sec->PMKIDList[iEntry].PMKID));
+
+		RTW_PUT_LE16(&ie[ie_len], 1);
+		ie_len += 2;
+
+		memcpy(&ie[ie_len], &sec->PMKIDList[iEntry].PMKID, 16);
+		ie_len += 16;
+
+		ie[13] += 18;/* PMKID length = 2+16 */
+	}
+
+exit:
+	return ie_len;
+}
+
+static int rtw_remove_pmkid(_adapter *adapter, u8 *ie, uint ie_len)
+{
+	struct security_priv *sec = &adapter->securitypriv;
+	int i;
+	u16 pmkid_cnt = RTW_GET_LE16(ie + 14 + 20);
+
+	if (ie[13] <= 20)
+		goto exit;
+
+	RTW_INFO(FUNC_ADPT_FMT" remove original PMKID, count:%u\n"
+		 , FUNC_ADPT_ARG(adapter), pmkid_cnt);
+
+	for (i = 0; i < pmkid_cnt; i++)
+		RTW_INFO("    "KEY_FMT"\n", KEY_ARG(ie + 14 + 20 + 2 + i * 16));
+
+	ie_len -= 2 + pmkid_cnt * 16;
+	ie[13] = 20;
+
+exit:
+	return ie_len;
+}
+
+sint rtw_restruct_sec_ie(_adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len)
+{
+	u8 authmode = 0x0, securitytype, match;
+	u8 sec_ie[255], uncst_oui[4], bkup_ie[255];
+	u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
+	uint	ielength, cnt, remove_cnt;
+	int iEntry;
+
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+	uint	ndisauthmode = psecuritypriv->ndisauthtype;
+	uint ndissecuritytype = psecuritypriv->ndisencryptstatus;
+
+
+
+	/* 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);
+		/* debug for CONFIG_IEEE80211W
+		{
+			int jj;
+			RTW_INFO("supplicant_ie_length=%d &&&&&&&&&&&&&&&&&&&\n", psecuritypriv->supplicant_ie[1]+2);
+			for(jj=0; jj < psecuritypriv->supplicant_ie[1]+2; jj++)
+				RTW_INFO(" %02x ", psecuritypriv->supplicant_ie[jj]);
+			RTW_INFO("\n");
+		}*/
+		ielength += psecuritypriv->supplicant_ie[1] + 2;
+		rtw_report_sec_ie(adapter, authmode, psecuritypriv->supplicant_ie);
+
+#ifdef CONFIG_DRVEXT_MODULE
+		drvext_report_sec_ie(&adapter->drvextpriv, authmode, sec_ie);
+#endif
+	}
+
+	iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid);
+	if (iEntry < 0) {
+		if (authmode == _WPA2_IE_ID_)
+			ielength = rtw_remove_pmkid(adapter, out_ie, ielength);
+	} else {
+		if (authmode == _WPA2_IE_ID_)
+			ielength = rtw_append_pmkid(adapter, iEntry, out_ie, ielength);
+	}
+
+
+	return ielength;
+}
+
+void rtw_init_registrypriv_dev_network(_adapter *adapter)
+{
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	WLAN_BSSID_EX    *pdev_network = &pregistrypriv->dev_network;
+	u8 *myhwaddr = adapter_mac_addr(adapter);
+
+
+	memcpy(pdev_network->MacAddress, myhwaddr, ETH_ALEN);
+
+	memcpy(&pdev_network->Ssid, &pregistrypriv->ssid, sizeof(NDIS_802_11_SSID));
+
+	pdev_network->Configuration.Length = sizeof(NDIS_802_11_CONFIGURATION);
+	pdev_network->Configuration.BeaconPeriod = 100;
+	pdev_network->Configuration.FHConfig.Length = 0;
+	pdev_network->Configuration.FHConfig.HopPattern = 0;
+	pdev_network->Configuration.FHConfig.HopSet = 0;
+	pdev_network->Configuration.FHConfig.DwellTime = 0;
+
+
+
+}
+
+void rtw_update_registrypriv_dev_network(_adapter *adapter)
+{
+	int sz = 0;
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	WLAN_BSSID_EX    *pdev_network = &pregistrypriv->dev_network;
+	struct	security_priv	*psecuritypriv = &adapter->securitypriv;
+	struct	wlan_network	*cur_network = &adapter->mlmepriv.cur_network;
+	/* struct	xmit_priv	*pxmitpriv = &adapter->xmitpriv; */
+	struct mlme_ext_priv	*pmlmeext = &adapter->mlmeextpriv;
+
+	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);
+
+	if (cur_network->network.InfrastructureMode == Ndis802_11IBSS) {
+		pdev_network->Configuration.ATIMWindow = (0);
+
+		if (pmlmeext->cur_channel != 0)
+			pdev_network->Configuration.DSConfig = pmlmeext->cur_channel;
+		else
+			pdev_network->Configuration.DSConfig = 1;
+	}
+
+	pdev_network->InfrastructureMode = (cur_network->network.InfrastructureMode);
+
+	/* 1. Supported rates */
+	/* 2. IE */
+
+	/* rtw_set_supported_rate(pdev_network->SupportedRates, pregistrypriv->wireless_mode) ; */ /* will be called in rtw_generate_ie */
+	sz = rtw_generate_ie(pregistrypriv);
+
+	pdev_network->IELength = sz;
+
+	pdev_network->Length = get_WLAN_BSSID_EX_sz((WLAN_BSSID_EX *)pdev_network);
+
+	/* notes: translate IELength & Length after assign the Length to cmdsz in createbss_cmd(); */
+	/* pdev_network->IELength = cpu_to_le32(sz); */
+
+
+}
+
+void rtw_get_encrypt_decrypt_from_registrypriv(_adapter *adapter)
+{
+
+
+
+}
+
+/* the fucntion is at passive_level */
+void rtw_joinbss_reset(_adapter *padapter)
+{
+	u8	threshold;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	/* todo: if you want to do something io/reg/hw setting before join_bss, please add code here */
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+
+	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));
+	}
+}
+
+void	rtw_ht_use_default_setting(_adapter *padapter)
+{
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	bool		bHwLDPCSupport = false, bHwSTBCSupport = false;
+#ifdef CONFIG_BEAMFORMING
+	bool		bHwSupportBeamformer = false, bHwSupportBeamformee = false;
+#endif /* CONFIG_BEAMFORMING */
+
+	if (pregistrypriv->wifi_spec)
+		phtpriv->bss_coexist = 1;
+	else
+		phtpriv->bss_coexist = 0;
+
+	phtpriv->sgi_40m = TEST_FLAG(pregistrypriv->short_gi, BIT1) ? true : false;
+	phtpriv->sgi_20m = TEST_FLAG(pregistrypriv->short_gi, BIT0) ? true : false;
+
+	/* LDPC support */
+	rtw_hal_get_def_var(padapter, HAL_DEF_RX_LDPC, (u8 *)&bHwLDPCSupport);
+	CLEAR_FLAGS(phtpriv->ldpc_cap);
+	if (bHwLDPCSupport) {
+		if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT4))
+			SET_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_RX);
+	}
+	rtw_hal_get_def_var(padapter, HAL_DEF_TX_LDPC, (u8 *)&bHwLDPCSupport);
+	if (bHwLDPCSupport) {
+		if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT5))
+			SET_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX);
+	}
+	if (phtpriv->ldpc_cap)
+		RTW_INFO("[HT] HAL Support LDPC = 0x%02X\n", phtpriv->ldpc_cap);
+
+	/* STBC */
+	rtw_hal_get_def_var(padapter, HAL_DEF_TX_STBC, (u8 *)&bHwSTBCSupport);
+	CLEAR_FLAGS(phtpriv->stbc_cap);
+	if (bHwSTBCSupport) {
+		if (TEST_FLAG(pregistrypriv->stbc_cap, BIT5))
+			SET_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX);
+	}
+	rtw_hal_get_def_var(padapter, HAL_DEF_RX_STBC, (u8 *)&bHwSTBCSupport);
+	if (bHwSTBCSupport) {
+		if (TEST_FLAG(pregistrypriv->stbc_cap, BIT4))
+			SET_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_RX);
+	}
+	if (phtpriv->stbc_cap)
+		RTW_INFO("[HT] HAL Support STBC = 0x%02X\n", phtpriv->stbc_cap);
+
+	/* Beamforming setting */
+	CLEAR_FLAGS(phtpriv->beamform_cap);
+#ifdef CONFIG_BEAMFORMING
+	rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMER, (u8 *)&bHwSupportBeamformer);
+	rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMEE, (u8 *)&bHwSupportBeamformee);
+	if (TEST_FLAG(pregistrypriv->beamform_cap, BIT4) && bHwSupportBeamformer) {
+		SET_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE);
+		RTW_INFO("[HT] HAL Support Beamformer\n");
+	}
+	if (TEST_FLAG(pregistrypriv->beamform_cap, BIT5) && bHwSupportBeamformee) {
+		SET_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE);
+		RTW_INFO("[HT] HAL Support Beamformee\n");
+	}
+#endif /* CONFIG_BEAMFORMING */
+}
+void rtw_build_wmm_ie_ht(_adapter *padapter, u8 *out_ie, uint *pout_len)
+{
+	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	int out_len;
+	u8 *pframe;
+
+	if (padapter->mlmepriv.qospriv.qos_option == 0) {
+		out_len = *pout_len;
+		pframe = rtw_set_ie(out_ie + out_len, _VENDOR_SPECIFIC_IE_,
+				    _WMM_IE_Length_, WMM_IE, pout_len);
+
+		padapter->mlmepriv.qospriv.qos_option = 1;
+	}
+}
+
+/* the fucntion is >= passive_level */
+unsigned int rtw_restructure_ht_ie(_adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len, u8 channel)
+{
+	u32 ielen, out_len;
+	u32 rx_packet_offset, max_recvbuf_sz;
+	HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor;
+	HT_CAP_AMPDU_DENSITY best_ampdu_density;
+	unsigned char *p, *pframe;
+	struct rtw_ieee80211_ht_cap ht_capie;
+	u8	cbw40_enable = 0, rf_type = 0, operation_bw = 0, rf_num = 0, rx_stbc_nss = 0, rx_nss = 0;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct hal_spec_t *hal_spec = GET_HAL_SPEC(padapter);
+
+	phtpriv->ht_option = false;
+
+	out_len = *pout_len;
+
+	memset(&ht_capie, 0, sizeof(struct rtw_ieee80211_ht_cap));
+
+	ht_capie.cap_info = cpu_to_le16(IEEE80211_HT_CAP_DSSSCCK40);
+
+	if (phtpriv->sgi_20m)
+		ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SGI_20);
+
+	/* Get HT BW */
+	if (in_ie == NULL) {
+		/* TDLS: TODO 20/40 issue */
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+			operation_bw = padapter->mlmeextpriv.cur_bwmode;
+			if (operation_bw > CHANNEL_WIDTH_40)
+				operation_bw = CHANNEL_WIDTH_40;
+		} else
+			/* TDLS: TODO 40? */
+			operation_bw = CHANNEL_WIDTH_40;
+	} else {
+		p = rtw_get_ie(in_ie, _HT_ADD_INFO_IE_, &ielen, in_len);
+		if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) {
+			struct HT_info_element *pht_info = (struct HT_info_element *)(p + 2);
+			if (pht_info->infos[0] & BIT(2)) {
+				switch (pht_info->infos[0] & 0x3) {
+				case 1:
+				case 3:
+					operation_bw = CHANNEL_WIDTH_40;
+					break;
+				default:
+					operation_bw = CHANNEL_WIDTH_20;
+					break;
+				}
+			} else
+				operation_bw = CHANNEL_WIDTH_20;
+		}
+	}
+
+	/* to disable 40M Hz support while gd_bw_40MHz_en = 0 */
+	if (hal_chk_bw_cap(padapter, BW_CAP_40M)) {
+		if (channel > 14) {
+			if (REGSTY_IS_BW_5G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40))
+				cbw40_enable = 1;
+		} else {
+			if (REGSTY_IS_BW_2G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40))
+				cbw40_enable = 1;
+		}
+	}
+
+	if ((cbw40_enable == 1) && (operation_bw == CHANNEL_WIDTH_40)) {
+		ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH);
+		if (phtpriv->sgi_40m)
+			ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SGI_40);
+	}
+
+	/* todo: disable SM power save mode */
+	ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SM_PS);
+
+	/* RX LDPC */
+	if (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_RX)) {
+		ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_LDPC_CODING);
+		RTW_INFO("[HT] Declare supporting RX LDPC\n");
+	}
+
+	/* TX STBC */
+	if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX)) {
+		ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_TX_STBC);
+		RTW_INFO("[HT] Declare supporting TX STBC\n");
+	}
+
+	/* RX STBC */
+	if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_RX)) {
+		if ((pregistrypriv->rx_stbc == 0x3) ||							/* enable for 2.4/5 GHz */
+		    ((channel <= 14) && (pregistrypriv->rx_stbc == 0x1)) ||		/* enable for 2.4GHz */
+		    ((channel > 14) && (pregistrypriv->rx_stbc == 0x2)) ||		/* enable for 5GHz */
+		    (pregistrypriv->wifi_spec == 1)) {
+			/* HAL_DEF_RX_STBC means STBC RX spatial stream, todo: VHT 4 streams */
+			rtw_hal_get_def_var(padapter, HAL_DEF_RX_STBC, (u8 *)(&rx_stbc_nss));
+			SET_HT_CAP_ELE_RX_STBC(&ht_capie, rx_stbc_nss);
+			RTW_INFO("[HT] Declare supporting RX STBC = %d\n", rx_stbc_nss);
+		}
+	}
+
+	/* fill default supported_mcs_set */
+	memcpy(ht_capie.supp_mcs_set, pmlmeext->default_supported_mcs_set, 16);
+
+	/* update default supported_mcs_set */
+	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	rx_nss = rtw_min(rf_type_to_rf_rx_cnt(rf_type), hal_spec->rx_nss_num);
+
+	switch (rx_nss) {
+	case 1:
+		set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_1R);
+		break;
+	case 2:
+		#ifdef CONFIG_DISABLE_MCS13TO15
+		if (((cbw40_enable == 1) && (operation_bw == CHANNEL_WIDTH_40)) && (pregistrypriv->wifi_spec != 1))
+			set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R_13TO15_OFF);
+		else
+		#endif
+			set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R);
+		break;
+	case 3:
+		set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_3R);
+		break;
+	case 4:
+		set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_4R);
+		break;
+	default:
+		RTW_WARN("rf_type:%d or rx_nss:%u is not expected\n", rf_type, hal_spec->rx_nss_num);
+	}
+
+	rtw_hal_get_def_var(padapter, HAL_DEF_RX_PACKET_OFFSET, &rx_packet_offset);
+	rtw_hal_get_def_var(padapter, HAL_DEF_MAX_RECVBUF_SZ, &max_recvbuf_sz);
+	if (max_recvbuf_sz - rx_packet_offset >= (8191 - 256)) {
+		RTW_INFO("%s IEEE80211_HT_CAP_MAX_AMSDU is set\n", __func__);
+		ht_capie.cap_info = ht_capie.cap_info | cpu_to_le16(IEEE80211_HT_CAP_MAX_AMSDU);
+	}
+	if (padapter->driver_rx_ampdu_factor != 0xFF)
+		max_rx_ampdu_factor = (HT_CAP_AMPDU_FACTOR)padapter->driver_rx_ampdu_factor;
+	else
+		rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
+
+	/* 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->driver_rx_ampdu_spacing != 0xFF)
+		ht_capie.ampdu_params_info |= ((padapter->driver_rx_ampdu_spacing & 0x07) << 2);
+	else {
+		if (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_) {
+			/*
+			*	Todo : Each chip must to ask DD , this chip best ampdu_density setting
+			*	By yiwei.sun
+			*/
+			rtw_hal_get_def_var(padapter, HW_VAR_BEST_AMPDU_DENSITY, &best_ampdu_density);
+
+			ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY & (best_ampdu_density << 2));
+
+		} else
+			ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY & 0x00);
+	}
+#ifdef CONFIG_BEAMFORMING
+	ht_capie.tx_BF_cap_info = 0;
+
+	/* HT Beamformer*/
+	if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE)) {
+		/* Transmit NDP Capable */
+		SET_HT_CAP_TXBF_TRANSMIT_NDP_CAP(&ht_capie, 1);
+		/* Explicit Compressed Steering Capable */
+		SET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(&ht_capie, 1);
+		/* Compressed Steering Number Antennas */
+		SET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(&ht_capie, 1);
+		rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMER_CAP, (u8 *)&rf_num);
+		SET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS(&ht_capie, rf_num);
+	}
+
+	/* HT Beamformee */
+	if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE)) {
+		/* Receive NDP Capable */
+		SET_HT_CAP_TXBF_RECEIVE_NDP_CAP(&ht_capie, 1);
+		/* Explicit Compressed Beamforming Feedback Capable */
+		SET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(&ht_capie, 2);
+		rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMEE_CAP, (u8 *)&rf_num);
+		SET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(&ht_capie, rf_num);
+	}
+#endif/*CONFIG_BEAMFORMING*/
+
+	pframe = 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;
+
+	if (in_ie != NULL) {
+		p = rtw_get_ie(in_ie, _HT_ADD_INFO_IE_, &ielen, in_len);
+		if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) {
+			out_len = *pout_len;
+			pframe = rtw_set_ie(out_ie + out_len, _HT_ADD_INFO_IE_, ielen, p + 2 , pout_len);
+		}
+	}
+
+	return phtpriv->ht_option;
+
+}
+
+/* the fucntion is > passive_level (in critical_section) */
+void rtw_update_ht_cap(_adapter *padapter, u8 *pie, uint ie_len, u8 channel)
+{
+	u8 *p, max_ampdu_sz;
+	int len;
+	/* struct sta_info *bmc_sta, *psta; */
+	struct rtw_ieee80211_ht_cap *pht_capie;
+	struct ieee80211_ht_addt_info *pht_addtinfo;
+	/* struct recv_reorder_ctrl *preorder_ctrl; */
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+	/* struct recv_priv *precvpriv = &padapter->recvpriv; */
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	/* struct wlan_network *pcur_network = &(pmlmepriv->cur_network);; */
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 cbw40_enable = 0;
+
+
+	if (!phtpriv->ht_option)
+		return;
+
+	if ((!pmlmeinfo->HT_info_enable) || (!pmlmeinfo->HT_caps_enable))
+		return;
+
+	RTW_INFO("+rtw_update_ht_cap()\n");
+
+	/* maybe needs check if ap supports rx ampdu. */
+	if ((phtpriv->ampdu_enable == false) && (pregistrypriv->ampdu_enable == 1)) {
+		if (pregistrypriv->wifi_spec == 1) {
+			/* remove this part because testbed AP should disable RX AMPDU */
+			/* phtpriv->ampdu_enable = false; */
+			phtpriv->ampdu_enable = true;
+		} else
+			phtpriv->ampdu_enable = true;
+	} 
+
+
+	/* check Max Rx A-MPDU Size */
+	len = 0;
+	p = rtw_get_ie(pie + sizeof(NDIS_802_11_FIXED_IEs), _HT_CAPABILITY_IE_, &len, ie_len - sizeof(NDIS_802_11_FIXED_IEs));
+	if (p && len > 0) {
+		pht_capie = (struct rtw_ieee80211_ht_cap *)(p + 2);
+		max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_CAP_AMPDU_FACTOR);
+		max_ampdu_sz = 1 << (max_ampdu_sz + 3); /* max_ampdu_sz (kbytes); */
+
+		/* RTW_INFO("rtw_update_ht_cap(): max_ampdu_sz=%d\n", max_ampdu_sz); */
+		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
+
+	}
+
+
+	len = 0;
+	p = rtw_get_ie(pie + sizeof(NDIS_802_11_FIXED_IEs), _HT_ADD_INFO_IE_, &len, ie_len - sizeof(NDIS_802_11_FIXED_IEs));
+	if (p && len > 0) {
+		pht_addtinfo = (struct ieee80211_ht_addt_info *)(p + 2);
+		/* todo: */
+	}
+
+	if (hal_chk_bw_cap(padapter, BW_CAP_40M)) {
+		if (channel > 14) {
+			if (REGSTY_IS_BW_5G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40))
+				cbw40_enable = 1;
+		} else {
+			if (REGSTY_IS_BW_2G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40))
+				cbw40_enable = 1;
+		}
+	}
+
+	/* update cur_bwmode & cur_ch_offset */
+	if ((cbw40_enable) &&
+	    (le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) & BIT(1)) &&
+	    (pmlmeinfo->HT_info.infos[0] & BIT(2))) {
+		struct hal_spec_t *hal_spec = GET_HAL_SPEC(padapter);
+		int i;
+		u8	rf_type = RF_1T1R;
+		u8 tx_nss = 0;
+
+		rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+		tx_nss = rtw_min(rf_type_to_rf_tx_cnt(rf_type), hal_spec->tx_nss_num);
+
+		/* update the MCS set */
+		for (i = 0; i < 16; i++)
+			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= pmlmeext->default_supported_mcs_set[i];
+
+		/* update the MCS rates */
+		switch (tx_nss) {
+		case 1:
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R);
+			break;
+		case 2:
+			#ifdef CONFIG_DISABLE_MCS13TO15
+			if (pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 && pregistrypriv->wifi_spec != 1)
+				set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R_13TO15_OFF);
+			else
+			#endif
+				set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R);
+			break;
+		case 3:
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_3R);
+			break;
+		case 4:
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_4R);
+			break;
+		default:
+			RTW_WARN("rf_type:%d or tx_nss_num:%u is not expected\n", rf_type, hal_spec->tx_nss_num);
+		}
+
+		/* switch to the 40M Hz mode accoring to the AP */
+		/* pmlmeext->cur_bwmode = CHANNEL_WIDTH_40; */
+		switch ((pmlmeinfo->HT_info.infos[0] & 0x3)) {
+		case EXTCHNL_OFFSET_UPPER:
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+			break;
+
+		case 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)
+		RTW_INFO("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__);
+
+	/* Config current HT Protection mode. */
+	pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3;
+}
+
+#ifdef CONFIG_TDLS
+void rtw_issue_addbareq_cmd_tdls(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct sta_info *ptdls_sta = NULL;
+	u8 issued;
+	int priority;
+	struct ht_priv	*phtpriv;
+
+	priority = pattrib->priority;
+
+	if (pattrib->direct_link) {
+		ptdls_sta = rtw_get_stainfo(&padapter->stapriv, pattrib->dst);
+		if ((ptdls_sta != NULL) && (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)) {
+			phtpriv = &ptdls_sta->htpriv;
+
+			if ((phtpriv->ht_option) && (phtpriv->ampdu_enable == true)) {
+				issued = (phtpriv->agg_enable_bitmap >> priority) & 0x1;
+				issued |= (phtpriv->candidate_tid_bitmap >> priority) & 0x1;
+
+				if (0 == issued) {
+					RTW_INFO("[%s], p=%d\n", __func__, priority);
+					ptdls_sta->htpriv.candidate_tid_bitmap |= BIT((u8)priority);
+					rtw_addbareq_cmd(padapter, (u8)priority, pattrib->dst);
+				}
+			}
+		}
+	}
+}
+#endif /* CONFIG_TDLS */
+
+void rtw_issue_addbareq_cmd(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	u8 issued;
+	int priority;
+	struct sta_info *psta = NULL;
+	struct ht_priv	*phtpriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	s32 bmcst = IS_MCAST(pattrib->ra);
+
+	/* if(bmcst || (padapter->mlmepriv.LinkDetectInfo.bTxBusyTraffic == false)) */
+	if (bmcst || (padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod < 100))
+		return;
+
+	priority = pattrib->priority;
+
+#ifdef CONFIG_TDLS
+	rtw_issue_addbareq_cmd_tdls(padapter, pxmitframe);
+#endif /* CONFIG_TDLS */
+
+	psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	if (pattrib->psta != psta) {
+		RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+		return;
+	}
+
+	if (psta == NULL) {
+		RTW_INFO("%s, psta==NUL\n", __func__);
+		return;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		RTW_INFO("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return;
+	}
+
+
+	phtpriv = &psta->htpriv;
+
+	if ((phtpriv->ht_option) && (phtpriv->ampdu_enable == true)) {
+		issued = (phtpriv->agg_enable_bitmap >> priority) & 0x1;
+		issued |= (phtpriv->candidate_tid_bitmap >> priority) & 0x1;
+
+		if (0 == issued) {
+			RTW_INFO("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_append_exented_cap(_adapter *padapter, u8 *out_ie, uint *pout_len)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
+	u8	cap_content[8] = { 0 };
+	u8	*pframe;
+	u8   null_content[8] = {0};
+
+	if (phtpriv->bss_coexist)
+		SET_EXT_CAPABILITY_ELE_BSS_COEXIST(cap_content, 1);
+	/*
+		From 802.11 specification,if a STA does not support any of capabilities defined
+		in the Extended Capabilities element, then the STA is not required to
+		transmit the Extended Capabilities element.
+	*/
+	if (false == !memcmp(cap_content, null_content, 8))
+		pframe = rtw_set_ie(out_ie + *pout_len, EID_EXTCapability, 8, cap_content , pout_len);
+}
+
+#ifdef CONFIG_LAYER2_ROAMING
+inline void rtw_set_to_roam(_adapter *adapter, u8 to_roam)
+{
+	if (to_roam == 0)
+		adapter->mlmepriv.to_join = false;
+	adapter->mlmepriv.to_roam = to_roam;
+}
+
+inline u8 rtw_dec_to_roam(_adapter *adapter)
+{
+	adapter->mlmepriv.to_roam--;
+	return adapter->mlmepriv.to_roam;
+}
+
+inline u8 rtw_to_roam(_adapter *adapter)
+{
+	return adapter->mlmepriv.to_roam;
+}
+
+void rtw_roaming(_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(_adapter *padapter, struct wlan_network *tgt_network)
+{
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *cur_network = &pmlmepriv->cur_network;
+	int do_join_r;
+
+	if (0 < rtw_to_roam(padapter)) {
+		RTW_INFO("roaming from %s("MAC_FMT"), length:%d\n",
+			cur_network->network.Ssid.Ssid, MAC_ARG(cur_network->network.MacAddress),
+			 cur_network->network.Ssid.SsidLength);
+		memcpy(&pmlmepriv->assoc_ssid, &cur_network->network.Ssid, sizeof(NDIS_802_11_SSID));
+
+		pmlmepriv->assoc_by_bssid = false;
+
+#ifdef CONFIG_WAPI_SUPPORT
+		rtw_wapi_return_all_sta_info(padapter);
+#endif
+
+		while (1) {
+			do_join_r = rtw_do_join(padapter);
+			if (_SUCCESS == do_join_r)
+				break;
+			else {
+				RTW_INFO("roaming do_join return %d\n", do_join_r);
+				rtw_dec_to_roam(padapter);
+
+				if (rtw_to_roam(padapter) > 0)
+					continue;
+				else {
+					RTW_INFO("%s(%d) -to roaming fail, indicate_disconnect\n", __func__, __LINE__);
+#ifdef CONFIG_RTW_80211R
+					rtw_clr_ft_flags(padapter, RTW_FT_SUPPORTED|RTW_FT_OVER_DS_SUPPORTED);
+					rtw_reset_ft_status(padapter);
+#endif
+					rtw_indicate_disconnect(padapter, 0, false);
+					break;
+				}
+			}
+		}
+	}
+
+}
+#endif /* CONFIG_LAYER2_ROAMING */
+
+bool rtw_adjust_chbw(_adapter *adapter, u8 req_ch, u8 *req_bw, u8 *req_offset)
+{
+	struct registry_priv *regsty = adapter_to_regsty(adapter);
+	u8 allowed_bw;
+
+	if (req_ch <= 14)
+		allowed_bw = REGSTY_BW_2G(regsty);
+	else
+		allowed_bw = REGSTY_BW_5G(regsty);
+
+	allowed_bw = hal_largest_bw(adapter, allowed_bw);
+
+	if (allowed_bw == CHANNEL_WIDTH_80 && *req_bw > CHANNEL_WIDTH_80)
+		*req_bw = CHANNEL_WIDTH_80;
+	else if (allowed_bw == CHANNEL_WIDTH_40 && *req_bw > CHANNEL_WIDTH_40)
+		*req_bw = CHANNEL_WIDTH_40;
+	else if (allowed_bw == CHANNEL_WIDTH_20 && *req_bw > CHANNEL_WIDTH_20) {
+		*req_bw = CHANNEL_WIDTH_20;
+		*req_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	} else
+		return false;
+
+	return true;
+}
+
+sint rtw_linked_check(_adapter *padapter)
+{
+	if ((check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE)) ||
+	    (check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE))) {
+		if (padapter->stapriv.asoc_sta_count > 2)
+			return true;
+	} else {
+		/* Station mode */
+		if (check_fwstate(&padapter->mlmepriv, _FW_LINKED))
+			return true;
+	}
+	return false;
+}
+u8 rtw_is_adapter_up(_adapter *padapter)
+{
+	if (padapter == NULL)
+		return false;
+
+	if (RTW_CANNOT_RUN(padapter)) {
+		RTW_INFO(FUNC_ADPT_FMT "-(bSurpriseRemoved) || ( bDriverStopped == true)\n", FUNC_ADPT_ARG(padapter));
+		return false;
+	}
+
+	if (!rtw_is_hw_init_completed(padapter)) {
+		/*RTW_INFO(FUNC_ADPT_FMT "-(hw_init_completed == false)\n", FUNC_ADPT_ARG(padapter));*/
+		return false;
+	}
+
+	if (padapter->bup == false) {
+		/*RTW_INFO(FUNC_ADPT_FMT "-(bup == false)\n", FUNC_ADPT_ARG(padapter));*/
+		return false;
+	}
+
+	return true;
+}
+
+bool is_miracast_enabled(_adapter *adapter)
+{
+	bool enabled = 0;
+#ifdef CONFIG_WFD
+	struct wifi_display_info *wfdinfo = &adapter->wfd_info;
+
+	enabled = (wfdinfo->stack_wfd_mode & (MIRACAST_SOURCE | MIRACAST_SINK))
+		  || (wfdinfo->op_wfd_mode & (MIRACAST_SOURCE | MIRACAST_SINK));
+#endif
+
+	return enabled;
+}
+
+bool rtw_chk_miracast_mode(_adapter *adapter, u8 mode)
+{
+	bool ret = 0;
+#ifdef CONFIG_WFD
+	struct wifi_display_info *wfdinfo = &adapter->wfd_info;
+
+	ret = (wfdinfo->stack_wfd_mode & mode) || (wfdinfo->op_wfd_mode & mode);
+#endif
+
+	return ret;
+}
+
+const char *get_miracast_mode_str(int mode)
+{
+	if (mode == MIRACAST_SOURCE)
+		return "SOURCE";
+	else if (mode == MIRACAST_SINK)
+		return "SINK";
+	else if (mode == (MIRACAST_SOURCE | MIRACAST_SINK))
+		return "SOURCE&SINK";
+	else if (mode == MIRACAST_DISABLED)
+		return "DISABLED";
+	else
+		return "INVALID";
+}
+
+#ifdef CONFIG_WFD
+static bool wfd_st_match_rule(_adapter *adapter, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port)
+{
+	struct wifi_display_info *wfdinfo = &adapter->wfd_info;
+
+	if (ntohs(*((__be16 *)local_port)) == wfdinfo->rtsp_ctrlport ||
+	    ntohs(*((__be16 *)local_port)) == wfdinfo->tdls_rtsp_ctrlport ||
+	    ntohs(*((__be16 *)remote_port)) == wfdinfo->peer_rtsp_ctrlport)
+		return true;
+	return false;
+}
+
+static struct st_register wfd_st_reg = {
+	.s_proto = 0x06,
+	.rule = wfd_st_match_rule,
+};
+#endif /* CONFIG_WFD */
+
+inline void rtw_wfd_st_switch(struct sta_info *sta, bool on)
+{
+#ifdef CONFIG_WFD
+	if (on)
+		rtw_st_ctl_register(&sta->st_ctl, SESSION_TRACKER_REG_ID_WFD, &wfd_st_reg);
+	else
+		rtw_st_ctl_unregister(&sta->st_ctl, SESSION_TRACKER_REG_ID_WFD);
+#endif
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
new file mode 100755
index 000000000000..2e9878e0b12a
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
@@ -0,0 +1,15277 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_MLME_EXT_C_
+
+#include <drv_types.h>
+#ifdef CONFIG_IOCTL_CFG80211
+	#include <rtw_wifi_regd.h>
+#endif /* CONFIG_IOCTL_CFG80211 */
+#include <hal_data.h>
+
+
+static struct mlme_handler mlme_sta_tbl[] = {
+	{WIFI_ASSOCREQ,		"OnAssocReq",	&OnAssocReq},
+	{WIFI_ASSOCRSP,		"OnAssocRsp",	&OnAssocRsp},
+	{WIFI_REASSOCREQ,	"OnReAssocReq",	&OnAssocReq},
+	{WIFI_REASSOCRSP,	"OnReAssocRsp",	&OnAssocRsp},
+	{WIFI_PROBEREQ,		"OnProbeReq",	&OnProbeReq},
+	{WIFI_PROBERSP,		"OnProbeRsp",		&OnProbeRsp},
+
+	/*----------------------------------------------------------
+					below 2 are reserved
+	-----------------------------------------------------------*/
+	{0,					"DoReserved",		&DoReserved},
+	{0,					"DoReserved",		&DoReserved},
+	{WIFI_BEACON,		"OnBeacon",		&OnBeacon},
+	{WIFI_ATIM,			"OnATIM",		&OnAtim},
+	{WIFI_DISASSOC,		"OnDisassoc",		&OnDisassoc},
+	{WIFI_AUTH,			"OnAuth",		&OnAuthClient},
+	{WIFI_DEAUTH,		"OnDeAuth",		&OnDeAuth},
+	{WIFI_ACTION,		"OnAction",		&OnAction},
+	{WIFI_ACTION_NOACK, "OnActionNoAck",	&OnAction},
+};
+
+#ifdef _CONFIG_NATIVEAP_MLME_
+struct mlme_handler mlme_ap_tbl[] = {
+	{WIFI_ASSOCREQ,		"OnAssocReq",	&OnAssocReq},
+	{WIFI_ASSOCRSP,		"OnAssocRsp",	&OnAssocRsp},
+	{WIFI_REASSOCREQ,	"OnReAssocReq",	&OnAssocReq},
+	{WIFI_REASSOCRSP,	"OnReAssocRsp",	&OnAssocRsp},
+	{WIFI_PROBEREQ,		"OnProbeReq",	&OnProbeReq},
+	{WIFI_PROBERSP,		"OnProbeRsp",		&OnProbeRsp},
+
+	/*----------------------------------------------------------
+					below 2 are reserved
+	-----------------------------------------------------------*/
+	{0,					"DoReserved",		&DoReserved},
+	{0,					"DoReserved",		&DoReserved},
+	{WIFI_BEACON,		"OnBeacon",		&OnBeacon},
+	{WIFI_ATIM,			"OnATIM",		&OnAtim},
+	{WIFI_DISASSOC,		"OnDisassoc",		&OnDisassoc},
+	{WIFI_AUTH,			"OnAuth",		&OnAuth},
+	{WIFI_DEAUTH,		"OnDeAuth",		&OnDeAuth},
+	{WIFI_ACTION,		"OnAction",		&OnAction},
+	{WIFI_ACTION_NOACK, "OnActionNoAck",	&OnAction},
+};
+#endif
+
+static struct action_handler OnAction_tbl[] = {
+	{RTW_WLAN_CATEGORY_SPECTRUM_MGMT,	 "ACTION_SPECTRUM_MGMT", on_action_spct},
+	{RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos},
+	{RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls},
+	{RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
+	{RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
+	{RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
+	{RTW_WLAN_CATEGORY_FT, "ACTION_FT",	&OnAction_ft},
+	{RTW_WLAN_CATEGORY_HT,	"ACTION_HT",	&OnAction_ht},
+#ifdef CONFIG_IEEE80211W
+	{RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &OnAction_sa_query},
+#else
+	{RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved},
+#endif /* CONFIG_IEEE80211W */
+#ifdef CONFIG_RTW_WNM
+	{RTW_WLAN_CATEGORY_WNM, "ACTION_WNM", &on_action_wnm},
+#endif
+	{RTW_WLAN_CATEGORY_UNPROTECTED_WNM, "ACTION_UNPROTECTED_WNM", &DoReserved},
+	{RTW_WLAN_CATEGORY_SELF_PROTECTED, "ACTION_SELF_PROTECTED", &DoReserved},
+	{RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm},
+	{RTW_WLAN_CATEGORY_VHT, "ACTION_VHT", &OnAction_vht},
+	{RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p},
+};
+
+
+static u8	null_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
+
+/**************************************************
+OUI definitions for the vendor specific IE
+***************************************************/
+unsigned char	RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
+unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
+unsigned char	WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
+unsigned char	P2P_OUI[] = {0x50, 0x6F, 0x9A, 0x09};
+unsigned char	WFD_OUI[] = {0x50, 0x6F, 0x9A, 0x0A};
+
+unsigned char	WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+unsigned char	WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+
+unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
+unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
+
+#ifdef LEGACY_CHANNEL_PLAN_REF
+/********************************************************
+ChannelPlan definitions
+*********************************************************/
+static RT_CHANNEL_PLAN legacy_channel_plan[] = {
+	/* 0x00, RTW_CHPLAN_FCC */						{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165}, 32},
+	/* 0x01, RTW_CHPLAN_IC */						{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165}, 31},
+	/* 0x02, RTW_CHPLAN_ETSI */						{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 32},
+	/* 0x03, RTW_CHPLAN_SPAIN */						{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	/* 0x04, RTW_CHPLAN_FRANCE */					{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	/* 0x05, RTW_CHPLAN_MKK */						{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	/* 0x06, RTW_CHPLAN_MKK1 */						{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	/* 0x07, RTW_CHPLAN_ISRAEL */					{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52, 56, 60, 64}, 21},
+	/* 0x08, RTW_CHPLAN_TELEC */						{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 36, 40, 44, 48, 52, 56, 60, 64}, 22},
+	/* 0x09, RTW_CHPLAN_GLOBAL_DOAMIN */			{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14},
+	/* 0x0A, RTW_CHPLAN_WORLD_WIDE_13 */			{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},
+	/* 0x0B, RTW_CHPLAN_TAIWAN */					{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165}, 26},
+	/* 0x0C, RTW_CHPLAN_CHINA */					{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 149, 153, 157, 161, 165}, 18},
+	/* 0x0D, RTW_CHPLAN_SINGAPORE_INDIA_MEXICO */	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165}, 24},
+	/* 0x0E, RTW_CHPLAN_KOREA */					{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 149, 153, 157, 161, 165}, 31},
+	/* 0x0F, RTW_CHPLAN_TURKEY */					{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 36, 40, 44, 48, 52, 56, 60, 64}, 19},
+	/* 0x10, RTW_CHPLAN_JAPAN */						{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 32},
+	/* 0x11, RTW_CHPLAN_FCC_NO_DFS */				{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 36, 40, 44, 48, 149, 153, 157, 161, 165}, 20},
+	/* 0x12, RTW_CHPLAN_JAPAN_NO_DFS */				{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48}, 17},
+	/* 0x13, RTW_CHPLAN_WORLD_WIDE_5G */			{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165}, 37},
+	/* 0x14, RTW_CHPLAN_TAIWAN_NO_DFS */			{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 56, 60, 64, 149, 153, 157, 161, 165}, 19},
+};
+#endif
+
+static struct ch_list_t RTW_ChannelPlan2G[] = {
+	/* 0, RTW_RD_2G_NULL */		CH_LIST_ENT(0),
+	/* 1, RTW_RD_2G_WORLD */	CH_LIST_ENT(13, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13),
+	/* 2, RTW_RD_2G_ETSI1 */		CH_LIST_ENT(13, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13),
+	/* 3, RTW_RD_2G_FCC1 */		CH_LIST_ENT(11, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11),
+	/* 4, RTW_RD_2G_MKK1 */		CH_LIST_ENT(14, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14),
+	/* 5, RTW_RD_2G_ETSI2 */		CH_LIST_ENT(4, 10, 11, 12, 13),
+	/* 6, RTW_RD_2G_GLOBAL */	CH_LIST_ENT(14, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14),
+	/* 7, RTW_RD_2G_MKK2 */		CH_LIST_ENT(13, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13),
+	/* 8, RTW_RD_2G_FCC2 */		CH_LIST_ENT(13, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13),
+};
+
+#ifdef CONFIG_IEEE80211_BAND_5GHZ
+static struct ch_list_t RTW_ChannelPlan5G[] = {
+	/* 0, RTW_RD_5G_NULL */		CH_LIST_ENT(0),
+	/* 1, RTW_RD_5G_ETSI1 */		CH_LIST_ENT(19, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140),
+	/* 2, RTW_RD_5G_ETSI2 */		CH_LIST_ENT(24, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165),
+	/* 3, RTW_RD_5G_ETSI3 */		CH_LIST_ENT(22, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 149, 153, 157, 161, 165),
+	/* 4, RTW_RD_5G_FCC1 */		CH_LIST_ENT(24, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165),
+	/* 5, RTW_RD_5G_FCC2 */		CH_LIST_ENT(9, 36, 40, 44, 48, 149, 153, 157, 161, 165),
+	/* 6, RTW_RD_5G_FCC3 */		CH_LIST_ENT(13, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165),
+	/* 7, RTW_RD_5G_FCC4 */		CH_LIST_ENT(12, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161),
+	/* 8, RTW_RD_5G_FCC5 */		CH_LIST_ENT(5, 149, 153, 157, 161, 165),
+	/* 9, RTW_RD_5G_FCC6 */		CH_LIST_ENT(8, 36, 40, 44, 48, 52, 56, 60, 64),
+	/* 10, RTW_RD_5G_FCC7 */	CH_LIST_ENT(21, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165),
+	/* 11, RTW_RD_5G_KCC1 */	CH_LIST_ENT(19, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 149, 153, 157, 161),
+	/* 12, RTW_RD_5G_MKK1 */	CH_LIST_ENT(19, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140),
+	/* 13, RTW_RD_5G_MKK2 */	CH_LIST_ENT(8, 36, 40, 44, 48, 52, 56, 60, 64),
+	/* 14, RTW_RD_5G_MKK3 */	CH_LIST_ENT(11, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140),
+	/* 15, RTW_RD_5G_NCC1 */	CH_LIST_ENT(16, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165),
+	/* 16, RTW_RD_5G_NCC2 */	CH_LIST_ENT(8, 56, 60, 64, 149, 153, 157, 161, 165),
+	/* 17, RTW_RD_5G_NCC3 */	CH_LIST_ENT(5, 149, 153, 157, 161, 165),
+	/* 18, RTW_RD_5G_ETSI4 */	CH_LIST_ENT(4, 36, 40, 44, 48),
+	/* 19, RTW_RD_5G_ETSI5 */	CH_LIST_ENT(21, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165),
+	/* 20, RTW_RD_5G_FCC8 */	CH_LIST_ENT(4, 149, 153, 157, 161),
+	/* 21, RTW_RD_5G_ETSI6 */	CH_LIST_ENT(8, 36, 40, 44, 48, 52, 56, 60, 64),
+	/* 22, RTW_RD_5G_ETSI7 */	CH_LIST_ENT(13, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165),
+	/* 23, RTW_RD_5G_ETSI8 */	CH_LIST_ENT(9, 36, 40, 44, 48, 149, 153, 157, 161, 165),
+	/* 24, RTW_RD_5G_ETSI9 */	CH_LIST_ENT(11, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140),
+	/* 25, RTW_RD_5G_ETSI10 */	CH_LIST_ENT(5, 149, 153, 157, 161, 165),
+	/* 26, RTW_RD_5G_ETSI11 */	CH_LIST_ENT(16, 36, 40, 44, 48, 52, 56, 60, 64, 132, 136, 140, 149, 153, 157, 161, 165),
+	/* 27, RTW_RD_5G_NCC4 */	CH_LIST_ENT(17, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165),
+	/* 28, RTW_RD_5G_ETSI12 */	CH_LIST_ENT(4, 149, 153, 157, 161),
+	/* 29, RTW_RD_5G_FCC9 */	CH_LIST_ENT(21, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165),
+	/* 30, RTW_RD_5G_ETSI13 */	CH_LIST_ENT(16, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140),
+	/* 31, RTW_RD_5G_FCC10 */	CH_LIST_ENT(20, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161),
+	/* 32, RTW_RD_5G_MKK4 */	CH_LIST_ENT(4, 36, 40, 44, 48),
+	/* 33, RTW_RD_5G_ETSI14 */	CH_LIST_ENT(11, 36, 40, 44, 48, 52, 56, 60, 64, 132, 136, 140),
+	/* 34, RTW_RD_5G_FCC11 */	CH_LIST_ENT(25, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 144, 149, 153, 157, 161, 165),
+
+	/* === Below are driver defined for legacy channel plan compatible, NO static index assigned ==== */
+	/* RTW_RD_5G_OLD_FCC1 */	CH_LIST_ENT(20, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165),
+	/* RTW_RD_5G_OLD_NCC1 */	CH_LIST_ENT(15, 56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165),
+	/* RTW_RD_5G_OLD_KCC1 */	CH_LIST_ENT(20, 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 149, 153, 157, 161, 165),
+};
+#endif /* CONFIG_IEEE80211_BAND_5GHZ */
+
+static RT_CHANNEL_PLAN_MAP	RTW_ChannelPlanMap[] = {
+	/* ===== 0x00 ~ 0x1F, legacy channel plan ===== */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_KCC1,		TXPWR_LMT_FCC),		/* 0x00, RTW_CHPLAN_FCC */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_OLD_FCC1,	TXPWR_LMT_FCC),		/* 0x01, RTW_CHPLAN_IC */
+	CHPLAN_ENT(RTW_RD_2G_ETSI1,		RTW_RD_5G_ETSI1,	TXPWR_LMT_ETSI),	/* 0x02, RTW_CHPLAN_ETSI */
+	CHPLAN_ENT(RTW_RD_2G_ETSI1,		RTW_RD_5G_NULL,		TXPWR_LMT_ETSI),	/* 0x03, RTW_CHPLAN_SPAIN */
+	CHPLAN_ENT(RTW_RD_2G_ETSI1,		RTW_RD_5G_NULL,		TXPWR_LMT_ETSI),	/* 0x04, RTW_CHPLAN_FRANCE */
+	CHPLAN_ENT(RTW_RD_2G_MKK1,		RTW_RD_5G_NULL,		TXPWR_LMT_MKK),		/* 0x05, RTW_CHPLAN_MKK */
+	CHPLAN_ENT(RTW_RD_2G_MKK1,		RTW_RD_5G_NULL,		TXPWR_LMT_MKK),		/* 0x06, RTW_CHPLAN_MKK1 */
+	CHPLAN_ENT(RTW_RD_2G_ETSI1,		RTW_RD_5G_FCC6,		TXPWR_LMT_ETSI),	/* 0x07, RTW_CHPLAN_ISRAEL */
+	CHPLAN_ENT(RTW_RD_2G_MKK1,		RTW_RD_5G_FCC6,		TXPWR_LMT_MKK),		/* 0x08, RTW_CHPLAN_TELEC */
+	CHPLAN_ENT(RTW_RD_2G_MKK1,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x09, RTW_CHPLAN_GLOBAL_DOAMIN */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x0A, RTW_CHPLAN_WORLD_WIDE_13 */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_OLD_NCC1,	TXPWR_LMT_FCC),		/* 0x0B, RTW_CHPLAN_TAIWAN */
+	CHPLAN_ENT(RTW_RD_2G_ETSI1,		RTW_RD_5G_FCC5,		TXPWR_LMT_ETSI),	/* 0x0C, RTW_CHPLAN_CHINA */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_FCC3,		TXPWR_LMT_WW),		/* 0x0D, RTW_CHPLAN_SINGAPORE_INDIA_MEXICO */ /* ETSI:Singapore, India. FCC:Mexico => WW */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_OLD_KCC1,	TXPWR_LMT_ETSI),	/* 0x0E, RTW_CHPLAN_KOREA */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_FCC6,		TXPWR_LMT_ETSI),	/* 0x0F, RTW_CHPLAN_TURKEY */
+	CHPLAN_ENT(RTW_RD_2G_ETSI1,		RTW_RD_5G_ETSI1,	TXPWR_LMT_MKK),		/* 0x10, RTW_CHPLAN_JAPAN */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_FCC2,		TXPWR_LMT_FCC),		/* 0x11, RTW_CHPLAN_FCC_NO_DFS */
+	CHPLAN_ENT(RTW_RD_2G_ETSI1,		RTW_RD_5G_FCC7,		TXPWR_LMT_MKK),		/* 0x12, RTW_CHPLAN_JAPAN_NO_DFS */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_FCC1,		TXPWR_LMT_WW),		/* 0x13, RTW_CHPLAN_WORLD_WIDE_5G */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_NCC2,		TXPWR_LMT_FCC),		/* 0x14, RTW_CHPLAN_TAIWAN_NO_DFS */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_FCC7,		TXPWR_LMT_ETSI),	/* 0x15, RTW_CHPLAN_ETSI_NO_DFS */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_NCC1,		TXPWR_LMT_ETSI),	/* 0x16, RTW_CHPLAN_KOREA_NO_DFS */
+	CHPLAN_ENT(RTW_RD_2G_MKK1,		RTW_RD_5G_FCC7,		TXPWR_LMT_MKK),		/* 0x17, RTW_CHPLAN_JAPAN_NO_DFS */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_FCC5,		TXPWR_LMT_ETSI),	/* 0x18, RTW_CHPLAN_PAKISTAN_NO_DFS */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_FCC5,		TXPWR_LMT_FCC),		/* 0x19, RTW_CHPLAN_TAIWAN2_NO_DFS */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x1A, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x1B, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x1C, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x1D, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x1E, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_FCC1,		TXPWR_LMT_WW),		/* 0x1F, RTW_CHPLAN_WORLD_WIDE_ONLY_5G */
+
+	/* ===== 0x20 ~ 0x7F, new channel plan ===== */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x20, RTW_CHPLAN_WORLD_NULL */
+	CHPLAN_ENT(RTW_RD_2G_ETSI1,		RTW_RD_5G_NULL,		TXPWR_LMT_ETSI),	/* 0x21, RTW_CHPLAN_ETSI1_NULL */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_NULL,		TXPWR_LMT_FCC),		/* 0x22, RTW_CHPLAN_FCC1_NULL */
+	CHPLAN_ENT(RTW_RD_2G_MKK1,		RTW_RD_5G_NULL,		TXPWR_LMT_MKK),		/* 0x23, RTW_CHPLAN_MKK1_NULL */
+	CHPLAN_ENT(RTW_RD_2G_ETSI2,		RTW_RD_5G_NULL,		TXPWR_LMT_ETSI),	/* 0x24, RTW_CHPLAN_ETSI2_NULL */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_FCC1,		TXPWR_LMT_FCC),		/* 0x25, RTW_CHPLAN_FCC1_FCC1 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI1,	TXPWR_LMT_ETSI),	/* 0x26, RTW_CHPLAN_WORLD_ETSI1 */
+	CHPLAN_ENT(RTW_RD_2G_MKK1,		RTW_RD_5G_MKK1,		TXPWR_LMT_MKK),		/* 0x27, RTW_CHPLAN_MKK1_MKK1 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_KCC1,		TXPWR_LMT_ETSI),	/* 0x28, RTW_CHPLAN_WORLD_KCC1 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_FCC2,		TXPWR_LMT_FCC),		/* 0x29, RTW_CHPLAN_WORLD_FCC2 */
+	CHPLAN_ENT(RTW_RD_2G_FCC2,		RTW_RD_5G_NULL,		TXPWR_LMT_FCC),		/* 0x2A, RTW_CHPLAN_FCC2_NULL */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x2B, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x2C, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x2D, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x2E, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x2F, */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_FCC3,		TXPWR_LMT_FCC),		/* 0x30, RTW_CHPLAN_WORLD_FCC3 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_FCC4,		TXPWR_LMT_FCC),		/* 0x31, RTW_CHPLAN_WORLD_FCC4 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_FCC5,		TXPWR_LMT_FCC),		/* 0x32, RTW_CHPLAN_WORLD_FCC5 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_FCC6,		TXPWR_LMT_FCC),		/* 0x33, RTW_CHPLAN_WORLD_FCC6 */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_FCC7,		TXPWR_LMT_FCC),		/* 0x34, RTW_CHPLAN_FCC1_FCC7 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI2,	TXPWR_LMT_ETSI),	/* 0x35, RTW_CHPLAN_WORLD_ETSI2 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI3,	TXPWR_LMT_ETSI),	/* 0x36, RTW_CHPLAN_WORLD_ETSI3 */
+	CHPLAN_ENT(RTW_RD_2G_MKK1,		RTW_RD_5G_MKK2,		TXPWR_LMT_MKK),		/* 0x37, RTW_CHPLAN_MKK1_MKK2 */
+	CHPLAN_ENT(RTW_RD_2G_MKK1,		RTW_RD_5G_MKK3,		TXPWR_LMT_MKK),		/* 0x38, RTW_CHPLAN_MKK1_MKK3 */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_NCC1,		TXPWR_LMT_FCC),		/* 0x39, RTW_CHPLAN_FCC1_NCC1 */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x3A, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x3B, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x3C, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x3D, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x3E, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x3F, */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_NCC2,		TXPWR_LMT_FCC),		/* 0x40, RTW_CHPLAN_FCC1_NCC2 */
+	CHPLAN_ENT(RTW_RD_2G_GLOBAL,	RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x41, RTW_CHPLAN_GLOBAL_NULL */
+	CHPLAN_ENT(RTW_RD_2G_ETSI1,		RTW_RD_5G_ETSI4,	TXPWR_LMT_ETSI),	/* 0x42, RTW_CHPLAN_ETSI1_ETSI4 */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_FCC2,		TXPWR_LMT_FCC),		/* 0x43, RTW_CHPLAN_FCC1_FCC2 */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_NCC3,		TXPWR_LMT_FCC),		/* 0x44, RTW_CHPLAN_FCC1_NCC3 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI5,	TXPWR_LMT_ETSI),	/* 0x45, RTW_CHPLAN_WORLD_ETSI5 */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_FCC8,		TXPWR_LMT_FCC),		/* 0x46, RTW_CHPLAN_FCC1_FCC8 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI6,	TXPWR_LMT_ETSI),	/* 0x47, RTW_CHPLAN_WORLD_ETSI6 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI7,	TXPWR_LMT_ETSI),	/* 0x48, RTW_CHPLAN_WORLD_ETSI7 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI8,	TXPWR_LMT_ETSI),	/* 0x49, RTW_CHPLAN_WORLD_ETSI8 */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x4A, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x4B, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x4C, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x4D, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x4E, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x4F, */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI9,	TXPWR_LMT_ETSI),	/* 0x50, RTW_CHPLAN_WORLD_ETSI9 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI10,	TXPWR_LMT_ETSI),	/* 0x51, RTW_CHPLAN_WORLD_ETSI10 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI11,	TXPWR_LMT_ETSI),	/* 0x52, RTW_CHPLAN_WORLD_ETSI11 */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_NCC4,		TXPWR_LMT_FCC),		/* 0x53, RTW_CHPLAN_FCC1_NCC4 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI12,	TXPWR_LMT_ETSI),	/* 0x54, RTW_CHPLAN_WORLD_ETSI12 */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_FCC9,		TXPWR_LMT_FCC),		/* 0x55, RTW_CHPLAN_FCC1_FCC9 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI13,	TXPWR_LMT_ETSI),	/* 0x56, RTW_CHPLAN_WORLD_ETSI13 */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_FCC10,	TXPWR_LMT_FCC),		/* 0x57, RTW_CHPLAN_FCC1_FCC10 */
+	CHPLAN_ENT(RTW_RD_2G_MKK2,		RTW_RD_5G_MKK4,		TXPWR_LMT_MKK),		/* 0x58, RTW_CHPLAN_MKK2_MKK4 */
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_ETSI14,	TXPWR_LMT_ETSI),	/* 0x59, RTW_CHPLAN_WORLD_ETSI14 */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x5A, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x5B, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x5C, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x5D, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x5E, */
+	CHPLAN_ENT(RTW_RD_2G_NULL,		RTW_RD_5G_NULL,		TXPWR_LMT_WW),		/* 0x5F, */
+	CHPLAN_ENT(RTW_RD_2G_FCC1,		RTW_RD_5G_FCC5,		TXPWR_LMT_FCC),		/* 0x60, RTW_CHPLAN_FCC1_FCC5 */
+	CHPLAN_ENT(RTW_RD_2G_FCC2,		RTW_RD_5G_FCC7,		TXPWR_LMT_FCC),		/* 0x61, RTW_CHPLAN_FCC2_FCC7 */
+	CHPLAN_ENT(RTW_RD_2G_FCC2,		RTW_RD_5G_FCC1,		TXPWR_LMT_FCC),		/* 0x62, RTW_CHPLAN_FCC2_FCC1 */
+};
+
+static RT_CHANNEL_PLAN_MAP RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE =
+	CHPLAN_ENT(RTW_RD_2G_WORLD,		RTW_RD_5G_FCC1,		TXPWR_LMT_FCC);		/* 0x7F, Realtek Define */
+
+bool rtw_chplan_is_empty(u8 id)
+{
+	RT_CHANNEL_PLAN_MAP *chplan_map;
+
+	if (id == RTW_CHPLAN_REALTEK_DEFINE)
+		chplan_map = &RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE;
+	else
+		chplan_map = &RTW_ChannelPlanMap[id];
+
+	if (chplan_map->Index2G == RTW_RD_2G_NULL
+		#ifdef CONFIG_IEEE80211_BAND_5GHZ
+		&& chplan_map->Index5G == RTW_RD_5G_NULL
+		#endif
+	)
+		return true;
+
+	return false;
+}
+
+void rtw_rfctl_init(_adapter *adapter)
+{
+	struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter);
+
+	memset(rfctl, 0, sizeof(*rfctl));
+
+#ifdef CONFIG_DFS_MASTER
+	rfctl->cac_start_time = rfctl->cac_end_time = RTW_CAC_STOPPED;
+
+	/* TODO: dfs_master_timer */
+#endif
+}
+
+#ifdef CONFIG_DFS_MASTER
+/*
+* called in rtw_dfs_master_enable()
+* assume the request channel coverage is DFS range
+* base on the current status and the request channel coverage to check if need to reset complete CAC time
+*/
+bool rtw_is_cac_reset_needed(_adapter *adapter, u8 ch, u8 bw, u8 offset)
+{
+	struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter);
+	bool needed = false;
+	u32 cur_hi, cur_lo, hi, lo;
+
+	if (rfctl->radar_detected == 1) {
+		needed = true;
+		goto exit;
+	}
+
+	if (rfctl->radar_detect_ch == 0) {
+		needed = true;
+		goto exit;
+	}
+
+	if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == false) {
+		RTW_ERR("request detection range ch:%u, bw:%u, offset:%u\n", ch, bw, offset);
+		rtw_warn_on(1);
+	}
+
+	if (rtw_chbw_to_freq_range(rfctl->radar_detect_ch, rfctl->radar_detect_bw, rfctl->radar_detect_offset, &cur_hi, &cur_lo) == false) {
+		RTW_ERR("cur detection range ch:%u, bw:%u, offset:%u\n", rfctl->radar_detect_ch, rfctl->radar_detect_bw, rfctl->radar_detect_offset);
+		rtw_warn_on(1);
+	}
+
+	if (hi <= lo || cur_hi <= cur_lo) {
+		RTW_ERR("hi:%u, lo:%u, cur_hi:%u, cur_lo:%u\n", hi, lo, cur_hi, cur_lo);
+		rtw_warn_on(1);
+	}
+
+	if (rtw_is_range_a_in_b(hi, lo, cur_hi, cur_lo)) {
+		/* request is in current detect range */
+		goto exit;
+	}
+
+	/* check if request channel coverage has new range and the new range is in DFS range */
+	if (!rtw_is_range_overlap(hi, lo, cur_hi, cur_lo)) {
+		/* request has no overlap with current */
+		needed = true;
+	} else if (rtw_is_range_a_in_b(cur_hi, cur_lo, hi, lo)) {
+		/* request is supper set of current */
+		if ((hi != cur_hi && rtw_is_dfs_range(hi, cur_hi)) || (lo != cur_lo && rtw_is_dfs_range(cur_lo, lo)))
+			needed = true;
+	} else {
+		/* request is not supper set of current, but has overlap */
+		if ((lo < cur_lo && rtw_is_dfs_range(cur_lo, lo)) || (hi > cur_hi && rtw_is_dfs_range(hi, cur_hi)))
+			needed = true;
+	}
+
+exit:
+	return needed;
+}
+
+bool _rtw_rfctl_overlap_radar_detect_ch(struct rf_ctl_t *rfctl, u8 ch, u8 bw, u8 offset)
+{
+	bool ret = false;
+	u32 hi = 0, lo = 0;
+	u32 r_hi = 0, r_lo = 0;
+	int i;
+
+	if (rfctl->radar_detect_by_others)
+		goto exit;
+
+	if (rfctl->radar_detect_ch == 0)
+		goto exit;
+
+	if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == false) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	if (rtw_chbw_to_freq_range(rfctl->radar_detect_ch
+			, rfctl->radar_detect_bw, rfctl->radar_detect_offset
+			, &r_hi, &r_lo) == false) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	if (rtw_is_range_overlap(hi, lo, r_hi, r_lo))
+		ret = true;
+
+exit:
+	return ret;
+}
+
+bool rtw_rfctl_overlap_radar_detect_ch(struct rf_ctl_t *rfctl)
+{
+	return _rtw_rfctl_overlap_radar_detect_ch(rfctl
+				, rfctl_to_dvobj(rfctl)->oper_channel
+				, rfctl_to_dvobj(rfctl)->oper_bwmode
+				, rfctl_to_dvobj(rfctl)->oper_ch_offset);
+}
+
+bool rtw_rfctl_is_tx_blocked_by_ch_waiting(struct rf_ctl_t *rfctl)
+{
+	return rtw_rfctl_overlap_radar_detect_ch(rfctl) && IS_CH_WAITING(rfctl);
+}
+
+bool rtw_chset_is_ch_non_ocp(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset)
+{
+	bool ret = false;
+	u32 hi = 0, lo = 0;
+	int i;
+
+	if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == false)
+		goto exit;
+
+	for (i = 0; ch_set[i].ChannelNum != 0; i++) {
+		if (!rtw_ch2freq(ch_set[i].ChannelNum)) {
+			rtw_warn_on(1);
+			continue;
+		}
+
+		if (!CH_IS_NON_OCP(&ch_set[i]))
+			continue;
+
+		if (lo <= rtw_ch2freq(ch_set[i].ChannelNum)
+			&& rtw_ch2freq(ch_set[i].ChannelNum) <= hi
+		) {
+			ret = true;
+			break;
+		}
+	}
+
+exit:
+	return ret;
+}
+
+u32 rtw_chset_get_ch_non_ocp_ms(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset)
+{
+	int ms = 0;
+	u32 current_time;
+	u32 hi = 0, lo = 0;
+	int i;
+
+	if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == false)
+		goto exit;
+
+	current_time = jiffies;
+
+	for (i = 0; ch_set[i].ChannelNum != 0; i++) {
+		if (!rtw_ch2freq(ch_set[i].ChannelNum)) {
+			rtw_warn_on(1);
+			continue;
+		}
+
+		if (!CH_IS_NON_OCP(&ch_set[i]))
+			continue;
+
+		if (lo <= rtw_ch2freq(ch_set[i].ChannelNum)
+			&& rtw_ch2freq(ch_set[i].ChannelNum) <= hi
+		) {
+			if (rtw_systime_to_ms(ch_set[i].non_ocp_end_time - current_time) > ms)
+				ms = rtw_systime_to_ms(ch_set[i].non_ocp_end_time - current_time);
+		}
+	}
+
+exit:
+	return ms;
+}
+
+/**
+ * rtw_chset_update_non_ocp - update non_ocp_end_time according to the given @ch, @bw, @offset into @ch_set
+ * @ch_set: the given channel set
+ * @ch: channel number on which radar is detected
+ * @bw: bandwidth on which radar is detected
+ * @offset: bandwidth offset on which radar is detected
+ * @ms: ms to add from now to update non_ocp_end_time, ms < 0 means use NON_OCP_TIME_MS
+ */
+static void _rtw_chset_update_non_ocp(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset, int ms)
+{
+	u32 hi = 0, lo = 0;
+	int i;
+
+	if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == false)
+		goto exit;
+
+	for (i = 0; ch_set[i].ChannelNum != 0; i++) {
+		if (!rtw_ch2freq(ch_set[i].ChannelNum)) {
+			rtw_warn_on(1);
+			continue;
+		}
+
+		if (lo <= rtw_ch2freq(ch_set[i].ChannelNum)
+			&& rtw_ch2freq(ch_set[i].ChannelNum) <= hi
+		) {
+			if (ms >= 0)
+				ch_set[i].non_ocp_end_time = jiffies + rtw_ms_to_systime(ms);
+			else
+				ch_set[i].non_ocp_end_time = jiffies + rtw_ms_to_systime(NON_OCP_TIME_MS);
+		}
+	}
+
+exit:
+	return;
+}
+
+inline void rtw_chset_update_non_ocp(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset)
+{
+	_rtw_chset_update_non_ocp(ch_set, ch, bw, offset, -1);
+}
+
+inline void rtw_chset_update_non_ocp_ms(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset, int ms)
+{
+	_rtw_chset_update_non_ocp(ch_set, ch, bw, offset, ms);
+}
+
+u32 rtw_get_ch_waiting_ms(_adapter *adapter, u8 ch, u8 bw, u8 offset, u32 *r_non_ocp_ms, u32 *r_cac_ms)
+{
+	struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter);
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+	u32 non_ocp_ms;
+	u32 cac_ms;
+	u8 in_rd_range = 0; /* if in current radar detection range*/
+
+	if (rtw_chset_is_ch_non_ocp(mlmeext->channel_set, ch, bw, offset))
+		non_ocp_ms = rtw_chset_get_ch_non_ocp_ms(mlmeext->channel_set, ch, bw, offset);
+	else
+		non_ocp_ms = 0;
+
+	if (rfctl->dfs_master_enabled) {
+		u32 cur_hi, cur_lo, hi, lo;
+
+		if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == false) {
+			RTW_ERR("input range ch:%u, bw:%u, offset:%u\n", ch, bw, offset);
+			rtw_warn_on(1);
+		}
+
+		if (rtw_chbw_to_freq_range(rfctl->radar_detect_ch, rfctl->radar_detect_bw, rfctl->radar_detect_offset, &cur_hi, &cur_lo) == false) {
+			RTW_ERR("cur detection range ch:%u, bw:%u, offset:%u\n", rfctl->radar_detect_ch, rfctl->radar_detect_bw, rfctl->radar_detect_offset);
+			rtw_warn_on(1);
+		}
+
+		if (rtw_is_range_a_in_b(hi, lo, cur_hi, cur_lo))
+			in_rd_range = 1;
+	}
+
+	if (!rtw_is_dfs_ch(ch, bw, offset))
+		cac_ms = 0;
+	else if (in_rd_range && !non_ocp_ms) {
+		if (IS_CH_WAITING(rfctl))
+			cac_ms = rtw_systime_to_ms(rfctl->cac_end_time - jiffies);
+		else
+			cac_ms = 0;
+	} else if (rtw_is_long_cac_ch(ch, bw, offset, rtw_odm_get_dfs_domain(adapter)))
+		cac_ms = CAC_TIME_CE_MS;
+	else
+		cac_ms = CAC_TIME_MS;
+
+	if (r_non_ocp_ms)
+		*r_non_ocp_ms = non_ocp_ms;
+	if (r_cac_ms)
+		*r_cac_ms = cac_ms;
+
+	return non_ocp_ms + cac_ms;
+}
+
+void rtw_reset_cac(_adapter *adapter, u8 ch, u8 bw, u8 offset)
+{
+	struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter);
+	u32 non_ocp_ms;
+	u32 cac_ms;
+
+	rtw_get_ch_waiting_ms(adapter
+		, ch
+		, bw
+		, offset
+		, &non_ocp_ms
+		, &cac_ms
+	);
+
+	rfctl->cac_start_time = jiffies + rtw_ms_to_systime(non_ocp_ms);
+	rfctl->cac_end_time = rfctl->cac_start_time + rtw_ms_to_systime(cac_ms);
+
+	/* skip special value */
+	if (rfctl->cac_start_time == RTW_CAC_STOPPED) {
+		rfctl->cac_start_time++;
+		rfctl->cac_end_time++;
+	}
+	if (rfctl->cac_end_time == RTW_CAC_STOPPED)
+		rfctl->cac_end_time++;
+}
+#endif /* CONFIG_DFS_MASTER */
+
+/* choose channel with shortest waiting (non ocp + cac) time */
+bool rtw_choose_shortest_waiting_ch(_adapter *adapter, u8 req_bw, u8 *dec_ch, u8 *dec_bw, u8 *dec_offset, u8 d_flags)
+{
+#ifndef DBG_CHOOSE_SHORTEST_WAITING_CH
+#define DBG_CHOOSE_SHORTEST_WAITING_CH 0
+#endif
+
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+	struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter);
+	struct registry_priv *regsty = adapter_to_regsty(adapter);
+	u8 ch, bw, offset;
+	u8 ch_c = 0, bw_c = 0, offset_c = 0;
+	int i;
+	u32 min_waiting_ms = 0;
+
+	if (!dec_ch || !dec_bw || !dec_offset) {
+		rtw_warn_on(1);
+		return false;
+	}
+
+	/* full search and narrow bw judegement first to avoid potetial judegement timing issue */
+	for (bw = CHANNEL_WIDTH_20; bw <= req_bw; bw++) {
+		if (!hal_is_bw_support(adapter, bw))
+			continue;
+
+		for (i = 0; i < mlmeext->max_chan_nums; i++) {
+			u32 non_ocp_ms = 0;
+			u32 cac_ms = 0;
+			u32 waiting_ms = 0;
+
+			ch = mlmeext->channel_set[i].ChannelNum;
+
+			if ((d_flags & RTW_CHF_2G) && ch <= 14)
+				continue;
+
+			if ((d_flags & RTW_CHF_5G) && ch > 14)
+				continue;
+
+			if (ch > 14) {
+				if (bw > REGSTY_BW_5G(regsty))
+					continue;
+			} else {
+				if (bw > REGSTY_BW_2G(regsty))
+					continue;
+			}
+
+			if (!rtw_get_offset_by_chbw(ch, bw, &offset))
+				continue;
+
+			if (!rtw_chset_is_chbw_valid(mlmeext->channel_set, ch, bw, offset))
+				continue;
+
+			if ((d_flags & RTW_CHF_NON_OCP) && rtw_chset_is_ch_non_ocp(mlmeext->channel_set, ch, bw, offset))
+				continue;
+
+			if ((d_flags & RTW_CHF_DFS) && rtw_is_dfs_ch(ch, bw, offset))
+				continue;
+
+			if ((d_flags & RTW_CHF_LONG_CAC) && rtw_is_long_cac_ch(ch, bw, offset, rtw_odm_get_dfs_domain(adapter)))
+				continue;
+
+			if ((d_flags & RTW_CHF_NON_DFS) && !rtw_is_dfs_ch(ch, bw, offset))
+				continue;
+
+			if ((d_flags & RTW_CHF_NON_LONG_CAC) && !rtw_is_long_cac_ch(ch, bw, offset, rtw_odm_get_dfs_domain(adapter)))
+				continue;
+
+			#ifdef CONFIG_DFS_MASTER
+			waiting_ms = rtw_get_ch_waiting_ms(adapter, ch, bw, offset, &non_ocp_ms, &cac_ms);
+			#endif
+
+			if (DBG_CHOOSE_SHORTEST_WAITING_CH)
+				RTW_INFO(FUNC_ADPT_FMT":%u,%u,%u %u(non_ocp:%u, cac:%u)\n"
+					, FUNC_ADPT_ARG(adapter), ch, bw, offset, waiting_ms, non_ocp_ms, cac_ms);
+
+			if (ch_c == 0
+				|| min_waiting_ms > waiting_ms
+				|| (min_waiting_ms == waiting_ms && bw > bw_c) /* wider bw first */
+			) {
+				ch_c = ch;
+				bw_c = bw;
+				offset_c = offset;
+				min_waiting_ms = waiting_ms;
+			}
+		}
+	}
+
+	if (ch_c != 0) {
+		RTW_INFO(FUNC_ADPT_FMT": d_flags:0x%02x %u,%u,%u waiting_ms:%u\n"
+			, FUNC_ADPT_ARG(adapter), d_flags, ch_c, bw_c, offset_c, min_waiting_ms);
+
+		*dec_ch = ch_c;
+		*dec_bw = bw_c;
+		*dec_offset = offset_c;
+		return true;
+	}
+
+	if (d_flags == 0)
+		rtw_warn_on(1);
+
+	return false;
+}
+
+void dump_country_chplan(void *sel, const struct country_chplan *ent)
+{
+	_RTW_PRINT_SEL(sel, "\"%c%c\", 0x%02X%s\n"
+		, ent->alpha2[0], ent->alpha2[1], ent->chplan
+		, COUNTRY_CHPLAN_EN_11AC(ent) ? " ac" : ""
+	);
+}
+
+void dump_country_chplan_map(void *sel)
+{
+	const struct country_chplan *ent;
+	u8 code[2];
+
+#if RTW_DEF_MODULE_REGULATORY_CERT
+	_RTW_PRINT_SEL(sel, "RTW_DEF_MODULE_REGULATORY_CERT:0x%x\n", RTW_DEF_MODULE_REGULATORY_CERT);
+#endif
+#ifdef CONFIG_CUSTOMIZED_COUNTRY_CHPLAN_MAP
+	_RTW_PRINT_SEL(sel, "CONFIG_CUSTOMIZED_COUNTRY_CHPLAN_MAP\n");
+#endif
+
+	for (code[0] = 'A'; code[0] <= 'Z'; code[0]++) {
+		for (code[1] = 'A'; code[1] <= 'Z'; code[1]++) {
+			ent = rtw_get_chplan_from_country(code);
+			if (!ent)
+				continue;
+
+			dump_country_chplan(sel, ent);
+		}
+	}
+}
+
+void dump_chplan_id_list(void *sel)
+{
+	int i;
+
+	for (i = 0; i < RTW_CHPLAN_MAX; i++) {
+		if (!rtw_is_channel_plan_valid(i))
+			continue;
+
+		_RTW_PRINT_SEL(sel, "0x%02X ", i);
+	}
+
+	RTW_PRINT_SEL(sel, "0x7F\n");
+}
+
+void dump_chplan_test(void *sel)
+{
+	int i, j;
+
+	/* check invalid channel */
+	for (i = 0; i < RTW_RD_2G_MAX; i++) {
+		for (j = 0; j < CH_LIST_LEN(RTW_ChannelPlan2G[i]); j++) {
+			if (rtw_ch2freq(CH_LIST_CH(RTW_ChannelPlan2G[i], j)) == 0)
+				RTW_PRINT_SEL(sel, "invalid ch:%u at (%d,%d)\n", CH_LIST_CH(RTW_ChannelPlan2G[i], j), i, j);
+		}
+	}
+
+#ifdef CONFIG_IEEE80211_BAND_5GHZ
+	for (i = 0; i < RTW_RD_5G_MAX; i++) {
+		for (j = 0; j < CH_LIST_LEN(RTW_ChannelPlan5G[i]); j++) {
+			if (rtw_ch2freq(CH_LIST_CH(RTW_ChannelPlan5G[i], j)) == 0)
+				RTW_PRINT_SEL(sel, "invalid ch:%u at (%d,%d)\n", CH_LIST_CH(RTW_ChannelPlan5G[i], j), i, j);
+		}
+	}
+#endif
+}
+
+void dump_chset(void *sel, RT_CHANNEL_INFO *ch_set)
+{
+	u8	i;
+
+	for (i = 0; ch_set[i].ChannelNum != 0; i++) {
+		RTW_PRINT_SEL(sel, "ch:%3u, freq:%u, scan_type:%d"
+			, ch_set[i].ChannelNum, rtw_ch2freq(ch_set[i].ChannelNum), ch_set[i].ScanType);
+
+#ifdef CONFIG_FIND_BEST_CHANNEL
+		_RTW_PRINT_SEL(sel, ", rx_count:%u", ch_set[i].rx_count);
+#endif
+
+#ifdef CONFIG_DFS_MASTER
+		if (rtw_is_dfs_ch(ch_set[i].ChannelNum, CHANNEL_WIDTH_20, HAL_PRIME_CHNL_OFFSET_DONT_CARE)) {
+			if (CH_IS_NON_OCP(&ch_set[i]))
+				_RTW_PRINT_SEL(sel, ", non_ocp:%d"
+					, rtw_systime_to_ms(ch_set[i].non_ocp_end_time - jiffies));
+			else
+				_RTW_PRINT_SEL(sel, ", non_ocp:N/A");
+		}
+#endif
+
+		_RTW_PRINT_SEL(sel, "\n");
+	}
+
+	RTW_PRINT_SEL(sel, "total ch number:%d\n", i);
+}
+
+void dump_cur_chset(void *sel, _adapter *adapter)
+{
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+	struct registry_priv *regsty = adapter_to_regsty(adapter);
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+	int i;
+
+	if (mlme->country_ent)
+		dump_country_chplan(sel, mlme->country_ent);
+	else
+		RTW_PRINT_SEL(sel, "chplan:0x%02X\n", mlme->ChannelPlan);
+
+	RTW_PRINT_SEL(sel, "2G_PLS:%u, 5G_PLS:%u\n"
+		, hal_data->Regulation2_4G, hal_data->Regulation5G);
+
+#ifdef CONFIG_DFS_MASTER
+	RTW_PRINT_SEL(sel, "dfs_domain:%u\n", rtw_odm_get_dfs_domain(adapter));
+#endif
+
+	for (i = 0; i < MAX_CHANNEL_NUM; i++)
+		if (regsty->excl_chs[i] != 0)
+			break;
+
+	if (i < MAX_CHANNEL_NUM) {
+		_RTW_PRINT_SEL(sel, "excl_chs:");
+		for (i = 0; i < MAX_CHANNEL_NUM; i++) {
+			if (regsty->excl_chs[i] == 0)
+				break;
+			_RTW_PRINT_SEL(sel, "%u ", regsty->excl_chs[i]);
+		}
+		RTW_PRINT_SEL(sel, "\n");
+	}
+
+	dump_chset(sel, mlmeext->channel_set);
+}
+
+/*
+ * Search the @param ch in given @param ch_set
+ * @ch_set: the given channel set
+ * @ch: the given channel number
+ *
+ * return the index of channel_num in channel_set, -1 if not found
+ */
+int rtw_ch_set_search_ch(RT_CHANNEL_INFO *ch_set, const u32 ch)
+{
+	int i;
+	for (i = 0; ch_set[i].ChannelNum != 0; i++) {
+		if (ch == ch_set[i].ChannelNum)
+			break;
+	}
+
+	if (i >= ch_set[i].ChannelNum)
+		return -1;
+	return i;
+}
+
+/*
+ * Check if the @param ch, bw, offset is valid for the given @param ch_set
+ * @ch_set: the given channel set
+ * @ch: the given channel number
+ * @bw: the given bandwidth
+ * @offset: the given channel offset
+ *
+ * return valid (1) or not (0)
+ */
+u8 rtw_chset_is_chbw_valid(RT_CHANNEL_INFO *ch_set, u8 ch, u8 bw, u8 offset)
+{
+	u8 cch;
+	u8 *op_chs;
+	u8 op_ch_num;
+	u8 valid = 0;
+	int i;
+
+	cch = rtw_get_center_ch(ch, bw, offset);
+
+	if (!rtw_get_op_chs_by_cch_bw(cch, bw, &op_chs, &op_ch_num))
+		goto exit;
+
+	for (i = 0; i < op_ch_num; i++) {
+		if (0)
+			RTW_INFO("%u,%u,%u - cch:%u, bw:%u, op_ch:%u\n", ch, bw, offset, cch, bw, *(op_chs + i));
+		if (rtw_ch_set_search_ch(ch_set, *(op_chs + i)) == -1)
+			break;
+	}
+
+	if (op_ch_num != 0 && i == op_ch_num)
+		valid = 1;
+
+exit:
+	return valid;
+}
+
+/*
+ * Check the @param ch is fit with setband setting of @param adapter
+ * @adapter: the given adapter
+ * @ch: the given channel number
+ *
+ * return true when check valid, false not valid
+ */
+bool rtw_mlme_band_check(_adapter *adapter, const u32 ch)
+{
+	if (adapter->setband == WIFI_FREQUENCY_BAND_AUTO /* 2.4G and 5G */
+		|| (adapter->setband == WIFI_FREQUENCY_BAND_2GHZ && ch < 35) /* 2.4G only */
+		|| (adapter->setband == WIFI_FREQUENCY_BAND_5GHZ && ch > 35) /* 5G only */
+	)
+		return true;
+	return false;
+}
+inline void RTW_SET_SCAN_BAND_SKIP(_adapter *padapter, int skip_band)
+{
+	int bs = ATOMIC_READ(&padapter->bandskip);
+
+	bs |= skip_band;
+	ATOMIC_SET(&padapter->bandskip, bs);
+}
+
+inline void RTW_CLR_SCAN_BAND_SKIP(_adapter *padapter, int skip_band)
+{
+	int bs = ATOMIC_READ(&padapter->bandskip);
+
+	bs &= ~(skip_band);
+	ATOMIC_SET(&padapter->bandskip, bs);
+}
+inline int RTW_GET_SCAN_BAND_SKIP(_adapter *padapter)
+{
+	return ATOMIC_READ(&padapter->bandskip);
+}
+
+#define RTW_IS_SCAN_BAND_SKIP(padapter, skip_band) (ATOMIC_READ(&padapter->bandskip) & (skip_band))
+
+bool rtw_mlme_ignore_chan(_adapter *adapter, const u32 ch)
+{
+	if (RTW_IS_SCAN_BAND_SKIP(adapter, BAND_24G) && ch < 35) /* SKIP 2.4G Band channel */
+		return true;
+	if (RTW_IS_SCAN_BAND_SKIP(adapter, BAND_5G)  && ch > 35) /* SKIP 5G Band channel */
+		return true;
+
+	return false;
+}
+
+
+/****************************************************************************
+
+Following are the initialization functions for WiFi MLME
+
+*****************************************************************************/
+
+int init_hw_mlme_ext(_adapter *padapter)
+{
+	struct	mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	/* set_opmode_cmd(padapter, infra_client_with_mlme); */ /* removed */
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	return _SUCCESS;
+}
+
+void init_mlme_default_rate_set(_adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	unsigned char	mixed_datarate[NumRates] = {_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_, _9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_, _48M_RATE_, _54M_RATE_, 0xff};
+	unsigned char	mixed_basicrate[NumRates] = {_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_, _12M_RATE_, _24M_RATE_, 0xff,};
+	unsigned char	supported_mcs_set[16] = {0xff, 0xff, 0xff, 0x00, 0x00, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+
+	memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
+	memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
+
+	memcpy(pmlmeext->default_supported_mcs_set, supported_mcs_set, sizeof(pmlmeext->default_supported_mcs_set));
+}
+
+static void init_mlme_ext_priv_value(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	ATOMIC_SET(&pmlmeext->event_seq, 0);
+	pmlmeext->mgnt_seq = 0;/* reset to zero when disconnect at client mode */
+#ifdef CONFIG_IEEE80211W
+	pmlmeext->sa_query_seq = 0;
+	pmlmeext->mgnt_80211w_IPN = 0;
+	pmlmeext->mgnt_80211w_IPN_rx = 0;
+#endif /* CONFIG_IEEE80211W */
+	pmlmeext->cur_channel = padapter->registrypriv.channel;
+	pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
+	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	pmlmeext->retry = 0;
+
+	pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
+
+	init_mlme_default_rate_set(padapter);
+
+	if (pmlmeext->cur_channel > 14)
+		pmlmeext->tx_rate = IEEE80211_OFDM_RATE_6MB;
+	else
+		pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
+
+	mlmeext_set_scan_state(pmlmeext, SCAN_DISABLE);
+	pmlmeext->sitesurvey_res.channel_idx = 0;
+	pmlmeext->sitesurvey_res.bss_cnt = 0;
+	pmlmeext->sitesurvey_res.scan_ch_ms = SURVEY_TO;
+	pmlmeext->sitesurvey_res.rx_ampdu_accept = RX_AMPDU_ACCEPT_INVALID;
+	pmlmeext->sitesurvey_res.rx_ampdu_size = RX_AMPDU_SIZE_INVALID;
+#ifdef CONFIG_SCAN_BACKOP
+	mlmeext_assign_scan_backop_flags_sta(pmlmeext, /*SS_BACKOP_EN|*/SS_BACKOP_PS_ANNC | SS_BACKOP_TX_RESUME);
+	mlmeext_assign_scan_backop_flags_ap(pmlmeext, SS_BACKOP_EN | SS_BACKOP_PS_ANNC | SS_BACKOP_TX_RESUME);
+	pmlmeext->sitesurvey_res.scan_cnt = 0;
+	pmlmeext->sitesurvey_res.scan_cnt_max = RTW_SCAN_NUM_OF_CH;
+	pmlmeext->sitesurvey_res.backop_ms = RTW_BACK_OP_CH_MS;
+#endif
+#if defined(CONFIG_ANTENNA_DIVERSITY) || defined(DBG_SCAN_SW_ANTDIV_BL)
+	pmlmeext->sitesurvey_res.is_sw_antdiv_bl_scan = 0;
+#endif
+	pmlmeext->scan_abort = false;
+
+	pmlmeinfo->state = WIFI_FW_NULL_STATE;
+	pmlmeinfo->reauth_count = 0;
+	pmlmeinfo->reassoc_count = 0;
+	pmlmeinfo->link_count = 0;
+	pmlmeinfo->auth_seq = 0;
+	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+	pmlmeinfo->key_index = 0;
+	pmlmeinfo->iv = 0;
+
+	pmlmeinfo->enc_algo = _NO_PRIVACY_;
+	pmlmeinfo->authModeToggle = 0;
+
+	memset(pmlmeinfo->chg_txt, 0, 128);
+
+	pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+	pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
+
+	pmlmeinfo->dialogToken = 0;
+
+	pmlmeext->action_public_rxseq = 0xffff;
+	pmlmeext->action_public_dialog_token = 0xff;
+}
+
+static int has_channel(RT_CHANNEL_INFO *channel_set,
+		       u8 chanset_size,
+		       u8 chan)
+{
+	int i;
+
+	for (i = 0; i < chanset_size; i++) {
+		if (channel_set[i].ChannelNum == chan)
+			return 1;
+	}
+
+	return 0;
+}
+
+static void init_channel_list(_adapter *padapter, RT_CHANNEL_INFO *channel_set,
+			      u8 chanset_size,
+			      struct p2p_channels *channel_list)
+{
+	struct registry_priv *regsty = adapter_to_regsty(padapter);
+
+	struct p2p_oper_class_map op_class[] = {
+		{ IEEE80211G,  81,   1,  13,  1, BW20 },
+		{ IEEE80211G,  82,  14,  14,  1, BW20 },
+		{ IEEE80211A, 115,  36,  48,  4, BW20 },
+		{ IEEE80211A, 116,  36,  44,  8, BW40PLUS },
+		{ IEEE80211A, 117,  40,  48,  8, BW40MINUS },
+		{ IEEE80211A, 124, 149, 161,  4, BW20 },
+		{ IEEE80211A, 125, 149, 169,  4, BW20 },
+		{ IEEE80211A, 126, 149, 157,  8, BW40PLUS },
+		{ IEEE80211A, 127, 153, 161,  8, BW40MINUS },
+		{ -1, 0, 0, 0, 0, BW20 }
+	};
+
+	int cla, op;
+
+	cla = 0;
+
+	for (op = 0; op_class[op].op_class; op++) {
+		u8 ch;
+		struct p2p_oper_class_map *o = &op_class[op];
+		struct p2p_reg_class *reg = NULL;
+
+		for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
+			if (!has_channel(channel_set, chanset_size, ch))
+				continue;
+
+			if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
+				continue;
+
+			if ((REGSTY_IS_BW_5G_SUPPORT(regsty, CHANNEL_WIDTH_40)) &&
+			    ((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
+				continue;
+
+			if (reg == NULL) {
+				reg = &channel_list->reg_class[cla];
+				cla++;
+				reg->reg_class = o->op_class;
+				reg->channels = 0;
+			}
+			reg->channel[reg->channels] = ch;
+			reg->channels++;
+		}
+	}
+	channel_list->reg_classes = cla;
+
+}
+
+static bool rtw_regsty_is_excl_chs(struct registry_priv *regsty, u8 ch)
+{
+	int i;
+
+	for (i = 0; i < MAX_CHANNEL_NUM; i++) {
+		if (regsty->excl_chs[i] == 0)
+			break;
+		if (regsty->excl_chs[i] == ch)
+			return true;
+	}
+	return false;
+}
+
+static u8 init_channel_set(_adapter *padapter, u8 ChannelPlan, RT_CHANNEL_INFO *channel_set)
+{
+	struct registry_priv *regsty = adapter_to_regsty(padapter);
+	u8	index, chanset_size = 0;
+	u8	b5GBand = false, b2_4GBand = false;
+	u8	Index2G = 0, Index5G = 0;
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(padapter);
+	int i;
+
+	if (!rtw_is_channel_plan_valid(ChannelPlan)) {
+		RTW_ERR("ChannelPlan ID 0x%02X error !!!!!\n", ChannelPlan);
+		return chanset_size;
+	}
+
+	memset(channel_set, 0, sizeof(RT_CHANNEL_INFO) * MAX_CHANNEL_NUM);
+
+	if (IsSupported24G(padapter->registrypriv.wireless_mode))
+		b2_4GBand = true;
+
+	if (is_supported_5g(padapter->registrypriv.wireless_mode))
+		b5GBand = true;
+
+	if (b2_4GBand) {
+		if (RTW_CHPLAN_REALTEK_DEFINE == ChannelPlan)
+			Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
+		else
+			Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
+
+		for (index = 0; index < CH_LIST_LEN(RTW_ChannelPlan2G[Index2G]); index++) {
+			if (rtw_regsty_is_excl_chs(regsty, CH_LIST_CH(RTW_ChannelPlan2G[Index2G], index)))
+				continue;
+
+			channel_set[chanset_size].ChannelNum = CH_LIST_CH(RTW_ChannelPlan2G[Index2G], index);
+
+			if (RTW_CHPLAN_GLOBAL_DOAMIN == ChannelPlan
+				|| RTW_CHPLAN_GLOBAL_NULL == ChannelPlan
+			) {
+				/* Channel 1~11 is active, and 12~14 is passive */
+				if (channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else if ((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
+					channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
+			} else if (RTW_CHPLAN_WORLD_WIDE_13 == ChannelPlan
+				|| RTW_CHPLAN_WORLD_WIDE_5G == ChannelPlan
+				|| RTW_RD_2G_WORLD == Index2G
+			) {
+				/* channel 12~13, passive scan */
+				if (channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+			} else
+				channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+
+			chanset_size++;
+		}
+	}
+
+#ifdef CONFIG_IEEE80211_BAND_5GHZ
+	if (b5GBand) {
+		if (RTW_CHPLAN_REALTEK_DEFINE == ChannelPlan)
+			Index5G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index5G;
+		else
+			Index5G = RTW_ChannelPlanMap[ChannelPlan].Index5G;
+
+		for (index = 0; index < CH_LIST_LEN(RTW_ChannelPlan5G[Index5G]); index++) {
+			if (rtw_regsty_is_excl_chs(regsty, CH_LIST_CH(RTW_ChannelPlan5G[Index5G], index)))
+				continue;
+#ifdef CONFIG_DFS
+			channel_set[chanset_size].ChannelNum = CH_LIST_CH(RTW_ChannelPlan5G[Index5G], index);
+			if (channel_set[chanset_size].ChannelNum <= 48
+				|| channel_set[chanset_size].ChannelNum >= 149
+			) {
+				if (RTW_CHPLAN_WORLD_WIDE_5G == ChannelPlan) /* passive scan for all 5G channels */
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+			} else
+				channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+			chanset_size++;
+#else /* CONFIG_DFS */
+			if (CH_LIST_CH(RTW_ChannelPlan5G[Index5G], index) <= 48
+				|| CH_LIST_CH(RTW_ChannelPlan5G[Index5G], index) >= 149
+			) {
+				channel_set[chanset_size].ChannelNum = CH_LIST_CH(RTW_ChannelPlan5G[Index5G], index);
+				if (RTW_CHPLAN_WORLD_WIDE_5G == ChannelPlan) /* passive scan for all 5G channels */
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				chanset_size++;
+			}
+#endif /* CONFIG_DFS */
+		}
+	}
+
+	#ifdef CONFIG_DFS_MASTER
+	for (i = 0; i < chanset_size; i++)
+		channel_set[i].non_ocp_end_time = jiffies;
+	#endif
+#endif /* CONFIG_IEEE80211_BAND_5GHZ */
+
+	if (RTW_CHPLAN_REALTEK_DEFINE == ChannelPlan) {
+		hal_data->Regulation2_4G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.regd;
+		hal_data->Regulation5G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.regd;
+	} else {
+		hal_data->Regulation2_4G = RTW_ChannelPlanMap[ChannelPlan].regd;
+		hal_data->Regulation5G = RTW_ChannelPlanMap[ChannelPlan].regd;
+	}
+
+	RTW_INFO(FUNC_ADPT_FMT" ChannelPlan ID:0x%02x, ch num:%d\n"
+		, FUNC_ADPT_ARG(padapter), ChannelPlan, chanset_size);
+
+	return chanset_size;
+}
+
+int	init_mlme_ext_priv(_adapter *padapter)
+{
+	int	res = _SUCCESS;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */
+	/* memset((u8 *)pmlmeext, 0, sizeof(struct mlme_ext_priv)); */
+
+	pmlmeext->padapter = padapter;
+
+	/* fill_fwpriv(padapter, &(pmlmeext->fwpriv)); */
+
+	init_mlme_ext_priv_value(padapter);
+	pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
+
+	init_mlme_ext_timer(padapter);
+
+#ifdef CONFIG_AP_MODE
+	init_mlme_ap_info(padapter);
+#endif
+
+	pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan, pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+	pmlmeext->last_scan_time = 0;
+	pmlmeext->mlmeext_init = true;
+
+
+#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+	pmlmeext->active_keep_alive_check = true;
+#else
+	pmlmeext->active_keep_alive_check = false;
+#endif
+
+#ifdef DBG_FIXED_CHAN
+	pmlmeext->fixed_chan = 0xFF;
+#endif
+
+	return res;
+
+}
+
+void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
+{
+	_adapter *padapter = pmlmeext->padapter;
+
+	if (!padapter)
+		return;
+
+	if (rtw_is_drv_stopped(padapter)) {
+		_cancel_timer_ex(&pmlmeext->survey_timer);
+		_cancel_timer_ex(&pmlmeext->link_timer);
+		/* _cancel_timer_ex(&pmlmeext->ADDBA_timer); */
+	}
+}
+
+static u8 cmp_pkt_chnl_diff(_adapter *padapter, u8 *pframe, uint packet_len)
+{
+	/* if the channel is same, return 0. else return channel differential	 */
+	uint len;
+	u8 channel;
+	u8 *p;
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_, _DSSET_IE_, &len, packet_len - _BEACON_IE_OFFSET_);
+	if (p) {
+		channel = *(p + 2);
+		if (padapter->mlmeextpriv.cur_channel >= channel)
+			return padapter->mlmeextpriv.cur_channel - channel;
+		else
+			return channel - padapter->mlmeextpriv.cur_channel;
+	} else
+		return 0;
+}
+
+static void _mgt_dispatcher(_adapter *padapter, struct mlme_handler *ptable, union recv_frame *precv_frame)
+{
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+
+	if (ptable->func) {
+		/* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
+		if (memcmp(GetAddr1Ptr(pframe), adapter_mac_addr(padapter), ETH_ALEN) &&
+		    memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
+			return;
+
+		ptable->func(padapter, precv_frame);
+	}
+
+}
+
+void mgt_dispatcher(_adapter *padapter, union recv_frame *precv_frame)
+{
+	int index;
+	struct mlme_handler *ptable;
+#ifdef CONFIG_AP_MODE
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+#endif /* CONFIG_AP_MODE */
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, get_addr2_ptr(pframe));
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	if (GetFrameType(pframe) != WIFI_MGT_TYPE) {
+		return;
+	}
+
+	/* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
+	if (memcmp(GetAddr1Ptr(pframe), adapter_mac_addr(padapter), ETH_ALEN) &&
+	    memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
+		return;
+
+	ptable = mlme_sta_tbl;
+
+	index = get_frame_sub_type(pframe) >> 4;
+
+#ifdef CONFIG_TDLS
+	if ((index << 4) == WIFI_ACTION) {
+		/* category==public (4), action==TDLS_DISCOVERY_RESPONSE */
+		if (*(pframe + 24) == RTW_WLAN_CATEGORY_PUBLIC && *(pframe + 25) == TDLS_DISCOVERY_RESPONSE) {
+			RTW_INFO("[TDLS] Recv %s from "MAC_FMT"\n", rtw_tdls_action_txt(TDLS_DISCOVERY_RESPONSE), MAC_ARG(get_addr2_ptr(pframe)));
+			On_TDLS_Dis_Rsp(padapter, precv_frame);
+		}
+	}
+#endif /* CONFIG_TDLS */
+
+	if (index >= (sizeof(mlme_sta_tbl) / sizeof(struct mlme_handler))) {
+		return;
+	}
+	ptable += index;
+
+	if (psta != NULL) {
+		if (GetRetry(pframe)) {
+			if (le16_to_cpu(precv_frame->u.hdr.attrib.seq_num) == psta->RxMgmtFrameSeqNum) {
+				/* drop the duplicate management frame */
+				pdbgpriv->dbg_rx_dup_mgt_frame_drop_count++;
+				RTW_INFO("Drop duplicate management frame with seq_num = %d.\n", precv_frame->u.hdr.attrib.seq_num);
+				return;
+			}
+		}
+		psta->RxMgmtFrameSeqNum = le16_to_cpu(precv_frame->u.hdr.attrib.seq_num);
+	}
+
+#ifdef CONFIG_AP_MODE
+	switch (get_frame_sub_type(pframe)) {
+	case WIFI_AUTH:
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+			ptable->func = &OnAuth;
+		else
+			ptable->func = &OnAuthClient;
+		__attribute__ ((__fallthrough__));/* FALL THRU */
+	case WIFI_ASSOCREQ:
+	case WIFI_REASSOCREQ:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+#ifdef CONFIG_HOSTAPD_MLME
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+			rtw_hostapd_mlme_rx(padapter, precv_frame);
+#endif
+		break;
+	case WIFI_PROBEREQ:
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+#ifdef CONFIG_HOSTAPD_MLME
+			rtw_hostapd_mlme_rx(padapter, precv_frame);
+#else
+			_mgt_dispatcher(padapter, ptable, precv_frame);
+#endif
+		} else
+			_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	case WIFI_BEACON:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	case WIFI_ACTION:
+		/* if(check_fwstate(pmlmepriv, WIFI_AP_STATE)) */
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	default:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+			rtw_hostapd_mlme_rx(padapter, precv_frame);
+		break;
+	}
+#else
+
+	_mgt_dispatcher(padapter, ptable, precv_frame);
+
+#endif
+
+}
+
+#ifdef CONFIG_P2P
+static u32 p2p_listen_state_process(_adapter *padapter, unsigned char *da)
+{
+	bool response = true;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (padapter->wdinfo.driver_interface == DRIVER_CFG80211) {
+		if (rtw_cfg80211_get_is_roch(padapter) == false
+			|| rtw_get_oper_ch(padapter) != padapter->wdinfo.listen_channel
+			|| adapter_wdev_data(padapter)->p2p_enabled == false
+			|| padapter->mlmepriv.wps_probe_resp_ie == NULL
+			|| padapter->mlmepriv.p2p_probe_resp_ie == NULL
+		) {
+#ifdef CONFIG_DEBUG_CFG80211
+			RTW_INFO(ADPT_FMT" DON'T issue_probersp_p2p: p2p_enabled:%d, wps_probe_resp_ie:%p, p2p_probe_resp_ie:%p\n"
+				, ADPT_ARG(padapter)
+				, adapter_wdev_data(padapter)->p2p_enabled
+				, padapter->mlmepriv.wps_probe_resp_ie
+				, padapter->mlmepriv.p2p_probe_resp_ie);
+			RTW_INFO(ADPT_FMT" DON'T issue_probersp_p2p: is_ro_ch:%d, op_ch:%d, p2p_listen_channel:%d\n"
+				, ADPT_ARG(padapter)
+				, rtw_cfg80211_get_is_roch(padapter)
+				, rtw_get_oper_ch(padapter)
+				, padapter->wdinfo.listen_channel);
+#endif
+			response = false;
+		}
+	} else
+#endif /* CONFIG_IOCTL_CFG80211 */
+		if (padapter->wdinfo.driver_interface == DRIVER_WEXT) {
+			/*	do nothing if the device name is empty */
+			if (!padapter->wdinfo.device_name_len)
+				response	= false;
+		}
+
+	if (response)
+		issue_probersp_p2p(padapter, da);
+
+	return _SUCCESS;
+}
+#endif /* CONFIG_P2P */
+
+
+/****************************************************************************
+
+Following are the callback functions for each subtype of the management frames
+
+*****************************************************************************/
+
+unsigned int OnProbeReq(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int	ielen;
+	unsigned char	*p;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX	*cur = &(pmlmeinfo->network);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	u8 is_valid_p2p_probereq = false;
+
+#ifdef CONFIG_ATMEL_RC_PATCH
+	u8 *target_ie = NULL, *wps_ie = NULL;
+	u8 *start;
+	uint search_len = 0, wps_ielen = 0, target_ielen = 0;
+	struct sta_info	*psta;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+#endif
+
+
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	struct rx_pkt_attrib	*pattrib = &precv_frame->u.hdr.attrib;
+	u8 wifi_test_chk_rate = 1;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if ((pwdinfo->driver_interface == DRIVER_CFG80211)
+	    && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)
+	    && (GET_CFG80211_REPORT_MGMT(adapter_wdev_data(padapter), IEEE80211_STYPE_PROBE_REQ))
+	) {
+		rtw_cfg80211_rx_probe_request(padapter, precv_frame);
+		return _SUCCESS;
+	}
+#endif /* CONFIG_IOCTL_CFG80211 */
+
+	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
+	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) &&
+	    !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
+	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
+	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)
+	   ) {
+		/*	Commented by Albert 2011/03/17 */
+		/*	mcs_rate = 0->CCK 1M rate */
+		/*	mcs_rate = 1->CCK 2M rate */
+		/*	mcs_rate = 2->CCK 5.5M rate */
+		/*	mcs_rate = 3->CCK 11M rate */
+		/*	In the P2P mode, the driver should not support the CCK rate */
+
+		/*	Commented by Kurt 2012/10/16 */
+		/*	IOT issue: Google Nexus7 use 1M rate to send p2p_probe_req after GO nego completed and Nexus7 is client */
+		if (padapter->registrypriv.wifi_spec == 1) {
+			if (pattrib->data_rate <= 3)
+				wifi_test_chk_rate = 0;
+		}
+
+		if (wifi_test_chk_rate == 1) {
+			is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len);
+			if (is_valid_p2p_probereq) {
+				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
+					/* FIXME */
+					if (padapter->wdinfo.driver_interface == DRIVER_WEXT)
+						report_survey_event(padapter, precv_frame);
+
+					p2p_listen_state_process(padapter,  get_sa(pframe));
+
+					return _SUCCESS;
+				}
+
+				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
+					goto _continue;
+			}
+		}
+	}
+
+_continue:
+#endif /* CONFIG_P2P */
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		return _SUCCESS;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == false &&
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE) == false)
+		return _SUCCESS;
+
+
+	/* RTW_INFO("+OnProbeReq\n"); */
+
+
+#ifdef CONFIG_ATMEL_RC_PATCH
+	wps_ie = rtw_get_wps_ie(
+			      pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_,
+			      len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_,
+			      NULL, &wps_ielen);
+	if (wps_ie)
+		target_ie = rtw_get_wps_attr_content(wps_ie, wps_ielen, WPS_ATTR_MANUFACTURER, NULL, &target_ielen);
+	if ((target_ie && (target_ielen == 4)) && (!memcmp((void *)target_ie, "Ozmo", 4))) {
+		/* psta->flag_atmel_rc = 1; */
+		unsigned char *sa_addr = get_sa(pframe);
+		RTW_INFO("%s: Find Ozmo RC -- %02x:%02x:%02x:%02x:%02x:%02x  \n\n",
+		       __func__, *sa_addr, *(sa_addr + 1), *(sa_addr + 2), *(sa_addr + 3), *(sa_addr + 4), *(sa_addr + 5));
+		memcpy(pstapriv->atmel_rc_pattern, get_sa(pframe), ETH_ALEN);
+	}
+#endif
+
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    pmlmepriv->cur_network.join_res) {
+		unsigned long	irqL;
+		struct sta_info	*psta;
+		u8 *mac_addr, *peer_addr;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+		u8 RC_OUI[4] = {0x00, 0xE0, 0x4C, 0x0A};
+		/* EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2] */
+
+		p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _VENDOR_SPECIFIC_IE_, (int *)&ielen,
+			       len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+		if (!p || ielen != 14)
+			goto _non_rc_device;
+
+		if (memcmp(p + 2, RC_OUI, sizeof(RC_OUI)))
+			goto _non_rc_device;
+
+		if (memcmp(p + 6, get_sa(pframe), ETH_ALEN)) {
+			RTW_INFO("%s, do rc pairing ("MAC_FMT"), but mac addr mismatch!("MAC_FMT")\n", __func__,
+				 MAC_ARG(get_sa(pframe)), MAC_ARG(p + 6));
+
+			goto _non_rc_device;
+		}
+
+		RTW_INFO("%s, got the pairing device("MAC_FMT")\n", __func__,  MAC_ARG(get_sa(pframe)));
+
+		/* new a station */
+		psta = rtw_get_stainfo(pstapriv, get_sa(pframe));
+		if (psta == NULL) {
+			/* allocate a new one */
+			RTW_INFO("going to alloc stainfo for rc="MAC_FMT"\n",  MAC_ARG(get_sa(pframe)));
+			psta = rtw_alloc_stainfo(pstapriv, get_sa(pframe));
+			if (psta == NULL) {
+				/* TODO: */
+				RTW_INFO(" Exceed the upper limit of supported clients...\n");
+				return _SUCCESS;
+			}
+
+			_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+			if (list_empty(&psta->asoc_list)) {
+				psta->expire_to = pstapriv->expire_to;
+				list_add_tail(&psta->asoc_list, &pstapriv->asoc_list);
+				pstapriv->asoc_list_cnt++;
+			}
+			_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+			/* generate pairing ID */
+			mac_addr = adapter_mac_addr(padapter);
+			peer_addr = psta->hwaddr;
+			psta->pid = (u16)(((mac_addr[4] << 8) + mac_addr[5]) + ((peer_addr[4] << 8) + peer_addr[5]));
+
+			/* update peer stainfo */
+			psta->isrc = true;
+
+			/* get a unique AID */
+			if (psta->aid > 0)
+				RTW_INFO("old AID %d\n", psta->aid);
+			else {
+				for (psta->aid = 1; psta->aid <= NUM_STA; psta->aid++)
+					if (pstapriv->sta_aid[psta->aid - 1] == NULL)
+						break;
+
+				if (psta->aid > pstapriv->max_num_sta) {
+					psta->aid = 0;
+					RTW_INFO("no room for more AIDs\n");
+					return _SUCCESS;
+				} else {
+					pstapriv->sta_aid[psta->aid - 1] = psta;
+					RTW_INFO("allocate new AID = (%d)\n", psta->aid);
+				}
+			}
+
+			psta->qos_option = 1;
+			psta->bw_mode = CHANNEL_WIDTH_20;
+			psta->ieee8021x_blocked = false;
+			psta->htpriv.ht_option = true;
+			psta->htpriv.ampdu_enable = false;
+			psta->htpriv.sgi_20m = false;
+			psta->htpriv.sgi_40m = false;
+			psta->htpriv.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+			psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+
+			rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
+
+			memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+
+			_enter_critical_bh(&psta->lock, &irqL);
+			psta->state |= _FW_LINKED;
+			_exit_critical_bh(&psta->lock, &irqL);
+
+			report_add_sta_event(padapter, psta->hwaddr);
+
+		}
+
+		issue_probersp(padapter, get_sa(pframe), false);
+
+		return _SUCCESS;
+
+	}
+
+_non_rc_device:
+
+	return _SUCCESS;
+
+#endif /* CONFIG_AUTO_AP_MODE */
+
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) &&
+	    rtw_mi_buddy_check_fwstate(padapter, _FW_UNDER_LINKING | _FW_UNDER_SURVEY)) {
+		/* don't process probe req */
+		return _SUCCESS;
+	}
+#endif
+
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
+		       len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+
+	/* check (wildcard) SSID */
+	if (p != NULL) {
+		if (is_valid_p2p_probereq)
+			goto _issue_probersp;
+
+		if ((ielen != 0 && false == !memcmp((void *)(p + 2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength))
+		    || (ielen == 0 && pmlmeinfo->hidden_ssid_mode)
+		   )
+			return _SUCCESS;
+
+_issue_probersp:
+		if (((check_fwstate(pmlmepriv, _FW_LINKED) &&
+		      pmlmepriv->cur_network.join_res)) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+			/* RTW_INFO("+issue_probersp during ap mode\n"); */
+			issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
+		}
+
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnProbeRsp(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct sta_info		*psta;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	u8	*pframe = precv_frame->u.hdr.rx_data;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+#endif
+
+
+#ifdef CONFIG_P2P
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
+		if (pwdinfo->tx_prov_disc_info.benable) {
+			if (!memcmp(pwdinfo->tx_prov_disc_info.peerIFAddr, get_addr2_ptr(pframe), ETH_ALEN)) {
+				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+					pwdinfo->tx_prov_disc_info.benable = false;
+					issue_p2p_provision_request(padapter,
+						pwdinfo->tx_prov_disc_info.ssid.Ssid,
+						pwdinfo->tx_prov_disc_info.ssid.SsidLength,
+						pwdinfo->tx_prov_disc_info.peerDevAddr);
+				} else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+					pwdinfo->tx_prov_disc_info.benable = false;
+					issue_p2p_provision_request(padapter,
+								    NULL,
+								    0,
+						pwdinfo->tx_prov_disc_info.peerDevAddr);
+				}
+			}
+		}
+		return _SUCCESS;
+	} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
+		if (pwdinfo->nego_req_info.benable) {
+			RTW_INFO("[%s] P2P State is GONEGO ING!\n", __func__);
+			if (!memcmp(pwdinfo->nego_req_info.peerDevAddr, get_addr2_ptr(pframe), ETH_ALEN)) {
+				pwdinfo->nego_req_info.benable = false;
+				issue_p2p_GO_request(padapter, pwdinfo->nego_req_info.peerDevAddr);
+			}
+		}
+	} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) {
+		if (pwdinfo->invitereq_info.benable) {
+			RTW_INFO("[%s] P2P_STATE_TX_INVITE_REQ!\n", __func__);
+			if (!memcmp(pwdinfo->invitereq_info.peer_macaddr, get_addr2_ptr(pframe), ETH_ALEN)) {
+				pwdinfo->invitereq_info.benable = false;
+				issue_p2p_invitation_request(padapter, pwdinfo->invitereq_info.peer_macaddr);
+			}
+		}
+	}
+#endif
+
+
+	if (mlmeext_chk_scan_state(pmlmeext, SCAN_PROCESS)) {
+		rtw_mi_report_survey_event(padapter, precv_frame);
+		return _SUCCESS;
+	}
+
+	return _SUCCESS;
+}
+
+/* for 11n Logo 4.2.31/4.2.32 */
+static void rtw_check_legacy_ap(_adapter *padapter, u8 *pframe, u32 len)
+{
+
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (!padapter->registrypriv.wifi_spec)
+		return;
+	
+	if(!MLME_IS_AP(padapter))
+		return;
+	
+
+	if (pmlmeext->bstart_bss) {
+		int left;
+		u16 capability;
+		unsigned char *pos;
+		struct rtw_ieee802_11_elems elems;
+		struct HT_info_element *pht_info = NULL;
+		u16 cur_op_mode; 
+
+		/* checking IEs */
+		left = len - sizeof(struct rtw_ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_;
+		pos = pframe + sizeof(struct rtw_ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_;
+		if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed) {
+			RTW_INFO("%s: parse fail for "MAC_FMT"\n", __func__, MAC_ARG(GetAddr3Ptr(pframe)));
+			return;
+		}
+
+		cur_op_mode = pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+
+		/* for legacy ap */
+		if (elems.ht_capabilities == NULL && elems.ht_capabilities_len == 0) {
+
+			if (0)
+				RTW_INFO("%s: "MAC_FMT" is legacy ap\n", __func__, MAC_ARG(GetAddr3Ptr(pframe)));
+
+			ATOMIC_SET(&pmlmepriv->olbc, true);
+			ATOMIC_SET(&pmlmepriv->olbc_ht, true);
+		}
+			
+	}
+}
+
+unsigned int OnBeacon(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct sta_info	*psta;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	WLAN_BSSID_EX *pbss;
+	int ret = _SUCCESS;
+	u8 *p = NULL;
+	u32 ielen = 0;
+#ifdef CONFIG_TDLS
+	struct sta_info *ptdls_sta;
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+#ifdef CONFIG_TDLS_CH_SW
+	struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
+#endif
+#endif /* CONFIG_TDLS */
+
+	if (validate_beacon_len(pframe, len) == false)
+		return _SUCCESS;
+#ifdef CONFIG_ATTEMPT_TO_FIX_AP_BEACON_ERROR
+	p = rtw_get_ie(pframe + sizeof(struct rtw_ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ielen,
+		precv_frame->u.hdr.len - sizeof(struct rtw_ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_);
+	if ((p != NULL) && (ielen > 0)) {
+		if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D)) {
+			/* Invalid value 0x2D is detected in Extended Supported Rates (ESR) IE. Try to fix the IE length to avoid failed Beacon parsing. */
+			RTW_INFO("[WIFIDBG] Error in ESR IE is detected in Beacon of BSSID:"MAC_FMT". Fix the length of ESR IE to avoid failed Beacon parsing.\n", MAC_ARG(GetAddr3Ptr(pframe)));
+			*(p + 1) = ielen - 1;
+		}
+	}
+#endif
+
+	if (mlmeext_chk_scan_state(pmlmeext, SCAN_PROCESS)) {
+		rtw_mi_report_survey_event(padapter, precv_frame);
+		return _SUCCESS;
+	}
+
+
+	rtw_check_legacy_ap(padapter, pframe, len);
+
+	if (!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)) {
+		if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+			/* we should update current network before auth, or some IE is wrong */
+			pbss = (WLAN_BSSID_EX *)rtw_malloc(sizeof(WLAN_BSSID_EX));
+			if (pbss) {
+				if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
+					struct beacon_keys recv_beacon;
+
+					update_network(&(pmlmepriv->cur_network.network), pbss, padapter, true);
+					rtw_get_bcn_info(&(pmlmepriv->cur_network));
+
+					/* update bcn keys */
+					if (rtw_get_bcn_keys(padapter, pframe, len, &recv_beacon)) {
+						RTW_INFO("%s: beacon keys ready\n", __func__);
+						memcpy(&pmlmepriv->cur_beacon_keys,
+							&recv_beacon, sizeof(recv_beacon));
+						pmlmepriv->new_beacon_cnts = 0;
+					} else {
+						RTW_ERR("%s: get beacon keys failed\n", __func__);
+						memset(&pmlmepriv->cur_beacon_keys, 0, sizeof(recv_beacon));
+						pmlmepriv->new_beacon_cnts = 0;
+					}
+				}
+				rtw_mfree((u8 *)pbss, sizeof(WLAN_BSSID_EX));
+			}
+
+			/* check the vendor of the assoc AP */
+			pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe + sizeof(struct rtw_ieee80211_hdr_3addr), len - sizeof(struct rtw_ieee80211_hdr_3addr));
+
+			/* update TSF Value */
+			update_TSF(pmlmeext, pframe, len);
+
+			/* reset for adaptive_early_32k */
+			pmlmeext->adaptive_tsf_done = false;
+			pmlmeext->DrvBcnEarly = 0xff;
+			pmlmeext->DrvBcnTimeOut = 0xff;
+			pmlmeext->bcn_cnt = 0;
+			memset(pmlmeext->bcn_delay_cnt, 0, sizeof(pmlmeext->bcn_delay_cnt));
+			memset(pmlmeext->bcn_delay_ratio, 0, sizeof(pmlmeext->bcn_delay_ratio));
+
+#ifdef CONFIG_P2P_PS
+			process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
+#endif /* CONFIG_P2P_PS */
+
+#if defined(CONFIG_P2P) && defined(CONFIG_CONCURRENT_MODE)
+			if (padapter->registrypriv.wifi_spec) {
+				if (process_p2p_cross_connect_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN)) == false) {
+					if (rtw_mi_buddy_check_mlmeinfo_state(padapter, WIFI_FW_AP_STATE)) {
+						RTW_PRINT("no issue auth, P2P cross-connect does not permit\n ");
+						return _SUCCESS;
+					}
+				}
+			}
+#endif /* CONFIG_P2P CONFIG_P2P and CONFIG_CONCURRENT_MODE */
+
+			/* start auth */
+			start_clnt_auth(padapter);
+
+			return _SUCCESS;
+		}
+
+		if (((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+			psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe));
+			if (psta != NULL) {
+#ifdef CONFIG_PATCH_JOIN_WRONG_CHANNEL
+				/* Merge from 8712 FW code */
+				if (cmp_pkt_chnl_diff(padapter, pframe, len) != 0) {
+					/* join wrong channel, deauth and reconnect           */
+					issue_deauth(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
+
+					report_del_sta_event(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_JOIN_WRONG_CHANNEL, true, false);
+					pmlmeinfo->state &= (~WIFI_FW_ASSOC_SUCCESS);
+					return _SUCCESS;
+				}
+#endif /* CONFIG_PATCH_JOIN_WRONG_CHANNEL */
+
+				ret = rtw_check_bcn_info(padapter, pframe, len);
+				if (!ret) {
+					RTW_PRINT("ap has changed, disconnect now\n ");
+					receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 0, false);
+					return _SUCCESS;
+				}
+				/* update WMM, ERP in the beacon */
+				/* todo: the timer is used instead of the number of the beacon received */
+				if ((sta_rx_pkts(psta) & 0xf) == 0) {
+					/* RTW_INFO("update_bcn_info\n"); */
+					update_beacon_info(padapter, pframe, len, psta);
+				}
+
+				pmlmepriv->cur_network_scanned->network.Rssi = precv_frame->u.hdr.attrib.phy_info.RecvSignalPower;
+
+				adaptive_early_32k(pmlmeext, pframe, len);
+
+#ifdef CONFIG_TDLS
+#ifdef CONFIG_TDLS_CH_SW
+				if (rtw_tdls_is_chsw_allowed(padapter)) {
+					/* Send TDLS Channel Switch Request when receiving Beacon */
+					if ((padapter->tdlsinfo.chsw_info.ch_sw_state & TDLS_CH_SW_INITIATOR_STATE) && (ATOMIC_READ(&pchsw_info->chsw_on))
+					    && (pmlmeext->cur_channel == rtw_get_oper_ch(padapter))) {
+						ptdls_sta = rtw_get_stainfo(&padapter->stapriv, padapter->tdlsinfo.chsw_info.addr);
+						if (ptdls_sta != NULL) {
+							if (ptdls_sta->tdls_sta_state | TDLS_LINKED_STATE)
+								_set_timer(&ptdls_sta->stay_on_base_chnl_timer, TDLS_CH_SW_STAY_ON_BASE_CHNL_TIMEOUT);
+						}
+					}
+				}
+#endif
+#endif /* CONFIG_TDLS */
+
+#ifdef CONFIG_DFS
+				process_csa_ie(padapter, pframe, len);	/* channel switch announcement */
+#endif /* CONFIG_DFS */
+
+#ifdef CONFIG_P2P_PS
+				process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
+#endif /* CONFIG_P2P_PS */
+
+				if (pmlmeext->en_hw_update_tsf)
+					rtw_enable_hw_update_tsf_cmd(padapter);
+			}
+		} else if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
+			unsigned long irqL;
+			u8 rate_set[16];
+			u8 rate_num = 0;
+
+			psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe));
+			if (psta != NULL) {
+				/*
+				* update WMM, ERP in the beacon
+				* todo: the timer is used instead of the number of the beacon received
+				*/
+				if ((sta_rx_pkts(psta) & 0xf) == 0)
+					update_beacon_info(padapter, pframe, len, psta);
+
+				if (pmlmeext->en_hw_update_tsf)
+					rtw_enable_hw_update_tsf_cmd(padapter);
+			} else {
+				rtw_ies_get_supported_rate(pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_, len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_, rate_set, &rate_num);
+				if (rate_num == 0) {
+					RTW_INFO(FUNC_ADPT_FMT" RX beacon with no supported rate\n", FUNC_ADPT_ARG(padapter));
+					goto _END_ONBEACON_;
+				}
+
+				psta = rtw_alloc_stainfo(pstapriv, get_addr2_ptr(pframe));
+				if (psta == NULL) {
+					RTW_INFO(FUNC_ADPT_FMT" Exceed the upper limit of supported clients\n", FUNC_ADPT_ARG(padapter));
+					goto _END_ONBEACON_;
+				}
+
+				psta->expire_to = pstapriv->adhoc_expire_to;
+
+				memcpy(psta->bssrateset, rate_set, rate_num);
+				psta->bssratelen = rate_num;
+
+				/* update TSF Value */
+				update_TSF(pmlmeext, pframe, len);
+
+				/* report sta add event */
+				report_add_sta_event(padapter, get_addr2_ptr(pframe));
+			}
+		}
+	}
+
+_END_ONBEACON_:
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnAuth(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_AP_MODE
+	unsigned long irqL;
+	unsigned int	auth_mode, seq, ie_len;
+	unsigned char	*sa, *p;
+	u16	algorithm;
+	int	status;
+	static struct sta_info stat;
+	struct	sta_info	*pstat = NULL;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	u8	offset = 0;
+
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) &&
+	    rtw_mi_buddy_check_fwstate(padapter, _FW_UNDER_LINKING | _FW_UNDER_SURVEY)) {
+		/* don't process auth request; */
+		return _SUCCESS;
+	}
+#endif /* CONFIG_CONCURRENT_MODE */
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	RTW_INFO("+OnAuth\n");
+
+	sa = get_addr2_ptr(pframe);
+
+	auth_mode = psecuritypriv->dot11AuthAlgrthm;
+
+	if (GetPrivacy(pframe)) {
+		u8	*iv;
+		struct rx_pkt_attrib	*prxattrib = &(precv_frame->u.hdr.attrib);
+
+		prxattrib->hdrlen = WLAN_HDR_A3_LEN;
+		prxattrib->encrypt = _WEP40_;
+
+		iv = pframe + prxattrib->hdrlen;
+		prxattrib->key_index = ((iv[3] >> 6) & 0x3);
+
+		prxattrib->iv_len = 4;
+		prxattrib->icv_len = 4;
+
+		rtw_wep_decrypt(padapter, (u8 *)precv_frame);
+
+		offset = 4;
+	}
+
+	algorithm = le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset));
+	seq	= le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2));
+
+	RTW_INFO("auth alg=%x, seq=%X\n", algorithm, seq);
+
+	if (auth_mode == 2 &&
+	    psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
+	    psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
+		auth_mode = 0;
+
+	if ((algorithm > 0 && auth_mode == 0) ||	/* rx a shared-key auth but shared not enabled */
+	    (algorithm == 0 && auth_mode == 1)) {	/* rx a open-system auth but shared-key is enabled */
+		RTW_INFO("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n",
+			algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
+
+		status = _STATS_NO_SUPP_ALG_;
+
+		goto auth_fail;
+	}
+
+#if CONFIG_RTW_MACADDR_ACL
+	if (rtw_access_ctrl(padapter, sa) == false) {
+		status = _STATS_UNABLE_HANDLE_STA_;
+		goto auth_fail;
+	}
+#endif
+
+	pstat = rtw_get_stainfo(pstapriv, sa);
+	if (pstat == NULL) {
+
+		/* allocate a new one */
+		RTW_INFO("going to alloc stainfo for sa="MAC_FMT"\n",  MAC_ARG(sa));
+		pstat = rtw_alloc_stainfo(pstapriv, sa);
+		if (pstat == NULL) {
+			RTW_INFO(" Exceed the upper limit of supported clients...\n");
+			status = _STATS_UNABLE_HANDLE_STA_;
+			goto auth_fail;
+		}
+
+		pstat->state = WIFI_FW_AUTH_NULL;
+		pstat->auth_seq = 0;
+
+		/* pstat->flags = 0; */
+		/* pstat->capability = 0; */
+	} else {
+#ifdef CONFIG_IEEE80211W
+		if (pstat->bpairwise_key_installed != true && !(pstat->state & WIFI_FW_ASSOC_SUCCESS))
+#endif /* CONFIG_IEEE80211W */
+		{
+
+			_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+			if (list_empty(&pstat->asoc_list) == false) {
+				list_del_init(&pstat->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				if (pstat->expire_to > 0)
+					;/* TODO: STA re_auth within expire_to */
+			}
+			_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+			if (seq == 1)
+				; /* TODO: STA re_auth and auth timeout */
+
+		}
+	}
+
+#ifdef CONFIG_IEEE80211W
+	if (pstat->bpairwise_key_installed != true && !(pstat->state & WIFI_FW_ASSOC_SUCCESS))
+#endif /* CONFIG_IEEE80211W */
+	{
+		_enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
+		if (list_empty(&pstat->auth_list)) {
+
+			list_add_tail(&pstat->auth_list, &pstapriv->auth_list);
+			pstapriv->auth_list_cnt++;
+		}
+		_exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
+	}
+
+	if (pstat->auth_seq == 0)
+		pstat->expire_to = pstapriv->auth_to;
+
+
+	if ((pstat->auth_seq + 1) != seq) {
+		RTW_INFO("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+			 seq, pstat->auth_seq + 1);
+		status = _STATS_OUT_OF_AUTH_SEQ_;
+		goto auth_fail;
+	}
+
+	if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2 || auth_mode == 3)) {
+		if (seq == 1) {
+#ifdef CONFIG_IEEE80211W
+			if (pstat->bpairwise_key_installed != true && !(pstat->state & WIFI_FW_ASSOC_SUCCESS))
+#endif /* CONFIG_IEEE80211W */
+			{
+				pstat->state &= ~WIFI_FW_AUTH_NULL;
+				pstat->state |= WIFI_FW_AUTH_SUCCESS;
+				pstat->expire_to = pstapriv->assoc_to;
+			}
+			pstat->authalg = algorithm;
+		} else {
+			RTW_INFO("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+				 seq, pstat->auth_seq + 1);
+			status = _STATS_OUT_OF_AUTH_SEQ_;
+			goto auth_fail;
+		}
+	} else { /* shared system or auto authentication */
+		if (seq == 1) {
+			/* prepare for the challenging txt... */
+
+			/* get_random_bytes((void *)pstat->chg_txt, 128); */ /* TODO: */
+			memset((void *)pstat->chg_txt, 78, 128);
+#ifdef CONFIG_IEEE80211W
+			if (pstat->bpairwise_key_installed != true && !(pstat->state & WIFI_FW_ASSOC_SUCCESS))
+#endif /* CONFIG_IEEE80211W */
+			{
+				pstat->state &= ~WIFI_FW_AUTH_NULL;
+				pstat->state |= WIFI_FW_AUTH_STATE;
+			}
+			pstat->authalg = algorithm;
+			pstat->auth_seq = 2;
+		} else if (seq == 3) {
+			/* checking for challenging txt... */
+			RTW_INFO("checking for challenging txt...\n");
+
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len,
+				len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
+
+			if ((p == NULL) || (ie_len <= 0)) {
+				RTW_INFO("auth rejected because challenge failure!(1)\n");
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+
+			if (!memcmp((void *)(p + 2), pstat->chg_txt, 128)) {
+#ifdef CONFIG_IEEE80211W
+				if (pstat->bpairwise_key_installed != true && !(pstat->state & WIFI_FW_ASSOC_SUCCESS))
+#endif /* CONFIG_IEEE80211W */
+				{
+					pstat->state &= (~WIFI_FW_AUTH_STATE);
+					pstat->state |= WIFI_FW_AUTH_SUCCESS;
+					/* challenging txt is correct... */
+					pstat->expire_to =  pstapriv->assoc_to;
+				}
+			} else {
+				RTW_INFO("auth rejected because challenge failure!\n");
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+		} else {
+			RTW_INFO("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+				 seq, pstat->auth_seq + 1);
+			status = _STATS_OUT_OF_AUTH_SEQ_;
+			goto auth_fail;
+		}
+	}
+
+
+	/* Now, we are going to issue_auth... */
+	pstat->auth_seq = seq + 1;
+
+#ifdef CONFIG_NATIVEAP_MLME
+	issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
+#endif
+
+	if ((pstat->state & WIFI_FW_AUTH_SUCCESS) || (pstat->state & WIFI_FW_ASSOC_SUCCESS))
+		pstat->auth_seq = 0;
+
+
+	return _SUCCESS;
+
+auth_fail:
+
+	if (pstat)
+		rtw_free_stainfo(padapter , pstat);
+
+	pstat = &stat;
+	memset((char *)pstat, '\0', sizeof(stat));
+	pstat->auth_seq = 2;
+	memcpy(pstat->hwaddr, sa, 6);
+
+#ifdef CONFIG_NATIVEAP_MLME
+	issue_auth(padapter, pstat, (unsigned short)status);
+#endif
+
+#endif
+	return _FAIL;
+
+}
+
+unsigned int OnAuthClient(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int	seq, len, status, algthm, offset;
+	unsigned char	*p;
+	unsigned int	go2asoc = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+#ifdef CONFIG_RTW_80211R
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	ft_priv	*pftpriv = &pmlmepriv->ftpriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+#endif
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+
+	RTW_INFO("%s\n", __func__);
+
+	/* check A1 matches or not */
+	if (memcmp(adapter_mac_addr(padapter), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
+		return _SUCCESS;
+
+	offset = (GetPrivacy(pframe)) ? 4 : 0;
+
+	algthm	= le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset));
+	seq	= le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 2));
+	status	= le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 4));
+
+	if (status != 0) {
+		RTW_INFO("clnt auth fail, status: %d\n", status);
+		if (status == 13) { /* && pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
+			if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+			else
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
+			/* pmlmeinfo->reauth_count = 0; */
+		}
+
+		set_link_timer(pmlmeext, 1);
+		goto authclnt_fail;
+	}
+
+	if (seq == 2) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
+			/* legendary shared system */
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
+				pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
+
+			if (p == NULL) {
+				/* RTW_INFO("marc: no challenge text?\n"); */
+				goto authclnt_fail;
+			}
+
+			memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
+			pmlmeinfo->auth_seq = 3;
+			issue_auth(padapter, NULL, 0);
+			set_link_timer(pmlmeext, REAUTH_TO);
+
+			return _SUCCESS;
+		} else {
+			/* open, or 802.11r FTAA system */
+			go2asoc = 1;
+		}
+	} else if (seq == 4) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+			go2asoc = 1;
+		else
+			goto authclnt_fail;
+	} else {
+		/* this is also illegal */
+		/* RTW_INFO("marc: clnt auth failed due to illegal seq=%x\n", seq); */
+		goto authclnt_fail;
+	}
+
+	if (go2asoc) {
+#ifdef CONFIG_RTW_80211R
+		if ((rtw_to_roam(padapter) > 0) && rtw_chk_ft_flags(padapter, RTW_FT_SUPPORTED)) {
+			u8 target_ap_addr[ETH_ALEN] = {0};
+
+			if ((rtw_chk_ft_status(padapter, RTW_FT_AUTHENTICATED_STA)) ||
+				(rtw_chk_ft_status(padapter, RTW_FT_ASSOCIATING_STA)) ||
+				(rtw_chk_ft_status(padapter, RTW_FT_ASSOCIATED_STA))) {
+				/*report_ft_reassoc_event already, and waiting for cfg80211_rtw_update_ft_ies*/
+				return _SUCCESS;
+			}
+
+			rtw_buf_update(&pmlmepriv->auth_rsp, &pmlmepriv->auth_rsp_len, pframe, pkt_len);
+			pftpriv->ft_event.ies = pmlmepriv->auth_rsp + sizeof(struct rtw_ieee80211_hdr_3addr) + 6;
+			pftpriv->ft_event.ies_len = pmlmepriv->auth_rsp_len - sizeof(struct rtw_ieee80211_hdr_3addr) - 6;
+
+			/*Not support RIC*/
+			pftpriv->ft_event.ric_ies =  NULL;
+			pftpriv->ft_event.ric_ies_len =  0;
+			memcpy(target_ap_addr, pmlmepriv->assoc_bssid, ETH_ALEN);
+			report_ft_reassoc_event(padapter, target_ap_addr);
+			return _SUCCESS;
+		}
+#endif
+
+		RTW_PRINT("auth success, start assoc\n");
+		start_clnt_assoc(padapter);
+		return _SUCCESS;
+	}
+
+authclnt_fail:
+
+	/* pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE); */
+
+	return _FAIL;
+
+}
+
+unsigned int OnAssocReq(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_AP_MODE
+	unsigned long irqL;
+	u16 capab_info, listen_interval;
+	struct rtw_ieee802_11_elems elems;
+	struct sta_info	*pstat;
+	unsigned char		reassoc, *p, *pos, *wpa_ie;
+	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+	int		i, ie_len, wpa_ie_len, left;
+	u8 rate_set[16];
+	u8 rate_num;
+	unsigned short		status = _STATS_SUCCESSFUL_;
+	unsigned short		frame_type, ie_offset = 0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX	*cur = &(pmlmeinfo->network);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8 p2p_status_code = P2P_STATUS_SUCCESS;
+	u8 *p2pie;
+	u32 p2pielen = 0;
+#endif /* CONFIG_P2P */
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) &&
+	    rtw_mi_buddy_check_fwstate(padapter, _FW_UNDER_LINKING | _FW_UNDER_SURVEY)) {
+		/* don't process assoc request; */
+		return _SUCCESS;
+	}
+#endif /* CONFIG_CONCURRENT_MODE */
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	frame_type = get_frame_sub_type(pframe);
+	if (frame_type == WIFI_ASSOCREQ) {
+		reassoc = 0;
+		ie_offset = _ASOCREQ_IE_OFFSET_;
+	} else { /* WIFI_REASSOCREQ */
+		reassoc = 1;
+		ie_offset = _REASOCREQ_IE_OFFSET_;
+	}
+
+
+	if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
+		RTW_INFO("handle_assoc(reassoc=%d) - too short payload (len=%lu)"
+			 "\n", reassoc, (unsigned long)pkt_len);
+		return _FAIL;
+	}
+
+	pstat = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe));
+	if (pstat == (struct sta_info *)NULL) {
+		status = _RSON_CLS2_;
+		goto asoc_class2_error;
+	}
+
+	capab_info = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN);
+	/* capab_info = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));	 */
+	/* listen_interval = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN+2)); */
+	listen_interval = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN + 2);
+
+	left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
+	pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
+
+
+	RTW_INFO("%s\n", __func__);
+
+	/* check if this stat has been successfully authenticated/assocated */
+	if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) {
+		if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) {
+			status = _RSON_CLS2_;
+			goto asoc_class2_error;
+		} else {
+			pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
+			pstat->state |= WIFI_FW_ASSOC_STATE;
+		}
+	} else {
+		pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
+		pstat->state |= WIFI_FW_ASSOC_STATE;
+	}
+
+	pstat->capability = capab_info;
+
+	/* now parse all ieee802_11 ie to point to elems */
+	if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
+	    !elems.ssid) {
+		RTW_INFO("STA " MAC_FMT " sent invalid association request\n",
+			 MAC_ARG(pstat->hwaddr));
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+
+
+	/* now we should check all the fields... */
+	/* checking SSID */
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
+		       pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+	if (p == NULL)
+		status = _STATS_FAILURE_;
+
+	if (ie_len == 0) /* broadcast ssid, however it is not allowed in assocreq */
+		status = _STATS_FAILURE_;
+	else {
+		/* check if ssid match */
+		if (memcmp((void *)(p + 2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
+			status = _STATS_FAILURE_;
+
+		if (ie_len != cur->Ssid.SsidLength)
+			status = _STATS_FAILURE_;
+	}
+
+	if (_STATS_SUCCESSFUL_ != status)
+		goto OnAssocReqFail;
+
+	rtw_ies_get_supported_rate(pframe + WLAN_HDR_A3_LEN + ie_offset, pkt_len - WLAN_HDR_A3_LEN - ie_offset, rate_set, &rate_num);
+	if (rate_num == 0) {
+		RTW_INFO(FUNC_ADPT_FMT" RX assoc-req with no supported rate\n", FUNC_ADPT_ARG(padapter));
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+	memcpy(pstat->bssrateset, rate_set, rate_num);
+	pstat->bssratelen = rate_num;
+	UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
+
+	/* check RSN/WPA/WPS */
+	pstat->dot8021xalg = 0;
+	pstat->wpa_psk = 0;
+	pstat->wpa_group_cipher = 0;
+	pstat->wpa2_group_cipher = 0;
+	pstat->wpa_pairwise_cipher = 0;
+	pstat->wpa2_pairwise_cipher = 0;
+	memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
+	if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
+
+		int group_cipher = 0, pairwise_cipher = 0;
+
+		wpa_ie = elems.rsn_ie;
+		wpa_ie_len = elems.rsn_ie_len;
+
+		if (rtw_parse_wpa2_ie(wpa_ie - 2, wpa_ie_len + 2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			pstat->dot8021xalg = 1;/* psk,  todo:802.1x						 */
+			pstat->wpa_psk |= BIT(1);
+
+			pstat->wpa2_group_cipher = group_cipher & psecuritypriv->wpa2_group_cipher;
+			pstat->wpa2_pairwise_cipher = pairwise_cipher & psecuritypriv->wpa2_pairwise_cipher;
+
+			if (!pstat->wpa2_group_cipher)
+				status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
+
+			if (!pstat->wpa2_pairwise_cipher)
+				status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
+		} else
+			status = WLAN_STATUS_INVALID_IE;
+
+	} else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
+
+		int group_cipher = 0, pairwise_cipher = 0;
+
+		wpa_ie = elems.wpa_ie;
+		wpa_ie_len = elems.wpa_ie_len;
+
+		if (rtw_parse_wpa_ie(wpa_ie - 2, wpa_ie_len + 2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			pstat->dot8021xalg = 1;/* psk,  todo:802.1x						 */
+			pstat->wpa_psk |= BIT(0);
+
+			pstat->wpa_group_cipher = group_cipher & psecuritypriv->wpa_group_cipher;
+			pstat->wpa_pairwise_cipher = pairwise_cipher & psecuritypriv->wpa_pairwise_cipher;
+
+			if (!pstat->wpa_group_cipher)
+				status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
+
+			if (!pstat->wpa_pairwise_cipher)
+				status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
+
+		} else
+			status = WLAN_STATUS_INVALID_IE;
+
+	} else {
+		wpa_ie = NULL;
+		wpa_ie_len = 0;
+	}
+
+	if (_STATS_SUCCESSFUL_ != status)
+		goto OnAssocReqFail;
+
+	pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
+	/* if (hapd->conf->wps_state && wpa_ie == NULL) { */ /* todo: to check ap if supporting WPS */
+	if (wpa_ie == NULL) {
+		if (elems.wps_ie) {
+			RTW_INFO("STA included WPS IE in "
+				 "(Re)Association Request - assume WPS is "
+				 "used\n");
+			pstat->flags |= WLAN_STA_WPS;
+			/* wpabuf_free(sta->wps_ie); */
+			/* sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4, */
+			/*				elems.wps_ie_len - 4); */
+		} else {
+			RTW_INFO("STA did not include WPA/RSN IE "
+				 "in (Re)Association Request - possible WPS "
+				 "use\n");
+			pstat->flags |= WLAN_STA_MAYBE_WPS;
+		}
+
+
+		/* AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
+		/* that the selected registrar of AP is _FLASE */
+		if ((psecuritypriv->wpa_psk > 0)
+		    && (pstat->flags & (WLAN_STA_WPS | WLAN_STA_MAYBE_WPS))) {
+			if (pmlmepriv->wps_beacon_ie) {
+				u8 selected_registrar = 0;
+
+				rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR , &selected_registrar, NULL);
+
+				if (!selected_registrar) {
+					RTW_INFO("selected_registrar is false , or AP is not ready to do WPS\n");
+
+					status = _STATS_UNABLE_HANDLE_STA_;
+
+					goto OnAssocReqFail;
+				}
+			}
+		}
+
+	} else {
+		int copy_len;
+
+		if (psecuritypriv->wpa_psk == 0) {
+			RTW_INFO("STA " MAC_FMT ": WPA/RSN IE in association "
+				"request, but AP don't support WPA/RSN\n", MAC_ARG(pstat->hwaddr));
+
+			status = WLAN_STATUS_INVALID_IE;
+
+			goto OnAssocReqFail;
+
+		}
+
+		if (elems.wps_ie) {
+			RTW_INFO("STA included WPS IE in "
+				 "(Re)Association Request - WPS is "
+				 "used\n");
+			pstat->flags |= WLAN_STA_WPS;
+			copy_len = 0;
+		} else
+			copy_len = ((wpa_ie_len + 2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)) : (wpa_ie_len + 2);
+
+
+		if (copy_len > 0)
+			memcpy(pstat->wpa_ie, wpa_ie - 2, copy_len);
+
+	}
+
+
+	/* check if there is WMM IE & support WWM-PS */
+	pstat->flags &= ~WLAN_STA_WME;
+	pstat->qos_option = 0;
+	pstat->qos_info = 0;
+	pstat->has_legacy_ac = true;
+	pstat->uapsd_vo = 0;
+	pstat->uapsd_vi = 0;
+	pstat->uapsd_be = 0;
+	pstat->uapsd_bk = 0;
+	if (pmlmepriv->qospriv.qos_option) {
+		p = pframe + WLAN_HDR_A3_LEN + ie_offset;
+		ie_len = 0;
+		for (;;) {
+			p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+			if (p != NULL) {
+				if (!memcmp(p + 2, WMM_IE, 6)) {
+
+					pstat->flags |= WLAN_STA_WME;
+
+					pstat->qos_option = 1;
+					pstat->qos_info = *(p + 8);
+
+					pstat->max_sp_len = (pstat->qos_info >> 5) & 0x3;
+
+					if ((pstat->qos_info & 0xf) != 0xf)
+						pstat->has_legacy_ac = true;
+					else
+						pstat->has_legacy_ac = false;
+
+					if (pstat->qos_info & 0xf) {
+						if (pstat->qos_info & BIT(0))
+							pstat->uapsd_vo = BIT(0) | BIT(1);
+						else
+							pstat->uapsd_vo = 0;
+
+						if (pstat->qos_info & BIT(1))
+							pstat->uapsd_vi = BIT(0) | BIT(1);
+						else
+							pstat->uapsd_vi = 0;
+
+						if (pstat->qos_info & BIT(2))
+							pstat->uapsd_bk = BIT(0) | BIT(1);
+						else
+							pstat->uapsd_bk = 0;
+
+						if (pstat->qos_info & BIT(3))
+							pstat->uapsd_be = BIT(0) | BIT(1);
+						else
+							pstat->uapsd_be = 0;
+
+					}
+
+					break;
+				}
+			} else
+				break;
+			p = p + ie_len + 2;
+		}
+	}
+
+
+	if (pmlmepriv->htpriv.ht_option == false)
+		goto bypass_ht_chk;
+
+	/* save HT capabilities in the sta object */
+	memset(&pstat->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));
+	if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap)) {
+		pstat->flags |= WLAN_STA_HT;
+
+		pstat->flags |= WLAN_STA_WME;
+
+		memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap));
+
+	} else
+		pstat->flags &= ~WLAN_STA_HT;
+bypass_ht_chk:
+
+	if ((pmlmepriv->htpriv.ht_option == false) && (pstat->flags & WLAN_STA_HT)) {
+		rtw_warn_on(1);
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+
+	if (((pstat->flags & WLAN_STA_HT) || (pstat->flags & WLAN_STA_VHT)) &&
+	    ((pstat->wpa2_pairwise_cipher & WPA_CIPHER_TKIP) ||
+	     (pstat->wpa_pairwise_cipher & WPA_CIPHER_TKIP))) {
+
+		RTW_INFO("(V)HT: " MAC_FMT " tried to use TKIP with (V)HT association\n", MAC_ARG(pstat->hwaddr));
+
+		pstat->flags &= ~WLAN_STA_HT;
+		pstat->flags &= ~WLAN_STA_VHT;
+		/*status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY;
+		  * goto OnAssocReqFail;
+		*/
+	}
+
+	/*
+	 * if (hapd->iface->current_mode->mode == HOSTAPD_MODE_IEEE80211G) */ /* ? */
+	pstat->flags |= WLAN_STA_NONERP;
+	for (i = 0; i < pstat->bssratelen; i++) {
+		if ((pstat->bssrateset[i] & 0x7f) > 22) {
+			pstat->flags &= ~WLAN_STA_NONERP;
+			break;
+		}
+	}
+
+	if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
+	else
+		pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
+
+
+
+	if (status != _STATS_SUCCESSFUL_)
+		goto OnAssocReqFail;
+
+#ifdef CONFIG_P2P
+	pstat->is_p2p_device = false;
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , NULL, &p2pielen);
+		if (p2pie) {
+			pstat->is_p2p_device = true;
+			p2p_status_code = (u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat);
+			if (p2p_status_code > 0) {
+				pstat->p2p_status_code = p2p_status_code;
+				status = _STATS_CAP_FAIL_;
+				goto OnAssocReqFail;
+			}
+		}
+#ifdef CONFIG_WFD
+		rtw_process_wfd_ies(padapter, pframe + WLAN_HDR_A3_LEN + ie_offset, pkt_len - WLAN_HDR_A3_LEN - ie_offset, __func__);
+#endif
+	}
+	pstat->p2p_status_code = p2p_status_code;
+#endif /* CONFIG_P2P */
+
+	/* TODO: identify_proprietary_vendor_ie(); */
+	/* Realtek proprietary IE */
+	/* identify if this is Broadcom sta */
+	/* identify if this is ralink sta */
+	/* Customer proprietary IE */
+
+
+
+	/* get a unique AID */
+	if (pstat->aid > 0)
+		RTW_INFO("  old AID %d\n", pstat->aid);
+	else {
+		for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++) {
+			if (pstapriv->sta_aid[pstat->aid - 1] == NULL) {
+				if (pstat->aid > pstapriv->max_num_sta) {
+					pstat->aid = 0;
+
+					RTW_INFO("  no room for more AIDs\n");
+
+					status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
+
+					goto OnAssocReqFail;
+
+
+				} else {
+					pstapriv->sta_aid[pstat->aid - 1] = pstat;
+					RTW_INFO("allocate new AID = (%d)\n", pstat->aid);
+					break;
+				}
+			}
+		}
+	}
+
+
+	pstat->state &= (~WIFI_FW_ASSOC_STATE);
+	pstat->state |= WIFI_FW_ASSOC_SUCCESS;
+	/* RTW_INFO("==================%s, %d,  (%x), bpairwise_key_installed=%d, MAC:"MAC_FMT"\n"
+	, __func__, __LINE__, pstat->state, pstat->bpairwise_key_installed, MAC_ARG(pstat->hwaddr)); */
+#ifdef CONFIG_IEEE80211W
+	if (pstat->bpairwise_key_installed != true)
+#endif /* CONFIG_IEEE80211W */
+	{
+		_enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
+		if (!list_empty(&pstat->auth_list)) {
+			list_del_init(&pstat->auth_list);
+			pstapriv->auth_list_cnt--;
+		}
+		_exit_critical_bh(&pstapriv->auth_list_lock, &irqL);
+
+		_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+		if (list_empty(&pstat->asoc_list)) {
+			pstat->expire_to = pstapriv->expire_to;
+			list_add_tail(&pstat->asoc_list, &pstapriv->asoc_list);
+			pstapriv->asoc_list_cnt++;
+		}
+		_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+	}
+
+	/* now the station is qualified to join our BSS...	 */
+	if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_ == status)) {
+#ifdef CONFIG_NATIVEAP_MLME
+#ifdef CONFIG_IEEE80211W
+		if (pstat->bpairwise_key_installed != true)
+#endif /* CONFIG_IEEE80211W */
+		{
+			/* .1 bss_cap_update & sta_info_update */
+			bss_cap_update_on_sta_join(padapter, pstat);
+			sta_info_update(padapter, pstat);
+		}
+#ifdef CONFIG_IEEE80211W
+		if (pstat->bpairwise_key_installed)
+			status = _STATS_REFUSED_TEMPORARILY_;
+#endif /* CONFIG_IEEE80211W */
+		/* .2 issue assoc rsp before notify station join event. */
+		if (frame_type == WIFI_ASSOCREQ)
+			issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+		else
+			issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+
+#ifdef CONFIG_IOCTL_CFG80211
+		_enter_critical_bh(&pstat->lock, &irqL);
+		if (pstat->passoc_req) {
+			rtw_mfree(pstat->passoc_req, pstat->assoc_req_len);
+			pstat->passoc_req = NULL;
+			pstat->assoc_req_len = 0;
+		}
+
+		pstat->passoc_req =  rtw_zmalloc(pkt_len);
+		if (pstat->passoc_req) {
+			memcpy(pstat->passoc_req, pframe, pkt_len);
+			pstat->assoc_req_len = pkt_len;
+		}
+		_exit_critical_bh(&pstat->lock, &irqL);
+#endif /* CONFIG_IOCTL_CFG80211 */
+#ifdef CONFIG_IEEE80211W
+		if (pstat->bpairwise_key_installed != true)
+#endif /* CONFIG_IEEE80211W */
+		{
+			/* .3-(1) report sta add event */
+			report_add_sta_event(padapter, pstat->hwaddr);
+		}
+#ifdef CONFIG_IEEE80211W
+		if (pstat->bpairwise_key_installed && padapter->securitypriv.binstallBIPkey == true) {
+			RTW_INFO(MAC_FMT"\n", MAC_ARG(pstat->hwaddr));
+			issue_action_SA_Query(padapter, pstat->hwaddr, 0, 0, IEEE80211W_RIGHT_KEY);
+		}
+#endif /* CONFIG_IEEE80211W */
+#endif /* CONFIG_NATIVEAP_MLME */
+	}
+
+	return _SUCCESS;
+
+asoc_class2_error:
+
+#ifdef CONFIG_NATIVEAP_MLME
+	issue_deauth(padapter, (void *)get_addr2_ptr(pframe), status);
+#endif
+
+	return _FAIL;
+
+OnAssocReqFail:
+
+
+#ifdef CONFIG_NATIVEAP_MLME
+	pstat->aid = 0;
+	if (frame_type == WIFI_ASSOCREQ)
+		issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+	else
+		issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+#endif
+
+
+#endif /* CONFIG_AP_MODE */
+
+	return _FAIL;
+
+}
+
+unsigned int OnAssocRsp(_adapter *padapter, union recv_frame *precv_frame)
+{
+	uint i;
+	int res;
+	unsigned short	status;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	/* WLAN_BSSID_EX 		*cur_network = &(pmlmeinfo->network); */
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+	PNDIS_802_11_VARIABLE_IEs	pWapiIE = NULL;
+
+	RTW_INFO("%s\n", __func__);
+
+	/* check A1 matches or not */
+	if (memcmp(adapter_mac_addr(padapter), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
+		return _SUCCESS;
+
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+		return _SUCCESS;
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	/* status */
+	status = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 2));
+	if (status > 0) {
+		RTW_INFO("assoc reject, status code: %d\n", status);
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		res = -4;
+		goto report_assoc_result;
+	}
+
+	/* get capabilities */
+	pmlmeinfo->capability = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	/* set slot time */
+	pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10)) ? 9 : 20;
+
+	/* AID */
+	res = pmlmeinfo->aid = (int)(le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 4)) & 0x3fff);
+
+	/* following are moved to join event callback function */
+	/* to handle HT, WMM, rate adaptive, update MAC reg */
+	/* for not to handle the synchronous IO in the tasklet */
+	for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;) {
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			if (!memcmp(pIE->data, WMM_PARA_OUI, 6))	/* WMM */
+				WMM_param_handler(padapter, pIE);
+#if defined(CONFIG_P2P) && defined(CONFIG_WFD)
+			else if (!memcmp(pIE->data, WFD_OUI, 4))		/* WFD */
+				rtw_process_wfd_ie(padapter, (u8 *)pIE, pIE->Length, __func__);
+#endif
+			break;
+
+#ifdef CONFIG_WAPI_SUPPORT
+		case _WAPI_IE_:
+			pWapiIE = pIE;
+			break;
+#endif
+
+		case _HT_CAPABILITY_IE_:	/* HT caps */
+			HT_caps_handler(padapter, pIE);
+			break;
+
+		case _HT_EXTRA_INFO_IE_:	/* HT info */
+			HT_info_handler(padapter, pIE);
+			break;
+
+		case _ERPINFO_IE_:
+			ERP_IE_handler(padapter, pIE);
+			break;
+#ifdef CONFIG_TDLS
+		case _EXT_CAP_IE_:
+			if (check_ap_tdls_prohibited(pIE->data, pIE->Length))
+				padapter->tdlsinfo.ap_prohibited = true;
+			if (check_ap_tdls_ch_switching_prohibited(pIE->data, pIE->Length))
+				padapter->tdlsinfo.ch_switch_prohibited = true;
+			break;
+#endif /* CONFIG_TDLS */
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+#ifdef CONFIG_WAPI_SUPPORT
+	rtw_wapi_on_assoc_ok(padapter, pIE);
+#endif
+
+	pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
+	pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+
+	/* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
+	UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);
+
+report_assoc_result:
+	if (res > 0)
+		rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
+	else
+		rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
+
+	report_join_res(padapter, res);
+
+	return _SUCCESS;
+}
+
+unsigned int OnDeAuth(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned short	reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_P2P */
+
+	/* check A3 */
+	if (!(!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+	RTW_INFO(FUNC_ADPT_FMT" - Start to Disconnect\n", FUNC_ADPT_ARG(padapter));
+
+#ifdef CONFIG_P2P
+	if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
+		_cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+		_set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
+	}
+#endif /* CONFIG_P2P */
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	rtw_lock_rx_suspend_timeout(8000);
+
+#ifdef CONFIG_AP_MODE
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		unsigned long irqL;
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		/* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		 */
+		/* rtw_free_stainfo(padapter, psta); */
+		/* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		 */
+
+		RTW_PRINT(FUNC_ADPT_FMT" reason=%u, ta=%pM\n"
+			, FUNC_ADPT_ARG(padapter), reason, get_addr2_ptr(pframe));
+
+		psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe));
+		if (psta) {
+			u8 updated = false;
+
+			_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+			if (list_empty(&psta->asoc_list) == false) {
+				list_del_init(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, false, reason, true);
+
+			}
+			_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+			associated_clients_update(padapter, updated, STA_INFO_UPDATE_ALL);
+		}
+
+
+		return _SUCCESS;
+	} else
+#endif
+	{
+		int	ignore_received_deauth = 0;
+
+		/*	Commented by Albert 20130604 */
+		/*	Before sending the auth frame to start the STA/GC mode connection with AP/GO,  */
+		/*	we will send the deauth first. */
+		/*	However, the Win8.1 with BRCM Wi-Fi will send the deauth with reason code 6 to us after receieving our deauth. */
+		/*	Added the following code to avoid this case. */
+		if ((pmlmeinfo->state & WIFI_FW_AUTH_STATE) ||
+		    (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)) {
+			if (reason == WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA)
+				ignore_received_deauth = 1;
+			else if (WLAN_REASON_PREV_AUTH_NOT_VALID == reason) {
+				/* TODO: 802.11r */
+				ignore_received_deauth = 1;
+			}
+		}
+
+		RTW_PRINT(FUNC_ADPT_FMT" reason=%u, ta=%pM, ignore=%d\n"
+			, FUNC_ADPT_ARG(padapter), reason, get_addr2_ptr(pframe), ignore_received_deauth);
+
+		if (0 == ignore_received_deauth)
+			receive_disconnect(padapter, get_addr2_ptr(pframe), reason, false);
+	}
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+	return _SUCCESS;
+
+}
+
+unsigned int OnDisassoc(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned short	reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_P2P */
+
+	/* check A3 */
+	if (!(!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+	RTW_INFO(FUNC_ADPT_FMT" - Start to Disconnect\n", FUNC_ADPT_ARG(padapter));
+
+#ifdef CONFIG_P2P
+	if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
+		_cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+		_set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
+	}
+#endif /* CONFIG_P2P */
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	rtw_lock_rx_suspend_timeout(8000);
+
+#ifdef CONFIG_AP_MODE
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		unsigned long irqL;
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		/* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);	 */
+		/* rtw_free_stainfo(padapter, psta); */
+		/* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);		 */
+
+		RTW_PRINT(FUNC_ADPT_FMT" reason=%u, ta=%pM\n"
+			, FUNC_ADPT_ARG(padapter), reason, get_addr2_ptr(pframe));
+
+		psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe));
+		if (psta) {
+			u8 updated = false;
+
+			_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+			if (list_empty(&psta->asoc_list) == false) {
+				list_del_init(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, false, reason, true);
+
+			}
+			_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+			associated_clients_update(padapter, updated, STA_INFO_UPDATE_ALL);
+		}
+
+		return _SUCCESS;
+	} else
+#endif
+	{
+		RTW_PRINT(FUNC_ADPT_FMT" reason=%u, ta=%pM\n"
+			, FUNC_ADPT_ARG(padapter), reason, get_addr2_ptr(pframe));
+
+		receive_disconnect(padapter, get_addr2_ptr(pframe), reason, false);
+	}
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+	return _SUCCESS;
+
+}
+
+unsigned int OnAtim(_adapter *padapter, union recv_frame *precv_frame)
+{
+	RTW_INFO("%s\n", __func__);
+	return _SUCCESS;
+}
+
+static unsigned int on_action_spct_ch_switch(_adapter *padapter, struct sta_info *psta, u8 *ies, uint ies_len)
+{
+	unsigned int ret = _FAIL;
+	struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(mlmeext->mlmext_info);
+
+	if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+		ret = _SUCCESS;
+		goto exit;
+	}
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
+
+		int ch_switch_mode = -1, ch = -1, ch_switch_cnt = -1;
+		int ch_offset = -1;
+		u8 bwmode;
+		struct ieee80211_info_element *ie;
+
+		RTW_INFO(FUNC_NDEV_FMT" from "MAC_FMT"\n",
+			FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(psta->hwaddr));
+
+		for_each_ie(ie, ies, ies_len) {
+			if (ie->id == WLAN_EID_CHANNEL_SWITCH) {
+				ch_switch_mode = ie->data[0];
+				ch = ie->data[1];
+				ch_switch_cnt = ie->data[2];
+				RTW_INFO("ch_switch_mode:%d, ch:%d, ch_switch_cnt:%d\n",
+					 ch_switch_mode, ch, ch_switch_cnt);
+			} else if (ie->id == WLAN_EID_SECONDARY_CHANNEL_OFFSET) {
+				ch_offset = secondary_ch_offset_to_hal_ch_offset(ie->data[0]);
+				RTW_INFO("ch_offset:%d\n", ch_offset);
+			}
+		}
+
+		if (ch == -1)
+			return _SUCCESS;
+
+		if (ch_offset == -1)
+			bwmode = mlmeext->cur_bwmode;
+		else
+			bwmode = (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) ?
+				 CHANNEL_WIDTH_20 : CHANNEL_WIDTH_40;
+
+		ch_offset = (ch_offset == -1) ? mlmeext->cur_ch_offset : ch_offset;
+
+		/* todo:
+		 * 1. the decision of channel switching
+		 * 2. things after channel switching
+		 */
+
+		ret = rtw_set_ch_cmd(padapter, ch, bwmode, ch_offset, true);
+	}
+
+exit:
+	return ret;
+}
+
+unsigned int on_action_spct(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint frame_len = precv_frame->u.hdr.len;
+	u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+	u8 category;
+	u8 action;
+
+	RTW_INFO(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+
+	psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe));
+
+	if (!psta)
+		goto exit;
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case RTW_WLAN_ACTION_SPCT_MSR_REQ:
+	case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
+	case RTW_WLAN_ACTION_SPCT_TPC_REQ:
+	case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
+		break;
+	case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
+#ifdef CONFIG_SPCT_CH_SWITCH
+		ret = on_action_spct_ch_switch(padapter, psta, &frame_body[2],
+				       frame_len - (frame_body - pframe) - 2);
+#endif
+		break;
+	default:
+		break;
+	}
+
+exit:
+	return ret;
+}
+
+unsigned int OnAction_qos(_adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int OnAction_dls(_adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+#ifdef CONFIG_RTW_WNM
+unsigned int on_action_wnm(_adapter *adapter, union recv_frame *rframe)
+{
+	unsigned int ret = _FAIL;
+	struct sta_info *sta = NULL;
+	struct sta_priv *stapriv = &adapter->stapriv;
+	u8 *frame = rframe->u.hdr.rx_data;
+	uint frame_len = rframe->u.hdr.len;
+	u8 *frame_body = (u8 *)(frame + sizeof(struct rtw_ieee80211_hdr_3addr));
+	u8 category;
+	u8 action;
+	int cnt = 0;
+	char msg[16];
+
+	sta = rtw_get_stainfo(stapriv, get_addr2_ptr(frame));
+	if (!sta)
+		goto exit;
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_WNM)
+		goto exit;
+
+	action = frame_body[1];
+
+	switch (action) {
+	default:
+		#ifdef CONFIG_IOCTL_CFG80211
+		cnt += sprintf((msg + cnt), "ACT_WNM %u", action);
+		rtw_cfg80211_rx_action(adapter, rframe, msg);
+		#endif
+		ret = _SUCCESS;
+		break;
+	}
+
+exit:
+	return ret;
+}
+#endif /* CONFIG_RTW_WNM */
+
+/**
+ * rtw_rx_ampdu_size - Get the target RX AMPDU buffer size for the specific @adapter
+ * @adapter: the adapter to get target RX AMPDU buffer size
+ *
+ * Returns: the target RX AMPDU buffer size
+ */
+u8 rtw_rx_ampdu_size(_adapter *adapter)
+{
+	u8 size;
+	HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor;
+
+	if (adapter->fix_rx_ampdu_size != RX_AMPDU_SIZE_INVALID) {
+		size = adapter->fix_rx_ampdu_size;
+		goto exit;
+	}
+
+#ifdef CONFIG_BT_COEXIST
+	if (rtw_btcoex_IsBTCoexCtrlAMPDUSize(adapter)) {
+		size = rtw_btcoex_GetAMPDUSize(adapter);
+		goto exit;
+	}
+#endif
+
+	/* for scan */
+	if (!mlmeext_chk_scan_state(&adapter->mlmeextpriv, SCAN_DISABLE)
+	    && !mlmeext_chk_scan_state(&adapter->mlmeextpriv, SCAN_COMPLETE)
+	    && adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_size != RX_AMPDU_SIZE_INVALID
+	   ) {
+		size = adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_size;
+		goto exit;
+	}
+
+	/* default value based on max_rx_ampdu_factor */
+	if (adapter->driver_rx_ampdu_factor != 0xFF)
+		max_rx_ampdu_factor = (HT_CAP_AMPDU_FACTOR)adapter->driver_rx_ampdu_factor;
+	else
+		rtw_hal_get_def_var(adapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
+	
+	/* In Maximum A-MPDU Length Exponent subfield of A-MPDU Parameters field of HT Capabilities element,
+		the unit of max_rx_ampdu_factor are octets. 8K, 16K, 32K, 64K is right.
+		But the buffer size subfield of Block Ack Parameter Set field in ADDBA action frame indicates
+		the number of buffers available for this particular TID. Each buffer is equal to max. size of 
+		MSDU or AMSDU. 
+		The size variable means how many MSDUs or AMSDUs, it's not Kbytes.
+	*/
+	if (MAX_AMPDU_FACTOR_64K == max_rx_ampdu_factor)
+		size = 64;
+	else if (MAX_AMPDU_FACTOR_32K == max_rx_ampdu_factor)
+		size = 32;
+	else if (MAX_AMPDU_FACTOR_16K == max_rx_ampdu_factor)
+		size = 16;
+	else if (MAX_AMPDU_FACTOR_8K == max_rx_ampdu_factor)
+		size = 8;
+	else
+		size = 64;
+
+exit:
+
+	if (size > 127)
+		size = 127;
+
+	return size;
+}
+
+/**
+ * rtw_rx_ampdu_is_accept - Get the permission if RX AMPDU should be set up for the specific @adapter
+ * @adapter: the adapter to get the permission if RX AMPDU should be set up
+ *
+ * Returns: accept or not
+ */
+bool rtw_rx_ampdu_is_accept(_adapter *adapter)
+{
+	bool accept;
+
+	if (adapter->fix_rx_ampdu_accept != RX_AMPDU_ACCEPT_INVALID) {
+		accept = adapter->fix_rx_ampdu_accept;
+		goto exit;
+	}
+
+#ifdef CONFIG_BT_COEXIST
+	if (rtw_btcoex_IsBTCoexRejectAMPDU(adapter)) {
+		accept = false;
+		goto exit;
+	}
+#endif
+
+	/* for scan */
+	if (!mlmeext_chk_scan_state(&adapter->mlmeextpriv, SCAN_DISABLE)
+	    && !mlmeext_chk_scan_state(&adapter->mlmeextpriv, SCAN_COMPLETE)
+	    && adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_accept != RX_AMPDU_ACCEPT_INVALID
+	   ) {
+		accept = adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_accept;
+		goto exit;
+	}
+
+	/* default value for other cases */
+	accept = adapter->mlmeextpriv.mlmext_info.bAcceptAddbaReq;
+
+exit:
+	return accept;
+}
+
+/**
+ * rtw_rx_ampdu_set_size - Set the target RX AMPDU buffer size for the specific @adapter and specific @reason
+ * @adapter: the adapter to set target RX AMPDU buffer size
+ * @size: the target RX AMPDU buffer size to set
+ * @reason: reason for the target RX AMPDU buffer size setting
+ *
+ * Returns: whether the target RX AMPDU buffer size is changed
+ */
+bool rtw_rx_ampdu_set_size(_adapter *adapter, u8 size, u8 reason)
+{
+	bool is_adj = false;
+	struct mlme_ext_priv *mlmeext;
+	struct mlme_ext_info *mlmeinfo;
+
+	mlmeext = &adapter->mlmeextpriv;
+	mlmeinfo = &mlmeext->mlmext_info;
+
+	if (reason == RX_AMPDU_DRV_FIXED) {
+		if (adapter->fix_rx_ampdu_size != size) {
+			adapter->fix_rx_ampdu_size = size;
+			is_adj = true;
+			RTW_INFO(FUNC_ADPT_FMT" fix_rx_ampdu_size:%u\n", FUNC_ADPT_ARG(adapter), size);
+		}
+	} else if (reason == RX_AMPDU_DRV_SCAN) {
+		struct ss_res *ss = &adapter->mlmeextpriv.sitesurvey_res;
+
+		if (ss->rx_ampdu_size != size) {
+			ss->rx_ampdu_size = size;
+			is_adj = true;
+			RTW_INFO(FUNC_ADPT_FMT" ss.rx_ampdu_size:%u\n", FUNC_ADPT_ARG(adapter), size);
+		}
+	}
+
+	return is_adj;
+}
+
+/**
+ * rtw_rx_ampdu_set_accept - Set the permission if RX AMPDU should be set up for the specific @adapter and specific @reason
+ * @adapter: the adapter to set if RX AMPDU should be set up
+ * @accept: if RX AMPDU should be set up
+ * @reason: reason for the permission if RX AMPDU should be set up
+ *
+ * Returns: whether the permission if RX AMPDU should be set up is changed
+ */
+bool rtw_rx_ampdu_set_accept(_adapter *adapter, u8 accept, u8 reason)
+{
+	bool is_adj = false;
+	struct mlme_ext_priv *mlmeext;
+	struct mlme_ext_info *mlmeinfo;
+
+	mlmeext = &adapter->mlmeextpriv;
+	mlmeinfo = &mlmeext->mlmext_info;
+
+	if (reason == RX_AMPDU_DRV_FIXED) {
+		if (adapter->fix_rx_ampdu_accept != accept) {
+			adapter->fix_rx_ampdu_accept = accept;
+			is_adj = true;
+			RTW_INFO(FUNC_ADPT_FMT" fix_rx_ampdu_accept:%u\n", FUNC_ADPT_ARG(adapter), accept);
+		}
+	} else if (reason == RX_AMPDU_DRV_SCAN) {
+		if (adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_accept != accept) {
+			adapter->mlmeextpriv.sitesurvey_res.rx_ampdu_accept = accept;
+			is_adj = true;
+			RTW_INFO(FUNC_ADPT_FMT" ss.rx_ampdu_accept:%u\n", FUNC_ADPT_ARG(adapter), accept);
+		}
+	}
+
+	return is_adj;
+}
+
+/**
+ * rx_ampdu_apply_sta_tid - Apply RX AMPDU setting to the specific @sta and @tid
+ * @adapter: the adapter to which @sta belongs
+ * @sta: the sta to be checked
+ * @tid: the tid to be checked
+ * @accept: the target permission if RX AMPDU should be set up
+ * @size: the target RX AMPDU buffer size
+ *
+ * Returns:
+ * 0: no canceled
+ * 1: canceled by no permission
+ * 2: canceled by different buffer size
+ * 3: canceled by potential mismatched status
+ *
+ * Blocking function, may sleep
+ */
+u8 rx_ampdu_apply_sta_tid(_adapter *adapter, struct sta_info *sta, u8 tid, u8 accept, u8 size)
+{
+	u8 ret = 0;
+	struct recv_reorder_ctrl *reorder_ctl = &sta->recvreorder_ctrl[tid];
+
+	if (reorder_ctl->enable == false) {
+		if (reorder_ctl->ampdu_size != RX_AMPDU_SIZE_INVALID) {
+			send_delba_sta_tid_wait_ack(adapter, 0, sta, tid, 1);
+			ret = 3;
+		}
+		goto exit;
+	}
+
+	if (accept == false) {
+		send_delba_sta_tid_wait_ack(adapter, 0, sta, tid, 0);
+		ret = 1;
+	} else if (reorder_ctl->ampdu_size != size) {
+		send_delba_sta_tid_wait_ack(adapter, 0, sta, tid, 0);
+		ret = 2;
+	}
+
+exit:
+	return ret;
+}
+
+/**
+ * rx_ampdu_apply_sta - Apply RX AMPDU setting to the specific @sta
+ * @adapter: the adapter to which @sta belongs
+ * @sta: the sta to be checked
+ * @accept: the target permission if RX AMPDU should be set up
+ * @size: the target RX AMPDU buffer size
+ *
+ * Returns: number of the RX AMPDU assciation canceled for applying current target setting
+ *
+ * Blocking function, may sleep
+ */
+u8 rx_ampdu_apply_sta(_adapter *adapter, struct sta_info *sta, u8 accept, u8 size)
+{
+	u8 change_cnt = 0;
+	int i;
+
+	for (i = 0; i < TID_NUM; i++) {
+		if (rx_ampdu_apply_sta_tid(adapter, sta, i, accept, size) != 0)
+			change_cnt++;
+	}
+
+	return change_cnt;
+}
+
+/**
+ * rtw_rx_ampdu_apply - Apply the current target RX AMPDU setting for the specific @adapter
+ * @adapter: the adapter to be applied
+ *
+ * Returns: number of the RX AMPDU assciation canceled for applying current target setting
+ */
+u16 rtw_rx_ampdu_apply(_adapter *adapter)
+{
+	u16 adj_cnt = 0;
+	struct mlme_ext_priv *mlmeext;
+	struct sta_info *sta;
+	u8 accept = rtw_rx_ampdu_is_accept(adapter);
+	u8 size = rtw_rx_ampdu_size(adapter);
+
+	mlmeext = &adapter->mlmeextpriv;
+
+	if (mlmeext_msr(mlmeext) == WIFI_FW_STATION_STATE) {
+		sta = rtw_get_stainfo(&adapter->stapriv, get_bssid(&adapter->mlmepriv));
+		if (sta)
+			adj_cnt += rx_ampdu_apply_sta(adapter, sta, accept, size);
+
+	} else if (mlmeext_msr(mlmeext) == WIFI_FW_AP_STATE) {
+		unsigned long irqL;
+		_list *phead, *plist;
+		u8 peer_num = 0;
+		char peers[NUM_STA];
+		struct sta_priv *pstapriv = &adapter->stapriv;
+		int i;
+
+		_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+		phead = &pstapriv->asoc_list;
+		plist = get_next(phead);
+
+		while ((rtw_end_of_queue_search(phead, plist)) == false) {
+			int stainfo_offset;
+
+			sta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+			plist = get_next(plist);
+
+			stainfo_offset = rtw_stainfo_offset(pstapriv, sta);
+			if (stainfo_offset_valid(stainfo_offset))
+				peers[peer_num++] = stainfo_offset;
+		}
+
+		_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+		for (i = 0; i < peer_num; i++) {
+			sta = rtw_get_stainfo_by_offset(pstapriv, peers[i]);
+			if (sta)
+				adj_cnt += rx_ampdu_apply_sta(adapter, sta, accept, size);
+		}
+	}
+
+	return adj_cnt;
+}
+
+unsigned int OnAction_back(_adapter *padapter, union recv_frame *precv_frame)
+{
+	u8 *addr;
+	struct sta_info *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	unsigned char		*frame_body;
+	unsigned char		category, action;
+	unsigned short	tid, status, reason_code = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+
+	RTW_INFO("%s\n", __func__);
+
+	/* check RA matches or not	 */
+	if (memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+			return _SUCCESS;
+
+	addr = get_addr2_ptr(pframe);
+	psta = rtw_get_stainfo(pstapriv, addr);
+
+	if (psta == NULL)
+		return _SUCCESS;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+	if (category == RTW_WLAN_CATEGORY_BACK) { /* representing Block Ack */
+#ifdef CONFIG_TDLS
+		if ((psta->tdls_sta_state & TDLS_LINKED_STATE) &&
+		    (psta->htpriv.ht_option) &&
+		    (psta->htpriv.ampdu_enable))
+			RTW_INFO("Recv [%s] from direc link\n", __func__);
+		else
+#endif /* CONFIG_TDLS */
+			if (!pmlmeinfo->HT_enable)
+				return _SUCCESS;
+
+		action = frame_body[1];
+		RTW_INFO("%s, action=%d\n", __func__, action);
+		switch (action) {
+		case RTW_WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
+
+			memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
+			/* process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), GetAddr3Ptr(pframe)); */
+			process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), addr);
+
+			break;
+
+		case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
+
+			/* status = frame_body[3] | (frame_body[4] << 8); */ /* endian issue */
+			status = RTW_GET_LE16(&frame_body[3]);
+			tid = ((frame_body[5] >> 2) & 0x7);
+			if (status == 0) {
+				/* successful					 */
+				RTW_INFO("agg_enable for TID=%d\n", tid);
+				psta->htpriv.agg_enable_bitmap |= 1 << tid;
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+				/* amsdu in ampdu */
+				if (frame_body[5] & 1)
+					psta->htpriv.tx_amsdu_enable = true;
+			} else
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+
+			if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+				RTW_INFO("%s alive check - rx ADDBA response\n", __func__);
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+				psta->expire_to = pstapriv->expire_to;
+				psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+			}
+
+			/* RTW_INFO("marc: ADDBA RSP: %x\n", pmlmeinfo->agg_enable_bitmap); */
+			break;
+
+		case RTW_WLAN_ACTION_DELBA: /* DELBA */
+			if ((frame_body[3] & BIT(3)) == 0) {
+				psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+				psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+
+				/* reason_code = frame_body[4] | (frame_body[5] << 8); */
+				reason_code = RTW_GET_LE16(&frame_body[4]);
+			} else if ((frame_body[3] & BIT(3)) == BIT(3)) {
+				tid = (frame_body[3] >> 4) & 0x0F;
+
+				preorder_ctrl = &psta->recvreorder_ctrl[tid];
+				preorder_ctrl->enable = false;
+				preorder_ctrl->ampdu_size = RX_AMPDU_SIZE_INVALID;
+			}
+
+			RTW_INFO("%s(): DELBA: %x(%x)\n", __func__, pmlmeinfo->agg_enable_bitmap, reason_code);
+			/* todo: how to notify the host while receiving DELETE BA */
+			break;
+
+		default:
+			break;
+		}
+	}
+	return _SUCCESS;
+}
+
+#ifdef CONFIG_P2P
+
+static int get_reg_classes_full_count(struct p2p_channels channel_list)
+{
+	int cnt = 0;
+	int i;
+
+	for (i = 0; i < channel_list.reg_classes; i++)
+		cnt += channel_list.reg_class[i].channels;
+
+	return cnt;
+}
+
+static void get_channel_cnt_24g_5gl_5gh(struct mlme_ext_priv *pmlmeext, u8 *p24g_cnt, u8 *p5gl_cnt, u8 *p5gh_cnt)
+{
+	int	i = 0;
+
+	*p24g_cnt = 0;
+	*p5gl_cnt = 0;
+	*p5gh_cnt = 0;
+
+	for (i = 0; i < pmlmeext->max_chan_nums; i++) {
+		if (pmlmeext->channel_set[i].ChannelNum <= 14)
+			(*p24g_cnt)++;
+		else if ((pmlmeext->channel_set[i].ChannelNum > 14) && (pmlmeext->channel_set[i].ChannelNum <= 48)) {
+			/*	Just include the channel 36, 40, 44, 48 channels for 5G low */
+			(*p5gl_cnt)++;
+		} else if ((pmlmeext->channel_set[i].ChannelNum >= 149) && (pmlmeext->channel_set[i].ChannelNum <= 161)) {
+			/*	Just include the channel 149, 153, 157, 161 channels for 5G high */
+			(*p5gh_cnt)++;
+		}
+	}
+}
+
+void issue_p2p_GO_request(_adapter *padapter, u8 *raddr)
+{
+
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_GO_NEGO_REQ;
+	u8			wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+	u8			wpsielen = 0, p2pielen = 0, i;
+	u8			channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
+	u16			len_channellist_attr = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	RTW_INFO("[%s] In\n", __func__);
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	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, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+	pwdinfo->negotiation_dialog_token = 1;	/*	Initialize the dialog value */
+	pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &(pattrib->pktlen));
+
+
+
+	/*	WPS Section */
+	wpsielen = 0;
+	/*	WPS OUI */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	/*	Device Password ID */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+
+	if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
+	else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
+	else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
+
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen);
+
+
+	/*	P2P IE Section. */
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20110306 */
+	/*	According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes */
+	/*	1. P2P Capability */
+	/*	2. Group Owner Intent */
+	/*	3. Configuration Timeout */
+	/*	4. Listen Channel */
+	/*	5. Extended Listen Timing */
+	/*	6. Intended P2P Interface Address */
+	/*	7. Channel List */
+	/*	8. P2P Device Info */
+	/*	9. Operating Channel */
+
+
+	/*	P2P Capability */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Device Capability Bitmap, 1 byte */
+	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+	/*	Group Capability Bitmap, 1 byte */
+	if (pwdinfo->persistent_supported)
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+	else
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
+
+
+	/*	Group Owner Intent */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Todo the tie breaker bit. */
+	p2pie[p2pielen++] = ((pwdinfo->intent << 1) &  0xFE);
+
+	/*	Configuration Timeout */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P GO */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P Client */
+
+
+	/*	Listen Channel */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Operating Class */
+	p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
+
+	/*	Channel Number */
+	p2pie[p2pielen++] = pwdinfo->listen_channel;	/*	listening channel number */
+
+
+	/*	Extended Listen Timing ATTR */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Availability Period */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+	p2pielen += 2;
+
+	/*	Availability Interval */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+	p2pielen += 2;
+
+
+	/*	Intended P2P Interface Address */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_INTENDED_IF_ADDR;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+
+	/*	Channel List */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+	/* Length: */
+	/* Country String(3) */
+	/* + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */
+	/* + number of channels in all classes */
+	len_channellist_attr = 3
+		       + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
+		       + get_reg_classes_full_count(pmlmeext->channel_list);
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(5 + 1);
+	else
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+#else
+
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+
+#endif
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Channel Entry List */
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) {
+		u8 union_ch = rtw_mi_get_union_chan(padapter);
+
+		/*	Operating Class */
+		if (union_ch > 14) {
+			if (union_ch >= 149)
+				p2pie[p2pielen++] = 0x7c;
+			else
+				p2pie[p2pielen++] = 0x73;
+		} else
+			p2pie[p2pielen++] = 0x51;
+
+
+		/*	Number of Channels */
+		/*	Just support 1 channel and this channel is AP's channel */
+		p2pie[p2pielen++] = 1;
+
+		/*	Channel List */
+		p2pie[p2pielen++] = union_ch;
+	} else {
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			/*	Number of Channels */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			/*	Channel List */
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++)
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+		}
+	}
+#else /* CONFIG_CONCURRENT_MODE */
+	{
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			/*	Number of Channels */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			/*	Channel List */
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++)
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+		}
+	}
+#endif /* CONFIG_CONCURRENT_MODE */
+
+	/*	Device Info */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	21->P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)  */
+	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Config Method */
+	/*	This field should be big endian. Noted by P2P specification. */
+
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+
+	p2pielen += 2;
+
+	/*	Primary Device Type */
+	/*	Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+	p2pielen += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+	p2pielen += 2;
+
+	/*	Number of Secondary Device Types */
+	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
+
+	/*	Device Name */
+	/*	Type: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len);
+	p2pielen += pwdinfo->device_name_len;
+
+
+	/*	Operating Channel */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Operating Class */
+	if (pwdinfo->operating_channel <= 14) {
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x51;
+	} else if ((pwdinfo->operating_channel >= 36) && (pwdinfo->operating_channel <= 48)) {
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x73;
+	} else {
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x7c;
+	}
+
+	/*	Channel Number */
+	p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen);
+
+#ifdef CONFIG_WFD
+	wfdielen = build_nego_req_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+static void issue_p2p_GO_response(_adapter *padapter, u8 *raddr, u8 *frame_body, uint len, u8 result)
+{
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_GO_NEGO_RESP;
+	u8			wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+	u8			p2pielen = 0, i;
+	uint			wpsielen = 0;
+	u16			wps_devicepassword_id = 0x0000;
+	uint			wps_devicepassword_id_len = 0;
+	u8			channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh;
+	u16			len_channellist_attr = 0;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	__be16 be_tmp;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	RTW_INFO("[%s] In, result = %d\n", __func__,  result);
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	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, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+	pwdinfo->negotiation_dialog_token = frame_body[7];	/*	The Dialog Token of provisioning discovery request frame. */
+	pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
+
+	/*	Commented by Albert 20110328 */
+	/*	Try to get the device password ID from the WPS IE of group negotiation request frame */
+	/*	WiFi Direct test plan 5.1.15 */
+	rtw_get_wps_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
+	rtw_get_wps_attr_content(wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, (u8 *) &be_tmp, &wps_devicepassword_id_len);
+	wps_devicepassword_id = be16_to_cpu(be_tmp);
+
+	memset(wpsie, 0x00, 255);
+	wpsielen = 0;
+
+	/*	WPS Section */
+	wpsielen = 0;
+	/*	WPS OUI */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	/*	Device Password ID */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
+	else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
+	else
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
+	wpsielen += 2;
+
+	/*	Commented by Kurt 20120113 */
+	/*	If some device wants to do p2p handshake without sending prov_disc_req */
+	/*	We have to get peer_req_cm from here. */
+	if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) {
+		if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
+			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
+		else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
+			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
+		else
+			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
+	}
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen);
+
+	/*	P2P IE Section. */
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20100908 */
+	/*	According to the P2P Specification, the group negoitation response frame should contain 9 P2P attributes */
+	/*	1. Status */
+	/*	2. P2P Capability */
+	/*	3. Group Owner Intent */
+	/*	4. Configuration Timeout */
+	/*	5. Operating Channel */
+	/*	6. Intended P2P Interface Address */
+	/*	7. Channel List */
+	/*	8. Device Info */
+	/*	9. Group ID	( Only GO ) */
+
+	/*	ToDo: */
+
+	/*	P2P Status */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_STATUS;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = result;
+
+	/*	P2P Capability */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Device Capability Bitmap, 1 byte */
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+		/*	Commented by Albert 2011/03/08 */
+		/*	According to the P2P specification */
+		/*	if the sending device will be client, the P2P Capability should be reserved of group negotation response frame */
+		p2pie[p2pielen++] = 0;
+	} else {
+		/*	Be group owner or meet the error case */
+		p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+	}
+
+	/*	Group Capability Bitmap, 1 byte */
+	if (pwdinfo->persistent_supported)
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+	else
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
+
+	/*	Group Owner Intent */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	if (pwdinfo->peer_intent & 0x01) {
+		/*	Peer's tie breaker bit is 1, our tie breaker bit should be 0 */
+		p2pie[p2pielen++] = (pwdinfo->intent << 1);
+	} else {
+		/*	Peer's tie breaker bit is 0, our tie breaker bit should be 1 */
+		p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
+	}
+
+	/*	Configuration Timeout */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P GO */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P Client */
+
+	/*	Operating Channel */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Operating Class */
+	if (pwdinfo->operating_channel <= 14) {
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x51;
+	} else if ((pwdinfo->operating_channel >= 36) && (pwdinfo->operating_channel <= 48)) {
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x73;
+	} else {
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x7c;
+	}
+
+	/*	Channel Number */
+	p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
+
+	/*	Intended P2P Interface Address	 */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_INTENDED_IF_ADDR;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Channel List */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+	/* Country String(3) */
+	/* + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */
+	/* + number of channels in all classes */
+	len_channellist_attr = 3
+		       + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+		       + get_reg_classes_full_count(pmlmeext->channel_list);
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(5 + 1);
+	else
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+#else
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+
+#endif
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Channel Entry List */
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) {
+
+		u8 union_chan = rtw_mi_get_union_chan(padapter);
+
+		/*Operating Class*/
+		if (union_chan > 14) {
+			if (union_chan >= 149)
+				p2pie[p2pielen++] = 0x7c;
+			else
+				p2pie[p2pielen++] = 0x73;
+
+		} else
+			p2pie[p2pielen++] = 0x51;
+
+		/*	Number of Channels
+			Just support 1 channel and this channel is AP's channel*/
+		p2pie[p2pielen++] = 1;
+
+		/*Channel List*/
+		p2pie[p2pielen++] = union_chan;
+	} else {
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			/*	Number of Channels */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			/*	Channel List */
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++)
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+		}
+	}
+#else /* CONFIG_CONCURRENT_MODE */
+	{
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			/*	Number of Channels */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			/*	Channel List */
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++)
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+		}
+	}
+#endif /* CONFIG_CONCURRENT_MODE */
+
+
+	/*	Device Info */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	21->P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)  */
+	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Config Method */
+	/*	This field should be big endian. Noted by P2P specification. */
+
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+
+	p2pielen += 2;
+
+	/*	Primary Device Type */
+	/*	Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+	p2pielen += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+	p2pielen += 2;
+
+	/*	Number of Secondary Device Types */
+	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
+
+	/*	Device Name */
+	/*	Type: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len);
+	p2pielen += pwdinfo->device_name_len;
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		/*	Group ID Attribute */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	p2P Device Address */
+		memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN);
+		p2pielen += ETH_ALEN;
+
+		/*	SSID */
+		memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
+		p2pielen += pwdinfo->nego_ssidlen;
+
+	}
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen);
+
+#ifdef CONFIG_WFD
+	wfdielen = build_nego_resp_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+static void issue_p2p_GO_confirm(_adapter *padapter, u8 *raddr, u8 result)
+{
+
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	__be32 p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_GO_NEGO_CONF;
+	u8			wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+	u8			wpsielen = 0, p2pielen = 0;
+
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	RTW_INFO("[%s] In\n", __func__);
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	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, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
+
+
+
+	/*	P2P IE Section. */
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20110306 */
+	/*	According to the P2P Specification, the group negoitation request frame should contain 5 P2P attributes */
+	/*	1. Status */
+	/*	2. P2P Capability */
+	/*	3. Operating Channel */
+	/*	4. Channel List */
+	/*	5. Group ID	( if this WiFi is GO ) */
+
+	/*	P2P Status */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_STATUS;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = result;
+
+	/*	P2P Capability */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Device Capability Bitmap, 1 byte */
+	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+	/*	Group Capability Bitmap, 1 byte */
+	if (pwdinfo->persistent_supported)
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
+	else
+		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
+
+
+	/*	Operating Channel */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+		if (pwdinfo->peer_operating_ch <= 14) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x51;
+		} else if ((pwdinfo->peer_operating_ch >= 36) && (pwdinfo->peer_operating_ch <= 48)) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x73;
+		} else {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x7c;
+		}
+
+		p2pie[p2pielen++] = pwdinfo->peer_operating_ch;
+	} else {
+		if (pwdinfo->operating_channel <= 14) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x51;
+		} else if ((pwdinfo->operating_channel >= 36) && (pwdinfo->operating_channel <= 48)) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x73;
+		} else {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x7c;
+		}
+
+		/*	Channel Number */
+		p2pie[p2pielen++] = pwdinfo->operating_channel;		/*	Use the listen channel as the operating channel */
+	}
+
+
+	/*	Channel List */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+	*(u16 *)(p2pie + p2pielen) = 6;
+	p2pielen += 2;
+
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Value: */
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
+		if (pwdinfo->peer_operating_ch <= 14) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x51;
+		} else if ((pwdinfo->peer_operating_ch >= 36) && (pwdinfo->peer_operating_ch <= 48)) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x73;
+		} else {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x7c;
+		}
+		p2pie[p2pielen++] = 1;
+		p2pie[p2pielen++] = pwdinfo->peer_operating_ch;
+	} else {
+		if (pwdinfo->operating_channel <= 14) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x51;
+		} else if ((pwdinfo->operating_channel >= 36) && (pwdinfo->operating_channel <= 48)) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x73;
+		} else {
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x7c;
+		}
+
+		/*	Channel Number */
+		p2pie[p2pielen++] = 1;
+		p2pie[p2pielen++] = pwdinfo->operating_channel;		/*	Use the listen channel as the operating channel */
+	}
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		/*	Group ID Attribute */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	p2P Device Address */
+		memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN);
+		p2pielen += ETH_ALEN;
+
+		/*	SSID */
+		memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
+		p2pielen += pwdinfo->nego_ssidlen;
+	}
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen);
+
+#ifdef CONFIG_WFD
+	wfdielen = build_nego_confirm_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+void issue_p2p_invitation_request(_adapter *padapter, u8 *raddr)
+{
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8 action = P2P_PUB_ACTION_ACTION;
+	__be32 p2poui = cpu_to_be32(P2POUI);
+	u8 oui_subtype = P2P_INVIT_REQ;
+	u8 p2pie[255] = { 0x00 };
+	u8 p2pielen = 0, i;
+	u8 dialogToken = 3;
+	u8 channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
+	u16 len_channellist_attr = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	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, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+	/*	P2P IE Section. */
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20101011 */
+	/*	According to the P2P Specification, the P2P Invitation request frame should contain 7 P2P attributes */
+	/*	1. Configuration Timeout */
+	/*	2. Invitation Flags */
+	/*	3. Operating Channel	( Only GO ) */
+	/*	4. P2P Group BSSID	( Should be included if I am the GO ) */
+	/*	5. Channel List */
+	/*	6. P2P Group ID */
+	/*	7. P2P Device Info */
+
+	/*	Configuration Timeout */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P GO */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P Client */
+
+	/*	Invitation Flags */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_INVITATION_FLAGS;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = P2P_INVITATION_FLAGS_PERSISTENT;
+
+
+	/*	Operating Channel */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Operating Class */
+	if (pwdinfo->invitereq_info.operating_ch <= 14)
+		p2pie[p2pielen++] = 0x51;
+	else if ((pwdinfo->invitereq_info.operating_ch >= 36) && (pwdinfo->invitereq_info.operating_ch <= 48))
+		p2pie[p2pielen++] = 0x73;
+	else
+		p2pie[p2pielen++] = 0x7c;
+
+	/*	Channel Number */
+	p2pie[p2pielen++] = pwdinfo->invitereq_info.operating_ch;	/*	operating channel number */
+
+	if (!memcmp(adapter_mac_addr(padapter), pwdinfo->invitereq_info.go_bssid, ETH_ALEN)) {
+		/*	P2P Group BSSID */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	P2P Device Address for GO */
+		memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
+		p2pielen += ETH_ALEN;
+	}
+
+	/*	Channel List */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+
+	/*	Length: */
+	/* Country String(3) */
+	/* + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */
+	/* + number of channels in all classes */
+	len_channellist_attr = 3
+		       + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+		       + get_reg_classes_full_count(pmlmeext->channel_list);
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(5 + 1);
+	else
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+#else
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+#endif
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Country String */
+	p2pie[p2pielen++] = 'X';
+	p2pie[p2pielen++] = 'X';
+
+	/*	The third byte should be set to 0x04. */
+	/*	Described in the "Operating Channel Attribute" section. */
+	p2pie[p2pielen++] = 0x04;
+
+	/*	Channel Entry List */
+#ifdef CONFIG_CONCURRENT_MODE
+	if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) {
+		u8 union_ch =  rtw_mi_get_union_chan(padapter);
+
+		/*	Operating Class */
+		if (union_ch > 14) {
+			if (union_ch >= 149)
+				p2pie[p2pielen++] = 0x7c;
+			else
+				p2pie[p2pielen++] = 0x73;
+		} else
+			p2pie[p2pielen++] = 0x51;
+
+
+		/*	Number of Channels */
+		/*	Just support 1 channel and this channel is AP's channel */
+		p2pie[p2pielen++] = 1;
+
+		/*	Channel List */
+		p2pie[p2pielen++] = union_ch;
+	} else {
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			/*	Number of Channels */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			/*	Channel List */
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++)
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+		}
+	}
+#else /* CONFIG_CONCURRENT_MODE */
+	{
+		int i, j;
+		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+			/*	Operating Class */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+			/*	Number of Channels */
+			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+			/*	Channel List */
+			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++)
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+		}
+	}
+#endif /* CONFIG_CONCURRENT_MODE */
+
+
+	/*	P2P Group ID */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(6 + pwdinfo->invitereq_info.ssidlen);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address for GO */
+	memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	SSID */
+	memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid, pwdinfo->invitereq_info.ssidlen);
+	p2pielen += pwdinfo->invitereq_info.ssidlen;
+
+
+	/*	Device Info */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	21->P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)  */
+	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
+	p2pielen += ETH_ALEN;
+
+	/*	Config Method */
+	/*	This field should be big endian. Noted by P2P specification. */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
+	p2pielen += 2;
+
+	/*	Primary Device Type */
+	/*	Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+	p2pielen += 2;
+
+	/*	OUI */
+	*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+	p2pielen += 4;
+
+	/*	Sub Category ID */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+	p2pielen += 2;
+
+	/*	Number of Secondary Device Types */
+	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
+
+	/*	Device Name */
+	/*	Type: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+	p2pielen += pwdinfo->device_name_len;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen);
+
+#ifdef CONFIG_WFD
+	wfdielen = build_invitation_req_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+void issue_p2p_invitation_response(_adapter *padapter, u8 *raddr, u8 dialogToken, u8 status_code)
+{
+
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	__be32 p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_INVIT_RESP;
+	u8			p2pie[255] = { 0x00 };
+	u8			p2pielen = 0, i;
+	u8			channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
+	u16			len_channellist_attr = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	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, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+	/*	P2P IE Section. */
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20101005 */
+	/*	According to the P2P Specification, the P2P Invitation response frame should contain 5 P2P attributes */
+	/*	1. Status */
+	/*	2. Configuration Timeout */
+	/*	3. Operating Channel	( Only GO ) */
+	/*	4. P2P Group BSSID	( Only GO ) */
+	/*	5. Channel List */
+
+	/*	P2P Status */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_STATUS;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE. */
+	/*	Sent the event receiving the P2P Invitation Req frame to DMP UI. */
+	/*	DMP had to compare the MAC address to find out the profile. */
+	/*	So, the WiFi driver will send the P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB. */
+	/*	If the UI found the corresponding profile, the WiFi driver sends the P2P Invitation Req */
+	/*	to NB to rebuild the persistent group. */
+	p2pie[p2pielen++] = status_code;
+
+	/*	Configuration Timeout */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
+
+	/*	Length: */
+	*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P GO */
+	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P Client */
+
+	if (status_code == P2P_STATUS_SUCCESS) {
+		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+			/*	The P2P Invitation request frame asks this Wi-Fi device to be the P2P GO */
+			/*	In this case, the P2P Invitation response frame should carry the two more P2P attributes. */
+			/*	First one is operating channel attribute. */
+			/*	Second one is P2P Group BSSID attribute. */
+
+			/*	Operating Channel */
+			/*	Type: */
+			p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+			/*	Length: */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+			p2pielen += 2;
+
+			/*	Value: */
+			/*	Country String */
+			p2pie[p2pielen++] = 'X';
+			p2pie[p2pielen++] = 'X';
+
+			/*	The third byte should be set to 0x04. */
+			/*	Described in the "Operating Channel Attribute" section. */
+			p2pie[p2pielen++] = 0x04;
+
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
+
+			/*	Channel Number */
+			p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
+
+
+			/*	P2P Group BSSID */
+			/*	Type: */
+			p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
+
+			/*	Length: */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
+			p2pielen += 2;
+
+			/*	Value: */
+			/*	P2P Device Address for GO */
+			memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
+			p2pielen += ETH_ALEN;
+
+		}
+
+		/*	Channel List */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
+
+		/*	Length: */
+		/* Country String(3) */
+		/* + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */
+		/* + number of channels in all classes */
+		len_channellist_attr = 3
+			+ (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
+			+ get_reg_classes_full_count(pmlmeext->channel_list);
+
+#ifdef CONFIG_CONCURRENT_MODE
+		if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(5 + 1);
+		else
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+#else
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
+#endif
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	Country String */
+		p2pie[p2pielen++] = 'X';
+		p2pie[p2pielen++] = 'X';
+
+		/*	The third byte should be set to 0x04. */
+		/*	Described in the "Operating Channel Attribute" section. */
+		p2pie[p2pielen++] = 0x04;
+
+		/*	Channel Entry List */
+#ifdef CONFIG_CONCURRENT_MODE
+		if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) {
+			u8 union_ch = rtw_mi_get_union_chan(padapter);
+
+			/*	Operating Class */
+			if (union_ch > 14) {
+				if (union_ch >= 149)
+					p2pie[p2pielen++]  = 0x7c;
+				else
+					p2pie[p2pielen++] = 0x73;
+			} else
+				p2pie[p2pielen++] = 0x51;
+
+
+			/*	Number of Channels */
+			/*	Just support 1 channel and this channel is AP's channel */
+			p2pie[p2pielen++] = 1;
+
+			/*	Channel List */
+			p2pie[p2pielen++] = union_ch;
+		} else {
+			int i, j;
+			for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+				/*	Operating Class */
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+				/*	Number of Channels */
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+				/*	Channel List */
+				for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++)
+					p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+			}
+		}
+#else /* CONFIG_CONCURRENT_MODE */
+		{
+			int i, j;
+			for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
+				/*	Operating Class */
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
+
+				/*	Number of Channels */
+				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
+
+				/*	Channel List */
+				for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++)
+					p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
+			}
+		}
+#endif /* CONFIG_CONCURRENT_MODE */
+	}
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen);
+
+#ifdef CONFIG_WFD
+	wfdielen = build_invitation_resp_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+void issue_p2p_provision_request(_adapter *padapter, u8 *pssid, u8 ussidlen, u8 *pdev_raddr)
+{
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	u8			dialogToken = 1;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_PROVISION_DISC_REQ;
+	u8			wpsie[100] = { 0x00 };
+	u8			wpsielen = 0;
+	u32			p2pielen = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	RTW_INFO("[%s] In\n", __func__);
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, pdev_raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, pdev_raddr, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+	p2pielen = build_prov_disc_request_p2p_ie(pwdinfo, pframe, pssid, ussidlen, pdev_raddr);
+
+	pframe += p2pielen;
+	pattrib->pktlen += p2pielen;
+
+	wpsielen = 0;
+	/*	WPS OUI */
+	*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+	wpsielen += 4;
+
+	/*	WPS version */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+	wpsielen += 2;
+
+	/*	Value: */
+	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+	/*	Config Method */
+	/*	Type: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+	wpsielen += 2;
+
+	/*	Length: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->tx_prov_disc_info.wps_config_method_request);
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen);
+
+
+#ifdef CONFIG_WFD
+	wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+
+static u8 is_matched_in_profilelist(u8 *peermacaddr, struct profile_info *profileinfo)
+{
+	u8 i, match_result = 0;
+
+	RTW_INFO("[%s] peermac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
+		peermacaddr[0], peermacaddr[1], peermacaddr[2], peermacaddr[3], peermacaddr[4], peermacaddr[5]);
+
+	for (i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++) {
+		RTW_INFO("[%s] profileinfo_mac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
+			profileinfo->peermac[0], profileinfo->peermac[1], profileinfo->peermac[2], profileinfo->peermac[3], profileinfo->peermac[4], profileinfo->peermac[5]);
+		if (!memcmp(peermacaddr, profileinfo->peermac, ETH_ALEN)) {
+			match_result = 1;
+			RTW_INFO("[%s] Match!\n", __func__);
+			break;
+		}
+	}
+
+	return match_result ;
+}
+
+void issue_probersp_p2p(_adapter *padapter, unsigned char *da)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	unsigned char					*mac;
+	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	/* WLAN_BSSID_EX 		*cur_network = &(pmlmeinfo->network); */
+	u16					beacon_interval = 100;
+	u16					capInfo = 0;
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8					wpsie[255] = { 0x00 };
+	u32					wpsielen = 0, p2pielen = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+#ifdef CONFIG_INTEL_WIDI
+	u8 zero_array_check[L2SDTA_SERVICE_VE_LEN] = { 0x00 };
+#endif /* CONFIG_INTEL_WIDI */
+
+	/* RTW_INFO("%s\n", __func__); */
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	if (IS_CCK_RATE(pattrib->rate)) {
+		/* force OFDM 6M rate */
+		pattrib->rate = MGN_6M;
+		pattrib->raid = rtw_get_mgntframe_raid(padapter, WIRELESS_11G);
+	}
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	mac = adapter_mac_addr(padapter);
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	/*	Use the device address for BSSID field.	 */
+	memcpy(pwlanhdr->addr3, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(fctrl, WIFI_PROBERSP);
+
+	pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+	/* timestamp will be inserted by hardware */
+	pframe += 8;
+	pattrib->pktlen += 8;
+
+	/* beacon interval: 2 bytes */
+	memcpy(pframe, (unsigned char *) &beacon_interval, 2);
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/*	capability info: 2 bytes */
+	/*	ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of WiFi Direct Spec) */
+	capInfo |= cap_ShortPremble;
+	capInfo |= cap_ShortSlot;
+
+	memcpy(pframe, (unsigned char *) &capInfo, 2);
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+
+	/* SSID */
+	pframe = rtw_set_ie(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid, &pattrib->pktlen);
+
+	/* supported rates... */
+	/*	Use the OFDM rate in the P2P probe response frame. ( 6(B), 9(B), 12, 18, 24, 36, 48, 54 ) */
+	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
+
+	/* DS parameter set */
+	pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&pwdinfo->listen_channel, &pattrib->pktlen);
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) {
+		if (pmlmepriv->wps_probe_resp_ie != NULL && pmlmepriv->p2p_probe_resp_ie != NULL) {
+			/* WPS IE */
+			memcpy(pframe, pmlmepriv->wps_probe_resp_ie, pmlmepriv->wps_probe_resp_ie_len);
+			pattrib->pktlen += pmlmepriv->wps_probe_resp_ie_len;
+			pframe += pmlmepriv->wps_probe_resp_ie_len;
+
+			/* P2P IE */
+			memcpy(pframe, pmlmepriv->p2p_probe_resp_ie, pmlmepriv->p2p_probe_resp_ie_len);
+			pattrib->pktlen += pmlmepriv->p2p_probe_resp_ie_len;
+			pframe += pmlmepriv->p2p_probe_resp_ie_len;
+		}
+	} else
+#endif /* CONFIG_IOCTL_CFG80211		 */
+	{
+
+		/*	Todo: WPS IE */
+		/*	Noted by Albert 20100907 */
+		/*	According to the WPS specification, all the WPS attribute is presented by Big Endian. */
+
+		wpsielen = 0;
+		/*	WPS OUI */
+		*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+		wpsielen += 4;
+
+		/*	WPS version */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+		wpsielen += 2;
+
+		/*	Value: */
+		wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+#ifdef CONFIG_INTEL_WIDI
+		/*	Commented by Kurt */
+		/*	Appended WiDi info. only if we did issued_probereq_widi(), and then we saved ven. ext. in pmlmepriv->sa_ext. */
+		if (!memcmp(pmlmepriv->sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN) == false
+		    || pmlmepriv->num_p2p_sdt != 0) {
+			/* Sec dev type */
+			*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SEC_DEV_TYPE_LIST);
+			wpsielen += 2;
+
+			/*	Length: */
+			*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008);
+			wpsielen += 2;
+
+			/*	Value: */
+			/*	Category ID */
+			*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_DISPLAYS);
+			wpsielen += 2;
+
+			/*	OUI */
+			*(__be32 *)(wpsie + wpsielen) = cpu_to_be32(INTEL_DEV_TYPE_OUI);
+			wpsielen += 4;
+
+			*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_WIDI_CONSUMER_SINK);
+			wpsielen += 2;
+
+			if (!memcmp(pmlmepriv->sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN) == false) {
+				/*	Vendor Extension */
+				memcpy(wpsie + wpsielen, pmlmepriv->sa_ext, L2SDTA_SERVICE_VE_LEN);
+				wpsielen += L2SDTA_SERVICE_VE_LEN;
+			}
+		}
+#endif /* CONFIG_INTEL_WIDI */
+
+		/*	WiFi Simple Config State */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SIMPLE_CONF_STATE);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+		wpsielen += 2;
+
+		/*	Value: */
+		wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG;	/*	Not Configured. */
+
+		/*	Response Type */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_RESP_TYPE);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+		wpsielen += 2;
+
+		/*	Value: */
+		wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
+
+		/*	UUID-E */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010);
+		wpsielen += 2;
+
+		/*	Value: */
+		if (pwdinfo->external_uuid == 0) {
+			memset(wpsie + wpsielen, 0x0, 16);
+			memcpy(wpsie + wpsielen, mac, ETH_ALEN);
+		} else
+			memcpy(wpsie + wpsielen, pwdinfo->uuid, 0x10);
+		wpsielen += 0x10;
+
+		/*	Manufacturer */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MANUFACTURER);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0007);
+		wpsielen += 2;
+
+		/*	Value: */
+		memcpy(wpsie + wpsielen, "Realtek", 7);
+		wpsielen += 7;
+
+		/*	Model Name */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NAME);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0006);
+		wpsielen += 2;
+
+		/*	Value: */
+		memcpy(wpsie + wpsielen, "8192CU", 6);
+		wpsielen += 6;
+
+		/*	Model Number */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NUMBER);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+		wpsielen += 2;
+
+		/*	Value: */
+		wpsie[wpsielen++] = 0x31;		/*	character 1 */
+
+		/*	Serial Number */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_SERIAL_NUMBER);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(ETH_ALEN);
+		wpsielen += 2;
+
+		/*	Value: */
+		memcpy(wpsie + wpsielen, "123456" , ETH_ALEN);
+		wpsielen += ETH_ALEN;
+
+		/*	Primary Device Type */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008);
+		wpsielen += 2;
+
+		/*	Value: */
+		/*	Category ID */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+		wpsielen += 2;
+
+		/*	OUI */
+		*(__be32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI);
+		wpsielen += 4;
+
+		/*	Sub Category ID */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+		wpsielen += 2;
+
+		/*	Device Name */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len);
+		wpsielen += 2;
+
+		/*	Value: */
+		memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len);
+		wpsielen += pwdinfo->device_name_len;
+
+		/*	Config Method */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+		wpsielen += 2;
+
+		/*	Value: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+		wpsielen += 2;
+
+
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen);
+
+
+		p2pielen = build_probe_resp_p2p_ie(pwdinfo, pframe);
+		pframe += p2pielen;
+		pattrib->pktlen += p2pielen;
+	}
+
+#ifdef CONFIG_WFD
+	wfdielen = rtw_append_probe_resp_wfd_ie(padapter, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+static int _issue_probereq_p2p(_adapter *padapter, u8 *da, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	unsigned char			*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	unsigned char			*mac;
+	unsigned char			bssrate[NumRates];
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	int	bssrate_len = 0;
+	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8					wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
+	u16					wpsielen = 0, p2pielen = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	if (IS_CCK_RATE(pattrib->rate)) {
+		/* force OFDM 6M rate */
+		pattrib->rate = MGN_6M;
+		pattrib->raid = rtw_get_mgntframe_raid(padapter, WIRELESS_11G);
+	}
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	mac = adapter_mac_addr(padapter);
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	if (da) {
+		memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, da, ETH_ALEN);
+	} else {
+		if ((pwdinfo->p2p_info.scan_op_ch_only) || (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
+			/*	This two flags will be set when this is only the P2P client mode. */
+			memcpy(pwlanhdr->addr1, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
+			memcpy(pwlanhdr->addr3, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
+		} else {
+			/*	broadcast probe request frame */
+			memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+			memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
+		}
+	}
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_PROBEREQ);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
+		pframe = rtw_set_ie(pframe, _SSID_IE_, pwdinfo->tx_prov_disc_info.ssid.SsidLength, pwdinfo->tx_prov_disc_info.ssid.Ssid, &(pattrib->pktlen));
+	else
+		pframe = rtw_set_ie(pframe, _SSID_IE_, P2P_WILDCARD_SSID_LEN, pwdinfo->p2p_wildcard_ssid, &(pattrib->pktlen));
+	/*	Use the OFDM rate in the P2P probe request frame. ( 6(B), 9(B), 12(B), 24(B), 36, 48, 54 ) */
+	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) {
+		if (pmlmepriv->wps_probe_req_ie != NULL && pmlmepriv->p2p_probe_req_ie != NULL) {
+			/* WPS IE */
+			memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
+			pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
+			pframe += pmlmepriv->wps_probe_req_ie_len;
+
+			/* P2P IE */
+			memcpy(pframe, pmlmepriv->p2p_probe_req_ie, pmlmepriv->p2p_probe_req_ie_len);
+			pattrib->pktlen += pmlmepriv->p2p_probe_req_ie_len;
+			pframe += pmlmepriv->p2p_probe_req_ie_len;
+		}
+	} else
+#endif /* CONFIG_IOCTL_CFG80211 */
+	{
+
+		/*	WPS IE */
+		/*	Noted by Albert 20110221 */
+		/*	According to the WPS specification, all the WPS attribute is presented by Big Endian. */
+
+		wpsielen = 0;
+		/*	WPS OUI */
+		*(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
+		wpsielen += 4;
+
+		/*	WPS version */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
+		wpsielen += 2;
+
+		/*	Value: */
+		wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
+
+		if (pmlmepriv->wps_probe_req_ie == NULL) {
+			/*	UUID-E */
+			/*	Type: */
+			*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
+			wpsielen += 2;
+
+			/*	Length: */
+			*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0010);
+			wpsielen += 2;
+			/*	Value: */
+			if (pwdinfo->external_uuid == 0) {
+				memset(wpsie + wpsielen, 0x0, 16);
+				memcpy(wpsie + wpsielen, mac, ETH_ALEN);
+			} else
+				memcpy(wpsie + wpsielen, pwdinfo->uuid, 0x10);
+			wpsielen += 0x10;
+
+			/*	Config Method */
+			/*	Type: */
+			*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
+			wpsielen += 2;
+
+			/*	Length: */
+			*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+			wpsielen += 2;
+
+			/*	Value: */
+			*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
+			wpsielen += 2;
+		}
+
+		/*	Device Name */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(pwdinfo->device_name_len);
+		wpsielen += 2;
+
+		/*	Value: */
+		memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len);
+		wpsielen += pwdinfo->device_name_len;
+
+		/*	Primary Device Type */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0008);
+		wpsielen += 2;
+
+		/*	Value: */
+		/*	Category ID */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_RTK_WIDI);
+		wpsielen += 2;
+
+		/*	OUI */
+		*(__be32 *)(wpsie + wpsielen) = cpu_to_be32(WPSOUI);
+		wpsielen += 4;
+
+		/*	Sub Category ID */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_RTK_DMP);
+		wpsielen += 2;
+
+		/*	Device Password ID */
+		/*	Type: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
+		wpsielen += 2;
+
+		/*	Length: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
+		wpsielen += 2;
+
+		/*	Value: */
+		*(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);	/*	Registrar-specified */
+		wpsielen += 2;
+
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen);
+
+		/*	P2P OUI */
+		p2pielen = 0;
+		p2pie[p2pielen++] = 0x50;
+		p2pie[p2pielen++] = 0x6F;
+		p2pie[p2pielen++] = 0x9A;
+		p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+		/*	Commented by Albert 20110221 */
+		/*	According to the P2P Specification, the probe request frame should contain 5 P2P attributes */
+		/*	1. P2P Capability */
+		/*	2. P2P Device ID if this probe request wants to find the specific P2P device */
+		/*	3. Listen Channel */
+		/*	4. Extended Listen Timing */
+		/*	5. Operating Channel if this WiFi is working as the group owner now */
+
+		/*	P2P Capability */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	Device Capability Bitmap, 1 byte */
+		p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+		/*	Group Capability Bitmap, 1 byte */
+		if (pwdinfo->persistent_supported)
+			p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+		else
+			p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
+
+		/*	Listen Channel */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	Country String */
+		p2pie[p2pielen++] = 'X';
+		p2pie[p2pielen++] = 'X';
+
+		/*	The third byte should be set to 0x04. */
+		/*	Described in the "Operating Channel Attribute" section. */
+		p2pie[p2pielen++] = 0x04;
+
+		/*	Operating Class */
+		p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
+
+		/*	Channel Number */
+		p2pie[p2pielen++] = pwdinfo->listen_channel;	/*	listen channel */
+
+
+		/*	Extended Listen Timing */
+		/*	Type: */
+		p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+		/*	Length: */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+		p2pielen += 2;
+
+		/*	Value: */
+		/*	Availability Period */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+		p2pielen += 2;
+
+		/*	Availability Interval */
+		*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+		p2pielen += 2;
+
+		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+			/*	Operating Channel (if this WiFi is working as the group owner now) */
+			/*	Type: */
+			p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
+
+			/*	Length: */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
+			p2pielen += 2;
+
+			/*	Value: */
+			/*	Country String */
+			p2pie[p2pielen++] = 'X';
+			p2pie[p2pielen++] = 'X';
+
+			/*	The third byte should be set to 0x04. */
+			/*	Described in the "Operating Channel Attribute" section. */
+			p2pie[p2pielen++] = 0x04;
+
+			/*	Operating Class */
+			p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
+
+			/*	Channel Number */
+			p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
+
+		}
+
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen);
+
+	}
+
+#ifdef CONFIG_WFD
+	wfdielen = rtw_append_probe_req_wfd_ie(padapter, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+
+	if (wait_ack)
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+inline void issue_probereq_p2p(_adapter *adapter, u8 *da)
+{
+	_issue_probereq_p2p(adapter, da, false);
+}
+
+/*
+ * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT
+ * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX
+ * try_cnt means the maximal TX count to try
+ */
+int issue_probereq_p2p_ex(_adapter *adapter, u8 *da, int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	u32 start = jiffies;
+
+	do {
+		ret = _issue_probereq_p2p(adapter, da, wait_ms > 0 ? true : false);
+
+		i++;
+
+		if (RTW_CANNOT_RUN(adapter))
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			rtw_msleep_os(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+#ifndef DBG_XMIT_ACK
+		goto exit;
+#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(adapter), MAC_ARG(da), rtw_get_oper_ch(adapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(adapter), rtw_get_oper_ch(adapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+#endif /* CONFIG_P2P */
+
+static s32 rtw_action_public_decache(union recv_frame *rframe, u8 token_offset)
+{
+	_adapter *adapter = rframe->u.hdr.adapter;
+	struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
+	u8 *frame = rframe->u.hdr.rx_data;
+	u16 seq_ctrl = ((le16_to_cpu(rframe->u.hdr.attrib.seq_num) & 0xffff) << 4) |
+			((rframe->u.hdr.attrib.frag_num) & 0xf);
+	u8 token = *(rframe->u.hdr.rx_data + sizeof(struct rtw_ieee80211_hdr_3addr) + token_offset);
+
+	if (GetRetry(frame)) {
+		if ((seq_ctrl == mlmeext->action_public_rxseq)
+		    && (token == mlmeext->action_public_dialog_token)
+		   ) {
+			RTW_INFO(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x, token:%d\n",
+				FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
+			return _FAIL;
+		}
+	}
+
+	/* TODO: per sta seq & token */
+	mlmeext->action_public_rxseq = seq_ctrl;
+	mlmeext->action_public_dialog_token = token;
+
+	return _SUCCESS;
+}
+
+static unsigned int on_action_public_p2p(union recv_frame *precv_frame)
+{
+	_adapter *padapter = precv_frame->u.hdr.adapter;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	u8 *frame_body;
+#ifdef CONFIG_P2P
+	u8 *p2p_ie;
+	u32	p2p_ielen, wps_ielen;
+	struct	wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8	result = P2P_STATUS_SUCCESS;
+	u8	empty_addr[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+	u8 *merged_p2pie = NULL;
+	u32 merged_p2p_ielen = 0;
+#endif /* CONFIG_P2P */
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+#ifdef CONFIG_P2P
+	_cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+#ifdef CONFIG_IOCTL_CFG80211
+	if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211)
+		rtw_cfg80211_rx_p2p_action_public(padapter, precv_frame);
+	else
+#endif /* CONFIG_IOCTL_CFG80211 */
+	{
+		/*	Do nothing if the driver doesn't enable the P2P function. */
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
+			return _SUCCESS;
+
+		len -= sizeof(struct rtw_ieee80211_hdr_3addr);
+
+		switch (frame_body[6]) { /* OUI Subtype */
+		case P2P_GO_NEGO_REQ: {
+			RTW_INFO("[%s] Got GO Nego Req Frame\n", __func__);
+			memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
+
+			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
+				rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+
+			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL)) {
+				/*	Commented by Albert 20110526 */
+				/*	In this case, this means the previous nego fail doesn't be reset yet. */
+				_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+				/*	Restore the previous p2p state */
+				rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+				RTW_INFO("[%s] Restore the previous p2p state to %d\n", __func__, rtw_p2p_state(pwdinfo));
+			}
+#ifdef CONFIG_CONCURRENT_MODE
+			if (rtw_mi_buddy_check_fwstate(padapter, _FW_LINKED))
+				_cancel_timer_ex(&pwdinfo->ap_p2p_switch_timer);
+#endif /* CONFIG_CONCURRENT_MODE */
+
+			/*	Commented by Kurt 20110902 */
+			/* Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
+			if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
+				rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+
+			/*	Commented by Kurt 20120113 */
+			/*	Get peer_dev_addr here if peer doesn't issue prov_disc frame. */
+			if (!memcmp(pwdinfo->rx_prov_disc_info.peerDevAddr, empty_addr, ETH_ALEN))
+				memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, get_addr2_ptr(pframe), ETH_ALEN);
+
+			result = process_p2p_group_negotation_req(pwdinfo, frame_body, len);
+			issue_p2p_GO_response(padapter, get_addr2_ptr(pframe), frame_body, len, result);
+#ifdef CONFIG_INTEL_WIDI
+			if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) {
+				padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
+				_cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
+				intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
+			}
+#endif /* CONFIG_INTEL_WIDI */
+
+			/*	Commented by Albert 20110718 */
+			/*	No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer. */
+#ifdef CONFIG_CONCURRENT_MODE
+			/*	Commented by Albert 20120107 */
+			_set_timer(&pwdinfo->restore_p2p_state_timer, 3000);
+#else /* CONFIG_CONCURRENT_MODE */
+			_set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
+#endif /* CONFIG_CONCURRENT_MODE */
+			break;
+		}
+		case P2P_GO_NEGO_RESP: {
+			RTW_INFO("[%s] Got GO Nego Resp Frame\n", __func__);
+
+			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
+				/*	Commented by Albert 20110425 */
+				/*	The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function. */
+				_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+				pwdinfo->nego_req_info.benable = false;
+				result = process_p2p_group_negotation_resp(pwdinfo, frame_body, len);
+				issue_p2p_GO_confirm(pwdinfo->padapter, get_addr2_ptr(pframe), result);
+				if (P2P_STATUS_SUCCESS == result) {
+					if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT) {
+						pwdinfo->p2p_info.operation_ch[0] = pwdinfo->peer_operating_ch;
+#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
+						pwdinfo->p2p_info.operation_ch[1] = 1;	/* Check whether GO is operating in channel 1; */
+						pwdinfo->p2p_info.operation_ch[2] = 6;	/* Check whether GO is operating in channel 6; */
+						pwdinfo->p2p_info.operation_ch[3] = 11;	/* Check whether GO is operating in channel 11; */
+#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */
+						pwdinfo->p2p_info.scan_op_ch_only = 1;
+						_set_timer(&pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH);
+					}
+				}
+
+				/*	Reset the dialog token for group negotiation frames. */
+				pwdinfo->negotiation_dialog_token = 1;
+
+				if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
+					_set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
+			} else
+				RTW_INFO("[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __func__);
+
+			break;
+		}
+		case P2P_GO_NEGO_CONF: {
+			RTW_INFO("[%s] Got GO Nego Confirm Frame\n", __func__);
+			result = process_p2p_group_negotation_confirm(pwdinfo, frame_body, len);
+			if (P2P_STATUS_SUCCESS == result) {
+				if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT) {
+					pwdinfo->p2p_info.operation_ch[0] = pwdinfo->peer_operating_ch;
+#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
+					pwdinfo->p2p_info.operation_ch[1] = 1;	/* Check whether GO is operating in channel 1; */
+					pwdinfo->p2p_info.operation_ch[2] = 6;	/* Check whether GO is operating in channel 6; */
+					pwdinfo->p2p_info.operation_ch[3] = 11;	/* Check whether GO is operating in channel 11; */
+#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */
+					pwdinfo->p2p_info.scan_op_ch_only = 1;
+					_set_timer(&pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH);
+				}
+			}
+			break;
+		}
+		case P2P_INVIT_REQ: {
+			/*	Added by Albert 2010/10/05 */
+			/*	Received the P2P Invite Request frame. */
+
+			RTW_INFO("[%s] Got invite request frame!\n", __func__);
+			p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+			if (p2p_ie) {
+				/*	Parse the necessary information from the P2P Invitation Request frame. */
+				/*	For example: The MAC address of sending this P2P Invitation Request frame. */
+				u32	attr_contentlen = 0;
+				u8	status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+				struct group_id_info group_id;
+				u8	invitation_flag = 0;
+				int j = 0;
+
+				merged_p2p_ielen = rtw_get_p2p_merged_ies_len(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_);
+
+				merged_p2pie = rtw_zmalloc(merged_p2p_ielen + 2);	/* 2 is for EID and Length */
+				if (merged_p2pie == NULL) {
+					RTW_INFO("[%s] Malloc p2p ie fail\n", __func__);
+					goto exit;
+				}
+				memset(merged_p2pie, 0x00, merged_p2p_ielen);
+
+				merged_p2p_ielen = rtw_p2p_merge_ies(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, merged_p2pie);
+
+				rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen);
+				if (attr_contentlen) {
+
+					rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen);
+					/*	Commented by Albert 20120510 */
+					/*	Copy to the pwdinfo->p2p_peer_interface_addr. */
+					/*	So that the WFD UI ( or Sigma ) can get the peer interface address by using the following command. */
+					/*	#> iwpriv wlan0 p2p_get peer_ifa */
+					/*	After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant. */
+
+					if (attr_contentlen) {
+						RTW_INFO("[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
+							pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1],
+							pwdinfo->p2p_peer_interface_addr[2], pwdinfo->p2p_peer_interface_addr[3],
+							pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5]);
+					}
+
+					if (invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT) {
+						/*	Re-invoke the persistent group. */
+
+						memset(&group_id, 0x00, sizeof(struct group_id_info));
+						rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_ID, (u8 *) &group_id, &attr_contentlen);
+						if (attr_contentlen) {
+							if (!memcmp(group_id.go_device_addr, adapter_mac_addr(padapter), ETH_ALEN)) {
+								/*	The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO. */
+								rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO);
+								rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+								status_code = P2P_STATUS_SUCCESS;
+							} else {
+								/*	The p2p device sending this p2p invitation request wants to be the persistent GO. */
+								if (is_matched_in_profilelist(pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[0])) {
+									u8 operatingch_info[5] = { 0x00 };
+									if (rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info,
+										&attr_contentlen)) {
+										if (rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, (u32)operatingch_info[4]) >= 0) {
+											/*	The operating channel is acceptable for this device. */
+											pwdinfo->rx_invitereq_info.operation_ch[0] = operatingch_info[4];
+#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
+											pwdinfo->rx_invitereq_info.operation_ch[1] = 1;		/* Check whether GO is operating in channel 1; */
+											pwdinfo->rx_invitereq_info.operation_ch[2] = 6;		/* Check whether GO is operating in channel 6; */
+											pwdinfo->rx_invitereq_info.operation_ch[3] = 11;		/* Check whether GO is operating in channel 11; */
+#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */
+											pwdinfo->rx_invitereq_info.scan_op_ch_only = 1;
+											_set_timer(&pwdinfo->reset_ch_sitesurvey, P2P_RESET_SCAN_CH);
+											rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
+											rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+											status_code = P2P_STATUS_SUCCESS;
+										} else {
+											/*	The operating channel isn't supported by this device. */
+											rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
+											rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+											status_code = P2P_STATUS_FAIL_NO_COMMON_CH;
+											_set_timer(&pwdinfo->restore_p2p_state_timer, 3000);
+										}
+									} else {
+										/*	Commented by Albert 20121130 */
+										/*	Intel will use the different P2P IE to store the operating channel information */
+										/*	Workaround for Intel WiDi 3.5 */
+										rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
+										rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+										status_code = P2P_STATUS_SUCCESS;
+									}
+								} else {
+									rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
+#ifdef CONFIG_INTEL_WIDI
+									memcpy(pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN);
+									rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+#endif /* CONFIG_INTEL_WIDI */
+
+									status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
+								}
+							}
+						} else {
+							RTW_INFO("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
+							status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+						}
+					} else {
+						/*	Received the invitation to join a P2P group. */
+
+						memset(&group_id, 0x00, sizeof(struct group_id_info));
+						rtw_get_p2p_attr_content(merged_p2pie, merged_p2p_ielen, P2P_ATTR_GROUP_ID, (u8 *) &group_id, &attr_contentlen);
+						if (attr_contentlen) {
+							if (!memcmp(group_id.go_device_addr, adapter_mac_addr(padapter), ETH_ALEN)) {
+								/*	In this case, the GO can't be myself. */
+								rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
+								status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+							} else {
+								/*	The p2p device sending this p2p invitation request wants to join an existing P2P group */
+								/*	Commented by Albert 2012/06/28 */
+								/*	In this case, this Wi-Fi device should use the iwpriv command to get the peer device address. */
+								/*	The peer device address should be the destination address for the provisioning discovery request. */
+								/*	Then, this Wi-Fi device should use the iwpriv command to get the peer interface address. */
+								/*	The peer interface address should be the address for WPS mac address */
+								memcpy(pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN);
+								rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+								rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN);
+								status_code = P2P_STATUS_SUCCESS;
+							}
+						} else {
+							RTW_INFO("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
+							status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+						}
+					}
+				} else {
+					RTW_INFO("[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __func__);
+					status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+				}
+
+				RTW_INFO("[%s] status_code = %d\n", __func__, status_code);
+
+				pwdinfo->inviteresp_info.token = frame_body[7];
+				issue_p2p_invitation_response(padapter, get_addr2_ptr(pframe), pwdinfo->inviteresp_info.token, status_code);
+				_set_timer(&pwdinfo->restore_p2p_state_timer, 3000);
+			}
+#ifdef CONFIG_INTEL_WIDI
+			if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) {
+				padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
+				_cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
+				intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
+			}
+#endif /* CONFIG_INTEL_WIDI */
+			break;
+		}
+		case P2P_INVIT_RESP: {
+			u8	attr_content = 0x00;
+			u32	attr_contentlen = 0;
+
+			RTW_INFO("[%s] Got invite response frame!\n", __func__);
+			_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+			p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+			if (p2p_ie) {
+				rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+
+				if (attr_contentlen == 1) {
+					RTW_INFO("[%s] Status = %d\n", __func__, attr_content);
+					pwdinfo->invitereq_info.benable = false;
+
+					if (attr_content == P2P_STATUS_SUCCESS) {
+						if (!memcmp(pwdinfo->invitereq_info.go_bssid, adapter_mac_addr(padapter), ETH_ALEN))
+							rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+						else
+							rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_OK);
+					} else {
+						rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
+					}
+				} else {
+					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+					rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
+				}
+			} else {
+				rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
+			}
+
+			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL))
+				_set_timer(&pwdinfo->restore_p2p_state_timer, 5000);
+			break;
+		}
+		case P2P_DEVDISC_REQ:
+
+			process_p2p_devdisc_req(pwdinfo, pframe, len);
+
+			break;
+
+		case P2P_DEVDISC_RESP:
+
+			process_p2p_devdisc_resp(pwdinfo, pframe, len);
+
+			break;
+
+		case P2P_PROVISION_DISC_REQ:
+			RTW_INFO("[%s] Got Provisioning Discovery Request Frame\n", __func__);
+			process_p2p_provdisc_req(pwdinfo, pframe, len);
+			memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, get_addr2_ptr(pframe), ETH_ALEN);
+
+			/* 20110902 Kurt */
+			/* Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
+			if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
+				rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
+
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ);
+			_set_timer(&pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT);
+#ifdef CONFIG_INTEL_WIDI
+			if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) {
+				padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
+				_cancel_timer_ex(&(padapter->mlmepriv.listen_timer));
+				intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL, 0);
+			}
+#endif /* CONFIG_INTEL_WIDI */
+			break;
+
+		case P2P_PROVISION_DISC_RESP:
+			/*	Commented by Albert 20110707 */
+			/*	Should we check the pwdinfo->tx_prov_disc_info.bsent flag here?? */
+			RTW_INFO("[%s] Got Provisioning Discovery Response Frame\n", __func__);
+			/*	Commented by Albert 20110426 */
+			/*	The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function. */
+			_cancel_timer_ex(&pwdinfo->restore_p2p_state_timer);
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP);
+			process_p2p_provdisc_resp(pwdinfo, pframe);
+			_set_timer(&pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT);
+			break;
+
+		}
+	}
+
+
+exit:
+
+	if (merged_p2pie)
+		rtw_mfree(merged_p2pie, merged_p2p_ielen + 2);
+#endif /* CONFIG_P2P */
+	return _SUCCESS;
+}
+
+static unsigned int on_action_public_vendor(union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint frame_len = precv_frame->u.hdr.len;
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if (!memcmp(frame_body + 2, P2P_OUI, 4)) {
+		if (rtw_action_public_decache(precv_frame, 7) == _FAIL)
+			goto exit;
+
+		if (!hal_chk_wl_func(precv_frame->u.hdr.adapter, WL_FUNC_MIRACAST))
+			rtw_rframe_del_wfd_ie(precv_frame, 8);
+
+		ret = on_action_public_p2p(precv_frame);
+	}
+
+exit:
+	return ret;
+}
+
+static unsigned int on_action_public_default(union recv_frame *precv_frame, u8 action)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint frame_len = precv_frame->u.hdr.len;
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 token;
+	_adapter *adapter = precv_frame->u.hdr.adapter;
+	int cnt = 0;
+	char msg[64];
+
+	token = frame_body[2];
+
+	if (rtw_action_public_decache(precv_frame, 2) == _FAIL)
+		goto exit;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	cnt += sprintf((msg + cnt), "%s(token:%u)", action_public_str(action), token);
+	rtw_cfg80211_rx_action(adapter, precv_frame, msg);
+#endif
+
+	ret = _SUCCESS;
+
+exit:
+	return ret;
+}
+
+unsigned int on_action_public(_adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint frame_len = precv_frame->u.hdr.len;
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 category, action;
+
+	/* check RA matches or not */
+	if (memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN))
+		goto exit;
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_PUBLIC)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case ACT_PUBLIC_BSSCOEXIST:
+#ifdef CONFIG_AP_MODE
+		/*20/40 BSS Coexistence Management frame is a Public Action frame*/
+		if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE))
+			rtw_process_public_act_bsscoex(padapter, pframe, frame_len);
+#endif /*CONFIG_AP_MODE*/
+		break;
+	case ACT_PUBLIC_VENDOR:
+		ret = on_action_public_vendor(precv_frame);
+		break;
+	default:
+		ret = on_action_public_default(precv_frame, action);
+		break;
+	}
+
+exit:
+	return ret;
+}
+
+unsigned int OnAction_ft(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_RTW_80211R
+	u32	ret = _FAIL;
+	u32	frame_len = 0;
+	u8	action_code = 0;
+	u8	category = 0;
+	u8	*pframe = NULL;
+	u8	*pframe_body = NULL;
+	u8	sta_addr[ETH_ALEN] = {0};
+	u8	*pie = NULL;
+	u32	ft_ie_len = 0;
+	u32 status_code = 0;
+	struct mlme_ext_priv *pmlmeext = NULL;
+	struct mlme_ext_info *pmlmeinfo = NULL;
+	struct mlme_priv *pmlmepriv = NULL;
+	struct wlan_network *proam_target = NULL;
+	ft_priv *pftpriv = NULL;
+	unsigned long  irqL;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+	pmlmepriv = &padapter->mlmepriv;
+	pftpriv = &pmlmepriv->ftpriv;
+	pframe = precv_frame->u.hdr.rx_data;
+	frame_len = precv_frame->u.hdr.len;
+	pframe_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	category = pframe_body[0];
+
+	if (category != RTW_WLAN_CATEGORY_FT)
+		goto exit;
+
+	action_code = pframe_body[1];
+	switch (action_code) {
+	case RTW_WLAN_ACTION_FT_RESPONSE:
+		RTW_INFO("FT: %s RTW_WLAN_ACTION_FT_RESPONSE\n", __func__);
+		if (memcmp(adapter_mac_addr(padapter), &pframe_body[2], ETH_ALEN)) {
+			RTW_ERR("FT: Unmatched STA MAC Address "MAC_FMT"\n", MAC_ARG(&pframe_body[2]));
+			goto exit;
+		}
+
+		status_code = le16_to_cpu(*(u16 *)((SIZE_PTR)pframe +  sizeof(struct rtw_ieee80211_hdr_3addr) + 14));
+		if (status_code != 0) {
+			RTW_ERR("FT: WLAN ACTION FT RESPONSE fail, status: %d\n", status_code);
+			goto exit;
+		}
+
+		if (is_zero_mac_addr(&pframe_body[8]) || is_broadcast_mac_addr(&pframe_body[8])) {
+			RTW_ERR("FT: Invalid Target MAC Address "MAC_FMT"\n", MAC_ARG(padapter->mlmepriv.roam_tgt_addr));
+			goto exit;
+		}
+
+		pie = rtw_get_ie(pframe_body, _MDIE_, &ft_ie_len, frame_len);
+		if (pie) {
+			if (memcmp(&pftpriv->mdid, pie+2, 2)) {
+				RTW_ERR("FT: Invalid MDID\n");
+				goto exit;
+			}
+		}
+
+		rtw_set_ft_status(padapter, RTW_FT_REQUESTED_STA);
+		_cancel_timer_ex(&pmlmeext->ft_link_timer);
+
+		/*Disconnect current AP*/
+		receive_disconnect(padapter, pmlmepriv->cur_network.network.MacAddress, WLAN_REASON_ACTIVE_ROAM, false);
+
+		pftpriv->ft_action_len = frame_len;
+		memcpy(pftpriv->ft_action, pframe, rtw_min(frame_len, RTW_MAX_FTIE_SZ));
+		ret = _SUCCESS;
+		break;
+	case RTW_WLAN_ACTION_FT_REQUEST:
+	case RTW_WLAN_ACTION_FT_CONFIRM:
+	case RTW_WLAN_ACTION_FT_ACK:
+	default:
+		RTW_ERR("FT: Unsupported FT Action!\n");
+		break;
+	}
+
+exit:
+	return ret;
+#else
+	return _SUCCESS;
+#endif
+}
+
+unsigned int OnAction_ht(_adapter *padapter, union recv_frame *precv_frame)
+{
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint frame_len = precv_frame->u.hdr.len;
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 category, action;
+
+	/* check RA matches or not */
+	if (memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN))
+		goto exit;
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_HT)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case RTW_WLAN_ACTION_HT_SM_PS:
+#ifdef CONFIG_AP_MODE
+		if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE))
+			rtw_process_ht_action_smps(padapter, get_addr2_ptr(pframe), frame_body[2]);
+#endif /*CONFIG_AP_MODE*/
+		break;
+	case RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING:
+#ifdef CONFIG_BEAMFORMING
+		/*RTW_INFO("RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING\n");*/
+		rtw_beamforming_get_report_frame(padapter, precv_frame);
+#endif /*CONFIG_BEAMFORMING*/
+		break;
+	default:
+		break;
+	}
+
+exit:
+
+	return _SUCCESS;
+}
+
+#ifdef CONFIG_IEEE80211W
+unsigned int OnAction_sa_query(_adapter *padapter, union recv_frame *precv_frame)
+{
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct sta_info		*psta;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	u16 tid;
+	/* Baron */
+
+	RTW_INFO("OnAction_sa_query\n");
+
+	switch (pframe[WLAN_HDR_A3_LEN + 1]) {
+	case 0: /* SA Query req */
+		memcpy(&tid, &pframe[WLAN_HDR_A3_LEN + 2], sizeof(u16));
+		RTW_INFO("OnAction_sa_query request,action=%d, tid=%04x, pframe=%02x-%02x\n"
+			, pframe[WLAN_HDR_A3_LEN + 1], tid, pframe[WLAN_HDR_A3_LEN + 2], pframe[WLAN_HDR_A3_LEN + 3]);
+		issue_action_SA_Query(padapter, get_addr2_ptr(pframe), 1, tid, IEEE80211W_RIGHT_KEY);
+		break;
+
+	case 1: /* SA Query rsp */
+		psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe));
+		if (psta != NULL)
+			_cancel_timer_ex(&psta->dot11w_expire_timer);
+
+		memcpy(&tid, &pframe[WLAN_HDR_A3_LEN + 2], sizeof(u16));
+		RTW_INFO("OnAction_sa_query response,action=%d, tid=%04x, cancel timer\n", pframe[WLAN_HDR_A3_LEN + 1], tid);
+		break;
+	default:
+		break;
+	}
+	if (0) {
+		int pp;
+		RTW_INFO("pattrib->pktlen = %d =>", pattrib->pkt_len);
+		for (pp = 0; pp < pattrib->pkt_len; pp++)
+			RTW_INFO(" %02x ", pframe[pp]);
+		RTW_INFO("\n");
+	}
+
+	return _SUCCESS;
+}
+#endif /* CONFIG_IEEE80211W */
+
+unsigned int OnAction_wmm(_adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int OnAction_vht(_adapter *padapter, union recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+unsigned int OnAction_p2p(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_P2P
+	u8 *frame_body;
+	u8 category, OUI_Subtype, dialogToken = 0;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	struct	wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+	/* check RA matches or not */
+	if (memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_P2P)
+		return _SUCCESS;
+
+	if (be32_to_cpu(*((__be32 *)(frame_body + 1))) != P2POUI)
+		return _SUCCESS;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (adapter_wdev_data(padapter)->p2p_enabled
+		&& pwdinfo->driver_interface == DRIVER_CFG80211
+	) {
+		rtw_cfg80211_rx_action_p2p(padapter, precv_frame);
+		return _SUCCESS;
+	} else
+#endif /* CONFIG_IOCTL_CFG80211 */
+	{
+		len -= sizeof(struct rtw_ieee80211_hdr_3addr);
+		OUI_Subtype = frame_body[5];
+		dialogToken = frame_body[6];
+
+		switch (OUI_Subtype) {
+		case P2P_NOTICE_OF_ABSENCE:
+
+			break;
+
+		case P2P_PRESENCE_REQUEST:
+
+			process_p2p_presence_req(pwdinfo, pframe, len);
+
+			break;
+
+		case P2P_PRESENCE_RESPONSE:
+
+			break;
+
+		case P2P_GO_DISC_REQUEST:
+
+			break;
+
+		default:
+			break;
+
+		}
+	}
+#endif /* CONFIG_P2P */
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnAction(_adapter *padapter, union recv_frame *precv_frame)
+{
+	int i;
+	unsigned char	category;
+	struct action_handler *ptable;
+	unsigned char	*frame_body;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+
+	for (i = 0; i < sizeof(OnAction_tbl) / sizeof(struct action_handler); i++) {
+		ptable = &OnAction_tbl[i];
+
+		if (category == ptable->num)
+			ptable->func(padapter, precv_frame);
+
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int DoReserved(_adapter *padapter, union recv_frame *precv_frame)
+{
+
+	/* RTW_INFO("rcvd mgt frame(%x, %x)\n", (get_frame_sub_type(pframe) >> 4), *(unsigned int *)GetAddr1Ptr(pframe)); */
+	return _SUCCESS;
+}
+
+static struct xmit_frame *_alloc_mgtxmitframe(struct xmit_priv *pxmitpriv, bool once)
+{
+	struct xmit_frame *pmgntframe;
+	struct xmit_buf *pxmitbuf;
+
+	if (once)
+		pmgntframe = rtw_alloc_xmitframe_once(pxmitpriv);
+	else
+		pmgntframe = rtw_alloc_xmitframe_ext(pxmitpriv);
+
+	if (pmgntframe == NULL) {
+		RTW_INFO(FUNC_ADPT_FMT" alloc xmitframe fail, once:%d\n", FUNC_ADPT_ARG(pxmitpriv->adapter), once);
+		goto exit;
+	}
+
+	pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv);
+	if (pxmitbuf == NULL) {
+		RTW_INFO(FUNC_ADPT_FMT" alloc xmitbuf fail\n", FUNC_ADPT_ARG(pxmitpriv->adapter));
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		pmgntframe = NULL;
+		goto exit;
+	}
+
+	pmgntframe->frame_tag = MGNT_FRAMETAG;
+	pmgntframe->pxmitbuf = pxmitbuf;
+	pmgntframe->buf_addr = pxmitbuf->pbuf;
+	pxmitbuf->priv_data = pmgntframe;
+
+exit:
+	return pmgntframe;
+
+}
+
+inline struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
+{
+	return _alloc_mgtxmitframe(pxmitpriv, false);
+}
+
+inline struct xmit_frame *alloc_mgtxmitframe_once(struct xmit_priv *pxmitpriv)
+{
+	return _alloc_mgtxmitframe(pxmitpriv, true);
+}
+
+
+/****************************************************************************
+
+Following are some TX fuctions for WiFi MLME
+
+*****************************************************************************/
+
+void update_mgnt_tx_rate(_adapter *padapter, u8 rate)
+{
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+
+	pmlmeext->tx_rate = rate;
+	/* RTW_INFO("%s(): rate = %x\n",__func__, rate); */
+}
+
+
+void update_monitor_frame_attrib(_adapter *padapter, struct pkt_attrib *pattrib)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	u8	wireless_mode;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct sta_info		*psta = NULL;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct sta_info *pbcmc_sta = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+	pbcmc_sta = rtw_get_bcmc_stainfo(padapter);
+
+	pattrib->hdrlen = 24;
+	pattrib->nr_frags = 1;
+	pattrib->priority = 7;
+
+	if (pbcmc_sta)
+		pattrib->mac_id = pbcmc_sta->mac_id;
+	else {
+		pattrib->mac_id = 0;
+		RTW_INFO("mgmt use mac_id 0 will affect RA\n");
+	}
+	pattrib->qsel = QSLT_MGNT;
+
+	pattrib->pktlen = 0;
+
+	if (pmlmeext->tx_rate == IEEE80211_CCK_RATE_1MB)
+		wireless_mode = WIRELESS_11B;
+	else
+		wireless_mode = WIRELESS_11G;
+
+	pattrib->raid = rtw_get_mgntframe_raid(padapter, wireless_mode);
+	pattrib->rate = MGN_MCS7;
+
+	pattrib->encrypt = _NO_PRIVACY_;
+	pattrib->bswenc = false;
+
+	pattrib->qos_en = false;
+	pattrib->ht_en = 1;
+	pattrib->bwmode = CHANNEL_WIDTH_20;
+	pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	pattrib->sgi = false;
+
+	pattrib->seqnum = pmlmeext->mgnt_seq;
+
+	pattrib->retry_ctrl = true;
+
+	pattrib->mbssid = 0;
+	pattrib->hw_ssn_sel = pxmitpriv->hw_ssn_seq_no;
+
+}
+
+
+void update_mgntframe_attrib(_adapter *padapter, struct pkt_attrib *pattrib)
+{
+	u8	wireless_mode;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct sta_info *pbcmc_sta = NULL;
+	/* memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib)); */
+
+	pbcmc_sta = rtw_get_bcmc_stainfo(padapter);
+
+	pattrib->hdrlen = 24;
+	pattrib->nr_frags = 1;
+	pattrib->priority = 7;
+
+	if (pbcmc_sta)
+		pattrib->mac_id = pbcmc_sta->mac_id;
+	else {
+		pattrib->mac_id = 1; /* use STA's BCMC sta-info macid */
+
+		if (MLME_IS_AP(padapter) || MLME_IS_GO(padapter))
+			RTW_INFO("%s-"ADPT_FMT" get bcmc sta_info fail,use MACID=1\n", __func__, ADPT_ARG(padapter));
+	}
+	pattrib->qsel = QSLT_MGNT;
+
+#ifdef CONFIG_MCC_MODE
+	update_mcc_mgntframe_attrib(padapter, pattrib);
+#endif
+
+	pattrib->pktlen = 0;
+
+	if (IS_CCK_RATE(pmlmeext->tx_rate))
+		wireless_mode = WIRELESS_11B;
+	else
+		wireless_mode = WIRELESS_11G;
+	pattrib->raid =  rtw_get_mgntframe_raid(padapter, wireless_mode);
+	pattrib->rate = pmlmeext->tx_rate;
+
+	pattrib->encrypt = _NO_PRIVACY_;
+	pattrib->bswenc = false;
+
+	pattrib->qos_en = false;
+	pattrib->ht_en = false;
+	pattrib->bwmode = CHANNEL_WIDTH_20;
+	pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	pattrib->sgi = false;
+
+	pattrib->seqnum = pmlmeext->mgnt_seq;
+
+	pattrib->retry_ctrl = true;
+
+	pattrib->mbssid = 0;
+	pattrib->hw_ssn_sel = pxmitpriv->hw_ssn_seq_no;
+}
+
+void update_mgntframe_attrib_addr(_adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	u8	*pframe;
+	struct pkt_attrib	*pattrib = &pmgntframe->attrib;
+#ifdef CONFIG_BEAMFORMING
+	struct sta_info		*sta = NULL;
+#endif /* CONFIG_BEAMFORMING */
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+
+	memcpy(pattrib->ra, GetAddr1Ptr(pframe), ETH_ALEN);
+	memcpy(pattrib->ta, get_addr2_ptr(pframe), ETH_ALEN);
+
+#ifdef CONFIG_BEAMFORMING
+	sta = pattrib->psta;
+	if (!sta) {
+		sta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+		pattrib->psta = sta;
+	}
+	if (sta)
+		update_attrib_txbf_info(padapter, pattrib, sta);
+#endif /* CONFIG_BEAMFORMING */
+}
+
+void dump_mgntframe(_adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	if (RTW_CANNOT_RUN(padapter)) {
+		rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
+		return;
+	}
+
+	rtw_hal_mgnt_xmit(padapter, pmgntframe);
+}
+
+s32 dump_mgntframe_and_wait(_adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
+{
+	s32 ret = _FAIL;
+	unsigned long irqL;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
+	struct submit_ctx sctx;
+
+	if (RTW_CANNOT_RUN(padapter)) {
+		rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
+		return ret;
+	}
+
+	rtw_sctx_init(&sctx, timeout_ms);
+	pxmitbuf->sctx = &sctx;
+
+	ret = rtw_hal_mgnt_xmit(padapter, pmgntframe);
+
+	if (ret == _SUCCESS)
+		ret = rtw_sctx_wait(&sctx, __func__);
+
+	_enter_critical(&pxmitpriv->lock_sctx, &irqL);
+	pxmitbuf->sctx = NULL;
+	_exit_critical(&pxmitpriv->lock_sctx, &irqL);
+
+	return ret;
+}
+
+s32 dump_mgntframe_and_wait_ack_timeout(_adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
+{
+#ifdef CONFIG_XMIT_ACK
+	static u8 seq_no = 0;
+	s32 ret = _FAIL;
+	struct xmit_priv	*pxmitpriv = &(GET_PRIMARY_ADAPTER(padapter))->xmitpriv;
+
+	if (RTW_CANNOT_RUN(padapter)) {
+		rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
+		return -1;
+	}
+
+	_enter_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
+	pxmitpriv->ack_tx = true;
+	pxmitpriv->seq_no = seq_no++;
+	pmgntframe->ack_report = 1;
+	rtw_sctx_init(&(pxmitpriv->ack_tx_ops), timeout_ms);
+	if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS)
+		ret = rtw_sctx_wait(&(pxmitpriv->ack_tx_ops), __func__);
+
+	pxmitpriv->ack_tx = false;
+	_exit_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
+
+	return ret;
+#else /* !CONFIG_XMIT_ACK */
+	dump_mgntframe(padapter, pmgntframe);
+	rtw_msleep_os(50);
+	return _SUCCESS;
+#endif /* !CONFIG_XMIT_ACK */
+}
+
+s32 dump_mgntframe_and_wait_ack(_adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	/* In this case, use 500 ms as the default wait_ack timeout */
+	return dump_mgntframe_and_wait_ack_timeout(padapter, pmgntframe, 500);
+}
+
+
+static int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
+{
+	u8 *ssid_ie;
+	sint ssid_len_ori;
+	int len_diff = 0;
+
+	ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
+
+	/* RTW_INFO("%s hidden_ssid_mode:%u, ssid_ie:%p, ssid_len_ori:%d\n", __func__, hidden_ssid_mode, ssid_ie, ssid_len_ori); */
+
+	if (ssid_ie && ssid_len_ori > 0) {
+		switch (hidden_ssid_mode) {
+		case 1: {
+			u8 *next_ie = ssid_ie + 2 + ssid_len_ori;
+			u32 remain_len = 0;
+
+			remain_len = ies_len - (next_ie - ies);
+
+			ssid_ie[1] = 0;
+			memcpy(ssid_ie + 2, next_ie, remain_len);
+			len_diff -= ssid_len_ori;
+
+			break;
+		}
+		case 2:
+			memset(&ssid_ie[2], 0, ssid_len_ori);
+			break;
+		default:
+			break;
+		}
+	}
+
+	return len_diff;
+}
+
+#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE
+u32 rtw_build_vendor_ie(_adapter *padapter , unsigned char *pframe , u8 mgmt_frame_tyte)
+{
+	int vendor_ie_num = 0;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	u32 len = 0;
+
+	for (vendor_ie_num = 0 ; vendor_ie_num < WLAN_MAX_VENDOR_IE_NUM ; vendor_ie_num++) {
+		if (pmlmepriv->vendor_ielen[vendor_ie_num] > 0 && pmlmepriv->vendor_ie_mask[vendor_ie_num] & mgmt_frame_tyte) {
+			memcpy(pframe , pmlmepriv->vendor_ie[vendor_ie_num] , pmlmepriv->vendor_ielen[vendor_ie_num]);
+			pframe +=  pmlmepriv->vendor_ielen[vendor_ie_num];
+			len += pmlmepriv->vendor_ielen[vendor_ie_num];
+		}
+	}
+
+	return len;
+}
+#endif
+
+void issue_beacon(_adapter *padapter, int timeout_ms)
+{
+	struct xmit_frame	*pmgntframe;
+	struct pkt_attrib	*pattrib;
+	unsigned char	*pframe;
+	struct rtw_ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	unsigned int	rate_len;
+	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
+#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	unsigned long irqL;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+#endif /* #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*cur_network = &(pmlmeinfo->network);
+	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_P2P */
+
+
+	/* RTW_INFO("%s\n", __func__); */
+
+#ifdef CONFIG_BCN_ICF
+	pmgntframe = rtw_alloc_bcnxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+#else
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+#endif
+	{
+		RTW_INFO("%s, alloc mgnt frame fail\n", __func__);
+		return;
+	}
+#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	_enter_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
+#endif /* #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->qsel = QSLT_BEACON;
+
+#if defined(CONFIG_CONCURRENT_MODE) && (!defined(CONFIG_SWTIMER_BASED_TXBCN))
+	if (padapter->hw_port == HW_PORT1)
+		pattrib->mbssid = 1;
+#endif
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
+	/* pmlmeext->mgnt_seq++; */
+	set_frame_sub_type(pframe, WIFI_BEACON);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
+		/* RTW_INFO("ie len=%d\n", cur_network->IELength); */
+#ifdef CONFIG_P2P
+		/* for P2P : Primary Device Type & Device Name */
+		u32 wpsielen = 0, insert_len = 0;
+		u8 *wpsie = NULL;
+		wpsie = rtw_get_wps_ie(cur_network->IEs + _FIXED_IE_LENGTH_, cur_network->IELength - _FIXED_IE_LENGTH_, NULL, &wpsielen);
+
+		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wpsie && wpsielen > 0) {
+			uint wps_offset, remainder_ielen;
+			u8 *premainder_ie, *pframe_wscie;
+
+			wps_offset = (uint)(wpsie - cur_network->IEs);
+
+			premainder_ie = wpsie + wpsielen;
+
+			remainder_ielen = cur_network->IELength - wps_offset - wpsielen;
+
+#ifdef CONFIG_IOCTL_CFG80211
+			if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) {
+				if (pmlmepriv->wps_beacon_ie && pmlmepriv->wps_beacon_ie_len > 0) {
+					memcpy(pframe, cur_network->IEs, wps_offset);
+					pframe += wps_offset;
+					pattrib->pktlen += wps_offset;
+
+					memcpy(pframe, pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len);
+					pframe += pmlmepriv->wps_beacon_ie_len;
+					pattrib->pktlen += pmlmepriv->wps_beacon_ie_len;
+
+					/* copy remainder_ie to pframe */
+					memcpy(pframe, premainder_ie, remainder_ielen);
+					pframe += remainder_ielen;
+					pattrib->pktlen += remainder_ielen;
+				} else {
+					memcpy(pframe, cur_network->IEs, cur_network->IELength);
+					pframe += cur_network->IELength;
+					pattrib->pktlen += cur_network->IELength;
+				}
+			} else
+#endif /* CONFIG_IOCTL_CFG80211 */
+			{
+				pframe_wscie = pframe + wps_offset;
+				memcpy(pframe, cur_network->IEs, wps_offset + wpsielen);
+				pframe += (wps_offset + wpsielen);
+				pattrib->pktlen += (wps_offset + wpsielen);
+
+				/* now pframe is end of wsc ie, insert Primary Device Type & Device Name */
+				/*	Primary Device Type */
+				/*	Type: */
+				*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
+				insert_len += 2;
+
+				/*	Length: */
+				*(__be16 *)(pframe + insert_len) = cpu_to_be16(0x0008);
+				insert_len += 2;
+
+				/*	Value: */
+				/*	Category ID */
+				*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+				insert_len += 2;
+
+				/*	OUI */
+				*(__be32 *)(pframe + insert_len) = cpu_to_be32(WPSOUI);
+				insert_len += 4;
+
+				/*	Sub Category ID */
+				*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+				insert_len += 2;
+
+
+				/*	Device Name */
+				/*	Type: */
+				*(__be16 *)(pframe + insert_len) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+				insert_len += 2;
+
+				/*	Length: */
+				*(__be16 *)(pframe + insert_len) = cpu_to_be16(pwdinfo->device_name_len);
+				insert_len += 2;
+
+				/*	Value: */
+				memcpy(pframe + insert_len, pwdinfo->device_name, pwdinfo->device_name_len);
+				insert_len += pwdinfo->device_name_len;
+
+
+				/* update wsc ie length */
+				*(pframe_wscie + 1) = (wpsielen - 2) + insert_len;
+
+				/* pframe move to end */
+				pframe += insert_len;
+				pattrib->pktlen += insert_len;
+
+				/* copy remainder_ie to pframe */
+				memcpy(pframe, premainder_ie, remainder_ielen);
+				pframe += remainder_ielen;
+				pattrib->pktlen += remainder_ielen;
+			}
+		} else
+#endif /* CONFIG_P2P */
+		{
+			int len_diff;
+			memcpy(pframe, cur_network->IEs, cur_network->IELength);
+			len_diff = update_hidden_ssid(
+					   pframe + _BEACON_IE_OFFSET_
+				   , cur_network->IELength - _BEACON_IE_OFFSET_
+					   , pmlmeinfo->hidden_ssid_mode
+				   );
+			pframe += (cur_network->IELength + len_diff);
+			pattrib->pktlen += (cur_network->IELength + len_diff);
+		}
+
+		{
+			u8 *wps_ie;
+			uint wps_ielen;
+			u8 sr = 0;
+			wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr + TXDESC_OFFSET + sizeof(struct rtw_ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_,
+				pattrib->pktlen - sizeof(struct rtw_ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_, NULL, &wps_ielen);
+			if (wps_ie && wps_ielen > 0)
+				rtw_get_wps_attr_content(wps_ie,  wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8 *)(&sr), NULL);
+			if (sr != 0)
+				set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
+			else
+				_clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
+		}
+
+#ifdef CONFIG_P2P
+		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+			u32 len;
+#ifdef CONFIG_IOCTL_CFG80211
+			if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) {
+				len = pmlmepriv->p2p_beacon_ie_len;
+				if (pmlmepriv->p2p_beacon_ie && len > 0)
+					memcpy(pframe, pmlmepriv->p2p_beacon_ie, len);
+			} else
+#endif /* CONFIG_IOCTL_CFG80211 */
+			{
+				len = build_beacon_p2p_ie(pwdinfo, pframe);
+			}
+
+			pframe += len;
+			pattrib->pktlen += len;
+
+#ifdef CONFIG_MCC_MODE
+			pframe = rtw_hal_mcc_append_go_p2p_ie(padapter, pframe, &pattrib->pktlen);
+#endif /* CONFIG_MCC_MODE*/
+
+#ifdef CONFIG_WFD
+			len = rtw_append_beacon_wfd_ie(padapter, pframe);
+			pframe += len;
+			pattrib->pktlen += len;
+#endif
+		}
+#endif /* CONFIG_P2P */
+
+#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE
+		pattrib->pktlen += rtw_build_vendor_ie(padapter , pframe , WIFI_BEACON_VENDOR_IE_BIT);
+#endif
+		goto _issue_bcn;
+
+	}
+
+	/* below for ad-hoc mode */
+
+	/* timestamp will be inserted by hardware */
+	pframe += 8;
+	pattrib->pktlen += 8;
+
+	/* beacon interval: 2 bytes */
+
+	memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/* capability info: 2 bytes */
+
+	memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/* SSID */
+	pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
+
+	/* supported rates... */
+	rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
+	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen);
+
+	/* DS parameter set */
+	pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
+
+	/* if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) */
+	{
+		u8 erpinfo = 0;
+		u32 ATIMWindow;
+		/* IBSS Parameter Set... */
+		/* ATIMWindow = cur->Configuration.ATIMWindow; */
+		ATIMWindow = 0;
+		pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
+
+		/* ERP IE */
+		pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
+	}
+
+
+	/* EXTERNDED SUPPORTED RATE */
+	if (rate_len > 8)
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
+
+
+	/* todo:HT for adhoc */
+
+_issue_bcn:
+
+#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	pmlmepriv->update_bcn = false;
+
+	_exit_critical_bh(&pmlmepriv->bcn_update_lock, &irqL);
+#endif /* #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */
+
+	if ((pattrib->pktlen + TXDESC_SIZE) > 512) {
+		RTW_INFO("beacon frame too large\n");
+		return;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	/* RTW_INFO("issue bcn_sz=%d\n", pattrib->last_txcmdsz); */
+	if (timeout_ms > 0)
+		dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
+	else
+		dump_mgntframe(padapter, pmgntframe);
+
+}
+
+void issue_probersp(_adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	unsigned char					*mac, *bssid;
+	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
+#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	u8 *pwps_ie;
+	uint wps_ielen;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+#endif /* #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*cur_network = &(pmlmeinfo->network);
+	unsigned int	rate_len;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+#endif /* CONFIG_P2P */
+
+	/* RTW_INFO("%s\n", __func__); */
+
+	if (da == NULL)
+		return;
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		return;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		RTW_INFO("%s, alloc mgnt frame fail\n", __func__);
+		return;
+	}
+
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	mac = adapter_mac_addr(padapter);
+	bssid = cur_network->MacAddress;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(fctrl, WIFI_PROBERSP);
+
+	pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+
+	if (cur_network->IELength > MAX_IE_SZ)
+		return;
+
+#if defined(CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
+		pwps_ie = rtw_get_wps_ie(cur_network->IEs + _FIXED_IE_LENGTH_, cur_network->IELength - _FIXED_IE_LENGTH_, NULL, &wps_ielen);
+
+		/* inerset & update wps_probe_resp_ie */
+		if ((pmlmepriv->wps_probe_resp_ie != NULL) && pwps_ie && (wps_ielen > 0)) {
+			uint wps_offset, remainder_ielen;
+			u8 *premainder_ie;
+
+			wps_offset = (uint)(pwps_ie - cur_network->IEs);
+
+			premainder_ie = pwps_ie + wps_ielen;
+
+			remainder_ielen = cur_network->IELength - wps_offset - wps_ielen;
+
+			memcpy(pframe, cur_network->IEs, wps_offset);
+			pframe += wps_offset;
+			pattrib->pktlen += wps_offset;
+
+			wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];/* to get ie data len */
+			if ((wps_offset + wps_ielen + 2) <= MAX_IE_SZ) {
+				memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen + 2);
+				pframe += wps_ielen + 2;
+				pattrib->pktlen += wps_ielen + 2;
+			}
+
+			if ((wps_offset + wps_ielen + 2 + remainder_ielen) <= MAX_IE_SZ) {
+				memcpy(pframe, premainder_ie, remainder_ielen);
+				pframe += remainder_ielen;
+				pattrib->pktlen += remainder_ielen;
+			}
+		} else {
+			memcpy(pframe, cur_network->IEs, cur_network->IELength);
+			pframe += cur_network->IELength;
+			pattrib->pktlen += cur_network->IELength;
+		}
+
+		/* retrieve SSID IE from cur_network->Ssid */
+		{
+			u8 *ssid_ie;
+			sint ssid_ielen;
+			sint ssid_ielen_diff;
+			u8 buf[MAX_IE_SZ];
+			u8 *ies = pmgntframe->buf_addr + TXDESC_OFFSET + sizeof(struct rtw_ieee80211_hdr_3addr);
+
+			ssid_ie = rtw_get_ie(ies + _FIXED_IE_LENGTH_, _SSID_IE_, &ssid_ielen,
+				     (pframe - ies) - _FIXED_IE_LENGTH_);
+
+			ssid_ielen_diff = cur_network->Ssid.SsidLength - ssid_ielen;
+
+			if (ssid_ie &&  cur_network->Ssid.SsidLength) {
+				uint remainder_ielen;
+				u8 *remainder_ie;
+				remainder_ie = ssid_ie + 2;
+				remainder_ielen = (pframe - remainder_ie);
+
+				if (remainder_ielen > MAX_IE_SZ) {
+					RTW_WARN(FUNC_ADPT_FMT" remainder_ielen > MAX_IE_SZ\n", FUNC_ADPT_ARG(padapter));
+					remainder_ielen = MAX_IE_SZ;
+				}
+
+				memcpy(buf, remainder_ie, remainder_ielen);
+				memcpy(remainder_ie + ssid_ielen_diff, buf, remainder_ielen);
+				*(ssid_ie + 1) = cur_network->Ssid.SsidLength;
+				memcpy(ssid_ie + 2, cur_network->Ssid.Ssid, cur_network->Ssid.SsidLength);
+
+				pframe += ssid_ielen_diff;
+				pattrib->pktlen += ssid_ielen_diff;
+			}
+		}
+#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE
+		pattrib->pktlen += rtw_build_vendor_ie(padapter , pframe , WIFI_PROBERESP_VENDOR_IE_BIT);
+#endif
+	} else
+#endif
+	{
+
+		/* timestamp will be inserted by hardware */
+		pframe += 8;
+		pattrib->pktlen += 8;
+
+		/* beacon interval: 2 bytes */
+
+		memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
+
+		pframe += 2;
+		pattrib->pktlen += 2;
+
+		/* capability info: 2 bytes */
+
+		memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
+
+		pframe += 2;
+		pattrib->pktlen += 2;
+
+		/* below for ad-hoc mode */
+
+		/* SSID */
+		pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
+
+		/* supported rates... */
+		rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pattrib->pktlen);
+
+		/* DS parameter set */
+		pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
+
+		if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
+			u8 erpinfo = 0;
+			u32 ATIMWindow;
+			/* IBSS Parameter Set... */
+			/* ATIMWindow = cur->Configuration.ATIMWindow; */
+			ATIMWindow = 0;
+			pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
+
+			/* ERP IE */
+			pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
+		}
+
+
+		/* EXTERNDED SUPPORTED RATE */
+		if (rate_len > 8)
+			pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
+
+
+		/* todo:HT for adhoc */
+
+	}
+
+#ifdef CONFIG_P2P
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)
+	    /* IOT issue, When wifi_spec is not set, send probe_resp with P2P IE even if probe_req has no P2P IE */
+	    && (is_valid_p2p_probereq || !padapter->registrypriv.wifi_spec)) {
+		u32 len;
+#ifdef CONFIG_IOCTL_CFG80211
+		if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) {
+			/* if pwdinfo->role == P2P_ROLE_DEVICE will call issue_probersp_p2p() */
+			len = pmlmepriv->p2p_go_probe_resp_ie_len;
+			if (pmlmepriv->p2p_go_probe_resp_ie && len > 0)
+				memcpy(pframe, pmlmepriv->p2p_go_probe_resp_ie, len);
+		} else
+#endif /* CONFIG_IOCTL_CFG80211 */
+		{
+			len = build_probe_resp_p2p_ie(pwdinfo, pframe);
+		}
+
+		pframe += len;
+		pattrib->pktlen += len;
+
+#ifdef CONFIG_MCC_MODE
+		pframe = rtw_hal_mcc_append_go_p2p_ie(padapter, pframe, &pattrib->pktlen);
+#endif /* CONFIG_MCC_MODE*/
+
+#ifdef CONFIG_WFD
+		len = rtw_append_probe_resp_wfd_ie(padapter, pframe);
+		pframe += len;
+		pattrib->pktlen += len;
+#endif
+	}
+#endif /* CONFIG_P2P */
+
+
+#ifdef CONFIG_AUTO_AP_MODE
+	{
+		struct sta_info	*psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		RTW_INFO("(%s)\n", __func__);
+
+		/* check rc station */
+		psta = rtw_get_stainfo(pstapriv, da);
+		if (psta && psta->isrc && psta->pid > 0) {
+			u8 RC_OUI[4] = {0x00, 0xE0, 0x4C, 0x0A};
+			u8 RC_INFO[14] = {0};
+			/* EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2] */
+			u16 cu_ch = (u16)cur_network->Configuration.DSConfig;
+
+			RTW_INFO("%s, reply rc(pid=0x%x) device "MAC_FMT" in ch=%d\n", __func__,
+				 psta->pid, MAC_ARG(psta->hwaddr), cu_ch);
+
+			/* append vendor specific ie */
+			memcpy(RC_INFO, RC_OUI, sizeof(RC_OUI));
+			memcpy(&RC_INFO[4], mac, ETH_ALEN);
+			memcpy(&RC_INFO[10], (u8 *)&psta->pid, 2);
+			memcpy(&RC_INFO[12], (u8 *)&cu_ch, 2);
+
+			pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, sizeof(RC_INFO), RC_INFO, &pattrib->pktlen);
+		}
+	}
+#endif /* CONFIG_AUTO_AP_MODE */
+
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+
+}
+
+static int _issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da, u8 ch, bool append_wps, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	unsigned char			*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	unsigned char			*mac;
+	unsigned char			bssrate[NumRates];
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	int	bssrate_len = 0;
+	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		goto exit;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	mac = adapter_mac_addr(padapter);
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	if (da) {
+		/*	unicast probe request frame */
+		memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, da, ETH_ALEN);
+	} else {
+		/*	broadcast probe request frame */
+		memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
+	}
+
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_PROBEREQ);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if (pssid)
+		pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &(pattrib->pktlen));
+	else
+		pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &(pattrib->pktlen));
+
+	get_rate_set(padapter, bssrate, &bssrate_len);
+
+	if (bssrate_len > 8) {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
+	} else
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
+
+	if (ch)
+		pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, &ch, &pattrib->pktlen);
+
+	if (append_wps) {
+		/* add wps_ie for wps2.0 */
+		if (pmlmepriv->wps_probe_req_ie_len > 0 && pmlmepriv->wps_probe_req_ie) {
+			memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
+			pframe += pmlmepriv->wps_probe_req_ie_len;
+			pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
+			/* pmlmepriv->wps_probe_req_ie_len = 0 ; */ /* reset to zero */
+		}
+	}
+#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE
+	pattrib->pktlen += rtw_build_vendor_ie(padapter , pframe , WIFI_PROBEREQ_VENDOR_IE_BIT);
+#endif
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+
+	if (wait_ack)
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+inline void issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da)
+{
+	_issue_probereq(padapter, pssid, da, 0, 1, false);
+}
+
+/*
+ * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT
+ * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX
+ * try_cnt means the maximal TX count to try
+ */
+int issue_probereq_ex(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da, u8 ch, bool append_wps,
+		      int try_cnt, int wait_ms)
+{
+	int ret = _FAIL;
+	int i = 0;
+	u32 start = jiffies;
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		goto exit;
+
+	do {
+		ret = _issue_probereq(padapter, pssid, da, ch, append_wps, wait_ms > 0 ? true : false);
+
+		i++;
+
+		if (RTW_CANNOT_RUN(padapter))
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			rtw_msleep_os(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+#ifndef DBG_XMIT_ACK
+		goto exit;
+#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+/* if psta == NULL, indiate we are station(client) now... */
+void issue_auth(_adapter *padapter, struct sta_info *psta, unsigned short status)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	unsigned int					val32;
+	unsigned short				val16;
+	int use_shared_key = 0;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	__le16 le_val16;
+#ifdef CONFIG_RTW_80211R
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	ft_priv			*pftpriv = &pmlmepriv->ftpriv;
+	u8	is_ft_roaming = false;
+	u8	is_ft_roaming_with_rsn_ie = true;
+	u8 *pie = NULL;
+	u32 ft_ie_len = 0;
+#endif
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		return;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_AUTH);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+
+	if (psta) { /* for AP mode */
+#ifdef CONFIG_NATIVEAP_MLME
+		memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN);
+
+
+		/* setting auth algo number */
+		val16 = (u16)psta->authalg;
+
+		if (val16)	{
+			le_val16 = cpu_to_le16(val16);
+			use_shared_key = 1;
+		} else {
+			le_val16 = 0;
+		}
+
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_val16, &(pattrib->pktlen));
+
+		/* setting auth seq number */
+		val16 = (u16)psta->auth_seq;
+		le_val16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_val16, &(pattrib->pktlen));
+
+		/* setting status code... */
+		val16 = status;
+		le_val16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_val16, &(pattrib->pktlen));
+
+		/* added challenging text... */
+		if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1))
+			pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, psta->chg_txt, &(pattrib->pktlen));
+#endif
+	} else {
+		memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+		memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+#ifdef CONFIG_RTW_80211R
+		/*For Fast BSS Transition */
+		if ((rtw_to_roam(padapter) > 0) && rtw_chk_ft_flags(padapter, RTW_FT_SUPPORTED)) {
+			is_ft_roaming = true;
+			val16 = 2;	/* 2: 802.11R FTAA */
+			le_val16 = cpu_to_le16(val16);
+		} else
+#endif
+		{
+			/* setting auth algo number */
+			val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) ? 1 : 0;	/* 0:OPEN System, 1:Shared key */
+			if (val16) {
+				le_val16 = cpu_to_le16(val16);
+				use_shared_key = 1;
+			}
+		}
+
+		/* RTW_INFO("%s auth_algo= %s auth_seq=%d\n",__func__,(pmlmeinfo->auth_algo==0)?"OPEN":"SHARED",pmlmeinfo->auth_seq); */
+
+		/* setting IV for auth seq #3 */
+		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
+			__le32 le_val32;
+
+			/* RTW_INFO("==> iv(%d),key_index(%d)\n",pmlmeinfo->iv,pmlmeinfo->key_index); */
+			val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
+			le_val32 = cpu_to_le32(val32);
+			pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&le_val32, &(pattrib->pktlen));
+
+			pattrib->iv_len = 4;
+		}
+
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
+
+		/* setting auth seq number */
+		val16 = pmlmeinfo->auth_seq;
+		le_val16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_val16, &(pattrib->pktlen));
+
+
+		/* setting status code... */
+		val16 = status;
+		le_val16 = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_val16, &(pattrib->pktlen));
+
+#ifdef CONFIG_RTW_80211R
+		if (is_ft_roaming) {
+			pie = rtw_get_ie(pftpriv->updated_ft_ies, EID_WPA2, &ft_ie_len, pftpriv->updated_ft_ies_len);
+			if (pie)
+				pframe = rtw_set_ie(pframe, EID_WPA2, ft_ie_len, pie+2, &(pattrib->pktlen));
+			else
+				is_ft_roaming_with_rsn_ie = false;
+
+			pie = rtw_get_ie(pftpriv->updated_ft_ies, _MDIE_, &ft_ie_len, pftpriv->updated_ft_ies_len);
+			if (pie)
+				pframe = rtw_set_ie(pframe, _MDIE_, ft_ie_len , pie+2, &(pattrib->pktlen));
+
+			pie = rtw_get_ie(pftpriv->updated_ft_ies, _FTIE_, &ft_ie_len, pftpriv->updated_ft_ies_len);
+			if (pie && is_ft_roaming_with_rsn_ie)
+				pframe = rtw_set_ie(pframe, _FTIE_, ft_ie_len , pie+2, &(pattrib->pktlen));
+		}
+#endif
+
+		/* then checking to see if sending challenging text... */
+		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
+			pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen));
+
+			SetPrivacy(fctrl);
+
+			pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+			pattrib->encrypt = _WEP40_;
+
+			pattrib->icv_len = 4;
+
+			pattrib->pktlen += pattrib->icv_len;
+
+		}
+
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	rtw_wep_encrypt(padapter, (u8 *)pmgntframe);
+	RTW_INFO("%s\n", __func__);
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+
+void issue_asocrsp(_adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
+{
+#ifdef CONFIG_AP_MODE
+	struct xmit_frame	*pmgntframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	struct pkt_attrib *pattrib;
+	unsigned char	*pbuf, *pframe;
+	unsigned short val;
+	__le16 *fctrl, le_val;
+	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);
+	u8 *ie = pnetwork->IEs;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+
+#endif /* CONFIG_P2P */
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		return;
+
+	RTW_INFO("%s\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
+	memcpy((void *)get_addr2_ptr(pwlanhdr), adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
+		set_frame_sub_type(pwlanhdr, pkt_type);
+	else
+		return;
+
+	pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen += pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+	/* capability */
+	val = *(unsigned short *)rtw_get_capability_from_ie(ie);
+
+	pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_ , (unsigned char *)&val, &(pattrib->pktlen));
+
+	le_val = cpu_to_le16(status);
+	pframe = rtw_set_fixed_ie(pframe , _STATUS_CODE_ , (unsigned char *)&le_val, &(pattrib->pktlen));
+
+	le_val = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
+	pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_ , (unsigned char *)&le_val, &(pattrib->pktlen));
+
+	if (pstat->bssratelen <= 8)
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen));
+	else {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &(pattrib->pktlen));
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (pstat->bssratelen - 8), pstat->bssrateset + 8, &(pattrib->pktlen));
+	}
+
+#ifdef CONFIG_IEEE80211W
+	if (status == _STATS_REFUSED_TEMPORARILY_) {
+		u8 timeout_itvl[5];
+		u32 timeout_interval = 3000;
+		/* Association Comeback time */
+		timeout_itvl[0] = 0x03;
+		timeout_interval = cpu_to_le32(timeout_interval);
+		memcpy(timeout_itvl + 1, &timeout_interval, 4);
+		pframe = rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, 5, timeout_itvl, &(pattrib->pktlen));
+	}
+#endif /* CONFIG_IEEE80211W */
+
+	if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) {
+		uint ie_len = 0;
+
+		/* FILL HT CAP INFO IE */
+		/* p = hostapd_eid_ht_capabilities_info(hapd, p); */
+		pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+		if (pbuf && ie_len > 0) {
+			memcpy(pframe, pbuf, ie_len + 2);
+			pframe += (ie_len + 2);
+			pattrib->pktlen += (ie_len + 2);
+		}
+
+		/* FILL HT ADD INFO IE */
+		/* p = hostapd_eid_ht_operation(hapd, p); */
+		pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+		if (pbuf && ie_len > 0) {
+			memcpy(pframe, pbuf, ie_len + 2);
+			pframe += (ie_len + 2);
+			pattrib->pktlen += (ie_len + 2);
+		}
+
+	}
+
+	/*adding EXT_CAPAB_IE */
+	if (pmlmepriv->ext_capab_ie_len > 0) {
+		uint ie_len = 0;
+
+		pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _EXT_CAP_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+		if (pbuf && ie_len > 0) {
+			memcpy(pframe, pbuf, ie_len + 2);
+			pframe += (ie_len + 2);
+			pattrib->pktlen += (ie_len + 2);
+		}
+	}
+
+	/* FILL WMM IE */
+	if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option)) {
+		uint ie_len = 0;
+		unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+
+		for (pbuf = ie + _BEACON_IE_OFFSET_; ; pbuf += (ie_len + 2)) {
+			pbuf = rtw_get_ie(pbuf, _VENDOR_SPECIFIC_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
+			if (pbuf && !memcmp(pbuf + 2, WMM_PARA_IE, 6)) {
+				memcpy(pframe, pbuf, ie_len + 2);
+				pframe += (ie_len + 2);
+				pattrib->pktlen += (ie_len + 2);
+
+				break;
+			}
+
+			if ((pbuf == NULL) || (ie_len == 0))
+				break;
+		}
+
+	}
+
+
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
+
+	/* add WPS IE ie for wps 2.0 */
+	if (pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len > 0) {
+		memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len);
+
+		pframe += pmlmepriv->wps_assoc_resp_ie_len;
+		pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
+	}
+
+#ifdef CONFIG_P2P
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && (pstat->is_p2p_device)) {
+		u32 len;
+
+		if (padapter->wdinfo.driver_interface == DRIVER_CFG80211) {
+			len = 0;
+			if (pmlmepriv->p2p_assoc_resp_ie && pmlmepriv->p2p_assoc_resp_ie_len > 0) {
+				len = pmlmepriv->p2p_assoc_resp_ie_len;
+				memcpy(pframe, pmlmepriv->p2p_assoc_resp_ie, len);
+			}
+		} else
+			len = build_assoc_resp_p2p_ie(pwdinfo, pframe, pstat->p2p_status_code);
+		pframe += len;
+		pattrib->pktlen += len;
+	}
+
+#ifdef CONFIG_WFD
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		wfdielen = rtw_append_assoc_resp_wfd_ie(padapter, pframe);
+		pframe += wfdielen;
+		pattrib->pktlen += wfdielen;
+	}
+#endif
+
+#endif /* CONFIG_P2P */
+#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE
+	pattrib->pktlen += rtw_build_vendor_ie(padapter , pframe , WIFI_ASSOCRESP_VENDOR_IE_BIT);
+#endif
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+#endif
+}
+
+void _issue_assocreq(_adapter *padapter, u8 is_reassoc)
+{
+	int ret = _FAIL;
+	struct xmit_frame				*pmgntframe;
+	struct pkt_attrib				*pattrib;
+	unsigned char					*pframe, *p;
+	struct rtw_ieee80211_hdr			*pwlanhdr;
+	__le16 *fctrl;
+	__le16 val16;
+	unsigned int					i, j, ie_len, index = 0;
+	unsigned char					rf_type, bssrate[NumRates], sta_bssrate[NumRates];
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	int	bssrate_len = 0, sta_bssrate_len = 0;
+	u8	vs_ie_length = 0;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8					p2pie[255] = { 0x00 };
+	u16					p2pielen = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+#endif /* CONFIG_P2P */
+
+#ifdef CONFIG_DFS
+	u16	cap;
+
+	/* Dot H */
+	u8 pow_cap_ele[2] = { 0x00 };
+	u8 sup_ch[30 * 2] = {0x00 }, sup_ch_idx = 0, idx_5g = 2;	/* For supported channel */
+#endif /* CONFIG_DFS */
+#ifdef CONFIG_RTW_80211R
+	u8 *pie = NULL;
+	u32 ft_ie_len = 0;
+	ft_priv *pftpriv = &pmlmepriv->ftpriv;
+#endif
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		goto exit;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	if (is_reassoc)
+		set_frame_sub_type(pframe, WIFI_REASSOCREQ);
+	else
+		set_frame_sub_type(pframe, WIFI_ASSOCREQ);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	/* caps */
+
+#ifdef CONFIG_DFS
+	memcpy(&cap, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
+	cap |= cap_SpecMgmt;
+	memcpy(pframe, &cap, 2);
+#else
+	memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
+#endif /* CONFIG_DFS */
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/* listen interval */
+	/* todo: listen interval for power saving */
+	val16 = cpu_to_le16(3);
+	memcpy(pframe , (unsigned char *)&val16, 2);
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/*Construct Current AP Field for Reassoc-Req only*/
+	if (is_reassoc) {
+		memcpy(pframe, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+		pframe += ETH_ALEN;
+		pattrib->pktlen += ETH_ALEN;
+	}
+
+	/* SSID */
+	pframe = rtw_set_ie(pframe, _SSID_IE_,  pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &(pattrib->pktlen));
+
+#ifdef CONFIG_DFS
+	/* Dot H */
+	if (pmlmeext->cur_channel > 14) {
+		pow_cap_ele[0] = 13;	/* Minimum transmit power capability */
+		pow_cap_ele[1] = 21;	/* Maximum transmit power capability */
+		pframe = rtw_set_ie(pframe, EID_PowerCap, 2, pow_cap_ele, &(pattrib->pktlen));
+
+		/* supported channels */
+		do {
+			if (pmlmeext->channel_set[sup_ch_idx].ChannelNum <= 14) {
+				sup_ch[0] = 1;	/* First channel number */
+				sup_ch[1] = pmlmeext->channel_set[sup_ch_idx].ChannelNum;	/* Number of channel */
+			} else {
+				sup_ch[idx_5g++] = pmlmeext->channel_set[sup_ch_idx].ChannelNum;
+				sup_ch[idx_5g++] = 1;
+			}
+			sup_ch_idx++;
+		} while (pmlmeext->channel_set[sup_ch_idx].ChannelNum != 0);
+		pframe = rtw_set_ie(pframe, EID_SupportedChannels, idx_5g, sup_ch, &(pattrib->pktlen));
+	}
+#endif /* CONFIG_DFS */
+
+	/* supported rate & extended supported rate */
+
+	get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);
+	/* RTW_INFO("sta_bssrate_len=%d\n", sta_bssrate_len); */
+
+	if (pmlmeext->cur_channel == 14) /* for JAPAN, channel 14 can only uses B Mode(CCK) */
+		sta_bssrate_len = 4;
+
+
+	/* for (i = 0; i < sta_bssrate_len; i++) { */
+	/*	RTW_INFO("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]); */
+	/* } */
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		if (pmlmeinfo->network.SupportedRates[i] == 0)
+			break;
+		RTW_INFO("network.SupportedRates[%d]=%02X\n", i, pmlmeinfo->network.SupportedRates[i]);
+	}
+
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		if (pmlmeinfo->network.SupportedRates[i] == 0)
+			break;
+
+
+		/* Check if the AP's supported rates are also supported by STA. */
+		for (j = 0; j < sta_bssrate_len; j++) {
+			/* Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP */
+			if ((pmlmeinfo->network.SupportedRates[i] | IEEE80211_BASIC_RATE_MASK)
+			    == (sta_bssrate[j] | IEEE80211_BASIC_RATE_MASK)) {
+				/* RTW_INFO("match i = %d, j=%d\n", i, j); */
+				break;
+			} else {
+				/* RTW_INFO("not match: %02X != %02X\n", (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK), (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)); */
+			}
+		}
+
+		if (j == sta_bssrate_len) {
+			/* the rate is not supported by STA */
+			RTW_INFO("%s(): the rate[%d]=%02X is not supported by STA!\n", __func__, i, pmlmeinfo->network.SupportedRates[i]);
+		} else {
+			/* the rate is supported by STA */
+			bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
+		}
+	}
+
+	bssrate_len = index;
+	RTW_INFO("bssrate_len = %d\n", bssrate_len);
+
+	if ((bssrate_len == 0) && (pmlmeinfo->network.SupportedRates[0] != 0)) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit; /* don't connect to AP if no joint supported rate */
+	}
+
+
+	if (bssrate_len > 8) {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
+	} else if (bssrate_len > 0)
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
+	else
+		RTW_INFO("%s: Connect to AP without 11b and 11g data rate!\n", __func__);
+
+	/* vendor specific IE, such as WPA, WMM, WPS */
+	for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;) {
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) ||
+			    (!memcmp(pIE->data, WMM_OUI, 4)) ||
+			    (!memcmp(pIE->data, WPS_OUI, 4))) {
+				vs_ie_length = pIE->Length;
+				if ((!padapter->registrypriv.wifi_spec) && (!memcmp(pIE->data, WPS_OUI, 4))) {
+					/* Commented by Kurt 20110629 */
+					/* In some older APs, WPS handshake */
+					/* would be fail if we append vender extensions informations to AP */
+
+					vs_ie_length = 14;
+				}
+
+				pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, vs_ie_length, pIE->data, &(pattrib->pktlen));
+			}
+			break;
+
+		case EID_WPA2:
+#ifdef CONFIG_RTW_80211R
+			if ((is_reassoc) && (rtw_to_roam(padapter) > 0) && rtw_chk_ft_flags(padapter, RTW_FT_SUPPORTED)) {
+				pie = rtw_get_ie(pftpriv->updated_ft_ies, EID_WPA2, &ft_ie_len, pftpriv->updated_ft_ies_len);
+				if (pie)
+					pframe = rtw_set_ie(pframe, EID_WPA2, ft_ie_len, pie+2, &(pattrib->pktlen));
+			} else
+#endif
+				pframe = rtw_set_ie(pframe, EID_WPA2, pIE->Length, pIE->data, &(pattrib->pktlen));
+			break;
+		case EID_HTCapability:
+			if (padapter->mlmepriv.htpriv.ht_option) {
+				if (!(is_ap_in_tkip(padapter))) {
+					memcpy(&(pmlmeinfo->HT_caps), pIE->data, sizeof(struct HT_caps_element));
+					pframe = rtw_set_ie(pframe, EID_HTCapability, pIE->Length , (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
+				}
+			}
+			break;
+
+		case EID_EXTCapability:
+			if (padapter->mlmepriv.htpriv.ht_option)
+				pframe = rtw_set_ie(pframe, EID_EXTCapability, pIE->Length, pIE->data, &(pattrib->pktlen));
+			break;
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
+
+
+#ifdef CONFIG_WAPI_SUPPORT
+	rtw_build_assoc_req_wapi_ie(padapter, pframe, pattrib);
+#endif
+
+
+#ifdef CONFIG_P2P
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211) {
+		if (pmlmepriv->p2p_assoc_req_ie && pmlmepriv->p2p_assoc_req_ie_len > 0) {
+			memcpy(pframe, pmlmepriv->p2p_assoc_req_ie, pmlmepriv->p2p_assoc_req_ie_len);
+			pframe += pmlmepriv->p2p_assoc_req_ie_len;
+			pattrib->pktlen += pmlmepriv->p2p_assoc_req_ie_len;
+		}
+	} else
+#endif /* CONFIG_IOCTL_CFG80211 */
+	{
+		if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) {
+			/*	Should add the P2P IE in the association request frame.	 */
+			/*	P2P OUI */
+
+			p2pielen = 0;
+			p2pie[p2pielen++] = 0x50;
+			p2pie[p2pielen++] = 0x6F;
+			p2pie[p2pielen++] = 0x9A;
+			p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+			/*	Commented by Albert 20101109 */
+			/*	According to the P2P Specification, the association request frame should contain 3 P2P attributes */
+			/*	1. P2P Capability */
+			/*	2. Extended Listen Timing */
+			/*	3. Device Info */
+			/*	Commented by Albert 20110516 */
+			/*	4. P2P Interface */
+
+			/*	P2P Capability */
+			/*	Type: */
+			p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+			/*	Length: */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
+			p2pielen += 2;
+
+			/*	Value: */
+			/*	Device Capability Bitmap, 1 byte */
+			p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+			/*	Group Capability Bitmap, 1 byte */
+			if (pwdinfo->persistent_supported)
+				p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+			else
+				p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
+
+			/*	Extended Listen Timing */
+			/*	Type: */
+			p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+			/*	Length: */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
+			p2pielen += 2;
+
+			/*	Value: */
+			/*	Availability Period */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+			p2pielen += 2;
+
+			/*	Availability Interval */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
+			p2pielen += 2;
+
+			/*	Device Info */
+			/*	Type: */
+			p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+			/*	Length: */
+			/*	21->P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)  */
+			/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
+			p2pielen += 2;
+
+			/*	Value: */
+			/*	P2P Device Address */
+			memcpy(p2pie + p2pielen, adapter_mac_addr(padapter), ETH_ALEN);
+			p2pielen += ETH_ALEN;
+
+			/*	Config Method */
+			/*	This field should be big endian. Noted by P2P specification. */
+			if ((pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN) ||
+			    (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN))
+				*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
+			else
+				*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC);
+
+			p2pielen += 2;
+
+			/*	Primary Device Type */
+			/*	Category ID */
+			*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
+			p2pielen += 2;
+
+			/*	OUI */
+			*(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
+			p2pielen += 4;
+
+			/*	Sub Category ID */
+			*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
+			p2pielen += 2;
+
+			/*	Number of Secondary Device Types */
+			p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
+
+			/*	Device Name */
+			/*	Type: */
+			*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
+			p2pielen += 2;
+
+			/*	Length: */
+			*(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
+			p2pielen += 2;
+
+			/*	Value: */
+			memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
+			p2pielen += pwdinfo->device_name_len;
+
+			/*	P2P Interface */
+			/*	Type: */
+			p2pie[p2pielen++] = P2P_ATTR_INTERFACE;
+
+			/*	Length: */
+			*(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x000D);
+			p2pielen += 2;
+
+			/*	Value: */
+			memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);	/*	P2P Device Address */
+			p2pielen += ETH_ALEN;
+
+			p2pie[p2pielen++] = 1;	/*	P2P Interface Address Count */
+
+			memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);	/*	P2P Interface Address List */
+			p2pielen += ETH_ALEN;
+
+			pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen);
+		}
+	}
+
+#endif /* CONFIG_P2P */
+
+#ifdef CONFIG_WFD
+	wfdielen = rtw_append_assoc_req_wfd_ie(padapter, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif
+#ifdef CONFIG_APPEND_VENDOR_IE_ENABLE
+	pattrib->pktlen += rtw_build_vendor_ie(padapter , pframe , WIFI_ASSOCREQ_VENDOR_IE_BIT);
+#endif
+#ifdef CONFIG_RTW_80211R
+	if (rtw_chk_ft_flags(padapter, RTW_FT_SUPPORTED)) {
+		u8 mdieval[3] = {0};
+
+		memcpy(mdieval, &(pftpriv->mdid), 2);
+		mdieval[2] = pftpriv->ft_cap;
+		pframe = rtw_set_ie(pframe, _MDIE_, 3, mdieval, &(pattrib->pktlen));
+	}
+
+	if (is_reassoc) {
+		if ((rtw_to_roam(padapter) > 0) && rtw_chk_ft_flags(padapter, RTW_FT_SUPPORTED)) {
+			u8 is_ft_roaming_with_rsn_ie = true;
+
+			pie = rtw_get_ie(pftpriv->updated_ft_ies, EID_WPA2, &ft_ie_len, pftpriv->updated_ft_ies_len);
+			if (!pie)
+				is_ft_roaming_with_rsn_ie = false;
+
+			pie = rtw_get_ie(pftpriv->updated_ft_ies, _FTIE_, &ft_ie_len, pftpriv->updated_ft_ies_len);
+			if (pie && is_ft_roaming_with_rsn_ie)
+				pframe = rtw_set_ie(pframe, _FTIE_, ft_ie_len , pie+2, &(pattrib->pktlen));
+		}
+	}
+#endif
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	dump_mgntframe(padapter, pmgntframe);
+
+	ret = _SUCCESS;
+
+exit:
+	if (ret == _SUCCESS)
+		rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen);
+	else
+		rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
+
+	return;
+}
+
+void issue_assocreq(_adapter *padapter)
+{
+	_issue_assocreq(padapter, false);
+}
+
+void issue_reassocreq(_adapter *padapter)
+{
+	_issue_assocreq(padapter, true);
+}
+
+/* when wait_ack is ture, this function shoule be called at process context */
+static int _issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv	*pxmitpriv;
+	struct mlme_ext_priv	*pmlmeext;
+	struct mlme_ext_info	*pmlmeinfo;
+
+	/* RTW_INFO("%s:%d\n", __func__, power_mode); */
+
+	if (!padapter)
+		goto exit;
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		goto exit;
+
+	pxmitpriv = &(padapter->xmitpriv);
+	pmlmeext = &(padapter->mlmeextpriv);
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->retry_ctrl = false;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)
+		SetFrDs(fctrl);
+	else if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE)
+		SetToDs(fctrl);
+
+	if (power_mode)
+		SetPwrMgt(fctrl);
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_DATA_NULL);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack)
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+/*
+ * [IMPORTANT] Don't call this function in interrupt context
+ *
+ * When wait_ms > 0, this function should be called at process context
+ * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT
+ * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX
+ * try_cnt means the maximal TX count to try
+ * da == NULL for station mode
+ */
+int issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
+{
+	int ret = _FAIL;
+	int i = 0;
+	u32 start = jiffies;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_info *psta;
+	u8 macid_sleep_reg_access = true;
+
+#ifdef CONFIG_MCC_MODE
+	if (MCC_EN(padapter)) {
+		/* driver doesn't access macid sleep reg under MCC */
+		if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_DOING_MCC)) {
+			macid_sleep_reg_access = false;
+
+			if (da == NULL) {
+				RTW_INFO("Warning: Do not tx null data to AP under MCC mode\n");
+				rtw_warn_on(1);
+			}
+
+		}
+	}
+#endif
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		goto exit;
+
+	/* da == NULL, assum it's null data for sta to ap*/
+	if (da == NULL)
+		da = get_my_bssid(&(pmlmeinfo->network));
+
+	psta = rtw_get_stainfo(&padapter->stapriv, da);
+	if (psta) {
+		if (macid_sleep_reg_access) {
+			if (power_mode)
+				rtw_hal_macid_sleep(padapter, psta->mac_id);
+			else
+				rtw_hal_macid_wakeup(padapter, psta->mac_id);
+		}
+	} else {
+		RTW_INFO(FUNC_ADPT_FMT ": Can't find sta info for " MAC_FMT ", skip macid %s!!\n",
+			FUNC_ADPT_ARG(padapter), MAC_ARG(da), power_mode ? "sleep" : "wakeup");
+		rtw_warn_on(1);
+	}
+
+	do {
+		ret = _issue_nulldata(padapter, da, power_mode, wait_ms > 0 ? true : false);
+
+		i++;
+
+		if (RTW_CANNOT_RUN(padapter))
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			rtw_msleep_os(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+#ifndef DBG_XMIT_ACK
+		goto exit;
+#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+/*
+ * [IMPORTANT] This function run in interrupt context
+ *
+ * The null data packet would be sent without power bit,
+ * and not guarantee success.
+ */
+s32 issue_nulldata_in_interrupt(PADAPTER padapter, u8 *da, unsigned int power_mode)
+{
+	int ret;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	/* da == NULL, assum it's null data for sta to ap*/
+	if (da == NULL)
+		da = get_my_bssid(&(pmlmeinfo->network));
+
+	ret = _issue_nulldata(padapter, da, power_mode, false);
+
+	return ret;
+}
+
+/* when wait_ack is ture, this function shoule be called at process context */
+static int _issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl, *qc;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		goto exit;
+
+	RTW_INFO("%s\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	pattrib->hdrlen += 2;
+	pattrib->qos_en = true;
+	pattrib->eosp = 1;
+	pattrib->ack_policy = 0;
+	pattrib->mdata = 0;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)
+		SetFrDs(fctrl);
+	else if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE)
+		SetToDs(fctrl);
+
+	if (pattrib->mdata)
+		SetMData(fctrl);
+
+	qc = (__le16 *)(pframe + pattrib->hdrlen - 2);
+
+	SetPriority(qc, tid);
+
+	SetEOSP(qc, pattrib->eosp);
+
+	SetAckpolicy(qc, pattrib->ack_policy);
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_QOS_DATA_NULL);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack)
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+/*
+ * when wait_ms >0 , this function should be called at process context
+ * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT
+ * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX
+ * try_cnt means the maximal TX count to try
+ * da == NULL for station mode
+ */
+int issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms)
+{
+	int ret = _FAIL;
+	int i = 0;
+	u32 start = jiffies;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		goto exit;
+
+	/* da == NULL, assum it's null data for sta to ap*/
+	if (da == NULL)
+		da = get_my_bssid(&(pmlmeinfo->network));
+
+	do {
+		ret = _issue_qos_nulldata(padapter, da, tid, wait_ms > 0 ? true : false);
+
+		i++;
+
+		if (RTW_CANNOT_RUN(padapter))
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			rtw_msleep_os(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+#ifndef DBG_XMIT_ACK
+		goto exit;
+#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+static int _issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack, u8 key_type)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl, le_reason;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	int ret = _FAIL;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_P2P	 */
+
+	/* RTW_INFO("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); */
+
+#ifdef CONFIG_P2P
+	if (!(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) && (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
+		_cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
+		_set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
+	}
+#endif /* CONFIG_P2P */
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		goto exit;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->retry_ctrl = false;
+	pattrib->key_type = key_type;
+	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, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_DEAUTH);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	le_reason = cpu_to_le16(reason);
+	pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_ , (unsigned char *)&le_reason, &(pattrib->pktlen));
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+
+	if (wait_ack)
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+int issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason)
+{
+	RTW_INFO("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
+	return _issue_deauth(padapter, da, reason, false, IEEE80211W_RIGHT_KEY);
+}
+
+#ifdef CONFIG_IEEE80211W
+int issue_deauth_11w(_adapter *padapter, unsigned char *da, unsigned short reason, u8 key_type)
+{
+	RTW_INFO("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
+	return _issue_deauth(padapter, da, reason, false, key_type);
+}
+#endif /* CONFIG_IEEE80211W */
+
+/*
+ * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT
+ * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX
+ * try_cnt means the maximal TX count to try
+ */
+int issue_deauth_ex(_adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
+		    int wait_ms)
+{
+	int ret = _FAIL;
+	int i = 0;
+	u32 start = jiffies;
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		goto exit;
+
+	do {
+		ret = _issue_deauth(padapter, da, reason, wait_ms > 0 ? true : false, IEEE80211W_RIGHT_KEY);
+
+		i++;
+
+		if (RTW_CANNOT_RUN(padapter))
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			rtw_msleep_os(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+#ifndef DBG_XMIT_ACK
+		goto exit;
+#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+void issue_action_spct_ch_switch(_adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset)
+{
+	unsigned long	irqL;
+	_list		*plist, *phead;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char				*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		return;
+
+	RTW_INFO(FUNC_NDEV_FMT" ra="MAC_FMT", ch:%u, offset:%u\n",
+		FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(ra), new_ch, ch_offset);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, ra, ETH_ALEN); /* RA */
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); /* TA */
+	memcpy(pwlanhdr->addr3, ra, ETH_ALEN); /* DA = RA */
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	/* category, action */
+	{
+		u8 category, action;
+		category = RTW_WLAN_CATEGORY_SPECTRUM_MGMT;
+		action = RTW_WLAN_ACTION_SPCT_CHL_SWITCH;
+
+		pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+		pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	}
+
+	pframe = rtw_set_ie_ch_switch(pframe, &(pattrib->pktlen), 0, new_ch, 0);
+	pframe = rtw_set_ie_secondary_ch_offset(pframe, &(pattrib->pktlen),
+			hal_ch_offset_to_secondary_ch_offset(ch_offset));
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+}
+
+#ifdef CONFIG_IEEE80211W
+void issue_action_SA_Query(_adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short tid, u8 key_type)
+{
+	u8	category = RTW_WLAN_CATEGORY_SA_QUERY;
+	u16	reason_code;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	u8					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_info		*psta;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct registry_priv		*pregpriv = &padapter->registrypriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		return;
+
+	RTW_INFO("%s, %04x\n", __func__, tid);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		RTW_INFO("%s: alloc_mgtxmitframe fail\n", __func__);
+		return;
+	}
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->key_type = key_type;
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	if (raddr)
+		memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	else
+		memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(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);
+
+	switch (action) {
+	case 0: /* SA Query req */
+		pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&pmlmeext->sa_query_seq, &pattrib->pktlen);
+		pmlmeext->sa_query_seq++;
+		/* send sa query request to AP, AP should reply sa query response in 1 second */
+		if (pattrib->key_type == IEEE80211W_RIGHT_KEY) {
+			psta = rtw_get_stainfo(pstapriv, raddr);
+			if (psta != NULL) {
+				/* RTW_INFO("%s, %d, set dot11w_expire_timer\n", __func__, __LINE__); */
+				_set_timer(&psta->dot11w_expire_timer, 1000);
+			}
+		}
+		break;
+
+	case 1: /* SA Query rsp */
+		tid = cpu_to_le16(tid);
+		/* RTW_INFO("rtw_set_fixed_ie, %04x\n", tid); */
+		pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&tid, &pattrib->pktlen);
+		break;
+	default:
+		break;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+#endif /* CONFIG_IEEE80211W */
+
+/**
+ * issue_action_ba - internal function to TX Block Ack action frame
+ * @padapter: the adapter to TX
+ * @raddr: receiver address
+ * @action: Block Ack Action
+ * @tid: tid
+ * @size: the announced AMPDU buffer size. used by ADDBA_RESP
+ * @status: status/reason code. used by ADDBA_RESP, DELBA
+ * @initiator: if we are the initiator of AMPDU association. used by DELBA
+ * @wait_ack: used xmit ack
+ *
+ * Returns:
+ * _SUCCESS: No xmit ack is used or acked
+ * _FAIL: not acked when using xmit ack
+ */
+static int issue_action_ba(_adapter *padapter, unsigned char *raddr, unsigned char action
+		   , u8 tid, u8 size, u16 status, u8 initiator, int wait_ack)
+{
+	int ret = _FAIL;
+	u8	category = RTW_WLAN_CATEGORY_BACK;
+	u16	start_seq;
+	u16	BA_para_set;
+	u16	BA_timeout_value;
+	u16	BA_starting_seqctrl;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	u8					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl, le_val;
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_info		*psta;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct registry_priv		*pregpriv = &padapter->registrypriv;
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		goto exit;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	/* memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); */
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+
+	if (category == 3) {
+		switch (action) {
+		case RTW_WLAN_ACTION_ADDBA_REQ:
+			do {
+				pmlmeinfo->dialogToken++;
+			} while (pmlmeinfo->dialogToken == 0);
+			pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen));
+
+			BA_para_set = (0x1002 | ((tid & 0xf) << 2)); /* immediate ack & 64 buffer size */
+
+#ifdef CONFIG_TX_AMSDU
+			if (padapter->tx_amsdu >= 1) /* TX AMSDU  enabled */
+				BA_para_set |= BIT(0);
+			else /* TX AMSDU disabled */
+				BA_para_set &= ~BIT(0);
+#endif
+			le_val = cpu_to_le16(BA_para_set);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_val)), &(pattrib->pktlen));
+
+			/* BA_timeout_value = 0xffff; */ /* max: 65535 TUs(~ 65 ms) */
+			BA_timeout_value = 5000;/* ~ 5ms */
+			le_val = cpu_to_le16(BA_timeout_value);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_val)), &(pattrib->pktlen));
+
+			/* if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL) */
+			psta = rtw_get_stainfo(pstapriv, raddr);
+			if (psta != NULL) {
+				start_seq = (psta->sta_xmitpriv.txseq_tid[tid & 0x07] & 0xfff) + 1;
+
+				RTW_INFO("BA_starting_seqctrl = %d for TID=%d\n", start_seq, tid & 0x07);
+
+				psta->BA_starting_seqctrl[tid & 0x07] = start_seq;
+
+				BA_starting_seqctrl = start_seq << 4;
+			} else {
+				BA_starting_seqctrl = 0;
+			}
+			le_val = cpu_to_le16(BA_starting_seqctrl);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_val)), &(pattrib->pktlen));
+			break;
+
+		case RTW_WLAN_ACTION_ADDBA_RESP:
+			pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen));
+			le_val = cpu_to_le16(status);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&le_val), &(pattrib->pktlen));
+
+			BA_para_set = le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set);
+
+			BA_para_set &= ~IEEE80211_ADDBA_PARAM_TID_MASK;
+			BA_para_set |= (tid << 2) & IEEE80211_ADDBA_PARAM_TID_MASK;
+
+			BA_para_set &= ~RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+			BA_para_set |= (size << 6) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+
+			if (!padapter->registrypriv.wifi_spec) {
+				if (pregpriv->ampdu_amsdu == 0) /* disabled */
+					BA_para_set &= ~BIT(0);
+				else if (pregpriv->ampdu_amsdu == 1) /* enabled */
+					BA_para_set |= BIT(0);
+			}
+
+			le_val = cpu_to_le16(BA_para_set);
+
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_val)), &(pattrib->pktlen));
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen));
+			break;
+
+		case RTW_WLAN_ACTION_DELBA:
+			BA_para_set = 0;
+			BA_para_set |= (tid << 12) & IEEE80211_DELBA_PARAM_TID_MASK;
+			BA_para_set |= (initiator << 11) & IEEE80211_DELBA_PARAM_INITIATOR_MASK;
+
+			le_val = cpu_to_le16(BA_para_set);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_val)), &(pattrib->pktlen));
+			le_val = cpu_to_le16(status);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_val)), &(pattrib->pktlen));
+			break;
+		default:
+			break;
+		}
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack)
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+/**
+ * issue_addba_req - TX ADDBA_REQ
+ * @adapter: the adapter to TX
+ * @ra: receiver address
+ * @tid: tid
+ */
+inline void issue_addba_req(_adapter *adapter, unsigned char *ra, u8 tid)
+{
+	issue_action_ba(adapter, ra, RTW_WLAN_ACTION_ADDBA_REQ
+			, tid
+			, 0 /* unused */
+			, 0 /* unused */
+			, 0 /* unused */
+			, false
+		       );
+	RTW_INFO(FUNC_ADPT_FMT" ra="MAC_FMT" tid=%u\n"
+		 , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), tid);
+
+}
+
+/**
+ * issue_addba_rsp - TX ADDBA_RESP
+ * @adapter: the adapter to TX
+ * @ra: receiver address
+ * @tid: tid
+ * @status: status code
+ * @size: the announced AMPDU buffer size
+ */
+inline void issue_addba_rsp(_adapter *adapter, unsigned char *ra, u8 tid, u16 status, u8 size)
+{
+	issue_action_ba(adapter, ra, RTW_WLAN_ACTION_ADDBA_RESP
+			, tid
+			, size
+			, status
+			, 0 /* unused */
+			, false
+		       );
+	RTW_INFO(FUNC_ADPT_FMT" ra="MAC_FMT" status=%u, tid=%u, size=%u\n"
+		 , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), status, tid, size);
+}
+
+/**
+ * issue_addba_rsp_wait_ack - TX ADDBA_RESP and wait ack
+ * @adapter: the adapter to TX
+ * @ra: receiver address
+ * @tid: tid
+ * @status: status code
+ * @size: the announced AMPDU buffer size
+ * @try_cnt: the maximal TX count to try
+ * @wait_ms: == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT
+ *           > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX
+ */
+inline u8 issue_addba_rsp_wait_ack(_adapter *adapter, unsigned char *ra, u8 tid, u16 status, u8 size, int try_cnt, int wait_ms)
+{
+	int ret = _FAIL;
+	int i = 0;
+	u32 start = jiffies;
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(adapter)))
+		goto exit;
+
+	do {
+		ret = issue_action_ba(adapter, ra, RTW_WLAN_ACTION_ADDBA_RESP
+				      , tid
+				      , size
+				      , status
+				      , 0 /* unused */
+				      , true
+				     );
+
+		i++;
+
+		if (RTW_CANNOT_RUN(adapter))
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			rtw_msleep_os(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+#ifndef DBG_XMIT_ACK
+		/* goto exit; */
+#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		RTW_INFO(FUNC_ADPT_FMT" ra="MAC_FMT" tid=%u%s, %d/%d in %u ms\n"
+			, FUNC_ADPT_ARG(adapter), MAC_ARG(ra), tid
+			, ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+
+exit:
+	return ret;
+}
+
+/**
+ * issue_del_ba - TX DELBA
+ * @adapter: the adapter to TX
+ * @ra: receiver address
+ * @tid: tid
+ * @reason: reason code
+ * @initiator: if we are the initiator of AMPDU association. used by DELBA
+ */
+inline void issue_del_ba(_adapter *adapter, unsigned char *ra, u8 tid, u16 reason, u8 initiator)
+{
+	issue_action_ba(adapter, ra, RTW_WLAN_ACTION_DELBA
+			, tid
+			, 0 /* unused */
+			, reason
+			, initiator
+			, false
+		       );
+	RTW_INFO(FUNC_ADPT_FMT" ra="MAC_FMT" reason=%u, tid=%u, initiator=%u\n"
+		 , FUNC_ADPT_ARG(adapter), MAC_ARG(ra), reason, tid, initiator);
+}
+
+/**
+ * issue_del_ba_ex - TX DELBA with xmit ack options
+ * @adapter: the adapter to TX
+ * @ra: receiver address
+ * @tid: tid
+ * @reason: reason code
+ * @initiator: if we are the initiator of AMPDU association. used by DELBA
+ * @try_cnt: the maximal TX count to try
+ * @wait_ms: == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT
+ *           > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX
+ */
+int issue_del_ba_ex(_adapter *adapter, unsigned char *ra, u8 tid, u16 reason, u8 initiator
+		    , int try_cnt, int wait_ms)
+{
+	int ret = _FAIL;
+	int i = 0;
+	u32 start = jiffies;
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(adapter)))
+		goto exit;
+
+	do {
+		ret = issue_action_ba(adapter, ra, RTW_WLAN_ACTION_DELBA
+				      , tid
+				      , 0 /* unused */
+				      , reason
+				      , initiator
+				      , wait_ms > 0 ? true : false
+				     );
+
+		i++;
+
+		if (RTW_CANNOT_RUN(adapter))
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			rtw_msleep_os(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+#ifndef DBG_XMIT_ACK
+		/* goto exit; */
+#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		RTW_INFO(FUNC_ADPT_FMT" ra="MAC_FMT" reason=%u, tid=%u, initiator=%u%s, %d/%d in %u ms\n"
+			, FUNC_ADPT_ARG(adapter), MAC_ARG(ra), reason, tid, initiator
+			, ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+static void issue_action_BSSCoexistPacket(_adapter *padapter)
+{
+	unsigned long	irqL;
+	_list		*plist, *phead;
+	unsigned char category, action;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char				*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct	wlan_network	*pnetwork = NULL;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	_queue		*queue	= &(pmlmepriv->scanned_queue);
+	u8 InfoContent[16] = {0};
+	u8 ICS[8][15];
+
+	if ((pmlmepriv->num_FortyMHzIntolerant == 0) || (pmlmepriv->num_sta_no_ht == 0))
+		return;
+
+	if (pmlmeinfo->bwmode_updated)
+		return;
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		return;
+
+	RTW_INFO("%s\n", __func__);
+
+
+	category = RTW_WLAN_CATEGORY_PUBLIC;
+	action = ACT_PUBLIC_BSSCOEXIST;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+
+
+	/*  */
+	if (pmlmepriv->num_FortyMHzIntolerant > 0) {
+		u8 iedata = 0;
+
+		iedata |= BIT(2);/* 20 MHz BSS Width Request */
+
+		pframe = rtw_set_ie(pframe, EID_BSSCoexistence,  1, &iedata, &(pattrib->pktlen));
+
+	}
+
+
+	/*  */
+	memset(ICS, 0, sizeof(ICS));
+	if (pmlmepriv->num_sta_no_ht > 0) {
+		int i;
+
+		_enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+		phead = get_list_head(queue);
+		plist = get_next(phead);
+
+		while (1) {
+			int len;
+			u8 *p;
+			WLAN_BSSID_EX *pbss_network;
+
+			if (rtw_end_of_queue_search(phead, plist))
+				break;
+
+			pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+			plist = get_next(plist);
+
+			pbss_network = (WLAN_BSSID_EX *)&pnetwork->network;
+
+			p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
+			if ((p == NULL) || (len == 0)) { /* non-HT */
+				if ((pbss_network->Configuration.DSConfig <= 0) || (pbss_network->Configuration.DSConfig > 14))
+					continue;
+
+				ICS[0][pbss_network->Configuration.DSConfig] = 1;
+
+				if (ICS[0][0] == 0)
+					ICS[0][0] = 1;
+			}
+
+		}
+
+		_exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
+
+
+		for (i = 0; i < 8; i++) {
+			if (ICS[i][0] == 1) {
+				int j, k = 0;
+
+				InfoContent[k] = i;
+				/* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent,i); */
+				k++;
+
+				for (j = 1; j <= 14; j++) {
+					if (ICS[i][j] == 1) {
+						if (k < 16) {
+							InfoContent[k] = j; /* channel number */
+							/* SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j); */
+							k++;
+						}
+					}
+				}
+
+				pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &(pattrib->pktlen));
+
+			}
+
+		}
+
+
+	}
+
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+/* Spatial Multiplexing Powersave (SMPS) action frame */
+static int _issue_action_SM_PS(_adapter *padapter ,  unsigned char *raddr , u8 NewMimoPsMode ,  u8 wait_ack)
+{
+
+	int ret = _FAIL;
+	unsigned char category = RTW_WLAN_CATEGORY_HT;
+	u8 action = RTW_WLAN_ACTION_HT_SM_PS;
+	u8 sm_power_control = 0;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+
+	if (NewMimoPsMode == WLAN_HT_CAP_SM_PS_DISABLED) {
+		sm_power_control = sm_power_control  & ~(BIT(0)); /* SM Power Save Enable = 0 SM Power Save Disable */
+	} else if (NewMimoPsMode == WLAN_HT_CAP_SM_PS_STATIC) {
+		sm_power_control = sm_power_control | BIT(0);    /* SM Power Save Enable = 1 SM Power Save Enable  */
+		sm_power_control = sm_power_control & ~(BIT(1)); /* SM Mode = 0 Static Mode */
+	} else if (NewMimoPsMode == WLAN_HT_CAP_SM_PS_DYNAMIC) {
+		sm_power_control = sm_power_control | BIT(0); /* SM Power Save Enable = 1 SM Power Save Enable  */
+		sm_power_control = sm_power_control | BIT(1); /* SM Mode = 1 Dynamic Mode */
+	} else
+		return ret;
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		return ret;
+
+	RTW_INFO("%s, sm_power_control=%u, NewMimoPsMode=%u\n", __func__ , sm_power_control , NewMimoPsMode);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return ret;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	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); /* RA */
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN); /* TA */
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); /* DA = RA */
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	/* category, action */
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(sm_power_control), &(pattrib->pktlen));
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack)
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+	if (ret != _SUCCESS)
+		RTW_INFO("%s, ack to\n", __func__);
+
+	return ret;
+}
+
+/*
+ * wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT
+ * wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX
+ * try_cnt means the maximal TX count to try
+ */
+int issue_action_SM_PS_wait_ack(_adapter *padapter, unsigned char *raddr, u8 NewMimoPsMode, int try_cnt, int wait_ms)
+{
+	int ret = _FAIL;
+	int i = 0;
+	u32 start = jiffies;
+
+	if (rtw_rfctl_is_tx_blocked_by_ch_waiting(adapter_to_rfctl(padapter)))
+		goto exit;
+
+	do {
+		ret = _issue_action_SM_PS(padapter, raddr, NewMimoPsMode , wait_ms > 0 ? true : false);
+
+		i++;
+
+		if (RTW_CANNOT_RUN(padapter))
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			rtw_msleep_os(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+#ifndef DBG_XMIT_ACK
+		goto exit;
+#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (raddr)
+			RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", %s , %d/%d in %u ms\n",
+				 FUNC_ADPT_ARG(padapter), MAC_ARG(raddr),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			RTW_INFO(FUNC_ADPT_FMT", %s , %d/%d in %u ms\n",
+				 FUNC_ADPT_ARG(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+
+	return ret;
+}
+
+int issue_action_SM_PS(_adapter *padapter ,  unsigned char *raddr , u8 NewMimoPsMode)
+{
+	RTW_INFO("%s to "MAC_FMT"\n", __func__, MAC_ARG(raddr));
+	return _issue_action_SM_PS(padapter, raddr, NewMimoPsMode , false);
+}
+
+/**
+ * _send_delba_sta_tid - Cancel the AMPDU association for the specific @sta, @tid
+ * @adapter: the adapter to which @sta belongs
+ * @initiator: if we are the initiator of AMPDU association
+ * @sta: the sta to be checked
+ * @tid: the tid to be checked
+ * @force: cancel and send DELBA even when no AMPDU association is setup
+ * @wait_ack: send delba with xmit ack (valid when initiator == 0)
+ *
+ * Returns:
+ * _FAIL if sta is NULL
+ * when initiator is 1, always _SUCCESS
+ * when initiator is 0, _SUCCESS if DELBA is acked
+ */
+static unsigned int _send_delba_sta_tid(_adapter *adapter, u8 initiator, struct sta_info *sta, u8 tid
+					, u8 force, int wait_ack)
+{
+	int ret = _SUCCESS;
+
+	if (sta == NULL) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	if (initiator == 0) {
+		/* recipient */
+		if (force || sta->recvreorder_ctrl[tid].enable) {
+			u8 ampdu_size_bak = sta->recvreorder_ctrl[tid].ampdu_size;
+
+			sta->recvreorder_ctrl[tid].enable = false;
+			sta->recvreorder_ctrl[tid].ampdu_size = RX_AMPDU_SIZE_INVALID;
+
+			if (rtw_del_rx_ampdu_test_trigger_no_tx_fail())
+				ret = _FAIL;
+			else if (wait_ack)
+				ret = issue_del_ba_ex(adapter, sta->hwaddr, tid, 37, initiator, 3, 1);
+			else
+				issue_del_ba(adapter, sta->hwaddr, tid, 37, initiator);
+
+			if (ret == _FAIL && sta->recvreorder_ctrl[tid].enable == false)
+				sta->recvreorder_ctrl[tid].ampdu_size = ampdu_size_bak;
+		}
+	} else if (initiator == 1) {
+		/* originator */
+		if (force || sta->htpriv.agg_enable_bitmap & BIT(tid)) {
+			sta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+			sta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+			issue_del_ba(adapter, sta->hwaddr, tid, 37, initiator);
+		}
+	}
+
+exit:
+	return ret;
+}
+
+inline unsigned int send_delba_sta_tid(_adapter *adapter, u8 initiator, struct sta_info *sta, u8 tid
+				       , u8 force)
+{
+	return _send_delba_sta_tid(adapter, initiator, sta, tid, force, 0);
+}
+
+inline unsigned int send_delba_sta_tid_wait_ack(_adapter *adapter, u8 initiator, struct sta_info *sta, u8 tid
+		, u8 force)
+{
+	return _send_delba_sta_tid(adapter, initiator, sta, tid, force, 1);
+}
+
+unsigned int send_delba(_adapter *padapter, u8 initiator, u8 *addr)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u16 tid;
+
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+			return _SUCCESS;
+
+	psta = rtw_get_stainfo(pstapriv, addr);
+	if (psta == NULL)
+		return _SUCCESS;
+	for (tid = 0; tid < TID_NUM; tid++)
+		send_delba_sta_tid(padapter, initiator, psta, tid, 0);
+	return _SUCCESS;
+}
+
+unsigned int send_beacon(_adapter *padapter)
+{
+	u8	bxmitok = false;
+	int	issue = 0;
+	int poll = 0;
+#if defined(CONFIG_PCI_HCI) && defined(RTL8814AE_SW_BCN)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+#endif
+
+#ifdef CONFIG_PCI_HCI
+	/* RTW_INFO("%s\n", __func__); */
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
+
+	/* 8192EE Port select for Beacon DL */
+	rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
+
+	issue_beacon(padapter, 0);
+
+#ifdef RTL8814AE_SW_BCN
+	if (pHalData->bCorrectBCN != 0)
+		RTW_INFO("%s, line%d, Warnning, pHalData->bCorrectBCN != 0\n", __func__, __LINE__);
+	pHalData->bCorrectBCN = 1;
+#endif
+
+	return _SUCCESS;
+#endif
+
+	u32 start = jiffies;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
+	rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
+	do {
+		issue_beacon(padapter, 100);
+		issue++;
+		do {
+			rtw_yield_os();
+			rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
+			poll++;
+		} while ((poll % 10) != 0 && false == bxmitok && !RTW_CANNOT_RUN(padapter));
+
+	} while (false == bxmitok && issue < 100 && !RTW_CANNOT_RUN(padapter));
+
+	if (RTW_CANNOT_RUN(padapter))
+		return _FAIL;
+
+
+	if (false == bxmitok) {
+		RTW_INFO("%s fail! %u ms\n", __func__, rtw_get_passing_time_ms(start));
+		return _FAIL;
+	} else {
+		u32 passing_time = rtw_get_passing_time_ms(start);
+
+		if (passing_time > 100 || issue > 3)
+			RTW_INFO("%s success, issue:%d, poll:%d, %u ms\n", __func__, issue, poll, rtw_get_passing_time_ms(start));
+		else if (0)
+			RTW_INFO("%s success, issue:%d, poll:%d, %u ms\n", __func__, issue, poll, rtw_get_passing_time_ms(start));
+
+		rtw_hal_fw_correct_bcn(padapter);
+
+		return _SUCCESS;
+	}
+}
+
+/****************************************************************************
+
+Following are some utitity fuctions for WiFi MLME
+
+*****************************************************************************/
+
+bool IsLegal5GChannel(
+	PADAPTER			Adapter,
+	u8			channel)
+{
+
+	int i = 0;
+	u8 Channel_5G[45] = {36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58,
+		60, 62, 64, 100, 102, 104, 106, 108, 110, 112, 114, 116, 118, 120, 122,
+		124, 126, 128, 130, 132, 134, 136, 138, 140, 149, 151, 153, 155, 157, 159,
+			     161, 163, 165
+			    };
+	for (i = 0; i < sizeof(Channel_5G); i++)
+		if (channel == Channel_5G[i])
+			return true;
+	return false;
+}
+
+/* collect bss info from Beacon and Probe request/response frames. */
+u8 collect_bss_info(_adapter *padapter, union recv_frame *precv_frame, WLAN_BSSID_EX *bssid)
+{
+	int	i;
+	u32	len;
+	u8	*p;
+	u16	val16, subtype;
+	u8	*pframe = precv_frame->u.hdr.rx_data;
+	u32	packet_len = precv_frame->u.hdr.len;
+	u8 ie_offset;
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ) {
+		/* RTW_INFO("IE too long for survey event\n"); */
+		return _FAIL;
+	}
+
+	memset(bssid, 0, sizeof(WLAN_BSSID_EX));
+
+	subtype = get_frame_sub_type(pframe);
+
+	if (subtype == WIFI_BEACON) {
+		bssid->Reserved[0] = 1;
+		ie_offset = _BEACON_IE_OFFSET_;
+	} else {
+		/* FIXME : more type */
+		if (subtype == WIFI_PROBERSP) {
+			ie_offset = _PROBERSP_IE_OFFSET_;
+			bssid->Reserved[0] = 3;
+		} else if (subtype == WIFI_PROBEREQ) {
+			ie_offset = _PROBEREQ_IE_OFFSET_;
+			bssid->Reserved[0] = 2;
+		} else {
+			bssid->Reserved[0] = 0;
+			ie_offset = _FIXED_IE_LENGTH_;
+		}
+	}
+
+	bssid->Length = sizeof(WLAN_BSSID_EX) - MAX_IE_SZ + len;
+
+	/* below is to copy the information element */
+	bssid->IELength = len;
+	memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength);
+
+	/* get the signal strength */
+	/* bssid->Rssi = precv_frame->u.hdr.attrib.SignalStrength; */ /* 0-100 index. */
+	bssid->Rssi = precv_frame->u.hdr.attrib.phy_info.RecvSignalPower; /* in dBM.raw data	 */
+	bssid->PhyInfo.SignalQuality = precv_frame->u.hdr.attrib.phy_info.SignalQuality;/* in percentage */
+	bssid->PhyInfo.SignalStrength = precv_frame->u.hdr.attrib.phy_info.SignalStrength;/* in percentage */
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	rtw_hal_get_odm_var(padapter, HAL_ODM_ANTDIV_SELECT, &(bssid->PhyInfo.Optimum_antenna), NULL);
+#endif
+
+	/* checking SSID */
+	p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset);
+	if (p == NULL) {
+		RTW_INFO("marc: cannot find SSID for survey event\n");
+		return _FAIL;
+	}
+
+	if (*(p + 1)) {
+		if (len > NDIS_802_11_LENGTH_SSID) {
+			RTW_INFO("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+			return _FAIL;
+		}
+		memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
+		bssid->Ssid.SsidLength = *(p + 1);
+	} else
+		bssid->Ssid.SsidLength = 0;
+
+	memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+	/* checking rate info... */
+	i = 0;
+	p = rtw_get_ie(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
+	if (p != NULL) {
+		if (len > NDIS_802_11_LENGTH_RATES_EX) {
+			RTW_INFO("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+			return _FAIL;
+		}
+		memcpy(bssid->SupportedRates, (p + 2), len);
+		i = len;
+	}
+
+	p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
+	if (p != NULL) {
+		if (len > (NDIS_802_11_LENGTH_RATES_EX - i)) {
+			RTW_INFO("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+			return _FAIL;
+		}
+		memcpy(bssid->SupportedRates + i, (p + 2), len);
+	}
+
+	/* todo: */
+	bssid->NetworkTypeInUse = Ndis802_11OFDM24;
+
+#ifdef CONFIG_P2P
+	if (subtype == WIFI_PROBEREQ) {
+		u8 *p2p_ie;
+		u32	p2p_ielen;
+		/* Set Listion Channel */
+		p2p_ie = rtw_get_p2p_ie(bssid->IEs, bssid->IELength, NULL, &p2p_ielen);
+		if (p2p_ie) {
+			u32	attr_contentlen = 0;
+			u8 listen_ch[5] = { 0x00 };
+
+			rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_LISTEN_CH, listen_ch, &attr_contentlen);
+			bssid->Configuration.DSConfig = listen_ch[4];
+		} else {
+			/* use current channel */
+			bssid->Configuration.DSConfig = padapter->mlmeextpriv.cur_channel;
+			RTW_INFO("%s()-%d: Cannot get p2p_ie. set DSconfig to op_ch(%d)\n", __func__, __LINE__, bssid->Configuration.DSConfig);
+		}
+
+		/* FIXME */
+		bssid->InfrastructureMode = Ndis802_11Infrastructure;
+		memcpy(bssid->MacAddress, get_addr2_ptr(pframe), ETH_ALEN);
+		bssid->Privacy = 1;
+		return _SUCCESS;
+	}
+#endif /* CONFIG_P2P */
+
+	if (bssid->IELength < 12)
+		return _FAIL;
+
+	/* Checking for DSConfig */
+	p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
+
+	bssid->Configuration.DSConfig = 0;
+	bssid->Configuration.Length = 0;
+
+	if (p)
+		bssid->Configuration.DSConfig = *(p + 2);
+	else {
+		/* In 5G, some ap do not have DSSET IE */
+		/* checking HT info for channel */
+		p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
+		if (p) {
+			struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
+			bssid->Configuration.DSConfig = HT_info->primary_channel;
+		} else {
+			/* use current channel */
+			bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter);
+		}
+	}
+
+	memcpy(&bssid->Configuration.BeaconPeriod, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
+
+	val16 = rtw_get_capability((WLAN_BSSID_EX *)bssid);
+
+	if (val16 & BIT(0)) {
+		bssid->InfrastructureMode = Ndis802_11Infrastructure;
+		memcpy(bssid->MacAddress, get_addr2_ptr(pframe), ETH_ALEN);
+	} else {
+		bssid->InfrastructureMode = Ndis802_11IBSS;
+		memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN);
+	}
+
+	if (val16 & BIT(4))
+		bssid->Privacy = 1;
+	else
+		bssid->Privacy = 0;
+
+	bssid->Configuration.ATIMWindow = 0;
+
+	/* 20/40 BSS Coexistence check */
+	if ((pregistrypriv->wifi_spec == 1) && (false == pmlmeinfo->bwmode_updated)) {
+		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+		p = rtw_get_ie(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset);
+		if (p && len > 0) {
+			struct HT_caps_element	*pHT_caps;
+			pHT_caps = (struct HT_caps_element *)(p + 2);
+
+			if (pHT_caps->u.HT_cap_element.HT_caps_info & cpu_to_le16(BIT(14)))
+				pmlmepriv->num_FortyMHzIntolerant++;
+		} else
+			pmlmepriv->num_sta_no_ht++;
+	}
+
+#ifdef CONFIG_INTEL_WIDI
+	/* process_intel_widi_query_or_tigger(padapter, bssid); */
+	if (process_intel_widi_query_or_tigger(padapter, bssid))
+		return _FAIL;
+#endif /* CONFIG_INTEL_WIDI */
+
+#if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) & 1
+	if (strcmp(bssid->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
+		RTW_INFO("Receiving %s("MAC_FMT", DSConfig:%u) from ch%u with ss:%3u, sq:%3u, RawRSSI:%3ld\n"
+			, bssid->Ssid.Ssid, MAC_ARG(bssid->MacAddress), bssid->Configuration.DSConfig
+			 , rtw_get_oper_ch(padapter)
+			, bssid->PhyInfo.SignalStrength, bssid->PhyInfo.SignalQuality, bssid->Rssi
+			);
+	}
+#endif
+
+	/* mark bss info receving from nearby channel as SignalQuality 101 */
+	if (bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
+		bssid->PhyInfo.SignalQuality = 101;
+
+	return _SUCCESS;
+}
+
+void start_create_ibss(_adapter *padapter)
+{
+	unsigned short	caps;
+	u8	val8;
+	u8	join_type;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network));
+	u8 doiqk = false;
+	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
+
+	/* update wireless mode */
+	update_wireless_mode(padapter);
+
+	/* udpate capability */
+	caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
+	update_capinfo(padapter, caps);
+	if (caps & cap_IBSS) { /* adhoc master */
+		/* set_opmode_cmd(padapter, adhoc); */ /* removed */
+
+		val8 = 0xcf;
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+		doiqk = true;
+		rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk);
+
+		/* switch channel */
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+
+		doiqk = false;
+		rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk);
+
+		beacon_timing_control(padapter);
+
+		/* set msr to WIFI_FW_ADHOC_STATE */
+		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+		Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+		/* issue beacon */
+		if (send_beacon(padapter) == _FAIL) {
+
+			report_join_res(padapter, -1);
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		} else {
+			rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
+			join_type = 0;
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+			report_join_res(padapter, 1);
+			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+			rtw_indicate_connect(padapter);
+		}
+	} else {
+		RTW_INFO("start_create_ibss, invalid cap:%x\n", caps);
+		return;
+	}
+	/* update bc/mc sta_info */
+	update_bmc_sta(padapter);
+
+}
+
+void start_clnt_join(_adapter *padapter)
+{
+	unsigned short	caps;
+	u8	val8;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network));
+	int beacon_timeout;
+	u8 ASIX_ID[] = {0x00, 0x0E, 0xC6};
+
+	/* update wireless mode */
+	update_wireless_mode(padapter);
+
+	/* udpate capability */
+	caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
+	update_capinfo(padapter, caps);
+
+	/* check if sta is ASIX peer and fix IOT issue if it is. */
+	if (!memcmp(get_my_bssid(&pmlmeinfo->network) , ASIX_ID , 3)) {
+		u8 iot_flag = true;
+		rtw_hal_set_hwreg(padapter, HW_VAR_ASIX_IOT, (u8 *)(&iot_flag));
+	}
+
+	if (caps & cap_ESS) {
+		Set_MSR(padapter, WIFI_FW_STATION_STATE);
+
+		val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf;
+
+#ifdef CONFIG_WAPI_SUPPORT
+		if (padapter->wapiInfo.bWapiEnable && pmlmeinfo->auth_algo == dot11AuthAlgrthm_WAPI) {
+			/* Disable TxUseDefaultKey, RxUseDefaultKey, RxBroadcastUseDefaultKey. */
+			val8 = 0x4c;
+		}
+#endif
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+#ifdef CONFIG_DEAUTH_BEFORE_CONNECT
+		/* Because of AP's not receiving deauth before */
+		/* AP may: 1)not response auth or 2)deauth us after link is complete */
+		/* issue deauth before issuing auth to deal with the situation */
+
+		/*	Commented by Albert 2012/07/21 */
+		/*	For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it. */
+		{
+#ifdef CONFIG_P2P
+			_queue *queue = &(padapter->mlmepriv.scanned_queue);
+			_list	*head = get_list_head(queue);
+			_list *pos = get_next(head);
+			struct wlan_network *scanned = NULL;
+			u8 ie_offset = 0;
+			unsigned long irqL;
+			bool has_p2p_ie = false;
+
+			_enter_critical_bh(&(padapter->mlmepriv.scanned_queue.lock), &irqL);
+
+			for (pos = get_next(head); !rtw_end_of_queue_search(head, pos); pos = get_next(pos)) {
+
+				scanned = LIST_CONTAINOR(pos, struct wlan_network, list);
+
+				if (!memcmp(&(scanned->network.Ssid), &(pnetwork->Ssid), sizeof(NDIS_802_11_SSID))
+				    && !memcmp(scanned->network.MacAddress, pnetwork->MacAddress, sizeof(NDIS_802_11_MAC_ADDRESS))
+				   ) {
+					ie_offset = (scanned->network.Reserved[0] == 2 ? 0 : 12);
+					if (rtw_get_p2p_ie(scanned->network.IEs + ie_offset, scanned->network.IELength - ie_offset, NULL, NULL))
+						has_p2p_ie = true;
+					break;
+				}
+			}
+
+			_exit_critical_bh(&(padapter->mlmepriv.scanned_queue.lock), &irqL);
+
+			if (scanned == NULL || rtw_end_of_queue_search(head, pos) || has_p2p_ie == false)
+#endif /* CONFIG_P2P */
+				/* To avoid connecting to AP fail during resume process, change retry count from 5 to 1 */
+				issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
+		}
+#endif /* CONFIG_DEAUTH_BEFORE_CONNECT */
+
+		/* here wait for receiving the beacon to start auth */
+		/* and enable a timer */
+		beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
+		set_link_timer(pmlmeext, beacon_timeout);
+		_set_timer(&padapter->mlmepriv.assoc_timer,
+			(REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO * REASSOC_LIMIT) + beacon_timeout);
+
+#ifdef CONFIG_RTW_80211R
+		if ((rtw_to_roam(padapter) > 0) && rtw_chk_ft_flags(padapter, RTW_FT_SUPPORTED)) {
+			if (rtw_chk_ft_flags(padapter, RTW_FT_OVER_DS_SUPPORTED)) {
+				struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+				ft_priv *pftpriv = &pmlmepriv->ftpriv;
+
+				pmlmeinfo->state = WIFI_FW_AUTH_SUCCESS | WIFI_FW_STATION_STATE;
+				pftpriv->ft_event.ies =  pftpriv->ft_action + sizeof(struct rtw_ieee80211_hdr_3addr) + 16;
+				pftpriv->ft_event.ies_len = pftpriv->ft_action_len - sizeof(struct rtw_ieee80211_hdr_3addr);
+
+				/*Not support RIC*/
+				pftpriv->ft_event.ric_ies =  NULL;
+				pftpriv->ft_event.ric_ies_len = 0;
+				report_ft_event(padapter);
+			} else {
+				pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
+				start_clnt_auth(padapter);
+			}
+		} else
+#endif
+			pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
+	} else if (caps & cap_IBSS) { /* adhoc client */
+		Set_MSR(padapter, WIFI_FW_ADHOC_STATE);
+
+		val8 = 0xcf;
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+		beacon_timing_control(padapter);
+
+		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+
+		report_join_res(padapter, 1);
+	} else {
+		/* RTW_INFO("marc: invalid cap:%x\n", caps); */
+		return;
+	}
+
+}
+
+void start_clnt_auth(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
+	pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
+
+	pmlmeinfo->auth_seq = 1;
+	pmlmeinfo->reauth_count = 0;
+	pmlmeinfo->reassoc_count = 0;
+	pmlmeinfo->link_count = 0;
+	pmlmeext->retry = 0;
+
+#ifdef CONFIG_RTW_80211R
+	if ((rtw_to_roam(padapter) > 0) && rtw_chk_ft_flags(padapter, RTW_FT_SUPPORTED)) {
+		rtw_set_ft_status(padapter, RTW_FT_AUTHENTICATING_STA);
+		RTW_PRINT("start ft auth\n");
+	} else
+#endif
+		RTW_PRINT("start auth\n");
+	issue_auth(padapter, NULL, 0);
+
+	set_link_timer(pmlmeext, REAUTH_TO);
+
+}
+
+
+void start_clnt_assoc(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
+	pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
+
+#ifdef CONFIG_RTW_80211R
+	if ((rtw_to_roam(padapter) > 0) && rtw_chk_ft_flags(padapter, RTW_FT_SUPPORTED))
+		issue_reassocreq(padapter);
+	else
+#endif
+		issue_assocreq(padapter);
+
+	set_link_timer(pmlmeext, REASSOC_TO);
+}
+
+unsigned int receive_disconnect(_adapter *padapter, unsigned char *MacAddr, unsigned short reason, u8 locally_generated)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (!(!memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+	RTW_INFO("%s\n", __func__);
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
+			if (report_del_sta_event(padapter, MacAddr, reason, true, locally_generated) != _FAIL)
+				pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		} else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) {
+			if (report_join_res(padapter, -2) != _FAIL)
+				pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		} else
+			RTW_INFO(FUNC_ADPT_FMT" - End to Disconnect\n", FUNC_ADPT_ARG(padapter));
+#ifdef CONFIG_RTW_80211R
+		if ((rtw_to_roam(padapter) > 0) && !rtw_chk_ft_status(padapter, RTW_FT_REQUESTED_STA))
+			rtw_reset_ft_status(padapter);
+#endif
+	}
+
+	return _SUCCESS;
+}
+
+#ifdef CONFIG_80211D
+static void process_80211d(PADAPTER padapter, WLAN_BSSID_EX *bssid)
+{
+	struct registry_priv *pregistrypriv;
+	struct mlme_ext_priv *pmlmeext;
+	RT_CHANNEL_INFO *chplan_new;
+	u8 channel;
+	u8 i;
+
+
+	pregistrypriv = &padapter->registrypriv;
+	pmlmeext = &padapter->mlmeextpriv;
+
+	/* Adjust channel plan by AP Country IE */
+	if (pregistrypriv->enable80211d
+	    && (!pmlmeext->update_channel_plan_by_ap_done)) {
+		u8 *ie, *p;
+		u32 len;
+		RT_CHANNEL_PLAN chplan_ap;
+		RT_CHANNEL_INFO chplan_sta[MAX_CHANNEL_NUM];
+		u8 country[4];
+		u8 fcn; /* first channel number */
+		u8 noc; /* number of channel */
+		u8 j, k;
+
+		ie = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+		if (!ie)
+			return;
+		if (len < 6)
+			return;
+
+		ie += 2;
+		p = ie;
+		ie += len;
+
+		memset(country, 0, 4);
+		memcpy(country, p, 3);
+		p += 3;
+		RTW_INFO("%s: 802.11d country=%s\n", __func__, country);
+
+		i = 0;
+		while ((ie - p) >= 3) {
+			fcn = *(p++);
+			noc = *(p++);
+			p++;
+
+			for (j = 0; j < noc; j++) {
+				if (fcn <= 14)
+					channel = fcn + j; /* 2.4 GHz */
+				else
+					channel = fcn + j * 4; /* 5 GHz */
+
+				chplan_ap.Channel[i++] = channel;
+			}
+		}
+		chplan_ap.Len = i;
+
+#ifdef CONFIG_RTW_DEBUG
+		i = 0;
+		RTW_INFO("%s: AP[%s] channel plan {", __func__, bssid->Ssid.Ssid);
+		while ((i < chplan_ap.Len) && (chplan_ap.Channel[i] != 0)) {
+			_RTW_INFO("%02d,", chplan_ap.Channel[i]);
+			i++;
+		}
+		_RTW_INFO("}\n");
+#endif
+
+		memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
+#ifdef CONFIG_RTW_DEBUG
+		i = 0;
+		RTW_INFO("%s: STA channel plan {", __func__);
+		while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
+			_RTW_INFO("%02d(%c),", chplan_sta[i].ChannelNum, chplan_sta[i].ScanType == SCAN_PASSIVE ? 'p' : 'a');
+			i++;
+		}
+		_RTW_INFO("}\n");
+#endif
+
+		memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
+		chplan_new = pmlmeext->channel_set;
+
+		i = j = k = 0;
+		if (pregistrypriv->wireless_mode & WIRELESS_11G) {
+			do {
+				if ((i == MAX_CHANNEL_NUM)
+				    || (chplan_sta[i].ChannelNum == 0)
+				    || (chplan_sta[i].ChannelNum > 14))
+					break;
+
+				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
+					break;
+
+				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					i++;
+					j++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+					chplan_new[k].ScanType = SCAN_PASSIVE;
+					i++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					j++;
+					k++;
+				}
+			} while (1);
+
+			/* change AP not support channel to Passive scan */
+			while ((i < MAX_CHANNEL_NUM)
+			       && (chplan_sta[i].ChannelNum != 0)
+			       && (chplan_sta[i].ChannelNum <= 14)) {
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+				chplan_new[k].ScanType = SCAN_PASSIVE;
+				i++;
+				k++;
+			}
+
+			/* add channel AP supported */
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
+				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				chplan_new[k].ScanType = SCAN_ACTIVE;
+				j++;
+				k++;
+			}
+		} else {
+			/* keep original STA 2.4G channel plan */
+			while ((i < MAX_CHANNEL_NUM)
+			       && (chplan_sta[i].ChannelNum != 0)
+			       && (chplan_sta[i].ChannelNum <= 14)) {
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+				chplan_new[k].ScanType = chplan_sta[i].ScanType;
+				i++;
+				k++;
+			}
+
+			/* skip AP 2.4G channel plan */
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
+				j++;
+		}
+
+		if (pregistrypriv->wireless_mode & WIRELESS_11A) {
+			do {
+				if ((i >= MAX_CHANNEL_NUM)
+				    || (chplan_sta[i].ChannelNum == 0))
+					break;
+
+				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0))
+					break;
+
+				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					i++;
+					j++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+					chplan_new[k].ScanType = SCAN_PASSIVE;
+					i++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					j++;
+					k++;
+				}
+			} while (1);
+
+			/* change AP not support channel to Passive scan */
+			while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+				chplan_new[k].ScanType = SCAN_PASSIVE;
+				i++;
+				k++;
+			}
+
+			/* add channel AP supported */
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] != 0)) {
+				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				chplan_new[k].ScanType = SCAN_ACTIVE;
+				j++;
+				k++;
+			}
+		} else {
+			/* keep original STA 5G channel plan */
+			while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+				chplan_new[k].ScanType = chplan_sta[i].ScanType;
+				i++;
+				k++;
+			}
+		}
+
+		pmlmeext->update_channel_plan_by_ap_done = 1;
+
+#ifdef CONFIG_RTW_DEBUG
+		k = 0;
+		RTW_INFO("%s: new STA channel plan {", __func__);
+		while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0)) {
+			_RTW_INFO("%02d(%c),", chplan_new[k].ChannelNum, chplan_new[k].ScanType == SCAN_PASSIVE ? 'p' : 'c');
+			k++;
+		}
+		_RTW_INFO("}\n");
+#endif
+	}
+
+	/* If channel is used by AP, set channel scan type to active */
+	channel = bssid->Configuration.DSConfig;
+	chplan_new = pmlmeext->channel_set;
+	i = 0;
+	while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0)) {
+		if (chplan_new[i].ChannelNum == channel) {
+			if (chplan_new[i].ScanType == SCAN_PASSIVE) {
+				/* 5G Bnad 2, 3 (DFS) doesn't change to active scan */
+				if (channel >= 52 && channel <= 144)
+					break;
+
+				chplan_new[i].ScanType = SCAN_ACTIVE;
+				RTW_INFO("%s: change channel %d scan type from passive to active\n",
+					 __func__, channel);
+			}
+			break;
+		}
+		i++;
+	}
+}
+#endif
+
+/****************************************************************************
+
+Following are the functions to report events
+
+*****************************************************************************/
+
+void report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct survey_event	*psurvey_evt;
+	struct C2HEvent_Header *pc2h_evt_hdr;
+	struct mlme_ext_priv *pmlmeext;
+	struct cmd_priv *pcmdpriv;
+	/* u8 *pframe = precv_frame->u.hdr.rx_data; */
+	/* uint len = precv_frame->u.hdr.len; */
+
+	if (!padapter)
+		return;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pcmdpriv = &padapter->cmdpriv;
+
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct survey_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	psurvey_evt = (struct survey_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+
+	if (collect_bss_info(padapter, precv_frame, (WLAN_BSSID_EX *)&psurvey_evt->bss) == _FAIL) {
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		rtw_mfree((u8 *)pevtcmd, cmdsz);
+		return;
+	}
+
+#ifdef CONFIG_80211D
+	process_80211d(padapter, &psurvey_evt->bss);
+#endif
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	pmlmeext->sitesurvey_res.bss_cnt++;
+
+	return;
+
+}
+
+void report_surveydone_event(_adapter *padapter)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct surveydone_event *psurveydone_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct surveydone_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	psurveydone_evt = (struct surveydone_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
+
+	RTW_INFO("survey done event(%x) band:%d for "ADPT_FMT"\n", psurveydone_evt->bss_cnt, padapter->setband, ADPT_ARG(padapter));
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+
+}
+
+u32 report_join_res(_adapter *padapter, int res)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct joinbss_event		*pjoinbss_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u32 ret = _FAIL;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		goto exit;
+
+	cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		goto exit;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct joinbss_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	pjoinbss_evt = (struct joinbss_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
+	pjoinbss_evt->network.join_res	= pjoinbss_evt->network.aid = res;
+
+	RTW_INFO("report_join_res(%d)\n", res);
+
+
+	rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network);
+
+
+	ret = rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+exit:
+	return ret;
+}
+
+void report_wmm_edca_update(_adapter *padapter)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct wmm_event		*pwmm_event;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct wmm_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct wmm_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_WMM);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	pwmm_event = (struct wmm_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	pwmm_event->wmm = 0;
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+
+}
+
+u32 report_del_sta_event(_adapter *padapter, unsigned char *MacAddr, unsigned short reason, bool enqueue, u8 locally_generated)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct sta_info *psta;
+	int	mac_id = -1;
+	struct stadel_event			*pdel_sta_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	/* prepare cmd parameter */
+	cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct stadel_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	pdel_sta_evt = (struct stadel_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+	memcpy((unsigned char *)(pdel_sta_evt->rsvd), (unsigned char *)(&reason), 2);
+	psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
+	if (psta)
+		mac_id = (int)psta->mac_id;
+	else
+		mac_id = (-1);
+	pdel_sta_evt->mac_id = mac_id;
+	pdel_sta_evt->locally_generated = locally_generated;
+
+	if (!enqueue) {
+		/* do directly */
+		rtw_stadel_event_callback(padapter, (u8 *)pdel_sta_evt);
+		rtw_mfree(pevtcmd, cmdsz);
+	} else {
+		pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (pcmd_obj == NULL) {
+			rtw_mfree(pevtcmd, cmdsz);
+			res = _FAIL;
+			goto exit;
+		}
+
+		INIT_LIST_HEAD(&pcmd_obj->list);
+		pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+		pcmd_obj->cmdsz = cmdsz;
+		pcmd_obj->parmbuf = pevtcmd;
+
+		pcmd_obj->rsp = NULL;
+		pcmd_obj->rspsz  = 0;
+
+		res = rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+	}
+
+exit:
+
+	RTW_INFO(FUNC_ADPT_FMT" "MAC_FMT" mac_id=%d, enqueue:%d, res:%u\n"
+		, FUNC_ADPT_ARG(padapter), MAC_ARG(MacAddr), mac_id, enqueue, res);
+
+	return res;
+}
+
+void report_add_sta_event(_adapter *padapter, unsigned char *MacAddr)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct stassoc_event		*padd_sta_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct stassoc_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	padd_sta_evt = (struct stassoc_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+
+	RTW_INFO("report_add_sta_event: add STA\n");
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+}
+
+
+bool rtw_port_switch_chk(_adapter *adapter)
+{
+	bool switch_needed = false;
+#ifdef CONFIG_CONCURRENT_MODE
+#ifdef CONFIG_RUNTIME_PORT_SWITCH
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct pwrctrl_priv *pwrctl = dvobj_to_pwrctl(dvobj);
+	_adapter *if_port0 = NULL;
+	_adapter *if_port1 = NULL;
+	struct mlme_ext_info *if_port0_mlmeinfo = NULL;
+	struct mlme_ext_info *if_port1_mlmeinfo = NULL;
+	int i;
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		if (get_hw_port(dvobj->padapters[i]) == HW_PORT0) {
+			if_port0 = dvobj->padapters[i];
+			if_port0_mlmeinfo = &(if_port0->mlmeextpriv.mlmext_info);
+		} else if (get_hw_port(dvobj->padapters[i]) == HW_PORT1) {
+			if_port1 = dvobj->padapters[i];
+			if_port1_mlmeinfo = &(if_port1->mlmeextpriv.mlmext_info);
+		}
+	}
+
+	if (if_port0 == NULL) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	if (if_port1 == NULL) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+#ifdef DBG_RUNTIME_PORT_SWITCH
+	RTW_INFO(FUNC_ADPT_FMT" wowlan_mode:%u\n"
+		 ADPT_FMT", port0, mlmeinfo->state:0x%08x, p2p_state:%d, %d\n"
+		 ADPT_FMT", port1, mlmeinfo->state:0x%08x, p2p_state:%d, %d\n",
+		 FUNC_ADPT_ARG(adapter), pwrctl->wowlan_mode,
+		ADPT_ARG(if_port0), if_port0_mlmeinfo->state, rtw_p2p_state(&if_port0->wdinfo), rtw_p2p_chk_state(&if_port0->wdinfo, P2P_STATE_NONE),
+		ADPT_ARG(if_port1), if_port1_mlmeinfo->state, rtw_p2p_state(&if_port1->wdinfo), rtw_p2p_chk_state(&if_port1->wdinfo, P2P_STATE_NONE));
+#endif /* DBG_RUNTIME_PORT_SWITCH */
+
+#ifdef CONFIG_WOWLAN
+	/* WOWLAN interface(primary, for now) should be port0 */
+	if (pwrctl->wowlan_mode) {
+		if (!is_primary_adapter(if_port0)) {
+			RTW_INFO("%s "ADPT_FMT" enable WOWLAN\n", __func__, ADPT_ARG(if_port1));
+			switch_needed = true;
+		}
+		goto exit;
+	}
+#endif /* CONFIG_WOWLAN */
+
+	/* AP should use port0 for ctl frame's ack */
+	if ((if_port1_mlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
+		RTW_INFO("%s "ADPT_FMT" is AP/GO\n", __func__, ADPT_ARG(if_port1));
+		switch_needed = true;
+		goto exit;
+	}
+
+	/* GC should use port0 for p2p ps */
+	if (((if_port1_mlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE)
+	    && (if_port1_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+#ifdef CONFIG_P2P
+	    && !rtw_p2p_chk_state(&if_port1->wdinfo, P2P_STATE_NONE)
+#endif
+	    && !check_fwstate(&if_port1->mlmepriv, WIFI_UNDER_WPS)
+	   ) {
+		RTW_INFO("%s "ADPT_FMT" is GC\n", __func__, ADPT_ARG(if_port1));
+		switch_needed = true;
+		goto exit;
+	}
+
+	/* port1 linked, but port0 not linked */
+	if ((if_port1_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+	    && !(if_port0_mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+	    && ((if_port0_mlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
+	   ) {
+		RTW_INFO("%s "ADPT_FMT" is SINGLE_LINK\n", __func__, ADPT_ARG(if_port1));
+		switch_needed = true;
+		goto exit;
+	}
+
+exit:
+#ifdef DBG_RUNTIME_PORT_SWITCH
+	RTW_INFO(FUNC_ADPT_FMT" ret:%d\n", FUNC_ADPT_ARG(adapter), switch_needed);
+#endif /* DBG_RUNTIME_PORT_SWITCH */
+#endif /* CONFIG_RUNTIME_PORT_SWITCH */
+#endif /* CONFIG_CONCURRENT_MODE */
+	return switch_needed;
+}
+
+/****************************************************************************
+
+Following are the event callback functions
+
+*****************************************************************************/
+
+/* for sta/adhoc mode */
+void update_sta_info(_adapter *padapter, struct sta_info *psta)
+{
+	unsigned long	irqL;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* ERP */
+	VCS_update(padapter, psta);
+
+	/* HT */
+	if (pmlmepriv->htpriv.ht_option) {
+		psta->htpriv.ht_option = true;
+
+		psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
+
+		psta->htpriv.rx_ampdu_min_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & IEEE80211_HT_CAP_AMPDU_DENSITY) >> 2;
+
+		if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_20))
+			psta->htpriv.sgi_20m = true;
+
+		if (support_short_GI(padapter, &(pmlmeinfo->HT_caps), CHANNEL_WIDTH_40))
+			psta->htpriv.sgi_40m = true;
+
+		psta->qos_option = true;
+
+		psta->htpriv.ldpc_cap = pmlmepriv->htpriv.ldpc_cap;
+		psta->htpriv.stbc_cap = pmlmepriv->htpriv.stbc_cap;
+		psta->htpriv.beamform_cap = pmlmepriv->htpriv.beamform_cap;
+
+		memcpy(&psta->htpriv.ht_cap, &pmlmeinfo->HT_caps, sizeof(struct rtw_ieee80211_ht_cap));
+	} else
+	{
+		psta->htpriv.ht_option = false;
+		psta->htpriv.ampdu_enable = false;
+		psta->htpriv.tx_amsdu_enable = false;
+		psta->htpriv.sgi_20m = false;
+		psta->htpriv.sgi_40m = false;
+		psta->qos_option = false;
+
+	}
+
+	psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
+
+	psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+	psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+
+	psta->bw_mode = pmlmeext->cur_bwmode;
+
+	/* QoS */
+	if (pmlmepriv->qospriv.qos_option)
+		psta->qos_option = true;
+
+	update_ldpc_stbc_cap(psta);
+
+	_enter_critical_ex(&psta->lock, &irqL);
+	psta->state = _FW_LINKED;
+	_exit_critical_ex(&psta->lock, &irqL);
+
+}
+
+static void rtw_mlmeext_disconnect(_adapter *padapter)
+{
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network));
+	u8 state_backup = (pmlmeinfo->state & 0x03);
+	u8 ASIX_ID[] = {0x00, 0x0E, 0xC6};
+
+	/* set_opmode_cmd(padapter, infra_client_with_mlme); */
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
+
+	/* set MSR to no link state->infra. mode */
+	Set_MSR(padapter, _HW_STATE_STATION_);
+
+	/* check if sta is ASIX peer and fix IOT issue if it is. */
+	if (!memcmp(get_my_bssid(&pmlmeinfo->network) , ASIX_ID , 3)) {
+		u8 iot_flag = false;
+		rtw_hal_set_hwreg(padapter, HW_VAR_ASIX_IOT, (u8 *)(&iot_flag));
+	}
+	pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+#ifdef CONFIG_MCC_MODE
+	/* mcc disconnect setting before download LPS rsvd page */
+	rtw_hal_set_mcc_setting_disconnect(padapter);
+#endif /* CONFIG_MCC_MODE */
+
+	if (state_backup == WIFI_FW_STATION_STATE) {
+		if (rtw_port_switch_chk(padapter)) {
+			rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
+#ifdef CONFIG_LPS
+			{
+				_adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter));
+				if (port0_iface)
+					rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0);
+			}
+#endif
+		}
+	}
+
+	/* switch to the 20M Hz mode after disconnect */
+	pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
+	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+#ifdef CONFIG_FCS_MODE
+	if (EN_FCS(padapter))
+		rtw_hal_set_hwreg(padapter, HW_VAR_STOP_FCS_MODE, NULL);
+#endif
+
+#ifdef CONFIG_DFS_MASTER
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		rtw_dfs_master_status_apply(padapter, MLME_AP_STOPPED);
+	else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		rtw_dfs_master_status_apply(padapter, MLME_STA_DISCONNECTED);
+#endif
+
+	{
+		u8 ch, bw, offset;
+
+		if (rtw_mi_get_ch_setting_union_no_self(padapter, &ch, &bw, &offset) != 0) {
+			set_channel_bwmode(padapter, ch, offset, bw);
+			rtw_mi_update_union_chan_inf(padapter, ch, offset, bw);
+		}
+	}
+
+	flush_all_cam_entry(padapter);
+
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	/* pmlmepriv->LinkDetectInfo.TrafficBusyState = false; */
+	pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
+	pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
+
+#ifdef CONFIG_TDLS
+	padapter->tdlsinfo.ap_prohibited = false;
+
+	/* For TDLS channel switch, currently we only allow it to work in wifi logo test mode */
+	if (padapter->registrypriv.wifi_spec == 1)
+		padapter->tdlsinfo.ch_switch_prohibited = false;
+#endif /* CONFIG_TDLS */
+
+}
+
+void mlmeext_joinbss_event_callback(_adapter *padapter, int join_res)
+{
+	struct sta_info		*psta, *psta_bmc;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*cur_network = &(pmlmeinfo->network);
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	u8	join_type;
+#ifdef CONFIG_ARP_KEEP_ALIVE
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+#endif
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	if (join_res < 0) {
+		join_type = 1;
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+		rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
+
+		goto exit_mlmeext_joinbss_event_callback;
+	}
+#ifdef CONFIG_ARP_KEEP_ALIVE
+	pmlmepriv->bGetGateway = 1;
+	pmlmepriv->GetGatewayTryCnt = 0;
+#endif
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
+		/* update bc/mc sta_info */
+		update_bmc_sta(padapter);
+	}
+
+
+	/* turn on dynamic functions */
+	/* Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); */
+
+	/* update IOT-releated issue */
+	update_IOT_info(padapter);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates);
+
+	/* BCN interval */
+	rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
+
+	/* udpate capability */
+	update_capinfo(padapter, pmlmeinfo->capability);
+
+	/* WMM, Update EDCA param */
+	WMMOnAssocRsp(padapter);
+
+	/* HT */
+	HTOnAssocRsp(padapter);
+
+	psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
+	if (psta) { /* only for infra. mode */
+		/* RTW_INFO("set_sta_rate\n"); */
+
+		psta->wireless_mode = pmlmeext->cur_wireless_mode;
+
+#ifdef CONFIG_FW_MULTI_PORT_SUPPORT
+		rtw_hal_set_default_port_id_cmd(padapter, psta->mac_id);
+#endif
+
+		/* set per sta rate after updating HT cap. */
+		set_sta_rate(padapter, psta);
+
+		rtw_sta_media_status_rpt(padapter, psta, 1);
+
+		/* wakeup macid after join bss successfully to ensure
+			the subsequent data frames can be sent out normally */
+		rtw_hal_macid_wakeup(padapter, psta->mac_id);
+	}
+
+#ifndef CONFIG_IOCTL_CFG80211
+	if (is_wep_enc(psecuritypriv->dot11PrivacyAlgrthm))
+		rtw_sec_restore_wep_key(padapter);
+#endif /* CONFIG_IOCTL_CFG80211 */
+
+	if (rtw_port_switch_chk(padapter))
+		rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
+
+	join_type = 2;
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
+		/* correcting TSF */
+		correct_TSF(padapter, pmlmeext);
+
+		/* set_link_timer(pmlmeext, DISCONNECT_TO); */
+	}
+
+#ifdef CONFIG_LPS
+	#ifndef CONFIG_FW_MULTI_PORT_SUPPORT
+	if (get_hw_port(padapter) == HW_PORT0)
+	#endif
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
+#endif
+
+#ifdef CONFIG_BEAMFORMING
+	if (psta)
+		beamforming_wk_cmd(padapter, BEAMFORMING_CTRL_ENTER, (u8 *)psta, sizeof(struct sta_info), 0);
+#endif/*CONFIG_BEAMFORMING*/
+
+exit_mlmeext_joinbss_event_callback:
+
+	rtw_join_done_chk_ch(padapter, join_res);
+
+	RTW_INFO("=>%s - End to Connection without 4-way\n", __func__);
+}
+
+/* currently only adhoc mode will go here */
+void mlmeext_sta_add_event_callback(_adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8	join_type;
+
+	RTW_INFO("%s\n", __func__);
+
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) { /* adhoc master or sta_count>1 */
+			/* nothing to do */
+		} else { /* adhoc client */
+			/* update TSF Value */
+			/* update_TSF(pmlmeext, pframe, len);			 */
+
+			/* correcting TSF */
+			correct_TSF(padapter, pmlmeext);
+
+			/* start beacon */
+			if (send_beacon(padapter) == _FAIL)
+				rtw_warn_on(1);
+
+			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+		}
+
+		join_type = 2;
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	}
+
+	/* update adhoc sta_info */
+	update_sta_info(padapter, psta);
+
+	rtw_hal_update_sta_rate_mask(padapter, psta);
+
+	/* ToDo: HT for Ad-hoc */
+	psta->wireless_mode = rtw_check_network_type(psta->bssrateset, psta->bssratelen, pmlmeext->cur_channel);
+	psta->raid = rtw_hal_networktype_to_raid(padapter, psta);
+
+	/* rate radaptive */
+	Update_RA_Entry(padapter, psta);
+}
+
+void mlmeext_sta_del_event_callback(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter))
+		rtw_mlmeext_disconnect(padapter);
+
+}
+
+/****************************************************************************
+
+Following are the functions for the timer handlers
+
+*****************************************************************************/
+void _linked_info_dump(_adapter *padapter)
+{
+	int i;
+	struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
+	HAL_DATA_TYPE *HalData = GET_HAL_DATA(padapter);
+	int undecorated_smoothed_pwdb = 0;
+
+	if (padapter->bLinkInfoDump) {
+
+		RTW_INFO("\n============["ADPT_FMT"] linked status check ===================\n", ADPT_ARG(padapter));
+
+		if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
+			rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &undecorated_smoothed_pwdb);
+
+			RTW_INFO("AP[" MAC_FMT "] - undecorated_smoothed_pwdb:%d\n",
+				MAC_ARG(padapter->mlmepriv.cur_network.network.MacAddress), undecorated_smoothed_pwdb);
+		} else if ((pmlmeinfo->state & 0x03) == _HW_STATE_AP_) {
+			unsigned long irqL;
+			_list	*phead, *plist;
+
+			struct sta_info *psta = NULL;
+			struct sta_priv *pstapriv = &padapter->stapriv;
+
+			_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+			phead = &pstapriv->asoc_list;
+			plist = get_next(phead);
+			while ((rtw_end_of_queue_search(phead, plist)) == false) {
+				psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+				plist = get_next(plist);
+
+				RTW_INFO("STA[" MAC_FMT "]:undecorated_smoothed_pwdb:%d\n",
+					MAC_ARG(psta->hwaddr), psta->rssi_stat.undecorated_smoothed_pwdb);
+			}
+			_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+		}
+
+		/*============  tx info ============	*/
+		rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, RTW_DBGDUMP);
+
+		rtw_hal_set_odm_var(padapter, HAL_ODM_RX_INFO_DUMP, RTW_DBGDUMP, false);
+
+	}
+
+
+}
+/********************************************************************
+
+When station does not receive any packet in MAX_CONTINUAL_NORXPACKET_COUNT*2 seconds,
+recipient station will teardown the block ack by issuing DELBA frame.
+
+*********************************************************************/
+static void rtw_delba_check(_adapter *padapter, struct sta_info *psta, u8 from_timer)
+{
+	int	i = 0;
+	int ret = _SUCCESS;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/*
+		IOT issue,occur Broadcom ap(Buffalo WZR-D1800H,Netgear R6300).
+		AP is originator.AP does not transmit unicast packets when STA response its BAR.
+		This case probably occur ap issue BAR after AP builds BA.
+
+		Follow 802.11 spec, STA shall maintain an inactivity timer for every negotiated Block Ack setup.
+		The inactivity timer is not reset when MPDUs corresponding to other TIDs are received.
+	*/
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_BROADCOM) {
+		for (i = 0; i < TID_NUM ; i++) {
+			if ((psta->recvreorder_ctrl[i].enable) && 
+                        (sta_rx_data_qos_pkts(psta, i) == sta_last_rx_data_qos_pkts(psta, i)) ) {			
+					if (rtw_inc_and_chk_continual_no_rx_packet(psta, i)) {					
+						/* send a DELBA frame to the peer STA with the Reason Code field set to TIMEOUT */
+						if (!from_timer)
+							ret = issue_del_ba_ex(padapter, psta->hwaddr, i, 39, 0, 3, 1);
+						else
+							issue_del_ba(padapter,  psta->hwaddr, i, 39, 0);
+						psta->recvreorder_ctrl[i].enable = false;
+						if (ret != _FAIL)
+							psta->recvreorder_ctrl[i].ampdu_size = RX_AMPDU_SIZE_INVALID;
+						rtw_reset_continual_no_rx_packet(psta, i);
+					}				
+			} else {
+				/* The inactivity timer is reset when MPDUs to the TID is received. */
+				rtw_reset_continual_no_rx_packet(psta, i);
+			}
+		}
+	}
+}
+
+static u8 chk_ap_is_alive(_adapter *padapter, struct sta_info *psta)
+{
+	u8 ret = false;
+	int i = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+#ifdef DBG_EXPIRATION_CHK
+	RTW_INFO(FUNC_ADPT_FMT" rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu"
+		/*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/
+		 ", retry:%u\n"
+		 , FUNC_ADPT_ARG(padapter)
+		 , STA_RX_PKTS_DIFF_ARG(psta)
+		, psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts
+		, psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts
+		/*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts
+		, psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts
+		, psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts
+		 , pmlmeinfo->bcn_interval*/
+		 , pmlmeext->retry
+		);
+
+	RTW_INFO(FUNC_ADPT_FMT" tx_pkts:%llu, link_count:%u\n", FUNC_ADPT_ARG(padapter)
+		 , padapter->xmitpriv.tx_pkts
+		 , pmlmeinfo->link_count
+		);
+#endif
+
+	if ((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta))
+	    && sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta)
+	    && sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta)
+	   )
+		ret = false;
+	else
+		ret = true;
+
+	sta_update_last_rx_pkts(psta);
+
+	/*
+		record last rx data packets for every tid.
+	*/
+	for (i = 0; i < TID_NUM; i++)
+		psta->sta_stats.last_rx_data_qos_pkts[i] = psta->sta_stats.rx_data_qos_pkts[i];
+
+	return ret;
+}
+
+static u8 chk_adhoc_peer_is_alive(struct sta_info *psta)
+{
+	u8 ret = true;
+
+#ifdef DBG_EXPIRATION_CHK
+	RTW_INFO("sta:"MAC_FMT", rssi:%d, rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu"
+		/*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/
+		 ", expire_to:%u\n"
+		 , MAC_ARG(psta->hwaddr)
+		 , psta->rssi_stat.undecorated_smoothed_pwdb
+		 , STA_RX_PKTS_DIFF_ARG(psta)
+		, psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts
+		, psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts
+		/*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts
+		, psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts
+		, psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts
+		 , pmlmeinfo->bcn_interval*/
+		 , psta->expire_to
+		);
+#endif
+
+	if (sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta)
+	    && sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta)
+	    && sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta))
+		ret = false;
+
+	sta_update_last_rx_pkts(psta);
+
+	return ret;
+}
+
+#ifdef CONFIG_TDLS
+u8 chk_tdls_peer_sta_is_alive(_adapter *padapter, struct sta_info *psta)
+{
+	if ((psta->sta_stats.rx_data_pkts == psta->sta_stats.last_rx_data_pkts)
+	    && (psta->sta_stats.rx_tdls_disc_rsp_pkts == psta->sta_stats.last_rx_tdls_disc_rsp_pkts))
+		return false;
+
+	return true;
+}
+
+void linked_status_chk_tdls(_adapter *padapter)
+{
+	struct candidate_pool {
+		struct sta_info *psta;
+		u8 addr[ETH_ALEN];
+	};
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	unsigned long irqL;
+	u8 ack_chk;
+	struct sta_info *psta;
+	int i, num_teardown = 0, num_checkalive = 0;
+	_list	*plist, *phead;
+	struct tdls_txmgmt txmgmt;
+	struct candidate_pool checkalive[MAX_ALLOWED_TDLS_STA_NUM];
+	struct candidate_pool teardown[MAX_ALLOWED_TDLS_STA_NUM];
+	u8 tdls_sta_max = false;
+
+#define ALIVE_MIN 2
+#define ALIVE_MAX 5
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	memset(checkalive, 0x00, sizeof(checkalive));
+	memset(teardown, 0x00, sizeof(teardown));
+
+	if ((padapter->tdlsinfo.link_established)) {
+		_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+		for (i = 0; i < NUM_STA; i++) {
+			phead = &(pstapriv->sta_hash[i]);
+			plist = get_next(phead);
+
+			while ((rtw_end_of_queue_search(phead, plist)) == false) {
+				psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+				plist = get_next(plist);
+
+				if (psta->tdls_sta_state & TDLS_LINKED_STATE) {
+					psta->alive_count++;
+					if (psta->alive_count >= ALIVE_MIN) {
+						if (chk_tdls_peer_sta_is_alive(padapter, psta) == false) {
+							if (psta->alive_count < ALIVE_MAX) {
+								memcpy(checkalive[num_checkalive].addr, psta->hwaddr, ETH_ALEN);
+								checkalive[num_checkalive].psta = psta;
+								num_checkalive++;
+							} else {
+								memcpy(teardown[num_teardown].addr, psta->hwaddr, ETH_ALEN);
+								teardown[num_teardown].psta = psta;
+								num_teardown++;
+							}
+						} else
+							psta->alive_count = 0;
+					}
+					psta->sta_stats.last_rx_data_pkts = psta->sta_stats.rx_data_pkts;
+					psta->sta_stats.last_rx_tdls_disc_rsp_pkts = psta->sta_stats.rx_tdls_disc_rsp_pkts;
+
+					if ((num_checkalive >= MAX_ALLOWED_TDLS_STA_NUM) || (num_teardown >= MAX_ALLOWED_TDLS_STA_NUM)) {
+						tdls_sta_max = true;
+						break;
+					}
+				}
+			}
+
+			if (tdls_sta_max)
+				break;
+		}
+		_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+		if (num_checkalive > 0) {
+			for (i = 0; i < num_checkalive; i++) {
+				memcpy(txmgmt.peer, checkalive[i].addr, ETH_ALEN);
+				issue_tdls_dis_req(padapter, &txmgmt);
+				issue_tdls_dis_req(padapter, &txmgmt);
+				issue_tdls_dis_req(padapter, &txmgmt);
+			}
+		}
+
+		if (num_teardown > 0) {
+			for (i = 0; i < num_teardown; i++) {
+				RTW_INFO("[%s %d] Send teardown to "MAC_FMT"\n", __func__, __LINE__, MAC_ARG(teardown[i].addr));
+				txmgmt.status_code = _RSON_TDLS_TEAR_TOOFAR_;
+				memcpy(txmgmt.peer, teardown[i].addr, ETH_ALEN);
+				issue_tdls_teardown(padapter, &txmgmt, false);
+			}
+		}
+	}
+
+}
+#endif /* CONFIG_TDLS */
+
+/* from_timer == 1 means driver is in LPS */
+void linked_status_chk(_adapter *padapter, u8 from_timer)
+{
+	u32	i;
+	struct sta_info		*psta;
+	struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+#if defined(CONFIG_ARP_KEEP_ALIVE) || defined(CONFIG_LAYER2_ROAMING)
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+#endif
+#ifdef CONFIG_LAYER2_ROAMING
+	struct recv_priv	*precvpriv = &padapter->recvpriv;
+#endif
+
+	if (padapter->registrypriv.mp_mode)
+		return;
+
+	if (is_client_associated_to_ap(padapter)) {
+		/* linked infrastructure client mode */
+
+		int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
+		int rx_chk_limit;
+		int link_count_limit;
+
+#ifdef CONFIG_LAYER2_ROAMING
+		if (rtw_chk_roam_flags(padapter, RTW_ROAM_ACTIVE)) {
+			RTW_INFO("signal_strength_data.avg_val = %d\n", precvpriv->signal_strength_data.avg_val);
+			if (precvpriv->signal_strength_data.avg_val < pmlmepriv->roam_rssi_threshold) {
+				pmlmepriv->need_to_roam = true;
+				rtw_drv_scan_by_self(padapter, RTW_AUTO_SCAN_REASON_ROAM);
+			} else {
+				pmlmepriv->need_to_roam = false;
+			}
+		}
+#endif
+#ifdef CONFIG_MCC_MODE
+		/*
+		 * due to tx ps null date to ao, so ap doest not tx pkt to driver
+		 * we may check chk_ap_is_alive fail, and may issue_probereq to wrong channel under sitesurvey
+		 * don't keep alive check under MCC
+		 */
+		if (rtw_hal_mcc_link_status_chk(padapter, __func__) == false)
+			return;
+#endif
+
+#if defined(DBG_ROAMING_TEST)
+		rx_chk_limit = 1;
+#elif defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && !defined(CONFIG_LPS_LCLK_WD_TIMER)
+		rx_chk_limit = 4;
+#else
+		rx_chk_limit = 8;
+#endif
+#ifdef CONFIG_ARP_KEEP_ALIVE
+		if (!from_timer && pmlmepriv->bGetGateway == 1 && pmlmepriv->GetGatewayTryCnt < 3) {
+			RTW_INFO("do rtw_gw_addr_query() : %d\n", pmlmepriv->GetGatewayTryCnt);
+			pmlmepriv->GetGatewayTryCnt++;
+			if (rtw_gw_addr_query(padapter) == 0)
+				pmlmepriv->bGetGateway = 0;
+			else {
+				memset(pmlmepriv->gw_ip, 0, 4);
+				memset(pmlmepriv->gw_mac_addr, 0, 6);
+			}
+		}
+#endif
+#ifdef CONFIG_P2P
+		if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE)) {
+			if (!from_timer)
+				link_count_limit = 3; /* 8 sec */
+			else
+				link_count_limit = 15; /* 32 sec */
+		} else
+#endif /* CONFIG_P2P */
+		{
+			if (!from_timer)
+				link_count_limit = 7; /* 16 sec */
+			else
+				link_count_limit = 29; /* 60 sec */
+		}
+
+#ifdef CONFIG_TDLS
+#ifdef CONFIG_TDLS_CH_SW
+		if (ATOMIC_READ(&padapter->tdlsinfo.chsw_info.chsw_on))
+			return;
+#endif /* CONFIG_TDLS_CH_SW */
+
+#ifdef CONFIG_TDLS_AUTOCHECKALIVE
+		linked_status_chk_tdls(padapter);
+#endif /* CONFIG_TDLS_AUTOCHECKALIVE */
+#endif /* CONFIG_TDLS */
+
+		psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
+		if (psta != NULL) {
+			bool is_p2p_enable = false;
+#ifdef CONFIG_P2P
+			is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
+#endif
+
+#ifdef CONFIG_ISSUE_DELBA_WHEN_NO_TRAFFIC 
+			/*issue delba when ap does not tx data packet that is Broadcom ap */
+			rtw_delba_check(padapter, psta, from_timer);
+#endif
+			if (chk_ap_is_alive(padapter, psta) == false)
+				rx_chk = _FAIL;
+
+			if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
+				tx_chk = _FAIL;
+
+#if defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK) && !defined(CONFIG_LPS_LCLK_WD_TIMER)
+			if (pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL)
+				#ifdef CONFIG_MCC_MODE
+				/* Driver don't know operation channel under MCC*/
+				/* So driver don't  do KEEP_ALIVE_CHECK */
+				&& (!rtw_hal_check_mcc_status(padapter, MCC_STATUS_NEED_MCC))
+				#endif
+			) {
+				u8 backup_ch = 0, backup_bw, backup_offset;
+				u8 union_ch = 0, union_bw, union_offset;
+
+				if (!rtw_mi_get_ch_setting_union(padapter, &union_ch, &union_bw, &union_offset)
+					|| pmlmeext->cur_channel != union_ch)
+						goto bypass_active_keep_alive;
+
+				/* switch to correct channel of current network  before issue keep-alive frames */
+				if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
+					backup_ch = rtw_get_oper_ch(padapter);
+					backup_bw = rtw_get_oper_bw(padapter);
+					backup_offset = rtw_get_oper_choffset(padapter);
+					set_channel_bwmode(padapter, union_ch, union_offset, union_bw);
+				}
+
+				if (rx_chk != _SUCCESS)
+					issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 0, 0, 3, 1);
+
+				if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit) || rx_chk != _SUCCESS) {
+					tx_chk = issue_nulldata(padapter, psta->hwaddr, 0, 3, 1);
+					/* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */
+					if (tx_chk == _SUCCESS && !is_p2p_enable)
+						rx_chk = _SUCCESS;
+				}
+
+				/* back to the original operation channel */
+				if (backup_ch > 0)
+					set_channel_bwmode(padapter, backup_ch, backup_offset, backup_bw);
+
+bypass_active_keep_alive:
+				;
+			} else
+#endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */
+			{
+				if (rx_chk != _SUCCESS) {
+					if (pmlmeext->retry == 0) {
+#ifdef DBG_EXPIRATION_CHK
+						RTW_INFO("issue_probereq to trigger probersp, retry=%d\n", pmlmeext->retry);
+#endif
+						issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
+						issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
+						issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
+					}
+				}
+
+				if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit
+#ifdef CONFIG_MCC_MODE
+				    /* FW tx nulldata under MCC mode, we just check  ap is alive */
+				    && (!rtw_hal_check_mcc_status(padapter, MCC_STATUS_NEED_MCC))
+#endif /* CONFIG_MCC_MODE */
+				   ) {
+#ifdef DBG_EXPIRATION_CHK
+					RTW_INFO("%s issue_nulldata(%d)\n", __func__, from_timer ? 1 : 0);
+#endif
+					tx_chk = issue_nulldata_in_interrupt(padapter, NULL, from_timer ? 1 : 0);
+				}
+			}
+
+			if (rx_chk == _FAIL) {
+				pmlmeext->retry++;
+				if (pmlmeext->retry > rx_chk_limit) {
+					RTW_PRINT(FUNC_ADPT_FMT" disconnect or roaming\n",
+						  FUNC_ADPT_ARG(padapter));
+					receive_disconnect(padapter, pmlmeinfo->network.MacAddress
+						, WLAN_REASON_EXPIRATION_CHK, false);
+					return;
+				}
+			} else
+				pmlmeext->retry = 0;
+
+			if (tx_chk == _FAIL)
+				pmlmeinfo->link_count %= (link_count_limit + 1);
+			else {
+				pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
+				pmlmeinfo->link_count = 0;
+			}
+
+		} /* end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL) */
+
+	} else if (is_client_associated_to_ibss(padapter)) {
+		unsigned long irqL;
+		_list *phead, *plist, dlist;
+
+		INIT_LIST_HEAD(&dlist);
+
+		_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+		for (i = 0; i < NUM_STA; i++) {
+
+			phead = &(pstapriv->sta_hash[i]);
+			plist = get_next(phead);
+			while ((rtw_end_of_queue_search(phead, plist)) == false) {
+				psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+				plist = get_next(plist);
+
+				if (is_broadcast_mac_addr(psta->hwaddr))
+					continue;
+
+				if (chk_adhoc_peer_is_alive(psta) || !psta->expire_to)
+					psta->expire_to = pstapriv->adhoc_expire_to;
+				else
+					psta->expire_to--;
+
+				if (psta->expire_to <= 0) {
+					list_del_init(&psta->list);
+					list_add_tail(&psta->list, &dlist);
+				}
+			}
+		}
+
+		_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+		plist = get_next(&dlist);
+		while (rtw_end_of_queue_search(&dlist, plist) == false) {
+			psta = LIST_CONTAINOR(plist, struct sta_info, list);
+			plist = get_next(plist);
+			list_del_init(&psta->list);
+			RTW_INFO(FUNC_ADPT_FMT" ibss expire "MAC_FMT"\n"
+				, FUNC_ADPT_ARG(padapter), MAC_ARG(psta->hwaddr));
+			report_del_sta_event(padapter, psta->hwaddr, WLAN_REASON_EXPIRATION_CHK, from_timer ? true : false, false);
+		}
+	}
+
+}
+
+void survey_timer_hdl(_adapter *padapter)
+{
+	struct cmd_obj *cmd;
+	struct sitesurvey_parm *psurveyPara;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif
+
+	if (mlmeext_scan_state(pmlmeext) > SCAN_DISABLE) {
+		cmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (cmd == NULL) {
+			rtw_warn_on(1);
+			goto exit;
+		}
+
+		psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm));
+		if (psurveyPara == NULL) {
+			rtw_warn_on(1);
+			rtw_mfree((unsigned char *)cmd, sizeof(struct cmd_obj));
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(cmd, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+		rtw_enqueue_cmd(pcmdpriv, cmd);
+	}
+
+exit:
+	return;
+}
+
+void link_timer_hdl(_adapter *padapter)
+{
+	/* static unsigned int		rx_pkt = 0; */
+	/* static u64				tx_cnt = 0; */
+	/* struct xmit_priv		*pxmitpriv = &(padapter->xmitpriv); */
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	/* struct sta_priv		*pstapriv = &padapter->stapriv; */
+#ifdef CONFIG_RTW_80211R
+	struct	sta_priv		*pstapriv = &padapter->stapriv;
+	struct	sta_info		*psta = NULL;
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network));
+#endif
+
+	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+		RTW_INFO("link_timer_hdl:no beacon while connecting\n");
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		report_join_res(padapter, -3);
+	} else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) {
+		/* re-auth timer */
+		if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) {
+			/* if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto) */
+			/* { */
+			pmlmeinfo->state = 0;
+			report_join_res(padapter, -1);
+			return;
+			/* } */
+			/* else */
+			/* { */
+			/*	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */
+			/*	pmlmeinfo->reauth_count = 0; */
+			/* } */
+		}
+
+		RTW_INFO("link_timer_hdl: auth timeout and try again\n");
+		pmlmeinfo->auth_seq = 1;
+		issue_auth(padapter, NULL, 0);
+		set_link_timer(pmlmeext, REAUTH_TO);
+	} else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) {
+		/* re-assoc timer */
+		if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) {
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+#ifdef CONFIG_RTW_80211R
+			if ((rtw_to_roam(padapter) > 0) && rtw_chk_ft_flags(padapter, RTW_FT_SUPPORTED)) {
+				psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
+				if (psta)
+					rtw_free_stainfo(padapter,  psta);
+			}
+#endif
+			report_join_res(padapter, -2);
+			return;
+		}
+
+#ifdef CONFIG_RTW_80211R
+		if ((rtw_to_roam(padapter) > 0) && rtw_chk_ft_flags(padapter, RTW_FT_SUPPORTED)) {
+			RTW_INFO("link_timer_hdl: reassoc timeout and try again\n");
+			issue_reassocreq(padapter);
+		} else
+#endif
+		{
+			RTW_INFO("link_timer_hdl: assoc timeout and try again\n");
+			issue_assocreq(padapter);
+		}
+
+		set_link_timer(pmlmeext, REASSOC_TO);
+	}
+
+	return;
+}
+
+void addba_timer_hdl(struct sta_info *psta)
+{
+	struct ht_priv	*phtpriv;
+
+	if (!psta)
+		return;
+
+	phtpriv = &psta->htpriv;
+
+	if ((phtpriv->ht_option) && (phtpriv->ampdu_enable == true)) {
+		if (phtpriv->candidate_tid_bitmap)
+			phtpriv->candidate_tid_bitmap = 0x0;
+
+	}
+}
+
+#ifdef CONFIG_IEEE80211W
+void report_sta_timeout_event(_adapter *padapter, u8 *MacAddr, unsigned short reason)
+{
+	struct cmd_obj *pcmd_obj;
+	u8	*pevtcmd;
+	u32 cmdsz;
+	struct sta_info *psta;
+	int	mac_id;
+	struct stadel_event			*pdel_sta_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct stadel_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_TimeoutSTA);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	pdel_sta_evt = (struct stadel_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+	memcpy((unsigned char *)(pdel_sta_evt->rsvd), (unsigned char *)(&reason), 2);
+
+
+	psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
+	if (psta)
+		mac_id = (int)psta->mac_id;
+	else
+		mac_id = (-1);
+
+	pdel_sta_evt->mac_id = mac_id;
+
+	RTW_INFO("report_del_sta_event: delete STA, mac_id=%d\n", mac_id);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+}
+
+void clnt_sa_query_timeout(_adapter *padapter)
+{
+
+	rtw_disassoc_cmd(padapter, 0, true);
+	rtw_indicate_disconnect(padapter, 0, false);
+	rtw_free_assoc_resources(padapter, 1);
+
+	RTW_INFO("SA query timeout client disconnect\n");
+}
+
+void sa_query_timer_hdl(struct sta_info *psta)
+{
+	_adapter *padapter = psta->padapter;
+	unsigned long irqL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) &&
+	    check_fwstate(pmlmepriv, _FW_LINKED))
+		clnt_sa_query_timeout(padapter);
+	else if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		report_sta_timeout_event(padapter, psta->hwaddr, WLAN_REASON_PREV_AUTH_NOT_VALID);
+}
+
+#endif /* CONFIG_IEEE80211W */
+
+#ifdef CONFIG_RTW_80211R
+void start_clnt_ft_action(_adapter *padapter, u8 *pTargetAddr)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	rtw_set_ft_status(padapter, RTW_FT_REQUESTING_STA);
+	issue_action_ft_request(padapter, pTargetAddr);
+	_set_timer(&pmlmeext->ft_link_timer, REASSOC_TO);
+}
+
+void ft_link_timer_hdl(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	ft_priv		*pftpriv = &pmlmepriv->ftpriv;
+
+	if (rtw_chk_ft_status(padapter, RTW_FT_REQUESTING_STA)) {
+		if (pftpriv->ft_req_retry_cnt < FT_ACTION_REQ_LIMIT) {
+			pftpriv->ft_req_retry_cnt++;
+			issue_action_ft_request(padapter, (u8 *)pmlmepriv->roam_network->network.MacAddress);
+			_set_timer(&pmlmeext->ft_link_timer, REASSOC_TO);
+		} else {
+			pftpriv->ft_req_retry_cnt = 0;
+
+			if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+				rtw_set_ft_status(padapter, RTW_FT_ASSOCIATED_STA);
+			else
+				rtw_reset_ft_status(padapter);
+		}
+	}
+}
+
+void ft_roam_timer_hdl(_adapter *padapter)
+{
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+
+	receive_disconnect(padapter, pmlmepriv->cur_network.network.MacAddress
+							, WLAN_REASON_ACTIVE_ROAM, false);
+}
+
+void issue_action_ft_request(_adapter *padapter, u8 *pTargetAddr)
+{
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct xmit_frame *pmgntframe = NULL;
+	struct rtw_ieee80211_hdr *pwlanhdr = NULL;
+	struct pkt_attrib *pattrib = NULL;
+	ft_priv *pftpriv = NULL;
+	u8 *pframe = NULL;
+	u8 category = RTW_WLAN_CATEGORY_FT;
+	u8 action = RTW_WLAN_ACTION_FT_REQUEST;
+	u8 is_ft_roaming_with_rsn_ie = true;
+	u8 *pie = NULL;
+	__le16 *fctrl;
+	u32 ft_ie_len = 0;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(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));
+
+	memcpy(pframe, adapter_mac_addr(padapter), ETH_ALEN);
+	pframe += ETH_ALEN;
+	pattrib->pktlen += ETH_ALEN;
+
+	memcpy(pframe, pTargetAddr, ETH_ALEN);
+	pframe += ETH_ALEN;
+	pattrib->pktlen += ETH_ALEN;
+
+	pftpriv = &pmlmepriv->ftpriv;
+	pie = rtw_get_ie(pftpriv->updated_ft_ies, EID_WPA2, &ft_ie_len, pftpriv->updated_ft_ies_len);
+	if (pie)
+		pframe = rtw_set_ie(pframe, EID_WPA2, ft_ie_len, pie+2, &(pattrib->pktlen));
+	else
+		is_ft_roaming_with_rsn_ie = false;
+
+	pie = rtw_get_ie(pftpriv->updated_ft_ies, _MDIE_, &ft_ie_len, pftpriv->updated_ft_ies_len);
+	if (pie)
+		pframe = rtw_set_ie(pframe, _MDIE_, ft_ie_len , pie+2, &(pattrib->pktlen));
+
+	pie = rtw_get_ie(pftpriv->updated_ft_ies, _FTIE_, &ft_ie_len, pftpriv->updated_ft_ies_len);
+	if (pie && is_ft_roaming_with_rsn_ie)
+		pframe = rtw_set_ie(pframe, _FTIE_, ft_ie_len , pie+2, &(pattrib->pktlen));
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+void report_ft_event(_adapter *padapter)
+{
+	struct mlme_priv		*pmlmepriv = &(padapter->mlmepriv);
+	ft_priv	*pftpriv = &pmlmepriv->ftpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network));
+	struct cfg80211_ft_event_params ft_evt_parms;
+	unsigned long irqL;
+
+	memset(&ft_evt_parms, 0, sizeof(ft_evt_parms));
+	rtw_update_ft_stainfo(padapter, pnetwork);
+
+	if (!pnetwork)
+		goto err_2;
+
+	ft_evt_parms.ies_len = pftpriv->ft_event.ies_len;
+	ft_evt_parms.ies =  rtw_zmalloc(ft_evt_parms.ies_len);
+	if (ft_evt_parms.ies)
+		memcpy((void *)ft_evt_parms.ies, pftpriv->ft_event.ies, ft_evt_parms.ies_len);
+	 else
+		goto err_2;
+
+	ft_evt_parms.target_ap = rtw_zmalloc(ETH_ALEN);
+	if (ft_evt_parms.target_ap)
+		memcpy((void *)ft_evt_parms.target_ap, pnetwork->MacAddress, ETH_ALEN);
+	else
+		goto err_1;
+
+	ft_evt_parms.ric_ies = pftpriv->ft_event.ric_ies;
+	ft_evt_parms.ric_ies_len = pftpriv->ft_event.ric_ies_len;
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+	rtw_set_ft_status(padapter, RTW_FT_AUTHENTICATED_STA);
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+	rtw_cfg80211_ft_event(padapter, &ft_evt_parms);
+	RTW_INFO("FT: report_ft_event\n");
+	rtw_mfree((u8 *)pftpriv->ft_event.target_ap, ETH_ALEN);
+err_1:
+	rtw_mfree((u8 *)ft_evt_parms.ies, ft_evt_parms.ies_len);
+err_2:
+	return;
+}
+
+void report_ft_reassoc_event(_adapter *padapter, u8 *pMacAddr)
+{
+	struct mlme_ext_priv		*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv			*pcmdpriv = &padapter->cmdpriv;
+	struct cmd_obj			*pcmd_obj = NULL;
+	struct stassoc_event		*passoc_sta_evt = NULL;
+	struct C2HEvent_Header	*pc2h_evt_hdr = NULL;
+	u8	*pevtcmd = NULL;
+	u32	cmdsz = 0;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
+		return;
+	}
+
+	INIT_LIST_HEAD(&pcmd_obj->list);
+	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
+	pcmd_obj->cmdsz = cmdsz;
+	pcmd_obj->parmbuf = pevtcmd;
+	pcmd_obj->rsp = NULL;
+	pcmd_obj->rspsz  = 0;
+
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
+	pc2h_evt_hdr->len = sizeof(struct stassoc_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_FT_REASSOC);
+	pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
+
+	passoc_sta_evt = (struct stassoc_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&(passoc_sta_evt->macaddr)), pMacAddr, ETH_ALEN);
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+}
+#endif
+
+u8 NULL_hdl(_adapter *padapter, u8 *pbuf)
+{
+	return H2C_SUCCESS;
+}
+
+#ifdef CONFIG_AUTO_AP_MODE
+void rtw_start_auto_ap(_adapter *adapter)
+{
+	RTW_INFO("%s\n", __func__);
+
+	rtw_set_802_11_infrastructure_mode(adapter, Ndis802_11APMode);
+
+	rtw_setopmode_cmd(adapter, Ndis802_11APMode, true);
+}
+
+static int rtw_auto_ap_start_beacon(_adapter *adapter)
+{
+	int ret = 0;
+	u8 *pbuf = NULL;
+	uint len;
+	u8	supportRate[16];
+	int	sz = 0, rateLen;
+	u8	*ie;
+	u8	wireless_mode, oper_channel;
+	u8 ssid[3] = {0}; /* hidden ssid */
+	u32 ssid_len = sizeof(ssid);
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+
+	len = 128;
+	pbuf = rtw_zmalloc(len);
+	if (!pbuf)
+		return -ENOMEM;
+
+
+	/* generate beacon */
+	ie = pbuf;
+
+	/* timestamp will be inserted by hardware */
+	sz += 8;
+	ie += sz;
+
+	/* beacon interval : 2bytes */
+	*(__le16 *)ie = cpu_to_le16((u16)100); /* BCN_INTERVAL=100; */
+	sz += 2;
+	ie += 2;
+
+	/* capability info */
+	*(u16 *)ie = 0;
+	*(u16 *)ie |= cpu_to_le16(cap_ESS);
+	*(u16 *)ie |= cpu_to_le16(cap_ShortPremble);
+	/* *(u16*)ie |= cpu_to_le16(cap_Privacy); */
+	sz += 2;
+	ie += 2;
+
+	/* SSID */
+	ie = rtw_set_ie(ie, _SSID_IE_, ssid_len, ssid, &sz);
+
+	/* supported rates */
+	wireless_mode = WIRELESS_11BG_24N;
+	rtw_set_supported_rate(supportRate, wireless_mode) ;
+	rateLen = rtw_get_rateset_len(supportRate);
+	if (rateLen > 8)
+		ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, supportRate, &sz);
+	else
+		ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, supportRate, &sz);
+
+
+	/* DS parameter set */
+	if (rtw_mi_check_status(adapter, MI_LINKED))
+		oper_channel = rtw_mi_get_union_chan(adapter);
+	else
+		oper_channel = adapter_to_dvobj(adapter)->oper_channel;
+
+	ie = rtw_set_ie(ie, _DSSET_IE_, 1, &oper_channel, &sz);
+
+	/* ext supported rates */
+	if (rateLen > 8)
+		ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (supportRate + 8), &sz);
+
+	RTW_INFO("%s, start auto ap beacon sz=%d\n", __func__, sz);
+
+	/* lunch ap mode & start to issue beacon */
+	if (rtw_check_beacon_data(adapter, pbuf,  sz) == _SUCCESS) {
+
+	} else
+		ret = -EINVAL;
+
+
+	rtw_mfree(pbuf, len);
+
+	return ret;
+
+}
+#endif/* CONFIG_AUTO_AP_MODE */
+
+u8 setopmode_hdl(_adapter *padapter, u8 *pbuf)
+{
+	u8	type;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
+
+	if (psetop->mode == Ndis802_11APMode) {
+		pmlmeinfo->state = WIFI_FW_AP_STATE;
+		type = _HW_STATE_AP_;
+#ifdef CONFIG_NATIVEAP_MLME
+		/* start_ap_mode(padapter); */
+#endif
+	} else if (psetop->mode == Ndis802_11Infrastructure) {
+		pmlmeinfo->state &= ~(BIT(0) | BIT(1)); /* clear state */
+		pmlmeinfo->state |= WIFI_FW_STATION_STATE;/* set to 	STATION_STATE */
+		type = _HW_STATE_STATION_;
+	} else if (psetop->mode == Ndis802_11IBSS)
+		type = _HW_STATE_ADHOC_;
+	else if (psetop->mode == Ndis802_11Monitor)
+		type = _HW_STATE_MONITOR_;
+	else
+		type = _HW_STATE_NOLINK_;
+
+#ifdef CONFIG_AP_PORT_SWAP
+	rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, (u8 *)(&type));
+#endif
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (psetop->mode == Ndis802_11APMode)
+		rtw_auto_ap_start_beacon(padapter);
+#endif
+
+	if (rtw_port_switch_chk(padapter)) {
+		rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
+
+		if (psetop->mode == Ndis802_11APMode)
+			adapter_to_pwrctl(padapter)->fw_psmode_iface_id = 0xff; /* ap mode won't dowload rsvd pages */
+		else if (psetop->mode == Ndis802_11Infrastructure) {
+#ifdef CONFIG_LPS
+			_adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter));
+			if (port0_iface)
+				rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0);
+#endif
+		}
+	}
+
+#ifdef CONFIG_BT_COEXIST
+	if (psetop->mode == Ndis802_11APMode ||
+		psetop->mode == Ndis802_11Monitor) {
+		/* Do this after port switch to */
+		/* prevent from downloading rsvd page to wrong port */
+		rtw_btcoex_MediaStatusNotify(padapter, 1); /* connect */
+	}
+#endif /* CONFIG_BT_COEXIST */
+
+	return H2C_SUCCESS;
+
+}
+
+u8 createbss_hdl(_adapter *padapter, u8 *pbuf)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX	*pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network));
+	WLAN_BSSID_EX	*pdev_network = &padapter->registrypriv.dev_network;
+	struct createbss_parm *parm = (struct createbss_parm *)pbuf;
+	u8 ret = H2C_SUCCESS;
+	/* u8	initialgain; */
+
+#ifdef CONFIG_AP_MODE
+	if (pmlmeinfo->state == WIFI_FW_AP_STATE) {
+		start_bss_network(padapter, parm);
+		goto exit;
+	}
+#endif
+
+	/* below is for ad-hoc master */
+	if (parm->adhoc) {
+		rtw_warn_on(pdev_network->InfrastructureMode != Ndis802_11IBSS);
+		rtw_joinbss_reset(padapter);
+
+		pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
+		pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+		pmlmeinfo->ERP_enable = 0;
+		pmlmeinfo->WMM_enable = 0;
+		pmlmeinfo->HT_enable = 0;
+		pmlmeinfo->HT_caps_enable = 0;
+		pmlmeinfo->HT_info_enable = 0;
+		pmlmeinfo->agg_enable_bitmap = 0;
+		pmlmeinfo->candidate_tid_bitmap = 0;
+
+		/* config the initial gain under linking, need to write the BB registers */
+		/* initialgain = 0x1E; */
+		/*rtw_hal_set_odm_var(padapter, HAL_ODM_INITIAL_GAIN, &initialgain, false);*/
+
+		/* disable dynamic functions, such as high power, DIG */
+		rtw_phydm_ability_backup(padapter);
+		rtw_phydm_func_disable_all(padapter);
+
+		/* cancel link timer */
+		_cancel_timer_ex(&pmlmeext->link_timer);
+
+		/* clear CAM */
+		flush_all_cam_entry(padapter);
+
+		pdev_network->Length = get_WLAN_BSSID_EX_sz(pdev_network);
+		memcpy(pnetwork, pdev_network, FIELD_OFFSET(WLAN_BSSID_EX, IELength));
+		pnetwork->IELength = pdev_network->IELength;
+
+		if (pnetwork->IELength > MAX_IE_SZ) {
+			ret = H2C_PARAMETERS_ERROR;
+			goto ibss_post_hdl;
+		}
+
+		memcpy(pnetwork->IEs, pdev_network->IEs, pnetwork->IELength);
+		start_create_ibss(padapter);
+	} else {
+		rtw_warn_on(1);
+		ret = H2C_PARAMETERS_ERROR;
+	}
+
+ibss_post_hdl:
+	rtw_create_ibss_post_hdl(padapter, ret);
+
+exit:
+	return ret;
+}
+
+u8 join_cmd_hdl(_adapter *padapter, u8 *pbuf)
+{
+	u8	join_type;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network));
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	struct joinbss_parm	*pparm = (struct joinbss_parm *)pbuf;
+#endif /* CONFIG_ANTENNA_DIVERSITY */
+	u32 i;
+	/* u8	initialgain; */
+	/* u32	acparm; */
+	u8 u_ch, u_bw, u_offset;
+	u8 doiqk = false;
+
+	/* check already connecting to AP or not */
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
+		if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
+			issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+		/* clear CAM */
+		flush_all_cam_entry(padapter);
+
+		_cancel_timer_ex(&pmlmeext->link_timer);
+
+		/* set MSR to nolink->infra. mode		 */
+		/* Set_MSR(padapter, _HW_STATE_NOLINK_); */
+		Set_MSR(padapter, _HW_STATE_STATION_);
+
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+	}
+
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	rtw_antenna_select_cmd(padapter, pparm->network.PhyInfo.Optimum_antenna, false);
+#endif
+
+#ifdef CONFIG_WAPI_SUPPORT
+	rtw_wapi_clear_all_cam_entry(padapter);
+#endif
+
+	rtw_joinbss_reset(padapter);
+
+	pmlmeinfo->ERP_enable = 0;
+	pmlmeinfo->WMM_enable = 0;
+	pmlmeinfo->HT_enable = 0;
+	pmlmeinfo->HT_caps_enable = 0;
+	pmlmeinfo->HT_info_enable = 0;
+	pmlmeinfo->agg_enable_bitmap = 0;
+	pmlmeinfo->candidate_tid_bitmap = 0;
+	pmlmeinfo->bwmode_updated = false;
+	/* pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX; */
+	pmlmeinfo->VHT_enable = 0;
+
+	memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength));
+	pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength;
+
+	if (pnetwork->IELength > MAX_IE_SZ) /* Check pbuf->IELength */
+		return H2C_PARAMETERS_ERROR;
+
+	if (pnetwork->IELength < 2) {
+		report_join_res(padapter, (-4));
+		return H2C_SUCCESS;
+	}
+	memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength);
+
+	pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
+
+	/* Check AP vendor to move rtw_joinbss_cmd() */
+	/* pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->IEs, pnetwork->IELength); */
+
+	/* sizeof(NDIS_802_11_FIXED_IEs)	 */
+	for (i = _FIXED_IE_LENGTH_ ; i < pnetwork->IELength - 2 ;) {
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pnetwork->IEs + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_: /* Get WMM IE. */
+			if (!memcmp(pIE->data, WMM_OUI, 4))
+				WMM_param_handler(padapter, pIE);
+			break;
+		case _HT_CAPABILITY_IE_:	/* Get HT Cap IE. */
+			pmlmeinfo->HT_caps_enable = 1;
+			break;
+
+		case _HT_EXTRA_INFO_IE_:	/* Get HT Info IE. */
+			pmlmeinfo->HT_info_enable = 1;
+			break;
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+	rtw_bss_get_chbw(pnetwork
+		, &pmlmeext->cur_channel, &pmlmeext->cur_bwmode, &pmlmeext->cur_ch_offset);
+
+	rtw_adjust_chbw(padapter, pmlmeext->cur_channel, &pmlmeext->cur_bwmode, &pmlmeext->cur_ch_offset);
+
+	/* check channel, bandwidth, offset and switch */
+	if (rtw_chk_start_clnt_join(padapter, &u_ch, &u_bw, &u_offset) == _FAIL) {
+		report_join_res(padapter, (-4));
+		return H2C_SUCCESS;
+	}
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
+	join_type = 0;
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	doiqk = true;
+	rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk);
+
+	set_channel_bwmode(padapter, u_ch, u_offset, u_bw);
+	rtw_mi_update_union_chan_inf(padapter, u_ch, u_offset, u_bw);
+
+	doiqk = false;
+	rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk);
+
+	/* cancel link timer */
+	_cancel_timer_ex(&pmlmeext->link_timer);
+
+	start_clnt_join(padapter);
+
+	return H2C_SUCCESS;
+}
+
+u8 disconnect_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network));
+	u8 val8;
+
+	if (is_client_associated_to_ap(padapter)) {
+#ifdef CONFIG_DFS
+		if (padapter->mlmepriv.handle_dfs == false)
+#endif /* CONFIG_DFS */
+#ifdef CONFIG_PLATFORM_ROCKCHIPS
+			/* To avoid connecting to AP fail during resume process, change retry count from 5 to 1 */
+			issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 1, 100);
+#else
+			issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms / 100, 100);
+#endif /* CONFIG_PLATFORM_ROCKCHIPS */
+	}
+
+#ifdef CONFIG_DFS
+	if (padapter->mlmepriv.handle_dfs)
+		padapter->mlmepriv.handle_dfs = false;
+#endif /* CONFIG_DFS */
+
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) {
+		/* Stop BCN */
+		val8 = 0;
+		rtw_hal_set_hwreg(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8));
+	}
+
+	rtw_mlmeext_disconnect(padapter);
+
+	rtw_free_uc_swdec_pending_queue(padapter);
+
+	return	H2C_SUCCESS;
+}
+
+static const char *const _scan_state_str[] = {
+	"SCAN_DISABLE",
+	"SCAN_START",
+	"SCAN_PS_ANNC_WAIT",
+	"SCAN_ENTER",
+	"SCAN_PROCESS",
+	"SCAN_BACKING_OP",
+	"SCAN_BACK_OP",
+	"SCAN_LEAVING_OP",
+	"SCAN_LEAVE_OP",
+	"SCAN_SW_ANTDIV_BL",
+	"SCAN_TO_P2P_LISTEN",
+	"SCAN_P2P_LISTEN",
+	"SCAN_COMPLETE",
+	"SCAN_STATE_MAX",
+};
+
+const char *scan_state_str(u8 state)
+{
+	state = (state >= SCAN_STATE_MAX) ? SCAN_STATE_MAX : state;
+	return _scan_state_str[state];
+}
+
+static bool scan_abort_hdl(_adapter *adapter)
+{
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct ss_res *ss = &pmlmeext->sitesurvey_res;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &adapter->wdinfo;
+#endif
+	bool ret = false;
+
+	if (pmlmeext->scan_abort) {
+#ifdef CONFIG_P2P
+		if (!rtw_p2p_chk_state(&adapter->wdinfo, P2P_STATE_NONE)) {
+			rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
+			ss->channel_idx = 3;
+			RTW_INFO("%s idx:%d, cnt:%u\n", __func__
+				 , ss->channel_idx
+				 , pwdinfo->find_phase_state_exchange_cnt
+				);
+		} else
+#endif
+		{
+			ss->channel_idx = ss->ch_num;
+			RTW_INFO("%s idx:%d\n", __func__
+				 , ss->channel_idx
+				);
+		}
+		pmlmeext->scan_abort = false;
+		ret = true;
+	}
+
+	return ret;
+}
+
+static u8 rtw_scan_sparse(_adapter *adapter, struct rtw_ieee80211_channel *ch, u8 ch_num)
+{
+	/* interval larger than this is treated as backgroud scan */
+#ifndef RTW_SCAN_SPARSE_BG_INTERVAL_MS
+#define RTW_SCAN_SPARSE_BG_INTERVAL_MS 12000
+#endif
+
+#ifndef RTW_SCAN_SPARSE_CH_NUM_MIRACAST
+#define RTW_SCAN_SPARSE_CH_NUM_MIRACAST 1
+#endif
+#ifndef RTW_SCAN_SPARSE_CH_NUM_BG
+#define RTW_SCAN_SPARSE_CH_NUM_BG 4
+#endif
+#ifdef CONFIG_LAYER2_ROAMING
+#ifndef RTW_SCAN_SPARSE_CH_NUM_ROAMING_ACTIVE
+#define RTW_SCAN_SPARSE_CH_NUM_ROAMING_ACTIVE 1
+#endif
+#endif
+
+#define SCAN_SPARSE_CH_NUM_INVALID 255
+
+	static u8 token = 255;
+	u32 interval;
+	bool busy_traffic = false;
+	bool miracast_enabled = false;
+	bool bg_scan = false;
+	u8 max_allow_ch = SCAN_SPARSE_CH_NUM_INVALID;
+	u8 scan_division_num;
+	u8 ret_num = ch_num;
+	struct registry_priv *regsty = dvobj_to_regsty(adapter_to_dvobj(adapter));
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+
+	if (regsty->wifi_spec)
+		goto exit;
+
+	/* assume ch_num > 6 is normal scan */
+	if (ch_num <= 6)
+		goto exit;
+
+	if (mlmeext->last_scan_time == 0)
+		mlmeext->last_scan_time = jiffies;
+
+	interval = rtw_get_passing_time_ms(mlmeext->last_scan_time);
+
+
+	if (rtw_mi_busy_traffic_check(adapter, false))
+		busy_traffic = true;
+
+	if (rtw_mi_check_miracast_enabled(adapter))
+		miracast_enabled = true;
+
+	if (interval > RTW_SCAN_SPARSE_BG_INTERVAL_MS)
+		bg_scan = true;
+
+	/* max_allow_ch by conditions*/
+
+#if RTW_SCAN_SPARSE_MIRACAST
+	if (miracast_enabled && busy_traffic == true)
+		max_allow_ch = rtw_min(max_allow_ch, RTW_SCAN_SPARSE_CH_NUM_MIRACAST);
+#endif
+
+#if RTW_SCAN_SPARSE_BG
+	if (bg_scan)
+		max_allow_ch = rtw_min(max_allow_ch, RTW_SCAN_SPARSE_CH_NUM_BG);
+#endif
+
+#if  defined(CONFIG_LAYER2_ROAMING) && defined(RTW_SCAN_SPARSE_ROAMING_ACTIVE)
+	if (rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE)) {
+		if (busy_traffic && adapter->mlmepriv.need_to_roam == true)
+			max_allow_ch = rtw_min(max_allow_ch, RTW_SCAN_SPARSE_CH_NUM_ROAMING_ACTIVE);
+	}
+#endif
+
+
+	if (max_allow_ch != SCAN_SPARSE_CH_NUM_INVALID) {
+		int i;
+		int k = 0;
+
+		scan_division_num = (ch_num / max_allow_ch) + ((ch_num % max_allow_ch) ? 1 : 0);
+		token = (token + 1) % scan_division_num;
+
+		if (0)
+			RTW_INFO("scan_division_num:%u, token:%u\n", scan_division_num, token);
+
+		for (i = 0; i < ch_num; i++) {
+			if (ch[i].hw_value && (i % scan_division_num) == token
+			   ) {
+				if (i != k)
+					memcpy(&ch[k], &ch[i], sizeof(struct rtw_ieee80211_channel));
+				k++;
+			}
+		}
+
+		memset(&ch[k], 0, sizeof(struct rtw_ieee80211_channel));
+
+		ret_num = k;
+		mlmeext->last_scan_time = jiffies;
+	}
+
+exit:
+	return ret_num;
+}
+
+static int rtw_scan_ch_decision(_adapter *padapter, struct rtw_ieee80211_channel *out,
+		u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
+{
+	int i, j;
+	int scan_ch_num = 0;
+	int set_idx;
+	u8 chan;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	/* clear first */
+	memset(out, 0, sizeof(struct rtw_ieee80211_channel) * out_num);
+
+	/* acquire channels from in */
+	j = 0;
+	for (i = 0; i < in_num; i++) {
+
+		if (0)
+			RTW_INFO(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[i]));
+
+		if (!in[i].hw_value || (in[i].flags & RTW_IEEE80211_CHAN_DISABLED))
+			continue;
+		if (rtw_mlme_band_check(padapter, in[i].hw_value) == false)
+			continue;
+
+		set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value);
+		if (set_idx >= 0) {
+			if (j >= out_num) {
+				RTW_PRINT(FUNC_ADPT_FMT" out_num:%u not enough\n",
+					  FUNC_ADPT_ARG(padapter), out_num);
+				break;
+			}
+
+			memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel));
+
+			if (pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
+				out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
+
+			j++;
+		}
+		if (j >= out_num)
+			break;
+	}
+
+	/* if out is empty, use channel_set as default */
+	if (j == 0) {
+		for (i = 0; i < pmlmeext->max_chan_nums; i++) {
+			chan = pmlmeext->channel_set[i].ChannelNum;
+			if (rtw_mlme_band_check(padapter, chan)) {
+				if (rtw_mlme_ignore_chan(padapter, chan))
+					continue;
+
+				if (0)
+					RTW_INFO(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter), chan);
+
+				if (j >= out_num) {
+					RTW_PRINT(FUNC_ADPT_FMT" out_num:%u not enough\n",
+						FUNC_ADPT_ARG(padapter), out_num);
+					break;
+				}
+
+				out[j].hw_value = chan;
+
+				if (pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
+					out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
+
+				j++;
+			}
+		}
+	}
+
+	/* scan_sparse */
+	j = rtw_scan_sparse(padapter, out, j);
+
+	return j;
+}
+
+static void sitesurvey_res_reset(_adapter *adapter, struct sitesurvey_parm *parm)
+{
+	struct ss_res *ss = &adapter->mlmeextpriv.sitesurvey_res;
+	int i;
+
+	ss->bss_cnt = 0;
+	ss->channel_idx = 0;
+	ss->igi_scan = 0;
+	ss->igi_before_scan = 0;
+#ifdef CONFIG_SCAN_BACKOP
+	ss->scan_cnt = 0;
+#endif
+#if defined(CONFIG_ANTENNA_DIVERSITY) || defined(DBG_SCAN_SW_ANTDIV_BL)
+	ss->is_sw_antdiv_bl_scan = 0;
+#endif
+
+	for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
+		if (parm->ssid[i].SsidLength) {
+			memcpy(ss->ssid[i].Ssid, parm->ssid[i].Ssid, IW_ESSID_MAX_SIZE);
+			ss->ssid[i].SsidLength = parm->ssid[i].SsidLength;
+		} else
+			ss->ssid[i].SsidLength = 0;
+	}
+
+	ss->ch_num = rtw_scan_ch_decision(adapter
+					  , ss->ch, RTW_CHANNEL_SCAN_AMOUNT
+					  , parm->ch, parm->ch_num
+					 );
+
+	ss->scan_mode = parm->scan_mode;
+}
+
+static u8 sitesurvey_pick_ch_behavior(_adapter *padapter, u8 *ch, RT_SCAN_TYPE *type)
+{
+	u8 next_state;
+	u8 scan_ch = 0;
+	RT_SCAN_TYPE scan_type = SCAN_PASSIVE;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	struct ss_res *ss = &pmlmeext->sitesurvey_res;
+
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+#endif
+
+	/* handle scan abort request */
+	scan_abort_hdl(padapter);
+
+#ifdef CONFIG_P2P
+	if (pwdinfo->rx_invitereq_info.scan_op_ch_only || pwdinfo->p2p_info.scan_op_ch_only) {
+		if (pwdinfo->rx_invitereq_info.scan_op_ch_only)
+			scan_ch = pwdinfo->rx_invitereq_info.operation_ch[ss->channel_idx];
+		else
+			scan_ch = pwdinfo->p2p_info.operation_ch[ss->channel_idx];
+		scan_type = SCAN_ACTIVE;
+	} else if (rtw_p2p_findphase_ex_is_social(pwdinfo)) {
+		/*
+		* Commented by Albert 2011/06/03
+		* The driver is in the find phase, it should go through the social channel.
+		*/
+		int ch_set_idx;
+
+		scan_ch = pwdinfo->social_chan[ss->channel_idx];
+		ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, scan_ch);
+		if (ch_set_idx >= 0)
+			scan_type = pmlmeext->channel_set[ch_set_idx].ScanType;
+		else
+			scan_type = SCAN_ACTIVE;
+	} else
+#endif /* CONFIG_P2P */
+	{
+		struct rtw_ieee80211_channel *ch;
+
+		if (ss->channel_idx < ss->ch_num) {
+			ch = &ss->ch[ss->channel_idx];
+			scan_ch = ch->hw_value;
+			scan_type = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
+		}
+	}
+
+	if (scan_ch != 0) {
+		next_state = SCAN_PROCESS;
+#ifdef CONFIG_SCAN_BACKOP
+		{
+			struct mi_state mstate;
+			u8 backop_flags = 0;
+
+			rtw_mi_status(padapter, &mstate);
+
+			if ((MSTATE_STA_LD_NUM(&mstate) && mlmeext_chk_scan_backop_flags_sta(pmlmeext, SS_BACKOP_EN))
+				|| (MSTATE_STA_NUM(&mstate) && mlmeext_chk_scan_backop_flags_sta(pmlmeext, SS_BACKOP_EN_NL)))
+				backop_flags |= mlmeext_scan_backop_flags_sta(pmlmeext);
+
+			if ((MSTATE_AP_LD_NUM(&mstate) && mlmeext_chk_scan_backop_flags_ap(pmlmeext, SS_BACKOP_EN))
+				|| (MSTATE_AP_NUM(&mstate) && mlmeext_chk_scan_backop_flags_ap(pmlmeext, SS_BACKOP_EN_NL)))
+				backop_flags |= mlmeext_scan_backop_flags_ap(pmlmeext);
+
+			if (backop_flags) {
+				if (ss->scan_cnt < ss->scan_cnt_max)
+					ss->scan_cnt++;
+				else {
+					mlmeext_assign_scan_backop_flags(pmlmeext, backop_flags);
+					next_state = SCAN_BACKING_OP;
+				}
+			}
+		}
+#endif /* CONFIG_SCAN_BACKOP */
+	} else if (rtw_p2p_findphase_ex_is_needed(pwdinfo)) {
+		/* go p2p listen */
+		next_state = SCAN_TO_P2P_LISTEN;
+
+#ifdef CONFIG_ANTENNA_DIVERSITY
+	} else if (rtw_hal_antdiv_before_linked(padapter)) {
+		/* go sw antdiv before link */
+		next_state = SCAN_SW_ANTDIV_BL;
+#endif
+	} else {
+		next_state = SCAN_COMPLETE;
+
+#if defined(DBG_SCAN_SW_ANTDIV_BL)
+		{
+			/* for SCAN_SW_ANTDIV_BL state testing */
+			struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+			int i;
+			bool is_linked = false;
+
+			for (i = 0; i < dvobj->iface_nums; i++) {
+				if (rtw_linked_check(dvobj->padapters[i]))
+					is_linked = true;
+			}
+
+			if (!is_linked) {
+				static bool fake_sw_antdiv_bl_state = 0;
+
+				if (fake_sw_antdiv_bl_state == 0) {
+					next_state = SCAN_SW_ANTDIV_BL;
+					fake_sw_antdiv_bl_state = 1;
+				} else
+					fake_sw_antdiv_bl_state = 0;
+			}
+		}
+#endif /* defined(DBG_SCAN_SW_ANTDIV_BL) */
+	}
+
+#ifdef CONFIG_SCAN_BACKOP
+	if (next_state != SCAN_PROCESS)
+		ss->scan_cnt = 0;
+#endif
+
+
+#ifdef DBG_FIXED_CHAN
+	if (pmlmeext->fixed_chan != 0xff && next_state == SCAN_PROCESS)
+		scan_ch = pmlmeext->fixed_chan;
+#endif
+
+	if (ch)
+		*ch = scan_ch;
+	if (type)
+		*type = scan_type;
+
+	return next_state;
+}
+
+void site_survey(_adapter *padapter, u8 survey_channel, RT_SCAN_TYPE ScanType)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#endif
+
+	if (survey_channel != 0) {
+		set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+
+#ifdef CONFIG_AUTO_CHNL_SEL_NHM
+		if (ACS_ENABLE == GET_ACS_STATE(padapter)) {
+			ACS_OP acs_op = ACS_RESET;
+
+			rtw_hal_set_odm_var(padapter, HAL_ODM_AUTO_CHNL_SEL, &acs_op, true);
+			rtw_set_acs_channel(padapter, survey_channel);
+#ifdef DBG_AUTO_CHNL_SEL_NHM
+			RTW_INFO("[ACS-"ADPT_FMT"]-set ch:%u\n",
+				ADPT_ARG(padapter), rtw_get_acs_channel(padapter));
+#endif
+		}
+#endif
+
+		if (ScanType == SCAN_ACTIVE) {
+#ifdef CONFIG_P2P
+			#ifdef CONFIG_IOCTL_CFG80211
+			if (rtw_cfg80211_is_p2p_scan(padapter))
+			#else
+			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)
+				|| rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
+			#endif
+			{
+				issue_probereq_p2p(padapter, NULL);
+				issue_probereq_p2p(padapter, NULL);
+				issue_probereq_p2p(padapter, NULL);
+			} else
+#endif /* CONFIG_P2P */
+			{
+				int i;
+
+				for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
+					if (pmlmeext->sitesurvey_res.ssid[i].SsidLength) {
+						/* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */
+						if (padapter->registrypriv.wifi_spec)
+							issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
+						else
+							issue_probereq_ex(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL, 0, 0, 0, 0);
+						issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
+					}
+				}
+
+				if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
+					/* IOT issue, When wifi_spec is not set, send one probe req without WPS IE. */
+					if (padapter->registrypriv.wifi_spec)
+						issue_probereq(padapter, NULL, NULL);
+					else
+						issue_probereq_ex(padapter, NULL, NULL, 0, 0, 0, 0);
+					issue_probereq(padapter, NULL, NULL);
+				}
+			}
+		}
+	} else {
+		/* channel number is 0 or this channel is not valid. */
+		rtw_warn_on(1);
+	}
+
+	return;
+}
+
+static void survey_done_set_ch_bw(_adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	u8 cur_channel = 0;
+	u8 cur_bwmode;
+	u8 cur_ch_offset;
+
+#ifdef CONFIG_MCC_MODE
+	if (!rtw_hal_mcc_change_scan_flag(padapter, &cur_channel, &cur_bwmode, &cur_ch_offset)) {
+		if (0)
+			RTW_INFO(FUNC_ADPT_FMT" back to AP channel - ch:%u, bw:%u, offset:%u\n",
+				FUNC_ADPT_ARG(padapter), cur_channel, cur_bwmode, cur_ch_offset);
+		goto exit;
+	}
+#endif
+
+	if (rtw_mi_get_ch_setting_union(padapter, &cur_channel, &cur_bwmode, &cur_ch_offset) != 0) {
+		if (0)
+			RTW_INFO(FUNC_ADPT_FMT" back to linked/linking union - ch:%u, bw:%u, offset:%u\n",
+				FUNC_ADPT_ARG(padapter), cur_channel, cur_bwmode, cur_ch_offset);
+	} else {
+#ifdef CONFIG_P2P
+		struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+		_adapter *iface;
+		int i;
+
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			iface = dvobj->padapters[i];
+			if (!iface)
+				continue;
+
+#ifdef CONFIG_IOCTL_CFG80211
+			if (iface->wdinfo.driver_interface == DRIVER_CFG80211 && !adapter_wdev_data(iface)->p2p_enabled)
+				continue;
+#endif
+
+			if (rtw_p2p_chk_state(&iface->wdinfo, P2P_STATE_LISTEN)) {
+				cur_channel = iface->wdinfo.listen_channel;
+				cur_bwmode = CHANNEL_WIDTH_20;
+				cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+				if (0)
+					RTW_INFO(FUNC_ADPT_FMT" back to "ADPT_FMT"'s listen ch - ch:%u, bw:%u, offset:%u\n",
+						FUNC_ADPT_ARG(padapter), ADPT_ARG(iface), cur_channel, cur_bwmode, cur_ch_offset);
+				break;
+			}
+		}
+#endif /* CONFIG_P2P */
+
+		if (cur_channel == 0) {
+			cur_channel = pmlmeext->cur_channel;
+			cur_bwmode = pmlmeext->cur_bwmode;
+			cur_ch_offset = pmlmeext->cur_ch_offset;
+			if (0)
+				RTW_INFO(FUNC_ADPT_FMT" back to ch:%u, bw:%u, offset:%u\n",
+					FUNC_ADPT_ARG(padapter), cur_channel, cur_bwmode, cur_ch_offset);
+		}
+	}
+exit:
+	set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
+}
+
+/**
+ * sitesurvey_ps_annc - check and doing ps announcement for all the adapters of given @dvobj
+ * @padapter
+ * @ps: power saving or not
+ *
+ * Returns: 0: no ps announcement is doing. 1: ps announcement is doing
+ */
+
+static u8 sitesurvey_ps_annc(_adapter *padapter, bool ps)
+{
+	u8 ps_anc = 0;
+
+	if (rtw_mi_issue_nulldata(padapter, NULL, ps, 3, 500))
+		ps_anc = 1;
+	return ps_anc;
+}
+
+static void sitesurvey_set_igi(_adapter *adapter)
+{
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+	struct ss_res *ss = &mlmeext->sitesurvey_res;
+	u8 igi;
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &adapter->wdinfo;
+#endif
+
+	switch (mlmeext_scan_state(mlmeext)) {
+	case SCAN_ENTER:
+		#ifdef CONFIG_P2P
+		#ifdef CONFIG_IOCTL_CFG80211
+		if (adapter_wdev_data(adapter)->p2p_enabled && pwdinfo->driver_interface == DRIVER_CFG80211)
+			igi = 0x30;
+		else
+		#endif /* CONFIG_IOCTL_CFG80211 */
+		if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+			igi = 0x28;
+		else
+		#endif /* CONFIG_P2P */
+			igi = 0x1e;
+
+		/* record IGI status */
+		ss->igi_scan = igi;
+		rtw_hal_get_odm_var(adapter, HAL_ODM_INITIAL_GAIN, &ss->igi_before_scan, NULL);
+
+		/* disable DIG and set IGI for scan */
+		rtw_hal_set_odm_var(adapter, HAL_ODM_INITIAL_GAIN, &igi, false);
+		break;
+	case SCAN_COMPLETE:
+	case SCAN_TO_P2P_LISTEN:
+		/* enable DIG and restore IGI */
+		igi = 0xff;
+		rtw_hal_set_odm_var(adapter, HAL_ODM_INITIAL_GAIN, &igi, false);
+		break;
+#ifdef CONFIG_SCAN_BACKOP
+	case SCAN_BACKING_OP:
+		/* write IGI for op channel when DIG is not enabled */
+		odm_write_dig(GET_ODM(adapter), ss->igi_before_scan);
+		break;
+	case SCAN_LEAVE_OP:
+		/* write IGI for scan when DIG is not enabled */
+		odm_write_dig(GET_ODM(adapter), ss->igi_scan);
+		break;
+#endif /* CONFIG_SCAN_BACKOP */
+	default:
+		rtw_warn_on(1);
+		break;
+	}
+}
+
+static void sitesurvey_set_msr(_adapter *adapter, bool enter)
+{
+	u8 network_type;
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (enter) {
+#ifdef CONFIG_MI_WITH_MBSSID_CAM
+		rtw_hal_get_hwreg(adapter, HW_VAR_MEDIA_STATUS, (u8 *)(&pmlmeinfo->hw_media_state));
+#endif
+		/* set MSR to no link state */
+		network_type = _HW_STATE_NOLINK_;
+	} else {
+#ifdef CONFIG_MI_WITH_MBSSID_CAM
+		network_type = pmlmeinfo->hw_media_state;
+#else
+		network_type = pmlmeinfo->state & 0x3;
+#endif
+	}
+	Set_MSR(adapter, network_type);
+}
+u8 sitesurvey_cmd_hdl(_adapter *padapter, u8 *pbuf)
+{
+	struct sitesurvey_parm	*pparm = (struct sitesurvey_parm *)pbuf;
+	struct dvobj_priv *dvobj = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &dvobj->drv_dbg;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct ss_res *ss = &pmlmeext->sitesurvey_res;
+	u8 val8;
+
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+#endif
+
+#ifdef DBG_CHECK_FW_PS_STATE
+	if (rtw_fw_ps_state(padapter) == _FAIL) {
+		RTW_INFO("scan without leave 32k\n");
+		pdbgpriv->dbg_scan_pwr_state_cnt++;
+	}
+#endif /* DBG_CHECK_FW_PS_STATE */
+
+	/* increase channel idx */
+	if (mlmeext_chk_scan_state(pmlmeext, SCAN_PROCESS))
+		ss->channel_idx++;
+
+	/* update scan state to next state (assigned by previous cmd hdl) */
+	if (mlmeext_scan_state(pmlmeext) != mlmeext_scan_next_state(pmlmeext))
+		mlmeext_set_scan_state(pmlmeext, mlmeext_scan_next_state(pmlmeext));
+
+operation_by_state:
+	switch (mlmeext_scan_state(pmlmeext)) {
+
+	case SCAN_DISABLE:
+		/*
+		* SW parameter initialization
+		*/
+
+		sitesurvey_res_reset(padapter, pparm);
+		mlmeext_set_scan_state(pmlmeext, SCAN_START);
+		goto operation_by_state;
+
+	case SCAN_START:
+		/*
+		* prepare to leave operating channel
+		*/
+
+#ifdef CONFIG_MCC_MODE
+		rtw_hal_set_mcc_setting_scan_start(padapter);
+#endif /* CONFIG_MCC_MODE */
+
+		/* apply rx ampdu setting */
+		if (ss->rx_ampdu_accept != RX_AMPDU_ACCEPT_INVALID
+			|| ss->rx_ampdu_size != RX_AMPDU_SIZE_INVALID)
+			rtw_rx_ampdu_apply(padapter);
+
+		/* clear HW TX queue before scan */
+		rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, NULL);
+
+		/* power save state announcement */
+		if (sitesurvey_ps_annc(padapter, 1)) {
+			mlmeext_set_scan_state(pmlmeext, SCAN_PS_ANNC_WAIT);
+			mlmeext_set_scan_next_state(pmlmeext, SCAN_ENTER);
+			set_survey_timer(pmlmeext, 50); /* delay 50ms to protect nulldata(1) */
+		} else {
+			mlmeext_set_scan_state(pmlmeext, SCAN_ENTER);
+			goto operation_by_state;
+		}
+
+		break;
+
+	case SCAN_ENTER:
+		/*
+		* HW register and DM setting for enter scan
+		*/
+
+		rtw_phydm_ability_backup(padapter);
+
+		sitesurvey_set_igi(padapter);
+
+		/* config dynamic functions for off channel */
+		rtw_phydm_func_for_offchannel(padapter);
+		/* set MSR to no link state */
+		sitesurvey_set_msr(padapter, true);
+
+		val8 = 1; /* under site survey */
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+		mlmeext_set_scan_state(pmlmeext, SCAN_PROCESS);
+		goto operation_by_state;
+
+	case SCAN_PROCESS: {
+		u8 scan_ch;
+		RT_SCAN_TYPE scan_type;
+		u8 next_state;
+		u32 scan_ms;
+
+#ifdef CONFIG_AUTO_CHNL_SEL_NHM
+		if ((ACS_ENABLE == GET_ACS_STATE(padapter)) && (0 != rtw_get_acs_channel(padapter))) {
+			ACS_OP acs_op = ACS_SELECT;
+
+			rtw_hal_set_odm_var(padapter, HAL_ODM_AUTO_CHNL_SEL, &acs_op, true);
+		}
+#endif
+
+		next_state = sitesurvey_pick_ch_behavior(padapter, &scan_ch, &scan_type);
+		if (next_state != SCAN_PROCESS) {
+#ifdef CONFIG_AUTO_CHNL_SEL_NHM
+			if (ACS_ENABLE == GET_ACS_STATE(padapter)) {
+				rtw_set_acs_channel(padapter, 0);
+#ifdef DBG_AUTO_CHNL_SEL_NHM
+				RTW_INFO("[ACS-"ADPT_FMT"]-set ch:%u\n", ADPT_ARG(padapter), rtw_get_acs_channel(padapter));
+#endif
+			}
+#endif
+
+			mlmeext_set_scan_state(pmlmeext, next_state);
+			goto operation_by_state;
+		}
+
+		/* still SCAN_PROCESS state */
+		if (0)
+#ifdef CONFIG_P2P
+			RTW_INFO(FUNC_ADPT_FMT" %s ch:%u (cnt:%u,idx:%d) at %dms, %c%c%c\n"
+				 , FUNC_ADPT_ARG(padapter)
+				 , mlmeext_scan_state_str(pmlmeext)
+				 , scan_ch
+				, pwdinfo->find_phase_state_exchange_cnt, ss->channel_idx
+				, rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time)
+				, scan_type ? 'A' : 'P', ss->scan_mode ? 'A' : 'P'
+				 , ss->ssid[0].SsidLength ? 'S' : ' '
+				);
+#else
+			RTW_INFO(FUNC_ADPT_FMT" %s ch:%u (idx:%d) at %dms, %c%c%c\n"
+				 , FUNC_ADPT_ARG(padapter)
+				 , mlmeext_scan_state_str(pmlmeext)
+				 , scan_ch
+				 , ss->channel_idx
+				, rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time)
+				, scan_type ? 'A' : 'P', ss->scan_mode ? 'A' : 'P'
+				 , ss->ssid[0].SsidLength ? 'S' : ' '
+				);
+#endif /* CONFIG_P2P */
+
+#ifdef DBG_FIXED_CHAN
+		if (pmlmeext->fixed_chan != 0xff)
+			RTW_INFO(FUNC_ADPT_FMT" fixed_chan:%u\n", pmlmeext->fixed_chan);
+#endif
+
+		site_survey(padapter, scan_ch, scan_type);
+
+#if defined(CONFIG_ATMEL_RC_PATCH)
+		if (check_fwstate(pmlmepriv, _FW_LINKED))
+			scan_ms = 20;
+		else
+			scan_ms = 40;
+#else
+		scan_ms = ss->scan_ch_ms;
+#endif
+
+#if defined(CONFIG_ANTENNA_DIVERSITY) || defined(DBG_SCAN_SW_ANTDIV_BL)
+		if (ss->is_sw_antdiv_bl_scan)
+			scan_ms = scan_ms / 2;
+#endif
+
+#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+		{
+			struct noise_info info;
+
+			info.bPauseDIG = false;
+			info.IGIValue = 0;
+			info.max_time = scan_ms / 2;
+			info.chan = scan_ch;
+			rtw_hal_set_odm_var(padapter, HAL_ODM_NOISE_MONITOR, &info, false);
+		}
+#endif
+
+		set_survey_timer(pmlmeext, scan_ms);
+		break;
+	}
+
+#ifdef CONFIG_SCAN_BACKOP
+	case SCAN_BACKING_OP: {
+		u8 back_ch, back_bw, back_ch_offset;
+		u8 need_ch_setting_union = true;
+
+#ifdef CONFIG_MCC_MODE
+		need_ch_setting_union = rtw_hal_mcc_change_scan_flag(padapter,
+				&back_ch, &back_bw, &back_ch_offset);
+#endif /* CONFIG_MCC_MODE */
+
+		if (need_ch_setting_union) {
+			if (rtw_mi_get_ch_setting_union(padapter, &back_ch, &back_bw, &back_ch_offset) == 0)
+				rtw_warn_on(1);
+		}
+
+		if (0)
+			RTW_INFO(FUNC_ADPT_FMT" %s ch:%u, bw:%u, offset:%u at %dms\n"
+				 , FUNC_ADPT_ARG(padapter)
+				 , mlmeext_scan_state_str(pmlmeext)
+				 , back_ch, back_bw, back_ch_offset
+				, rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time)
+				);
+
+		set_channel_bwmode(padapter, back_ch, back_ch_offset, back_bw);
+
+		sitesurvey_set_msr(padapter, false);
+
+		val8 = 0; /* survey done */
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+		if (mlmeext_chk_scan_backop_flags(pmlmeext, SS_BACKOP_PS_ANNC)) {
+			sitesurvey_set_igi(padapter);
+			sitesurvey_ps_annc(padapter, 0);
+		}
+
+		mlmeext_set_scan_state(pmlmeext, SCAN_BACK_OP);
+		ss->backop_time = jiffies;
+
+		if (mlmeext_chk_scan_backop_flags(pmlmeext, SS_BACKOP_TX_RESUME))
+			rtw_mi_os_xmit_schedule(padapter);
+
+
+		goto operation_by_state;
+	}
+
+	case SCAN_BACK_OP:
+		if (rtw_get_passing_time_ms(ss->backop_time) >= ss->backop_ms
+		    || pmlmeext->scan_abort
+		   ) {
+			mlmeext_set_scan_state(pmlmeext, SCAN_LEAVING_OP);
+			goto operation_by_state;
+		}
+		set_survey_timer(pmlmeext, 50);
+		break;
+
+	case SCAN_LEAVING_OP:
+		/*
+		* prepare to leave operating channel
+		*/
+
+		/* clear HW TX queue before scan */
+		rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
+
+		if (mlmeext_chk_scan_backop_flags(pmlmeext, SS_BACKOP_PS_ANNC)
+		    && sitesurvey_ps_annc(padapter, 1)
+		   ) {
+			mlmeext_set_scan_state(pmlmeext, SCAN_PS_ANNC_WAIT);
+			mlmeext_set_scan_next_state(pmlmeext, SCAN_LEAVE_OP);
+			set_survey_timer(pmlmeext, 50); /* delay 50ms to protect nulldata(1) */
+		} else {
+			mlmeext_set_scan_state(pmlmeext, SCAN_LEAVE_OP);
+			goto operation_by_state;
+		}
+
+		break;
+
+	case SCAN_LEAVE_OP:
+		/*
+		* HW register and DM setting for enter scan
+		*/
+
+		if (mlmeext_chk_scan_backop_flags(pmlmeext, SS_BACKOP_PS_ANNC))
+			sitesurvey_set_igi(padapter);
+
+		sitesurvey_set_msr(padapter, true);
+
+		val8 = 1; /* under site survey */
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+		mlmeext_set_scan_state(pmlmeext, SCAN_PROCESS);
+		goto operation_by_state;
+
+#endif /* CONFIG_SCAN_BACKOP */
+
+#if defined(CONFIG_ANTENNA_DIVERSITY) || defined(DBG_SCAN_SW_ANTDIV_BL)
+	case SCAN_SW_ANTDIV_BL:
+		/*
+		* 20100721
+		* For SW antenna diversity before link, it needs to switch to another antenna and scan again.
+		* It compares the scan result and select better one to do connection.
+		*/
+		ss->bss_cnt = 0;
+		ss->channel_idx = 0;
+		ss->is_sw_antdiv_bl_scan = 1;
+
+		mlmeext_set_scan_next_state(pmlmeext, SCAN_PROCESS);
+		set_survey_timer(pmlmeext, ss->scan_ch_ms);
+		break;
+#endif
+
+#ifdef CONFIG_P2P
+	case SCAN_TO_P2P_LISTEN:
+		/*
+		* Set the P2P State to the listen state of find phase
+		* and set the current channel to the listen channel
+		*/
+		set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN);
+
+		/* turn on phy-dynamic functions */
+		rtw_phydm_ability_restore(padapter);
+
+		sitesurvey_set_igi(padapter);
+
+		mlmeext_set_scan_state(pmlmeext, SCAN_P2P_LISTEN);
+		_set_timer(&pwdinfo->find_phase_timer, (u32)((u32)pwdinfo->listen_dwell * 100));
+		break;
+
+	case SCAN_P2P_LISTEN:
+		mlmeext_set_scan_state(pmlmeext, SCAN_PROCESS);
+		ss->channel_idx = 0;
+		goto operation_by_state;
+#endif /* CONFIG_P2P */
+
+	case SCAN_COMPLETE:
+#ifdef CONFIG_P2P
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)
+		    || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)
+		   ) {
+#ifdef CONFIG_CONCURRENT_MODE
+			if (pwdinfo->driver_interface == DRIVER_WEXT) {
+				if (rtw_mi_check_status(padapter, MI_LINKED))
+					_set_timer(&pwdinfo->ap_p2p_switch_timer, 500);
+			}
+#endif
+
+			rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+		}
+		rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
+#endif /* CONFIG_P2P */
+
+		/* switch channel */
+		survey_done_set_ch_bw(padapter);
+
+		sitesurvey_set_msr(padapter, false);
+
+		val8 = 0; /* survey done */
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+		/* turn on phy-dynamic functions */
+		rtw_phydm_ability_restore(padapter);
+
+		sitesurvey_set_igi(padapter);
+
+#ifdef CONFIG_MCC_MODE
+		/* start MCC fail, then tx null data */
+		if (!rtw_hal_set_mcc_setting_scan_complete(padapter))
+#endif /* CONFIG_MCC_MODE */
+			sitesurvey_ps_annc(padapter, 0);
+
+		/* apply rx ampdu setting */
+		rtw_rx_ampdu_apply(padapter);
+
+		mlmeext_set_scan_state(pmlmeext, SCAN_DISABLE);
+
+		report_surveydone_event(padapter);
+
+		issue_action_BSSCoexistPacket(padapter);
+		issue_action_BSSCoexistPacket(padapter);
+		issue_action_BSSCoexistPacket(padapter);
+	}
+
+	return H2C_SUCCESS;
+}
+
+u8 setauth_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct setauth_parm		*pparm = (struct setauth_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pparm->mode < 4)
+		pmlmeinfo->auth_algo = pparm->mode;
+
+	return	H2C_SUCCESS;
+}
+
+/*
+SEC CAM Entry format (32 bytes)
+DW0 - MAC_ADDR[15:0] | Valid[15] | MFB[14:8] | RSVD[7]  | GK[6] | MIC_KEY[5] | SEC_TYPE[4:2] | KID[1:0]
+DW0 - MAC_ADDR[15:0] | Valid[15] |RSVD[14:9] | RPT_MODE[8] | SPP_MODE[7]  | GK[6] | MIC_KEY[5] | SEC_TYPE[4:2] | KID[1:0] (92E/8812A/8814A)
+DW1 - MAC_ADDR[47:16]
+DW2 - KEY[31:0]
+DW3 - KEY[63:32]
+DW4 - KEY[95:64]
+DW5 - KEY[127:96]
+DW6 - RSVD
+DW7 - RSVD
+*/
+
+/*Set WEP key or Group Key*/
+u8 setkey_hdl(_adapter *padapter, u8 *pbuf)
+{
+	u16	ctrl = 0;
+	s16 cam_id = 0;
+	struct setkey_parm		*pparm = (struct setkey_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	unsigned char null_addr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	u8 *addr;
+	bool used = false;
+
+	/* main tx key for wep. */
+	if (pparm->set_tx)
+		pmlmeinfo->key_index = pparm->keyid;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE))
+		cam_id = rtw_iface_bcmc_id_get(padapter);
+	else
+#endif
+		cam_id = rtw_camid_alloc(padapter, NULL, pparm->keyid, &used);
+
+	if (cam_id < 0)
+		goto enable_mc;
+
+#ifndef CONFIG_CONCURRENT_MODE
+	if (cam_id >= 0 && cam_id <= 3)
+		addr = null_addr;
+	else
+#endif
+	{
+		if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE))
+			/* for AP mode ,we will force sec cam entry_id so hw dont search cam when tx*/
+			addr = adapter_mac_addr(padapter);
+		else
+			/* not default key, searched by A2 */
+			addr = get_bssid(&padapter->mlmepriv);
+	}
+
+	/* cam entry searched is pairwise key */
+	if (used && rtw_camid_is_gk(padapter, cam_id) == false) {
+		s16 camid_clr;
+
+		RTW_PRINT(FUNC_ADPT_FMT" group key with "MAC_FMT" id:%u the same key id as pairwise key\n"
+			, FUNC_ADPT_ARG(padapter), MAC_ARG(addr), pparm->keyid);
+
+		/* HW has problem to distinguish this group key with existing pairwise key, stop HW enc and dec for BMC */
+		rtw_camctl_set_flags(padapter, SEC_STATUS_STA_PK_GK_CONFLICT_DIS_BMC_SEARCH);
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, NULL);
+
+		/* clear group key */
+		while ((camid_clr = rtw_camid_search(padapter, addr, -1, 1)) >= 0) {
+			RTW_PRINT("clear group key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(addr), camid_clr);
+			clear_cam_entry(padapter, camid_clr);
+			rtw_camid_free(padapter, camid_clr);
+		}
+
+		goto enable_mc;
+	}
+
+	ctrl = BIT(15) | BIT(6) | ((pparm->algorithm) << 2) | pparm->keyid;
+
+	RTW_PRINT("set group key camid:%d, addr:"MAC_FMT", kid:%d, type:%s\n"
+		, cam_id, MAC_ARG(addr), pparm->keyid, security_type_str(pparm->algorithm));
+
+	write_cam(padapter, cam_id, ctrl, addr, pparm->key);
+
+	/* if ((cam_id > 3) && (((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)))*/
+#ifdef CONFIG_CONCURRENT_MODE
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE)) {
+		if (is_wep_enc(pparm->algorithm)) {
+			padapter->securitypriv.dot11Def_camid[pparm->keyid] = cam_id;
+			padapter->securitypriv.dot118021x_bmc_cam_id =
+				padapter->securitypriv.dot11Def_camid[padapter->securitypriv.dot11PrivacyKeyIndex];
+			RTW_PRINT("wep group key - force camid:%d\n", padapter->securitypriv.dot118021x_bmc_cam_id);
+		} else {
+			/*u8 org_cam_id = padapter->securitypriv.dot118021x_bmc_cam_id;*/
+
+			/*force GK's cam id*/
+			padapter->securitypriv.dot118021x_bmc_cam_id = cam_id;
+
+			/* for GTK rekey
+			if ((org_cam_id != INVALID_SEC_MAC_CAM_ID) &&
+				(org_cam_id != cam_id)) {
+				RTW_PRINT("clear group key for addr:"MAC_FMT", org_camid:%d new_camid:%d\n", MAC_ARG(addr), org_cam_id, cam_id);
+				clear_cam_entry(padapter, org_cam_id);
+				rtw_camid_free(padapter, org_cam_id);
+			}*/
+		}
+	}
+#endif
+
+
+#ifndef CONFIG_CONCURRENT_MODE
+	if (cam_id >= 0 && cam_id <= 3)
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8 *)true);
+#endif
+
+	/* 8814au should set both broadcast and unicast CAM entry for WEP key in STA mode */
+	if (is_wep_enc(pparm->algorithm) && check_mlmeinfo_state(pmlmeext, WIFI_FW_STATION_STATE) &&
+	    _rtw_camctl_chk_cap(padapter, SEC_CAP_CHK_BMC)) {
+		struct set_stakey_parm	sta_pparm;
+
+		sta_pparm.algorithm = pparm->algorithm;
+		sta_pparm.keyid = pparm->keyid;
+		memcpy(sta_pparm.key, pparm->key, 16);
+		memcpy(sta_pparm.addr, get_bssid(&padapter->mlmepriv), ETH_ALEN);
+		set_stakey_hdl(padapter, (u8 *)&sta_pparm);
+	}
+
+enable_mc:
+	/* allow multicast packets to driver */
+	rtw_hal_set_hwreg(padapter, HW_VAR_ON_RCR_AM, null_addr);
+
+	return H2C_SUCCESS;
+}
+
+void rtw_ap_wep_pk_setting(_adapter *adapter, struct sta_info *psta)
+{
+	struct security_priv *psecuritypriv = &(adapter->securitypriv);
+	struct set_stakey_parm	sta_pparm;
+	sint keyid;
+
+	if (!is_wep_enc(psecuritypriv->dot11PrivacyAlgrthm))
+		return;
+
+	for (keyid = 0; keyid < 4; keyid++) {
+		if ((psecuritypriv->key_mask & BIT(keyid)) && (keyid == psecuritypriv->dot11PrivacyKeyIndex)) {
+			sta_pparm.algorithm = psecuritypriv->dot11PrivacyAlgrthm;
+			sta_pparm.keyid = keyid;
+			memcpy(sta_pparm.key, &(psecuritypriv->dot11DefKey[keyid].skey[0]), 16);
+			memcpy(sta_pparm.addr, psta->hwaddr, ETH_ALEN);
+
+			RTW_PRINT(FUNC_ADPT_FMT"set WEP - PK with "MAC_FMT" keyid:%u\n"
+				, FUNC_ADPT_ARG(adapter), MAC_ARG(psta->hwaddr), keyid);
+
+			set_stakey_hdl(adapter, (u8 *)&sta_pparm);
+		}
+	}
+}
+
+u8 set_stakey_hdl(_adapter *padapter, u8 *pbuf)
+{
+	u16 ctrl = 0;
+	s16 cam_id = 0;
+	bool used;
+	u8 kid = 0;
+	u8 ret = H2C_SUCCESS;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct set_stakey_parm	*pparm = (struct set_stakey_parm *)pbuf;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta;
+
+	if (pparm->algorithm == _NO_PRIVACY_)
+		goto write_to_cam;
+
+	psta = rtw_get_stainfo(pstapriv, pparm->addr);
+	if (!psta) {
+		RTW_PRINT("%s sta:"MAC_FMT" not found\n", __func__, MAC_ARG(pparm->addr));
+		ret = H2C_REJECTED;
+		goto exit;
+	}
+
+	pmlmeinfo->enc_algo = pparm->algorithm;
+	if (is_wep_enc(pparm->algorithm))
+		kid = pparm->keyid;
+	cam_id = rtw_camid_alloc(padapter, psta, kid, &used);
+	if (cam_id < 0)
+		goto exit;
+
+	/* cam entry searched is group key */
+	if (used && rtw_camid_is_gk(padapter, cam_id) == true) {
+		s16 camid_clr;
+
+		RTW_PRINT(FUNC_ADPT_FMT" pairwise key with "MAC_FMT" id:%u the same key id as group key\n"
+			, FUNC_ADPT_ARG(padapter), MAC_ARG(pparm->addr), pparm->keyid);
+
+		/* HW has problem to distinguish this pairwise key with existing group key, stop HW enc and dec for BMC */
+		rtw_camctl_set_flags(padapter, SEC_STATUS_STA_PK_GK_CONFLICT_DIS_BMC_SEARCH);
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, NULL);
+
+		/* clear group key */
+		while ((camid_clr = rtw_camid_search(padapter, pparm->addr, -1, 1)) >= 0) {
+			RTW_PRINT("clear group key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(pparm->addr), camid_clr);
+			clear_cam_entry(padapter, camid_clr);
+			rtw_camid_free(padapter, camid_clr);
+		}
+	}
+
+write_to_cam:
+	if (pparm->algorithm == _NO_PRIVACY_) {
+		while ((cam_id = rtw_camid_search(padapter, pparm->addr, -1, -1)) >= 0) {
+			RTW_PRINT("clear key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(pparm->addr), cam_id);
+			clear_cam_entry(padapter, cam_id);
+			rtw_camid_free(padapter, cam_id);
+		}
+	} else {
+		RTW_PRINT("set pairwise key camid:%d, addr:"MAC_FMT", kid:%d, type:%s\n",
+			cam_id, MAC_ARG(pparm->addr), pparm->keyid, security_type_str(pparm->algorithm));
+		ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;
+		write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+	}
+	ret = H2C_SUCCESS_RSP;
+
+exit:
+	return ret;
+}
+
+u8 add_ba_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct addBaReq_parm	*pparm = (struct addBaReq_parm *)pbuf;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr);
+
+	if (!psta)
+		return	H2C_SUCCESS;
+
+	if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
+	    ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) {
+		/* pmlmeinfo->ADDBA_retry_count = 0; */
+		/* pmlmeinfo->candidate_tid_bitmap |= (0x1 << pparm->tid);		 */
+		/* psta->htpriv.candidate_tid_bitmap |= BIT(pparm->tid); */
+		issue_addba_req(padapter, pparm->addr, (u8)pparm->tid);
+		/* _set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO); */
+		_set_timer(&psta->addba_retry_timer, ADDBA_TO);
+	}
+#ifdef CONFIG_TDLS
+	else if ((psta->tdls_sta_state & TDLS_LINKED_STATE) &&
+		 (psta->htpriv.ht_option) &&
+		 (psta->htpriv.ampdu_enable)) {
+		issue_addba_req(padapter, pparm->addr, (u8)pparm->tid);
+		_set_timer(&psta->addba_retry_timer, ADDBA_TO);
+	}
+#endif /* CONFIG */
+	else
+		psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
+	return	H2C_SUCCESS;
+}
+
+
+u8 add_ba_rsp_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct addBaRsp_parm *pparm = (struct addBaRsp_parm *)pbuf;
+	u8 ret = true, i = 0, try_cnt = 3, wait_ms = 50;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta;
+
+	psta = rtw_get_stainfo(pstapriv, pparm->addr);
+	if (!psta)
+		goto exit;
+
+	preorder_ctrl = &psta->recvreorder_ctrl[pparm->tid];
+	ret = issue_addba_rsp_wait_ack(padapter, pparm->addr, pparm->tid, pparm->status, pparm->size, 3, 50);
+
+#ifdef CONFIG_UPDATE_INDICATE_SEQ_WHILE_PROCESS_ADDBA_REQ
+	/* status = 0 means accept this addba req, so update indicate seq = start_seq under this compile flag */
+	if (pparm->status == 0) {
+		preorder_ctrl->indicate_seq = pparm->start_seq;
+#ifdef DBG_RX_SEQ
+		RTW_INFO("DBG_RX_SEQ %s:%d IndicateSeq: %d, start_seq: %d\n", __func__, __LINE__,
+			 preorder_ctrl->indicate_seq, pparm->start_seq);
+#endif
+	}
+#else
+	preorder_ctrl->indicate_seq = 0xffff;
+#endif
+
+	/*
+	  * status = 0 means accept this addba req
+	  * status = 37 means reject this addba req
+	  */
+	if (pparm->status == 0) {
+		preorder_ctrl->enable = true;
+		preorder_ctrl->ampdu_size = pparm->size;
+	} else if (pparm->status == 37)
+		preorder_ctrl->enable = false;
+
+exit:
+	return H2C_SUCCESS;
+}
+
+u8 chk_bmc_sleepq_cmd(_adapter *padapter)
+{
+	struct cmd_obj *ph2c;
+	struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
+	u8 res = _SUCCESS;
+
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_parm_rsp(ph2c, GEN_CMD_CODE(_ChkBMCSleepq));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+
+	return res;
+}
+
+u8 set_tx_beacon_cmd(_adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct Tx_Beacon_param	*ptxBeacon_parm;
+	struct cmd_priv	*pcmdpriv = &(padapter->cmdpriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8	res = _SUCCESS;
+	int len_diff = 0;
+
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param));
+	if (ptxBeacon_parm == NULL) {
+		rtw_mfree((unsigned char *)ph2c, sizeof(struct	cmd_obj));
+		res = _FAIL;
+		goto exit;
+	}
+
+	memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
+
+	len_diff = update_hidden_ssid(
+			   ptxBeacon_parm->network.IEs + _BEACON_IE_OFFSET_
+		   , ptxBeacon_parm->network.IELength - _BEACON_IE_OFFSET_
+			   , pmlmeinfo->hidden_ssid_mode
+		   );
+	ptxBeacon_parm->network.IELength += len_diff;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+
+exit:
+
+
+	return res;
+}
+
+
+u8 mlme_evt_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	u8 evt_code, evt_seq;
+	u16 evt_sz;
+	uint	*peventbuf;
+	void (*event_callback)(_adapter *dev, u8 *pbuf);
+	struct evt_priv *pevt_priv = &(padapter->evtpriv);
+
+	if (pbuf == NULL)
+		goto _abort_event_;
+
+	peventbuf = (uint *)pbuf;
+	evt_sz = (u16)(*peventbuf & 0xffff);
+	evt_seq = (u8)((*peventbuf >> 24) & 0x7f);
+	evt_code = (u8)((*peventbuf >> 16) & 0xff);
+
+
+#ifdef CHECK_EVENT_SEQ
+	/* checking event sequence...		 */
+	if (evt_seq != (ATOMIC_READ(&pevt_priv->event_seq) & 0x7f)) {
+
+		pevt_priv->event_seq = (evt_seq + 1) & 0x7f;
+
+		goto _abort_event_;
+	}
+#endif
+
+	/* checking if event code is valid */
+	if (evt_code >= MAX_C2HEVT) {
+		goto _abort_event_;
+	}
+
+	/* checking if event size match the event parm size	 */
+	if ((wlanevents[evt_code].parmsize != 0) &&
+	    (wlanevents[evt_code].parmsize != evt_sz)) {
+
+		goto _abort_event_;
+
+	}
+
+	ATOMIC_INC(&pevt_priv->event_seq);
+
+	peventbuf += 2;
+
+	if (peventbuf) {
+		event_callback = wlanevents[evt_code].event_callback;
+		event_callback(padapter, (u8 *)peventbuf);
+
+		pevt_priv->evt_done_cnt++;
+	}
+
+
+_abort_event_:
+
+
+	return H2C_SUCCESS;
+
+}
+
+u8 h2c_msg_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	return H2C_SUCCESS;
+}
+
+u8 chk_bmc_sleepq_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+#ifdef CONFIG_AP_MODE
+	unsigned long irqL;
+	struct sta_info *psta_bmc;
+	_list	*xmitframe_plist, *xmitframe_phead;
+	struct xmit_frame *pxmitframe = NULL;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct sta_priv  *pstapriv = &padapter->stapriv;
+
+	/* for BC/MC Frames */
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+	if (!psta_bmc)
+		return H2C_SUCCESS;
+
+	if ((pstapriv->tim_bitmap & BIT(0)) && (psta_bmc->sleepq_len > 0)) {
+#ifndef CONFIG_PCI_HCI
+		rtw_msleep_os(10);/* 10ms, ATIM(HIQ) Windows */
+#endif
+		/* _enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL); */
+		_enter_critical_bh(&pxmitpriv->lock, &irqL);
+
+		xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+		xmitframe_plist = get_next(xmitframe_phead);
+
+		while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == false) {
+			pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+			xmitframe_plist = get_next(xmitframe_plist);
+
+			list_del_init(&pxmitframe->list);
+
+			psta_bmc->sleepq_len--;
+			if (psta_bmc->sleepq_len > 0)
+				pxmitframe->attrib.mdata = 1;
+			else
+				pxmitframe->attrib.mdata = 0;
+
+			pxmitframe->attrib.triggered = 1;
+
+			if (xmitframe_hiq_filter(pxmitframe))
+				pxmitframe->attrib.qsel = QSLT_HIGH;/* HIQ */
+
+			rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+		}
+
+		_exit_critical_bh(&pxmitpriv->lock, &irqL);
+
+		if (rtw_get_intf_type(padapter) != RTW_PCIE) {
+			/* check hi queue and bmc_sleepq */
+			rtw_chk_hi_queue_cmd(padapter);
+		}
+	}
+#endif
+
+	return H2C_SUCCESS;
+}
+
+u8 tx_beacon_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+
+#ifdef CONFIG_SWTIMER_BASED_TXBCN
+
+	tx_beacon_handler(padapter->dvobj);
+
+#else
+
+	if (send_beacon(padapter) == _FAIL) {
+		RTW_INFO("issue_beacon, fail!\n");
+		return H2C_PARAMETERS_ERROR;
+	}
+
+	/* tx bc/mc frames after update TIM */
+	chk_bmc_sleepq_hdl(padapter, NULL);
+#endif
+
+	return H2C_SUCCESS;
+}
+
+/*
+* according to channel
+* add/remove WLAN_BSSID_EX.IEs's ERP ie
+* set WLAN_BSSID_EX.SupportedRates
+* update WLAN_BSSID_EX.IEs's Supported Rate and Extended Supported Rate ie
+*/
+void change_band_update_ie(_adapter *padapter, WLAN_BSSID_EX *pnetwork, u8 ch)
+{
+	u8	network_type, rate_len, total_rate_len, remainder_rate_len;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8	erpinfo = 0x4;
+
+	if (ch >= 36) {
+		network_type = WIRELESS_11A;
+		total_rate_len = IEEE80211_NUM_OFDM_RATESLEN;
+		rtw_remove_bcn_ie(padapter, pnetwork, _ERPINFO_IE_);
+	} else {
+		network_type = WIRELESS_11BG;
+		total_rate_len = IEEE80211_CCK_RATE_LEN + IEEE80211_NUM_OFDM_RATESLEN;
+		rtw_add_bcn_ie(padapter, pnetwork, _ERPINFO_IE_, &erpinfo, 1);
+	}
+
+	rtw_set_supported_rate(pnetwork->SupportedRates, network_type);
+
+	UpdateBrateTbl(padapter, pnetwork->SupportedRates);
+
+	if (total_rate_len > 8) {
+		rate_len = 8;
+		remainder_rate_len = total_rate_len - 8;
+	} else {
+		rate_len = total_rate_len;
+		remainder_rate_len = 0;
+	}
+
+	rtw_add_bcn_ie(padapter, pnetwork, _SUPPORTEDRATES_IE_, pnetwork->SupportedRates, rate_len);
+
+	if (remainder_rate_len)
+		rtw_add_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_, (pnetwork->SupportedRates + 8), remainder_rate_len);
+	else
+		rtw_remove_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_);
+
+	pnetwork->Length = get_WLAN_BSSID_EX_sz(pnetwork);
+}
+
+void rtw_join_done_chk_ch(_adapter *adapter, int join_res)
+{
+#define DUMP_ADAPTERS_STATUS 0
+
+	struct dvobj_priv *dvobj;
+	_adapter *iface;
+	struct mlme_priv *mlme;
+	struct mlme_ext_priv *mlmeext;
+	u8 u_ch, u_offset, u_bw;
+	int i;
+
+	dvobj = adapter_to_dvobj(adapter);
+
+	if (DUMP_ADAPTERS_STATUS) {
+		RTW_INFO(FUNC_ADPT_FMT" enter\n", FUNC_ADPT_ARG(adapter));
+		dump_adapters_status(RTW_DBGDUMP , dvobj);
+	}
+
+	if (join_res >= 0) {
+
+#ifdef CONFIG_MCC_MODE
+		/* MCC setting success, don't go to ch union process */
+		if (rtw_hal_set_mcc_setting_join_done_chk_ch(adapter))
+			return;
+#endif /* CONFIG_MCC_MODE */
+
+		if (rtw_mi_get_ch_setting_union(adapter, &u_ch, &u_bw, &u_offset) <= 0) {
+			dump_adapters_status(RTW_DBGDUMP , dvobj);
+			rtw_warn_on(1);
+		}
+
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			iface = dvobj->padapters[i];
+			mlme = &iface->mlmepriv;
+			mlmeext = &iface->mlmeextpriv;
+
+			if (!iface || iface == adapter)
+				continue;
+
+			if (check_fwstate(mlme, WIFI_AP_STATE)
+			    && check_fwstate(mlme, WIFI_ASOC_STATE)
+			   ) {
+				bool is_grouped = rtw_is_chbw_grouped(u_ch, u_bw, u_offset
+					, mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset);
+
+				if (is_grouped == false) {
+					/* handle AP which need to switch ch setting */
+
+					/* restore original bw, adjust bw by registry setting on target ch */
+					mlmeext->cur_bwmode = mlme->ori_bw;
+					mlmeext->cur_channel = u_ch;
+					rtw_adjust_chbw(iface
+						, mlmeext->cur_channel, &mlmeext->cur_bwmode, &mlmeext->cur_ch_offset);
+
+					rtw_sync_chbw(&mlmeext->cur_channel, &mlmeext->cur_bwmode, &mlmeext->cur_ch_offset
+						, &u_ch, &u_bw, &u_offset);
+
+					rtw_ap_update_bss_chbw(iface, &(mlmeext->mlmext_info.network)
+						, mlmeext->cur_channel, mlmeext->cur_bwmode, mlmeext->cur_ch_offset);
+
+					memcpy(&(mlme->cur_network.network), &(mlmeext->mlmext_info.network), sizeof(WLAN_BSSID_EX));
+
+					rtw_start_bss_hdl_after_chbw_decided(iface);
+				}
+
+				update_beacon(iface, 0, NULL, true);
+			}
+
+			clr_fwstate(mlme, WIFI_OP_CH_SWITCHING);
+		}
+
+#ifdef CONFIG_DFS_MASTER
+		rtw_dfs_master_status_apply(adapter, MLME_STA_CONNECTED);
+#endif
+	} else {
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			iface = dvobj->padapters[i];
+			mlme = &iface->mlmepriv;
+			mlmeext = &iface->mlmeextpriv;
+
+			if (!iface || iface == adapter)
+				continue;
+
+			if (check_fwstate(mlme, WIFI_AP_STATE)
+			    && check_fwstate(mlme, WIFI_ASOC_STATE))
+				update_beacon(iface, 0, NULL, true);
+
+			clr_fwstate(mlme, WIFI_OP_CH_SWITCHING);
+		}
+#ifdef CONFIG_DFS_MASTER
+		rtw_dfs_master_status_apply(adapter, MLME_STA_DISCONNECTED);
+#endif
+	}
+
+	if (rtw_mi_get_ch_setting_union(adapter, &u_ch, &u_bw, &u_offset)) {
+		set_channel_bwmode(adapter, u_ch, u_offset, u_bw);
+		rtw_mi_update_union_chan_inf(adapter, u_ch, u_offset, u_bw);
+	}
+
+	if (DUMP_ADAPTERS_STATUS) {
+		RTW_INFO(FUNC_ADPT_FMT" exit\n", FUNC_ADPT_ARG(adapter));
+		dump_adapters_status(RTW_DBGDUMP , dvobj);
+	}
+}
+
+int rtw_chk_start_clnt_join(_adapter *adapter, u8 *ch, u8 *bw, u8 *offset)
+{
+	bool chbw_allow = true;
+	bool connect_allow = true;
+	struct mlme_ext_priv	*pmlmeext = &adapter->mlmeextpriv;
+	u8 cur_ch, cur_bw, cur_ch_offset;
+	u8 u_ch, u_offset, u_bw;
+
+	u_ch = cur_ch = pmlmeext->cur_channel;
+	u_bw = cur_bw = pmlmeext->cur_bwmode;
+	u_offset = cur_ch_offset = pmlmeext->cur_ch_offset;
+
+	if (!ch || !bw || !offset) {
+		connect_allow = false;
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	if (cur_ch == 0) {
+		connect_allow = false;
+		RTW_ERR(FUNC_ADPT_FMT" cur_ch:%u\n"
+			, FUNC_ADPT_ARG(adapter), cur_ch);
+		rtw_warn_on(1);
+		goto exit;
+	}
+	RTW_INFO(FUNC_ADPT_FMT" req: %u,%u,%u\n", FUNC_ADPT_ARG(adapter), u_ch, u_bw, u_offset);
+
+#ifdef CONFIG_CONCURRENT_MODE
+	{
+		struct dvobj_priv *dvobj;
+		_adapter *iface;
+		struct mlme_priv *mlme;
+		struct mlme_ext_priv *mlmeext;
+		struct mi_state mstate;
+		int i;
+
+		dvobj = adapter_to_dvobj(adapter);
+
+		rtw_mi_status_no_self(adapter, &mstate);
+		RTW_INFO(FUNC_ADPT_FMT" ld_sta_num:%u, ap_num:%u\n"
+			 , FUNC_ADPT_ARG(adapter), MSTATE_STA_LD_NUM(&mstate), MSTATE_AP_NUM(&mstate));
+
+		if (!MSTATE_STA_LD_NUM(&mstate) && !MSTATE_AP_NUM(&mstate)) {
+			/* consider linking STA? */
+			goto connect_allow_hdl;
+		}
+
+		if (rtw_mi_get_ch_setting_union_no_self(adapter, &u_ch, &u_bw, &u_offset) <= 0) {
+			dump_adapters_status(RTW_DBGDUMP , dvobj);
+			rtw_warn_on(1);
+		}
+		RTW_INFO(FUNC_ADPT_FMT" union no self: %u,%u,%u\n"
+			 , FUNC_ADPT_ARG(adapter), u_ch, u_bw, u_offset);
+
+		/* chbw_allow? */
+		chbw_allow = rtw_is_chbw_grouped(pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset
+						 , u_ch, u_bw, u_offset);
+
+		RTW_INFO(FUNC_ADPT_FMT" chbw_allow:%d\n"
+			 , FUNC_ADPT_ARG(adapter), chbw_allow);
+
+#ifdef CONFIG_MCC_MODE
+		/* check setting success, don't go to ch union process */
+		if (rtw_hal_set_mcc_setting_chk_start_clnt_join(adapter, &u_ch, &u_bw, &u_offset, chbw_allow))
+			goto exit;
+#endif
+
+		if (chbw_allow) {
+			rtw_sync_chbw(&cur_ch, &cur_bw, &cur_ch_offset, &u_ch, &u_bw, &u_offset);
+			rtw_warn_on(cur_ch != pmlmeext->cur_channel);
+			rtw_warn_on(cur_bw != pmlmeext->cur_bwmode);
+			rtw_warn_on(cur_ch_offset != pmlmeext->cur_ch_offset);
+			goto connect_allow_hdl;
+		}
+
+#ifdef CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT
+		/* chbw_allow is false, connect allow? */
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			iface = dvobj->padapters[i];
+			mlme = &iface->mlmepriv;
+			mlmeext = &iface->mlmeextpriv;
+
+			if (check_fwstate(mlme, WIFI_STATION_STATE)
+			    && check_fwstate(mlme, WIFI_ASOC_STATE)
+#if defined(CONFIG_P2P)
+			    && rtw_p2p_chk_state(&(iface->wdinfo), P2P_STATE_NONE)
+#endif
+			   ) {
+				connect_allow = false;
+				break;
+			}
+		}
+#endif /* CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT */
+
+		if (MSTATE_STA_LD_NUM(&mstate) + MSTATE_AP_LD_NUM(&mstate) >= 2)
+			connect_allow = false;
+
+		RTW_INFO(FUNC_ADPT_FMT" connect_allow:%d\n"
+			 , FUNC_ADPT_ARG(adapter), connect_allow);
+
+		if (connect_allow == false)
+			goto exit;
+
+connect_allow_hdl:
+		/* connect_allow */
+
+#ifdef CONFIG_DFS_MASTER
+		rtw_dfs_master_status_apply(adapter, MLME_STA_CONNECTING);
+#endif
+
+		if (chbw_allow == false) {
+			u_ch = cur_ch;
+			u_bw = cur_bw;
+			u_offset = cur_ch_offset;
+
+			for (i = 0; i < dvobj->iface_nums; i++) {
+				iface = dvobj->padapters[i];
+				mlme = &iface->mlmepriv;
+				mlmeext = &iface->mlmeextpriv;
+
+				if (!iface || iface == adapter)
+					continue;
+
+				if (check_fwstate(mlme, WIFI_AP_STATE)
+				    && check_fwstate(mlme, WIFI_ASOC_STATE)
+				   ) {
+#ifdef CONFIG_SPCT_CH_SWITCH
+					if (1)
+						rtw_ap_inform_ch_switch(iface, pmlmeext->cur_channel , pmlmeext->cur_ch_offset);
+					else
+#endif
+						rtw_sta_flush(iface, false);
+
+					rtw_hal_set_hwreg(iface, HW_VAR_CHECK_TXBUF, 0);
+					set_fwstate(mlme, WIFI_OP_CH_SWITCHING);
+				} else if (check_fwstate(mlme, WIFI_STATION_STATE)
+					&& check_fwstate(mlme, WIFI_ASOC_STATE)
+					  ) {
+					rtw_disassoc_cmd(iface, 500, false);
+					rtw_indicate_disconnect(iface, 0, false);
+					rtw_free_assoc_resources(iface, 1);
+				}
+			}
+		}
+	}
+#endif /* CONFIG_CONCURRENT_MODE */
+
+exit:
+
+	if (connect_allow) {
+		RTW_INFO(FUNC_ADPT_FMT" union: %u,%u,%u\n", FUNC_ADPT_ARG(adapter), u_ch, u_bw, u_offset);
+		*ch = u_ch;
+		*bw = u_bw;
+		*offset = u_offset;
+	}
+
+	return connect_allow ? _SUCCESS : _FAIL;
+}
+
+
+u8 set_ch_hdl(_adapter *padapter, u8 *pbuf)
+{
+	struct set_ch_parm *set_ch_parm;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	set_ch_parm = (struct set_ch_parm *)pbuf;
+
+	RTW_INFO(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
+		 FUNC_NDEV_ARG(padapter->pnetdev),
+		 set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
+
+	pmlmeext->cur_channel = set_ch_parm->ch;
+	pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
+	pmlmeext->cur_bwmode = set_ch_parm->bw;
+
+	set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
+
+	return	H2C_SUCCESS;
+}
+
+u8 set_chplan_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct SetChannelPlan_param *setChannelPlan_param;
+	struct mlme_priv *mlme = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
+
+	if (!rtw_is_channel_plan_valid(setChannelPlan_param->channel_plan))
+		return H2C_PARAMETERS_ERROR;
+
+	mlme->country_ent = setChannelPlan_param->country_ent;
+	mlme->ChannelPlan = setChannelPlan_param->channel_plan;
+
+	pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+
+	rtw_hal_set_odm_var(padapter, HAL_ODM_REGULATION, NULL, true);
+
+#ifdef CONFIG_IOCTL_CFG80211
+	rtw_reg_notify_by_driver(padapter);
+#endif /* CONFIG_IOCTL_CFG80211 */
+
+	return	H2C_SUCCESS;
+}
+
+u8 led_blink_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+	struct LedBlink_param *ledBlink_param;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	ledBlink_param = (struct LedBlink_param *)pbuf;
+
+#ifdef CONFIG_LED_HANDLED_BY_CMD_THREAD
+	BlinkHandler((PLED_DATA)ledBlink_param->pLed);
+#endif
+
+	return	H2C_SUCCESS;
+}
+
+u8 set_csa_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+#ifdef CONFIG_DFS
+	struct SetChannelSwitch_param *setChannelSwitch_param;
+	u8 new_ch_no;
+	u8 gval8 = 0x00, sval8 = 0xff;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	setChannelSwitch_param = (struct SetChannelSwitch_param *)pbuf;
+	new_ch_no = setChannelSwitch_param->new_ch_no;
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &sval8);
+
+	RTW_INFO("DFS detected! Swiching channel to %d!\n", new_ch_no);
+	set_channel_bwmode(padapter, new_ch_no, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
+
+	rtw_disassoc_cmd(padapter, 0, false);
+	rtw_indicate_disconnect(padapter, 0, false);
+	rtw_free_assoc_resources(padapter, 1);
+	rtw_free_network_queue(padapter, true);
+
+	if (((new_ch_no >= 52) && (new_ch_no <= 64)) || ((new_ch_no >= 100) && (new_ch_no <= 140)))
+		RTW_INFO("Switched to DFS band (ch %02x) again!!\n", new_ch_no);
+
+	return	H2C_SUCCESS;
+#else
+	return	H2C_REJECTED;
+#endif /* CONFIG_DFS */
+
+}
+
+u8 tdls_hdl(_adapter *padapter, unsigned char *pbuf)
+{
+#ifdef CONFIG_TDLS
+	unsigned long irqL;
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+#ifdef CONFIG_TDLS_CH_SW
+	struct tdls_ch_switch *pchsw_info = &ptdlsinfo->chsw_info;
+#endif
+	struct TDLSoption_param *TDLSoption;
+	struct sta_info *ptdls_sta = NULL;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	u8 survey_channel, i, min, option;
+	struct tdls_txmgmt txmgmt;
+	u32 setchtime, resp_sleep = 0, wait_time;
+	u8 zaddr[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	u8 ret;
+	u8 doiqk;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	TDLSoption = (struct TDLSoption_param *)pbuf;
+	option = TDLSoption->option;
+
+	if (memcmp(TDLSoption->addr, zaddr, ETH_ALEN)) {
+		ptdls_sta = rtw_get_stainfo(&(padapter->stapriv), TDLSoption->addr);
+		if (ptdls_sta == NULL)
+			return H2C_REJECTED;
+	} else {
+		if (!(option == TDLS_RS_RCR))
+			return H2C_REJECTED;
+	}
+
+	/* _enter_critical_bh(&(ptdlsinfo->hdl_lock), &irqL); */
+	/* RTW_INFO("[%s] option:%d\n", __func__, option); */
+
+	switch (option) {
+	case TDLS_ESTABLISHED: {
+		/* As long as TDLS handshake success, we should set RCR_CBSSID_DATA bit to 0 */
+		/* So we can receive all kinds of data frames. */
+		u8 sta_band = 0;
+
+		/* leave ALL PS when TDLS is established */
+		rtw_pwr_wakeup(padapter);
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_WRCR, 0);
+		RTW_INFO("Created Direct Link with "MAC_FMT"\n", MAC_ARG(ptdls_sta->hwaddr));
+
+		/* Set TDLS sta rate. */
+		/* Update station supportRate */
+		rtw_hal_update_sta_rate_mask(padapter, ptdls_sta);
+		if (pmlmeext->cur_channel > 14) {
+			if (ptdls_sta->ra_mask & 0xffff000)
+				sta_band |= WIRELESS_11_5N ;
+
+			if (ptdls_sta->ra_mask & 0xff0)
+				sta_band |= WIRELESS_11A;
+		} else {
+			/* 5G band */
+			if (ptdls_sta->ra_mask & 0xffff000)
+				sta_band |= WIRELESS_11_24N;
+
+			if (ptdls_sta->ra_mask & 0xff0)
+				sta_band |= WIRELESS_11G;
+
+			if (ptdls_sta->ra_mask & 0x0f)
+				sta_band |= WIRELESS_11B;
+		}
+		ptdls_sta->wireless_mode = sta_band;
+		ptdls_sta->raid = rtw_hal_networktype_to_raid(padapter, ptdls_sta);
+		set_sta_rate(padapter, ptdls_sta);
+		rtw_sta_media_status_rpt(padapter, ptdls_sta, 1);
+		/* Sta mode */
+		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, ptdls_sta, true);
+		break;
+	}
+	case TDLS_ISSUE_PTI:
+		ptdls_sta->tdls_sta_state |= TDLS_WAIT_PTR_STATE;
+		issue_tdls_peer_traffic_indication(padapter, ptdls_sta);
+		_set_timer(&ptdls_sta->pti_timer, TDLS_PTI_TIME);
+		break;
+#ifdef CONFIG_TDLS_CH_SW
+	case TDLS_CH_SW_RESP:
+		memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+		txmgmt.status_code = 0;
+		memcpy(txmgmt.peer, ptdls_sta->hwaddr, ETH_ALEN);
+
+		issue_nulldata(padapter, NULL, 1, 3, 3);
+
+		RTW_INFO("[TDLS ] issue tdls channel switch response\n");
+		ret = issue_tdls_ch_switch_rsp(padapter, &txmgmt, true);
+
+		/* If we receive TDLS_CH_SW_REQ at off channel which it's target is AP's channel */
+		/* then we just switch to AP's channel*/
+		if (padapter->mlmeextpriv.cur_channel == pchsw_info->off_ch_num) {
+			rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_END_TO_BASE_CHNL);
+			break;
+		}
+
+		if (ret == _SUCCESS)
+			rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_TO_OFF_CHNL);
+		else
+			RTW_INFO("[TDLS] issue_tdls_ch_switch_rsp wait ack fail !!!!!!!!!!\n");
+
+		break;
+	case TDLS_CH_SW_PREPARE:
+		pchsw_info->ch_sw_state |= TDLS_CH_SWITCH_PREPARE_STATE;
+
+		/* to collect IQK info of off-chnl */
+		doiqk = true;
+		rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, &doiqk);
+		set_channel_bwmode(padapter, pchsw_info->off_ch_num, pchsw_info->ch_offset, (pchsw_info->ch_offset) ? CHANNEL_WIDTH_40 : CHANNEL_WIDTH_20);
+		doiqk = false;
+		rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, &doiqk);
+
+		/* switch back to base-chnl */
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+		rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_START);
+
+		pchsw_info->ch_sw_state &= ~(TDLS_CH_SWITCH_PREPARE_STATE);
+
+		break;
+	case TDLS_CH_SW_START:
+		rtw_tdls_set_ch_sw_oper_control(padapter, true);
+		break;
+	case TDLS_CH_SW_TO_OFF_CHNL:
+		issue_nulldata(padapter, NULL, 1, 3, 3);
+
+		if (!(pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE))
+			_set_timer(&ptdls_sta->ch_sw_timer, (u32)(ptdls_sta->ch_switch_timeout) / 1000);
+
+		if (rtw_tdls_do_ch_sw(padapter, ptdls_sta, TDLS_CH_SW_OFF_CHNL, pchsw_info->off_ch_num,
+			pchsw_info->ch_offset, (pchsw_info->ch_offset) ? CHANNEL_WIDTH_40 : CHANNEL_WIDTH_20, ptdls_sta->ch_switch_time) == _SUCCESS) {
+			pchsw_info->ch_sw_state &= ~(TDLS_PEER_AT_OFF_STATE);
+			if (pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE) {
+				if (issue_nulldata_to_TDLS_peer_STA(ptdls_sta->padapter, ptdls_sta->hwaddr, 0, 1, 3) == _FAIL)
+					rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_TO_BASE_CHNL);
+			}
+		} else {
+			u8 bcancelled;
+
+			if (!(pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE))
+				_cancel_timer(&ptdls_sta->ch_sw_timer, &bcancelled);
+		}
+
+
+		break;
+	case TDLS_CH_SW_END:
+	case TDLS_CH_SW_END_TO_BASE_CHNL:
+		rtw_tdls_set_ch_sw_oper_control(padapter, false);
+		_cancel_timer_ex(&ptdls_sta->ch_sw_timer);
+		_cancel_timer_ex(&ptdls_sta->stay_on_base_chnl_timer);
+		_cancel_timer_ex(&ptdls_sta->ch_sw_monitor_timer);
+		if (option == TDLS_CH_SW_END_TO_BASE_CHNL)
+			rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_TO_BASE_CHNL);
+
+		break;
+	case TDLS_CH_SW_TO_BASE_CHNL_UNSOLICITED:
+	case TDLS_CH_SW_TO_BASE_CHNL:
+		pchsw_info->ch_sw_state &= ~(TDLS_PEER_AT_OFF_STATE | TDLS_WAIT_CH_RSP_STATE);
+
+		if (option == TDLS_CH_SW_TO_BASE_CHNL_UNSOLICITED) {
+			if (ptdls_sta != NULL) {
+				/* Send unsolicited channel switch rsp. to peer */
+				memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+				txmgmt.status_code = 0;
+				memcpy(txmgmt.peer, ptdls_sta->hwaddr, ETH_ALEN);
+				issue_tdls_ch_switch_rsp(padapter, &txmgmt, false);
+			}
+		}
+
+		if (rtw_tdls_do_ch_sw(padapter, ptdls_sta, TDLS_CH_SW_BASE_CHNL, pmlmeext->cur_channel,
+			pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode, ptdls_sta->ch_switch_time) == _SUCCESS) {
+			issue_nulldata(padapter, NULL, 0, 3, 3);
+			/* set ch sw monitor timer for responder */
+			if (!(pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE))
+				_set_timer(&ptdls_sta->ch_sw_monitor_timer, TDLS_CH_SW_MONITOR_TIMEOUT);
+		}
+
+		break;
+#endif
+	case TDLS_RS_RCR:
+		rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_RS_RCR, 0);
+		RTW_INFO("[TDLS] write REG_RCR, set bit6 on\n");
+		break;
+	case TDLS_TEARDOWN_STA:
+		memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+		txmgmt.status_code = 0;
+		memcpy(txmgmt.peer, ptdls_sta->hwaddr, ETH_ALEN);
+
+		issue_tdls_teardown(padapter, &txmgmt, true);
+		break;
+	case TDLS_TEARDOWN_STA_LOCALLY:
+#ifdef CONFIG_TDLS_CH_SW
+		if (!memcmp(TDLSoption->addr, pchsw_info->addr, ETH_ALEN)) {
+			pchsw_info->ch_sw_state &= ~(TDLS_CH_SW_INITIATOR_STATE |
+						     TDLS_CH_SWITCH_ON_STATE |
+						     TDLS_PEER_AT_OFF_STATE);
+			rtw_tdls_set_ch_sw_oper_control(padapter, false);
+			memset(pchsw_info->addr, 0x00, ETH_ALEN);
+		}
+#endif
+		rtw_sta_media_status_rpt(padapter, ptdls_sta, 0);
+		free_tdls_sta(padapter, ptdls_sta);
+		break;
+	}
+
+	/* _exit_critical_bh(&(ptdlsinfo->hdl_lock), &irqL); */
+
+	return H2C_SUCCESS;
+#else
+	return H2C_REJECTED;
+#endif /* CONFIG_TDLS */
+
+}
+
+u8 run_in_thread_hdl(_adapter *padapter, u8 *pbuf)
+{
+	struct RunInThread_param *p;
+
+
+	if (NULL == pbuf)
+		return H2C_PARAMETERS_ERROR;
+	p = (struct RunInThread_param *)pbuf;
+
+	if (p->func)
+		p->func(p->context);
+
+	return H2C_SUCCESS;
+}
+
+u8 rtw_getmacreg_hdl(_adapter *padapter, u8 *pbuf)
+{
+
+	struct readMAC_parm *preadmacparm = NULL;
+	u8 sz = 0;
+	u32	addr = 0;
+	u32	value = 0;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	preadmacparm = (struct readMAC_parm *) pbuf;
+	sz = preadmacparm->len;
+	addr = preadmacparm->addr;
+	value = 0;
+
+	switch (sz) {
+	case 1:
+		value = rtw_read8(padapter, addr);
+		break;
+	case 2:
+		value = rtw_read16(padapter, addr);
+		break;
+	case 4:
+		value = rtw_read32(padapter, addr);
+		break;
+	default:
+		RTW_INFO("%s: Unknown size\n", __func__);
+		break;
+	}
+	RTW_INFO("%s: addr:0x%02x valeu:0x%02x\n", __func__, addr, value);
+
+	return H2C_SUCCESS;
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_mp.c b/drivers/staging/rtl8188eu/core/rtw_mp.c
new file mode 100644
index 000000000000..8697cd7b60b1
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_mp.c
@@ -0,0 +1,2620 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_MP_C_
+#include <drv_types.h>
+
+#include "../hal/phydm_precomp.h"
+
+#ifdef CONFIG_MP_VHT_HW_TX_MODE
+#define CEILING_POS(X) ((X - (int)(X)) > 0 ? (int)(X + 1) : (int)(X))
+#define CEILING_NEG(X) ((X - (int)(X)) < 0 ? (int)(X - 1) : (int)(X))
+#define ceil(X) (((X) > 0) ? CEILING_POS(X) : CEILING_NEG(X))
+
+int rtfloor(float x)
+{
+	int i = x - 2;
+	while
+	(++i <= x - 1)
+		;
+	return i;
+}
+#endif
+
+#ifdef CONFIG_MP_INCLUDED
+u32 read_macreg(_adapter *padapter, u32 addr, u32 sz)
+{
+	u32 val = 0;
+
+	switch (sz) {
+	case 1:
+		val = rtw_read8(padapter, addr);
+		break;
+	case 2:
+		val = rtw_read16(padapter, addr);
+		break;
+	case 4:
+		val = rtw_read32(padapter, addr);
+		break;
+	default:
+		val = 0xffffffff;
+		break;
+	}
+
+	return val;
+
+}
+
+void write_macreg(_adapter *padapter, u32 addr, u32 val, u32 sz)
+{
+	switch (sz) {
+	case 1:
+		rtw_write8(padapter, addr, (u8)val);
+		break;
+	case 2:
+		rtw_write16(padapter, addr, (u16)val);
+		break;
+	case 4:
+		rtw_write32(padapter, addr, val);
+		break;
+	default:
+		break;
+	}
+
+}
+
+u32 read_bbreg(_adapter *padapter, u32 addr, u32 bitmask)
+{
+	return rtw_hal_read_bbreg(padapter, addr, bitmask);
+}
+
+void write_bbreg(_adapter *padapter, u32 addr, u32 bitmask, u32 val)
+{
+	rtw_hal_write_bbreg(padapter, addr, bitmask, val);
+}
+
+u32 _read_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 bitmask)
+{
+	return rtw_hal_read_rfreg(padapter, rfpath, addr, bitmask);
+}
+
+void _write_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 bitmask, u32 val)
+{
+	rtw_hal_write_rfreg(padapter, rfpath, addr, bitmask, val);
+}
+
+u32 read_rfreg(PADAPTER padapter, u8 rfpath, u32 addr)
+{
+	return _read_rfreg(padapter, rfpath, addr, bRFRegOffsetMask);
+}
+
+void write_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 val)
+{
+	_write_rfreg(padapter, rfpath, addr, bRFRegOffsetMask, val);
+}
+
+static void _init_mp_priv_(struct mp_priv *pmp_priv)
+{
+	WLAN_BSSID_EX *pnetwork;
+
+	memset(pmp_priv, 0, sizeof(struct mp_priv));
+
+	pmp_priv->mode = MP_OFF;
+
+	pmp_priv->channel = 1;
+	pmp_priv->bandwidth = CHANNEL_WIDTH_20;
+	pmp_priv->prime_channel_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	pmp_priv->rateidx = 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_bssidpktcount = 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;
+
+	pmp_priv->bSetRxBssid = false;
+	pmp_priv->bRTWSmbCfg = false;
+	pmp_priv->bloopback = false;
+
+	pmp_priv->bloadefusemap = false;
+
+	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);
+
+	pmp_priv->tx.payload = 2;
+	pmp_priv->tx.attrib.ht_en = 1;
+}
+
+static int init_mp_priv_by_os(struct mp_priv *pmp_priv)
+{
+	int i, res;
+	struct mp_xmit_frame *pmp_xmitframe;
+
+	if (pmp_priv == NULL)
+		return _FAIL;
+
+	_rtw_init_queue(&pmp_priv->free_mp_xmitqueue);
+
+	pmp_priv->pallocated_mp_xmitframe_buf = NULL;
+	pmp_priv->pallocated_mp_xmitframe_buf = rtw_zmalloc(NR_MP_XMITFRAME * sizeof(struct mp_xmit_frame) + 4);
+	if (pmp_priv->pallocated_mp_xmitframe_buf == NULL) {
+		res = _FAIL;
+		goto _exit_init_mp_priv;
+	}
+
+	pmp_priv->pmp_xmtframe_buf = pmp_priv->pallocated_mp_xmitframe_buf + 4 - ((SIZE_PTR)(pmp_priv->pallocated_mp_xmitframe_buf) & 3);
+
+	pmp_xmitframe = (struct mp_xmit_frame *)pmp_priv->pmp_xmtframe_buf;
+
+	for (i = 0; i < NR_MP_XMITFRAME; i++) {
+		INIT_LIST_HEAD(&pmp_xmitframe->list);
+		list_add_tail(&pmp_xmitframe->list, &pmp_priv->free_mp_xmitqueue.queue);
+
+		pmp_xmitframe->pkt = NULL;
+		pmp_xmitframe->frame_tag = MP_FRAMETAG;
+		pmp_xmitframe->padapter = pmp_priv->papdater;
+
+		pmp_xmitframe++;
+	}
+
+	pmp_priv->free_mp_xmitframe_cnt = NR_MP_XMITFRAME;
+
+	res = _SUCCESS;
+
+_exit_init_mp_priv:
+
+	return res;
+}
+
+static void mp_init_xmit_attrib(struct mp_tx *pmptx, PADAPTER padapter)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+
+	struct pkt_attrib *pattrib;
+
+	/* init xmitframe attribute */
+	pattrib = &pmptx->attrib;
+	memset(pattrib, 0, sizeof(struct pkt_attrib));
+	memset(pmptx->desc, 0, TXDESC_SIZE);
+
+	pattrib->ether_type = 0x8712;
+	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;
+
+	pattrib->pktlen = 1500;
+}
+
+s32 init_mp_priv(PADAPTER padapter)
+{
+	struct mp_priv *pmppriv = &padapter->mppriv;
+	PHAL_DATA_TYPE pHalData;
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	_init_mp_priv_(pmppriv);
+	pmppriv->papdater = padapter;
+	pmppriv->mp_dm = 0;
+	pmppriv->tx.stop = 1;
+	pmppriv->bSetTxPower = 0;		/*for  manually set tx power*/
+	pmppriv->bTxBufCkFail = false;
+	pmppriv->pktInterval = 0;
+	pmppriv->pktLength = 1000;
+
+	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_BC;
+		pmppriv->antenna_rx = ANTENNA_ABCD;
+		break;
+	}
+
+	pHalData->AntennaRxPath = pmppriv->antenna_rx;
+	pHalData->antenna_tx_path = pmppriv->antenna_tx;
+
+	return _SUCCESS;
+}
+
+void free_mp_priv(struct mp_priv *pmp_priv)
+{
+	if (pmp_priv->pallocated_mp_xmitframe_buf) {
+		rtw_mfree(pmp_priv->pallocated_mp_xmitframe_buf, 0);
+		pmp_priv->pallocated_mp_xmitframe_buf = NULL;
+	}
+	pmp_priv->pmp_xmtframe_buf = NULL;
+}
+
+
+static void PHY_IQCalibrate_default(
+	PADAPTER	pAdapter,
+	bool	bReCovery
+)
+{
+	RTW_INFO("%s\n", __func__);
+}
+
+static void PHY_LCCalibrate_default(
+	PADAPTER	pAdapter
+)
+{
+	RTW_INFO("%s\n", __func__);
+}
+
+static void PHY_SetRFPathSwitch_default(
+	PADAPTER	pAdapter,
+	bool		bMain
+)
+{
+	RTW_INFO("%s\n", __func__);
+}
+
+
+static void mpt_InitHWConfig(PADAPTER Adapter)
+{
+}
+
+static void PHY_IQCalibrate(PADAPTER padapter, u8 bReCovery)
+{
+	PHAL_DATA_TYPE pHalData;
+	u8 b2ant;	/* false:1ant, true:2-ant */
+	u8 RF_Path;	/* 0:S1, 1:S0 */
+
+	if (IS_HARDWARE_TYPE_8188E(padapter))
+		phy_iq_calibrate_8188e(padapter, bReCovery);
+}
+
+static void PHY_LCCalibrate(PADAPTER padapter)
+{
+	if (IS_HARDWARE_TYPE_8188E(padapter))
+		phy_lc_calibrate_8188e(&(GET_HAL_DATA(padapter)->odmpriv));
+}
+
+static u8 PHY_QueryRFPathSwitch(PADAPTER padapter)
+{
+	return 0;
+}
+
+static void  PHY_SetRFPathSwitch(PADAPTER padapter , bool bMain) {
+
+	if (IS_HARDWARE_TYPE_8188E(padapter))
+		phy_set_rf_path_switch_8188e(padapter, bMain);
+}
+
+s32
+MPT_InitializeAdapter(
+	PADAPTER			pAdapter,
+	u8				Channel
+)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(pAdapter);
+	s32		rtStatus = _SUCCESS;
+	PMPT_CONTEXT	pMptCtx = &pAdapter->mppriv.mpt_ctx;
+	u32		ledsetting;
+	struct mlme_priv *pmlmepriv = &pAdapter->mlmepriv;
+
+	pMptCtx->bMptDrvUnload = false;
+	pMptCtx->bMassProdTest = false;
+	pMptCtx->bMptIndexEven = true;	/* default gain index is -6.0db */
+	pMptCtx->h2cReqNum = 0x0;
+	/* init for BT MP */
+
+	mpt_InitHWConfig(pAdapter);
+
+	pMptCtx->bMptWorkItemInProgress = false;
+	pMptCtx->CurrMptAct = NULL;
+	pMptCtx->mpt_rf_path = ODM_RF_PATH_A;
+	/* ------------------------------------------------------------------------- */
+	/* Don't accept any packets */
+	rtw_write32(pAdapter, REG_RCR, 0);
+
+	ledsetting = rtw_read32(pAdapter, REG_LEDCFG0);
+
+	PHY_LCCalibrate(pAdapter);
+	PHY_IQCalibrate(pAdapter, false);
+
+	PHY_SetRFPathSwitch(pAdapter, 1/*pHalData->bDefaultAntenna*/); /* default use Main */
+
+	pMptCtx->backup0xc50 = (u8)phy_query_bb_reg(pAdapter, rOFDM0_XAAGCCore1, bMaskByte0);
+	pMptCtx->backup0xc58 = (u8)phy_query_bb_reg(pAdapter, rOFDM0_XBAGCCore1, bMaskByte0);
+	pMptCtx->backup0xc30 = (u8)phy_query_bb_reg(pAdapter, rOFDM0_RxDetector1, bMaskByte0);
+	pMptCtx->backup0x52_RF_A = (u8)phy_query_rf_reg(pAdapter, RF_PATH_A, RF_0x52, 0x000F0);
+	pMptCtx->backup0x52_RF_B = (u8)phy_query_rf_reg(pAdapter, RF_PATH_B, RF_0x52, 0x000F0);
+	rtw_write32(pAdapter, REG_MACID_NO_LINK_0, 0x0);
+	rtw_write32(pAdapter, REG_MACID_NO_LINK_1, 0x0);
+	return	rtStatus;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	MPT_DeInitAdapter()
+ *
+ * Overview:	Extra DeInitialization for Mass Production Test.
+ *
+ * Input:		PADAPTER	pAdapter
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	05/08/2007	MHC		Create Version 0.
+ *	05/18/2007	MHC		Add normal driver MPHalt code.
+ *
+ *---------------------------------------------------------------------------*/
+void
+MPT_DeInitAdapter(
+	PADAPTER	pAdapter
+)
+{
+	PMPT_CONTEXT		pMptCtx = &pAdapter->mppriv.mpt_ctx;
+
+	pMptCtx->bMptDrvUnload = true;
+}
+
+static u8 mpt_ProStartTest(PADAPTER padapter)
+{
+	PMPT_CONTEXT pMptCtx = &padapter->mppriv.mpt_ctx;
+
+	pMptCtx->bMassProdTest = true;
+	pMptCtx->is_start_cont_tx = false;
+	pMptCtx->bCckContTx = false;
+	pMptCtx->bOfdmContTx = false;
+	pMptCtx->bSingleCarrier = false;
+	pMptCtx->is_carrier_suppression = false;
+	pMptCtx->is_single_tone = false;
+	pMptCtx->HWTxmode = PACKETS_TX;
+
+	return _SUCCESS;
+}
+
+/*
+ * General use
+ */
+s32 SetPowerTracking(PADAPTER padapter, u8 enable)
+{
+	hal_mpt_SetPowerTracking(padapter, enable);
+	return 0;
+}
+
+void GetPowerTracking(PADAPTER padapter, u8 *enable)
+{
+	hal_mpt_GetPowerTracking(padapter, enable);
+}
+
+void rtw_mp_trigger_iqk(PADAPTER padapter)
+{
+	PHY_IQCalibrate(padapter, false);
+}
+
+void rtw_mp_trigger_lck(PADAPTER padapter)
+{
+	PHY_LCCalibrate(padapter);
+}
+
+static void disable_dm(PADAPTER padapter)
+{
+	u8 v8;
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct PHY_DM_STRUCT		*pDM_Odm = &pHalData->odmpriv;
+
+	/* 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 */
+	rtw_phydm_func_disable_all(padapter);
+
+	/* enable APK, LCK and IQK but disable power tracking */
+	pDM_Odm->rf_calibrate_info.txpowertrack_control = false;
+	rtw_phydm_func_set(padapter, ODM_RF_CALIBRATION);
+
+	/* #ifdef CONFIG_BT_COEXIST */
+	/* rtw_btcoex_Switch(padapter, 0); */ /* remove for BT MP Down. */
+	/* #endif */
+}
+
+
+void MPT_PwrCtlDM(PADAPTER padapter, u32 bstart)
+{
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct PHY_DM_STRUCT		*pDM_Odm = &pHalData->odmpriv;
+
+	if (bstart == 1) {
+		RTW_INFO("in MPT_PwrCtlDM start\n");
+		rtw_phydm_func_set(padapter, ODM_RF_TX_PWR_TRACK | ODM_RF_CALIBRATION);
+
+		pDM_Odm->rf_calibrate_info.txpowertrack_control = true;
+		padapter->mppriv.mp_dm = 1;
+
+	} else {
+		RTW_INFO("in MPT_PwrCtlDM stop\n");
+		disable_dm(padapter);
+		pDM_Odm->rf_calibrate_info.txpowertrack_control = false;
+		padapter->mppriv.mp_dm = 0;
+		{
+			struct _TXPWRTRACK_CFG	c;
+			u8	chnl = 0 ;
+			memset(&c, 0, sizeof(struct _TXPWRTRACK_CFG));
+			configure_txpower_track(pDM_Odm, &c);
+			odm_clear_txpowertracking_state(pDM_Odm);
+			if (*c.odm_tx_pwr_track_set_pwr) {
+				if (pDM_Odm->support_ic_type == ODM_RTL8188F)
+					(*c.odm_tx_pwr_track_set_pwr)(pDM_Odm, MIX_MODE, ODM_RF_PATH_A, chnl);
+				else if (pDM_Odm->support_ic_type == ODM_RTL8723D) {
+					(*c.odm_tx_pwr_track_set_pwr)(pDM_Odm, BBSWING, ODM_RF_PATH_A, chnl);
+					SetTxPower(padapter);
+				} else {
+					(*c.odm_tx_pwr_track_set_pwr)(pDM_Odm, BBSWING, ODM_RF_PATH_A, chnl);
+					(*c.odm_tx_pwr_track_set_pwr)(pDM_Odm, BBSWING, ODM_RF_PATH_B, chnl);
+				}
+			}
+		}
+	}
+
+}
+
+
+u32 mp_join(PADAPTER padapter, u8 mode)
+{
+	WLAN_BSSID_EX bssid;
+	struct sta_info *psta;
+	u32 length;
+	u8 val8, join_type;
+	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;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*pnetwork = (WLAN_BSSID_EX *)(&(pmlmeinfo->network));
+
+#ifdef CONFIG_IOCTL_CFG80211
+	struct wireless_dev *pwdev = padapter->rtw_wdev;
+#endif /* #ifdef CONFIG_IOCTL_CFG80211 */
+	/* 1. initialize a new WLAN_BSSID_EX */
+	memset(&bssid, 0, sizeof(WLAN_BSSID_EX));
+	RTW_INFO("%s ,pmppriv->network_macaddr=%x %x %x %x %x %x\n", __func__,
+		pmppriv->network_macaddr[0], pmppriv->network_macaddr[1], pmppriv->network_macaddr[2], pmppriv->network_macaddr[3], pmppriv->network_macaddr[4],
+		 pmppriv->network_macaddr[5]);
+	memcpy(bssid.MacAddress, pmppriv->network_macaddr, ETH_ALEN);
+
+	if (mode == WIFI_FW_ADHOC_STATE) {
+		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;
+		bssid.Configuration.DSConfig = pmppriv->channel;
+
+	} else if (mode == WIFI_FW_STATION_STATE) {
+		bssid.Ssid.SsidLength = strlen("mp_pseudo_STATION");
+		memcpy(bssid.Ssid.Ssid, (u8 *)"mp_pseudo_STATION", bssid.Ssid.SsidLength);
+		bssid.InfrastructureMode = Ndis802_11Infrastructure;
+		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, 0, false);
+		rtw_free_assoc_resources(padapter, 1);
+	}
+	pmppriv->prev_fw_state = get_fwstate(pmlmepriv);
+	/*pmlmepriv->fw_state = WIFI_MP_STATE;*/
+	init_fwstate(pmlmepriv, 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) {
+		/*pmlmepriv->fw_state = pmppriv->prev_fw_state;*/
+		init_fwstate(pmlmepriv, pmppriv->prev_fw_state);
+		res = _FAIL;
+		goto end_of_mp_start_test;
+	}
+	if (mode == WIFI_FW_ADHOC_STATE)
+	set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+	else
+		set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+	/* 3 3. join psudo AdHoc */
+	tgt_network->join_res = 1;
+	tgt_network->aid = psta->aid = 1;
+
+	memcpy(&padapter->registrypriv.dev_network, &bssid, length);
+	rtw_update_registrypriv_dev_network(padapter);
+	memcpy(&tgt_network->network, &padapter->registrypriv.dev_network, padapter->registrypriv.dev_network.Length);
+	memcpy(pnetwork, &padapter->registrypriv.dev_network, padapter->registrypriv.dev_network.Length);
+
+	rtw_indicate_connect(padapter);
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+	set_fwstate(pmlmepriv, _FW_LINKED);
+
+end_of_mp_start_test:
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+	if (1) { /* (res == _SUCCESS) */
+		/* set MSR to WIFI_FW_ADHOC_STATE */
+		if (mode == WIFI_FW_ADHOC_STATE) {
+			/* set msr to WIFI_FW_ADHOC_STATE */
+			pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+			Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+			rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
+			join_type = 0;
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+			report_join_res(padapter, 1);
+			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+		} else {
+			Set_MSR(padapter, WIFI_FW_STATION_STATE);
+
+			RTW_INFO("%s , pmppriv->network_macaddr =%x %x %x %x %x %x\n", __func__,
+				pmppriv->network_macaddr[0], pmppriv->network_macaddr[1], pmppriv->network_macaddr[2], pmppriv->network_macaddr[3], pmppriv->network_macaddr[4],
+				 pmppriv->network_macaddr[5]);
+
+			rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmppriv->network_macaddr);
+		}
+	}
+
+	return res;
+}
+/* This function initializes the DUT to the MP test mode */
+s32 mp_start_test(PADAPTER padapter)
+{
+	struct mp_priv *pmppriv = &padapter->mppriv;
+	s32 res = _SUCCESS;
+
+	padapter->registrypriv.mp_mode = 1;
+
+	/* 3 disable dynamic mechanism */
+	disable_dm(padapter);
+	rtl8188e_InitHalDm(padapter);
+	/* 3 0. update mp_priv */
+
+	if (!RF_TYPE_VALID(padapter->registrypriv.rf_config)) {
+		/*		switch (phal->rf_type) { */
+		switch (GET_RF_TYPE(padapter)) {
+		case RF_1T1R:
+			pmppriv->antenna_tx = ANTENNA_A;
+			pmppriv->antenna_rx = ANTENNA_A;
+			break;
+		case RF_1T2R:
+		default:
+			pmppriv->antenna_tx = ANTENNA_A;
+			pmppriv->antenna_rx = ANTENNA_AB;
+			break;
+		case RF_2T2R:
+		case RF_2T2R_GREEN:
+			pmppriv->antenna_tx = ANTENNA_AB;
+			pmppriv->antenna_rx = ANTENNA_AB;
+			break;
+		case RF_2T4R:
+			pmppriv->antenna_tx = ANTENNA_AB;
+			pmppriv->antenna_rx = ANTENNA_ABCD;
+			break;
+		}
+	}
+
+	mpt_ProStartTest(padapter);
+
+	mp_join(padapter, WIFI_FW_ADHOC_STATE);
+
+	return res;
+}
+/* ------------------------------------------------------------------------------
+ * This function change the DUT from the MP test mode into normal mode */
+void mp_stop_test(PADAPTER padapter)
+{
+	struct mp_priv *pmppriv = &padapter->mppriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+	struct sta_info *psta;
+
+	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, 0, false);
+
+		/* 3 2. clear psta used in mp test mode.
+		*	rtw_free_assoc_resources(padapter, 1); */
+		psta = rtw_get_stainfo(&padapter->stapriv, tgt_network->network.MacAddress);
+		if (psta)
+			rtw_free_stainfo(padapter, psta);
+
+		/* 3 3. return to normal state (default:station mode) */
+		/*pmlmepriv->fw_state = pmppriv->prev_fw_state; */ /* WIFI_STATION_STATE;*/
+		init_fwstate(pmlmepriv, pmppriv->prev_fw_state);
+
+		/* flush the cur_network */
+		memset(tgt_network, 0, sizeof(struct wlan_network));
+
+		_clr_fwstate_(pmlmepriv, WIFI_MP_STATE);
+
+end_of_mp_stop_test:
+
+		_exit_critical_bh(&pmlmepriv->lock, &irqL);
+	}
+}
+/*---------------------------hal\rtl8192c\MPT_Phy.c---------------------------*/
+/*-----------------------------------------------------------------------------
+ * Function:	mpt_SwitchRfSetting
+ *
+ * Overview:	Change RF Setting when we siwthc channel/rate/BW for MP.
+ *
+ * Input:       PADAPTER				pAdapter
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 01/08/2009	MHC		Suggestion from SD3 Willis for 92S series.
+ * 01/09/2009	MHC		Add CCK modification for 40MHZ. Suggestion from SD3.
+ *
+ *---------------------------------------------------------------------------*/
+static void mpt_SwitchRfSetting(PADAPTER pAdapter)
+{
+	hal_mpt_SwitchRfSetting(pAdapter);
+}
+
+/*---------------------------hal\rtl8192c\MPT_Phy.c---------------------------*/
+/*---------------------------hal\rtl8192c\MPT_HelperFunc.c---------------------------*/
+static void MPT_CCKTxPowerAdjust(PADAPTER Adapter, bool bInCH14)
+{
+	hal_mpt_CCKTxPowerAdjust(Adapter, bInCH14);
+}
+
+/*---------------------------hal\rtl8192c\MPT_HelperFunc.c---------------------------*/
+
+/*
+ * SetChannel
+ * Description
+ *	Use H2C command to change channel,
+ *	not only modify rf register, but also other setting need to be done.
+ */
+void SetChannel(PADAPTER pAdapter)
+{
+	hal_mpt_SetChannel(pAdapter);
+}
+
+/*
+ * Notice
+ *	Switch bandwitdth may change center frequency(channel)
+ */
+void SetBandwidth(PADAPTER pAdapter)
+{
+	hal_mpt_SetBandwidth(pAdapter);
+
+}
+
+void SetAntenna(PADAPTER pAdapter)
+{
+	hal_mpt_SetAntenna(pAdapter);
+}
+
+int SetTxPower(PADAPTER pAdapter)
+{
+
+	hal_mpt_SetTxPower(pAdapter);
+	return true;
+}
+
+static void SetTxAGCOffset(PADAPTER pAdapter, u32 ulTxAGCOffset)
+{
+	u32 TxAGCOffset_B, TxAGCOffset_C, TxAGCOffset_D, tmpAGC;
+
+	TxAGCOffset_B = (ulTxAGCOffset & 0x000000ff);
+	TxAGCOffset_C = ((ulTxAGCOffset & 0x0000ff00) >> 8);
+	TxAGCOffset_D = ((ulTxAGCOffset & 0x00ff0000) >> 16);
+
+	tmpAGC = (TxAGCOffset_D << 8 | TxAGCOffset_C << 4 | TxAGCOffset_B);
+	write_bbreg(pAdapter, rFPGA0_TxGainStage,
+		    (bXBTxAGC | bXCTxAGC | bXDTxAGC), tmpAGC);
+}
+
+void SetDataRate(PADAPTER pAdapter)
+{
+	hal_mpt_SetDataRate(pAdapter);
+}
+
+void MP_PHY_SetRFPathSwitch(PADAPTER pAdapter , bool bMain)
+{
+
+	PHY_SetRFPathSwitch(pAdapter, bMain);
+
+}
+
+u8 MP_PHY_QueryRFPathSwitch(PADAPTER pAdapter)
+{
+	return PHY_QueryRFPathSwitch(pAdapter);
+}
+
+s32 SetThermalMeter(PADAPTER pAdapter, u8 target_ther)
+{
+	return hal_mpt_SetThermalMeter(pAdapter, target_ther);
+}
+
+static void TriggerRFThermalMeter(PADAPTER pAdapter)
+{
+	hal_mpt_TriggerRFThermalMeter(pAdapter);
+}
+
+static u8 ReadRFThermalMeter(PADAPTER pAdapter)
+{
+	return hal_mpt_ReadRFThermalMeter(pAdapter);
+}
+
+void GetThermalMeter(PADAPTER pAdapter, u8 *value)
+{
+	hal_mpt_GetThermalMeter(pAdapter, value);
+}
+
+void SetSingleCarrierTx(PADAPTER pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	hal_mpt_SetSingleCarrierTx(pAdapter, bStart);
+}
+
+void SetSingleToneTx(PADAPTER pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	hal_mpt_SetSingleToneTx(pAdapter, bStart);
+}
+
+void SetCarrierSuppressionTx(PADAPTER pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	hal_mpt_SetCarrierSuppressionTx(pAdapter, bStart);
+}
+
+void SetContinuousTx(PADAPTER pAdapter, u8 bStart)
+{
+	PhySetTxPowerLevel(pAdapter);
+	hal_mpt_SetContinuousTx(pAdapter, bStart);
+}
+
+
+void PhySetTxPowerLevel(PADAPTER pAdapter)
+{
+	struct mp_priv *pmp_priv = &pAdapter->mppriv;
+
+
+	if (pmp_priv->bSetTxPower == 0) /* for NO manually set power index */
+		rtw_hal_set_tx_power_level(pAdapter, pmp_priv->channel);
+}
+
+/* ------------------------------------------------------------------------------ */
+static void dump_mpframe(PADAPTER padapter, struct xmit_frame *pmpframe)
+{
+	rtw_hal_mgnt_xmit(padapter, pmpframe);
+}
+
+static struct xmit_frame *alloc_mp_xmitframe(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_frame	*pmpframe;
+	struct xmit_buf	*pxmitbuf;
+
+	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 thread_return mp_xmit_packet_thread(thread_context context)
+{
+	struct xmit_frame	*pxmitframe;
+	struct mp_tx		*pmptx;
+	struct mp_priv	*pmp_priv;
+	struct xmit_priv	*pxmitpriv;
+	PADAPTER padapter;
+
+	pmp_priv = (struct mp_priv *)context;
+	pmptx = &pmp_priv->tx;
+	padapter = pmp_priv->papdater;
+	pxmitpriv = &(padapter->xmitpriv);
+
+	thread_enter("RTW_MP_THREAD");
+
+	RTW_INFO("%s:pkTx Start\n", __func__);
+	while (1) {
+		pxmitframe = alloc_mp_xmitframe(pxmitpriv);
+		if (pxmitframe == NULL) {
+			if (pmptx->stop ||
+			    RTW_CANNOT_RUN(padapter))
+				goto exit;
+			else {
+				rtw_usleep_os(10);
+				continue;
+			}
+		}
+		memcpy((u8 *)(pxmitframe->buf_addr + TXDESC_OFFSET), pmptx->buf, pmptx->write_size);
+		memcpy(&(pxmitframe->attrib), &(pmptx->attrib), sizeof(struct pkt_attrib));
+
+
+		rtw_usleep_os(padapter->mppriv.pktInterval);
+		dump_mpframe(padapter, pxmitframe);
+
+		pmptx->sended++;
+		pmp_priv->tx_pktcount++;
+
+		if (pmptx->stop ||
+		    RTW_CANNOT_RUN(padapter))
+			goto exit;
+		if ((pmptx->count != 0) &&
+		    (pmptx->count == pmptx->sended))
+			goto exit;
+
+		flush_signals_thread();
+	}
+
+exit:
+	/* RTW_INFO("%s:pkTx Exit\n", __func__); */
+	rtw_mfree(pmptx->pallocated_buf, pmptx->buf_size);
+	pmptx->pallocated_buf = NULL;
+	pmptx->stop = 1;
+
+	thread_exit();
+}
+
+void fill_txdesc_for_mp(PADAPTER padapter, u8 *ptxdesc)
+{
+	struct mp_priv *pmp_priv = &padapter->mppriv;
+	memcpy(ptxdesc, pmp_priv->tx.desc, TXDESC_SIZE);
+}
+
+static void fill_tx_desc_8188e(PADAPTER padapter)
+{
+	struct mp_priv *pmp_priv = &padapter->mppriv;
+	struct tx_desc *desc   = (struct tx_desc *)&(pmp_priv->tx.desc);
+	struct pkt_attrib *pattrib = &(pmp_priv->tx.attrib);
+	u32	pkt_size = pattrib->last_txcmdsz;
+	s32 bmcast = IS_MCAST(pattrib->ra);
+	/* 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);
+
+	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 */
+	/* desc->txdw2 |= cpu_to_le32(AGG_BK); */ /* AGG BK */
+
+	desc->txdw3 |= cpu_to_le32((pattrib->seqnum << 16) & 0x0fff0000);
+	desc->txdw4 |= cpu_to_le32(HW_SSN);
+
+	desc->txdw4 |= cpu_to_le32(USERATE);
+	desc->txdw4 |= cpu_to_le32(DISDATAFB);
+
+	if (pmp_priv->preamble) {
+		if (HwRateToMPTRate(pmp_priv->rateidx) <=  MPT_RATE_54M)
+			desc->txdw4 |= cpu_to_le32(DATA_SHORT); /* CCK Short Preamble */
+	}
+
+	if (pmp_priv->bandwidth == 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 (HwRateToMPTRate(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	 */
+
+
+}
+
+static void Rtw_MPSetMacTxEDCA(PADAPTER padapter)
+{
+	rtw_write32(padapter, 0x508 , 0x00a422); /* Disable EDCA BE Txop for MP pkt tx adjust Packet interval */
+	phy_set_mac_reg(padapter, 0x458 , bMaskDWord , 0x0);
+	phy_set_mac_reg(padapter, 0x460 , bMaskLWord , 0x0); /* fast EDCA queue packet interval & time out value*/
+	RTW_INFO("%s()!!!!! 0x460 = 0x%x\n" , __func__, phy_query_bb_reg(padapter, 0x460, bMaskDWord));
+}
+
+void SetPacketTx(PADAPTER padapter)
+{
+	u8 *ptr, *pkt_start, *pkt_end, *fctrl;
+	u32 pkt_size, offset, startPlace, i;
+	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, adapter_mac_addr(padapter), 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->mbssid = 0;
+
+	pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->pktlen;
+
+	/* 3 2. allocate xmit buffer */
+	pkt_size = pattrib->last_txcmdsz;
+
+	if (pmp_priv->tx.pallocated_buf)
+		rtw_mfree(pmp_priv->tx.pallocated_buf, pmp_priv->tx.buf_size);
+	pmp_priv->tx.write_size = pkt_size;
+	pmp_priv->tx.buf_size = pkt_size + XMITBUF_ALIGN_SZ;
+	pmp_priv->tx.pallocated_buf = rtw_zmalloc(pmp_priv->tx.buf_size);
+	if (pmp_priv->tx.pallocated_buf == NULL) {
+		RTW_INFO("%s: malloc(%d) fail!!\n", __func__, pmp_priv->tx.buf_size);
+		return;
+	}
+	pmp_priv->tx.buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pmp_priv->tx.pallocated_buf), XMITBUF_ALIGN_SZ);
+	ptr = pmp_priv->tx.buf;
+
+	memset(pmp_priv->tx.desc, 0, TXDESC_SIZE);
+	pkt_start = ptr;
+	pkt_end = pkt_start + pkt_size;
+
+	/* 3 3. init TX descriptor */
+	if (IS_HARDWARE_TYPE_8188E(padapter))
+		fill_tx_desc_8188e(padapter);
+
+	/* 3 4. make wlan header, make_wlanhdr() */
+	hdr = (struct rtw_ieee80211_hdr *)pkt_start;
+	set_frame_sub_type(&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;
+	}
+	pmp_priv->TXradomBuffer = rtw_zmalloc(4096);
+	if (pmp_priv->TXradomBuffer == NULL) {
+		RTW_INFO("mp create random buffer fail!\n");
+		goto exit;
+	}
+
+
+	for (i = 0; i < 4096; i++)
+		pmp_priv->TXradomBuffer[i] = rtw_random32() % 0xFF;
+
+	/* startPlace = (u32)(rtw_random32() % 3450); */
+	memcpy(ptr, pmp_priv->TXradomBuffer, pkt_end - ptr);
+	/* memset(ptr, payload, pkt_end - ptr); */
+	rtw_mfree(pmp_priv->TXradomBuffer, 4096);
+
+	/* 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))
+		RTW_INFO("Create PktTx Thread Fail !!!!!\n");
+
+	Rtw_MPSetMacTxEDCA(padapter);
+exit:
+	return;
+}
+
+void SetPacketRx(PADAPTER pAdapter, u8 bStartRx, u8 bAB)
+{
+	PHAL_DATA_TYPE pHalData = GET_HAL_DATA(pAdapter);
+	struct mp_priv *pmppriv = &pAdapter->mppriv;
+
+
+	if (bStartRx) {
+		pHalData->ReceiveConfig = RCR_AAP | RCR_APM | RCR_AM | RCR_AMF | RCR_HTC_LOC_CTRL;
+		pHalData->ReceiveConfig |= RCR_ACRC32;
+		pHalData->ReceiveConfig |= RCR_APP_PHYST_RXFF | RCR_APP_ICV | RCR_APP_MIC;
+
+		if (pmppriv->bSetRxBssid == true) {
+			RTW_INFO("%s: pmppriv->network_macaddr=" MAC_FMT "\n", __func__,
+				 MAC_ARG(pmppriv->network_macaddr));
+			pHalData->ReceiveConfig = 0;
+			pHalData->ReceiveConfig |= RCR_CBSSID_DATA | RCR_CBSSID_BCN |RCR_APM | RCR_AM | RCR_AB |RCR_AMF;
+
+			rtw_write16(pAdapter, REG_RXFLTMAP0, 0xFFEF); /* REG_RXFLTMAP0 (RX Filter Map Group 0) */
+		} else {
+			pHalData->ReceiveConfig |= RCR_ADF;
+			/* Accept all data frames */
+			rtw_write16(pAdapter, REG_RXFLTMAP2, 0xFFFF);
+		}
+
+		if (bAB)
+			pHalData->ReceiveConfig |= RCR_AB;
+	} else {
+		pHalData->ReceiveConfig = 0;
+		rtw_write16(pAdapter, REG_RXFLTMAP0, 0xFFFF); /* REG_RXFLTMAP0 (RX Filter Map Group 0) */
+	}
+
+	rtw_write32(pAdapter, REG_RCR, pHalData->ReceiveConfig);
+}
+
+void ResetPhyRxPktCount(PADAPTER pAdapter)
+{
+	u32 i, phyrx_set = 0;
+
+	for (i = 0; i <= 0xF; i++) {
+		phyrx_set = 0;
+		phyrx_set |= _RXERR_RPT_SEL(i);	/* select */
+		phyrx_set |= RXERR_RPT_RST;	/* set counter to zero */
+		rtw_write32(pAdapter, REG_RXERR_RPT, phyrx_set);
+	}
+}
+
+static u32 GetPhyRxPktCounts(PADAPTER pAdapter, u32 selbit)
+{
+	/* selection */
+	u32 phyrx_set = 0, count = 0;
+
+	phyrx_set = _RXERR_RPT_SEL(selbit & 0xF);
+	rtw_write32(pAdapter, REG_RXERR_RPT, phyrx_set);
+
+	/* Read packet count */
+	count = rtw_read32(pAdapter, REG_RXERR_RPT) & RXERR_COUNTER_MASK;
+
+	return count;
+}
+
+u32 GetPhyRxPktReceived(PADAPTER pAdapter)
+{
+	u32 OFDM_cnt = 0, CCK_cnt = 0, HT_cnt = 0;
+
+	OFDM_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_OFDM_MPDU_OK);
+	CCK_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_CCK_MPDU_OK);
+	HT_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_HT_MPDU_OK);
+
+	return OFDM_cnt + CCK_cnt + HT_cnt;
+}
+
+u32 GetPhyRxPktCRC32Error(PADAPTER pAdapter)
+{
+	u32 OFDM_cnt = 0, CCK_cnt = 0, HT_cnt = 0;
+
+	OFDM_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_OFDM_MPDU_FAIL);
+	CCK_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_CCK_MPDU_FAIL);
+	HT_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_HT_MPDU_FAIL);
+
+	return OFDM_cnt + CCK_cnt + HT_cnt;
+}
+
+/* reg 0x808[9:0]: FFT data x
+ * reg 0x808[22]:  0  -->  1  to get 1 FFT data y
+ * reg 0x8B4[15:0]: FFT data y report */
+static u32 rtw_GetPSDData(PADAPTER pAdapter, u32 point)
+{
+	u32 psd_val = 0;
+	u16 psd_reg = 0x808;
+	u16 psd_regL = 0x8B4;
+
+	psd_val = rtw_read32(pAdapter, psd_reg);
+
+	psd_val &= 0xFFBFFC00;
+	psd_val |= point;
+
+	rtw_write32(pAdapter, psd_reg, psd_val);
+	rtw_mdelay_os(1);
+	psd_val |= 0x00400000;
+
+	rtw_write32(pAdapter, psd_reg, psd_val);
+	rtw_mdelay_os(1);
+
+	psd_val = rtw_read32(pAdapter, psd_regL);
+	psd_val &= 0x0000FFFF;
+
+	return psd_val;
+}
+
+/*
+ * pts	start_point_min		stop_point_max
+ * 128	64			64 + 128 = 192
+ * 256	128			128 + 256 = 384
+ * 512	256			256 + 512 = 768
+ * 1024	512			512 + 1024 = 1536
+ *
+ */
+u32 mp_query_psd(PADAPTER pAdapter, u8 *data)
+{
+	u32 i, psd_pts = 0, psd_start = 0, psd_stop = 0;
+	u32 psd_data = 0;
+
+
+	if (!netif_running(pAdapter->pnetdev)) {
+		return 0;
+	}
+
+	if (check_fwstate(&pAdapter->mlmepriv, WIFI_MP_STATE) == false) {
+		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);
+
+	data[0] = '\0';
+
+	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 + strlen(data), "%x ", psd_data);
+		i++;
+	}
+
+#ifdef CONFIG_LONG_DELAY_ISSUE
+	rtw_msleep_os(100);
+#else
+	rtw_mdelay_os(100);
+#endif
+
+	return strlen(data) + 1;
+}
+
+u8
+mpt_to_mgnt_rate(
+	u32	MptRateIdx
+)
+{
+	/* Mapped to MGN_XXX defined in MgntGen.h */
+	switch (MptRateIdx) {
+	/* CCK rate. */
+	case	MPT_RATE_1M:
+		return MGN_1M;
+	case	MPT_RATE_2M:
+		return MGN_2M;
+	case	MPT_RATE_55M:
+		return MGN_5_5M;
+	case	MPT_RATE_11M:
+		return MGN_11M;
+
+	/* OFDM rate. */
+	case	MPT_RATE_6M:
+		return MGN_6M;
+	case	MPT_RATE_9M:
+		return MGN_9M;
+	case	MPT_RATE_12M:
+		return MGN_12M;
+	case	MPT_RATE_18M:
+		return MGN_18M;
+	case	MPT_RATE_24M:
+		return MGN_24M;
+	case	MPT_RATE_36M:
+		return MGN_36M;
+	case	MPT_RATE_48M:
+		return MGN_48M;
+	case	MPT_RATE_54M:
+		return MGN_54M;
+
+	/* HT rate. */
+	case	MPT_RATE_MCS0:
+		return MGN_MCS0;
+	case	MPT_RATE_MCS1:
+		return MGN_MCS1;
+	case	MPT_RATE_MCS2:
+		return MGN_MCS2;
+	case	MPT_RATE_MCS3:
+		return MGN_MCS3;
+	case	MPT_RATE_MCS4:
+		return MGN_MCS4;
+	case	MPT_RATE_MCS5:
+		return MGN_MCS5;
+	case	MPT_RATE_MCS6:
+		return MGN_MCS6;
+	case	MPT_RATE_MCS7:
+		return MGN_MCS7;
+	case	MPT_RATE_MCS8:
+		return MGN_MCS8;
+	case	MPT_RATE_MCS9:
+		return MGN_MCS9;
+	case	MPT_RATE_MCS10:
+		return MGN_MCS10;
+	case	MPT_RATE_MCS11:
+		return MGN_MCS11;
+	case	MPT_RATE_MCS12:
+		return MGN_MCS12;
+	case	MPT_RATE_MCS13:
+		return MGN_MCS13;
+	case	MPT_RATE_MCS14:
+		return MGN_MCS14;
+	case	MPT_RATE_MCS15:
+		return MGN_MCS15;
+	case	MPT_RATE_MCS16:
+		return MGN_MCS16;
+	case	MPT_RATE_MCS17:
+		return MGN_MCS17;
+	case	MPT_RATE_MCS18:
+		return MGN_MCS18;
+	case	MPT_RATE_MCS19:
+		return MGN_MCS19;
+	case	MPT_RATE_MCS20:
+		return MGN_MCS20;
+	case	MPT_RATE_MCS21:
+		return MGN_MCS21;
+	case	MPT_RATE_MCS22:
+		return MGN_MCS22;
+	case	MPT_RATE_MCS23:
+		return MGN_MCS23;
+	case	MPT_RATE_MCS24:
+		return MGN_MCS24;
+	case	MPT_RATE_MCS25:
+		return MGN_MCS25;
+	case	MPT_RATE_MCS26:
+		return MGN_MCS26;
+	case	MPT_RATE_MCS27:
+		return MGN_MCS27;
+	case	MPT_RATE_MCS28:
+		return MGN_MCS28;
+	case	MPT_RATE_MCS29:
+		return MGN_MCS29;
+	case	MPT_RATE_MCS30:
+		return MGN_MCS30;
+	case	MPT_RATE_MCS31:
+		return MGN_MCS31;
+
+	/* VHT rate. */
+	case	MPT_RATE_VHT1SS_MCS0:
+		return MGN_VHT1SS_MCS0;
+	case	MPT_RATE_VHT1SS_MCS1:
+		return MGN_VHT1SS_MCS1;
+	case	MPT_RATE_VHT1SS_MCS2:
+		return MGN_VHT1SS_MCS2;
+	case	MPT_RATE_VHT1SS_MCS3:
+		return MGN_VHT1SS_MCS3;
+	case	MPT_RATE_VHT1SS_MCS4:
+		return MGN_VHT1SS_MCS4;
+	case	MPT_RATE_VHT1SS_MCS5:
+		return MGN_VHT1SS_MCS5;
+	case	MPT_RATE_VHT1SS_MCS6:
+		return MGN_VHT1SS_MCS6;
+	case	MPT_RATE_VHT1SS_MCS7:
+		return MGN_VHT1SS_MCS7;
+	case	MPT_RATE_VHT1SS_MCS8:
+		return MGN_VHT1SS_MCS8;
+	case	MPT_RATE_VHT1SS_MCS9:
+		return MGN_VHT1SS_MCS9;
+	case	MPT_RATE_VHT2SS_MCS0:
+		return MGN_VHT2SS_MCS0;
+	case	MPT_RATE_VHT2SS_MCS1:
+		return MGN_VHT2SS_MCS1;
+	case	MPT_RATE_VHT2SS_MCS2:
+		return MGN_VHT2SS_MCS2;
+	case	MPT_RATE_VHT2SS_MCS3:
+		return MGN_VHT2SS_MCS3;
+	case	MPT_RATE_VHT2SS_MCS4:
+		return MGN_VHT2SS_MCS4;
+	case	MPT_RATE_VHT2SS_MCS5:
+		return MGN_VHT2SS_MCS5;
+	case	MPT_RATE_VHT2SS_MCS6:
+		return MGN_VHT2SS_MCS6;
+	case	MPT_RATE_VHT2SS_MCS7:
+		return MGN_VHT2SS_MCS7;
+	case	MPT_RATE_VHT2SS_MCS8:
+		return MGN_VHT2SS_MCS8;
+	case	MPT_RATE_VHT2SS_MCS9:
+		return MGN_VHT2SS_MCS9;
+	case	MPT_RATE_VHT3SS_MCS0:
+		return MGN_VHT3SS_MCS0;
+	case	MPT_RATE_VHT3SS_MCS1:
+		return MGN_VHT3SS_MCS1;
+	case	MPT_RATE_VHT3SS_MCS2:
+		return MGN_VHT3SS_MCS2;
+	case	MPT_RATE_VHT3SS_MCS3:
+		return MGN_VHT3SS_MCS3;
+	case	MPT_RATE_VHT3SS_MCS4:
+		return MGN_VHT3SS_MCS4;
+	case	MPT_RATE_VHT3SS_MCS5:
+		return MGN_VHT3SS_MCS5;
+	case	MPT_RATE_VHT3SS_MCS6:
+		return MGN_VHT3SS_MCS6;
+	case	MPT_RATE_VHT3SS_MCS7:
+		return MGN_VHT3SS_MCS7;
+	case	MPT_RATE_VHT3SS_MCS8:
+		return MGN_VHT3SS_MCS8;
+	case	MPT_RATE_VHT3SS_MCS9:
+		return MGN_VHT3SS_MCS9;
+	case	MPT_RATE_VHT4SS_MCS0:
+		return MGN_VHT4SS_MCS0;
+	case	MPT_RATE_VHT4SS_MCS1:
+		return MGN_VHT4SS_MCS1;
+	case	MPT_RATE_VHT4SS_MCS2:
+		return MGN_VHT4SS_MCS2;
+	case	MPT_RATE_VHT4SS_MCS3:
+		return MGN_VHT4SS_MCS3;
+	case	MPT_RATE_VHT4SS_MCS4:
+		return MGN_VHT4SS_MCS4;
+	case	MPT_RATE_VHT4SS_MCS5:
+		return MGN_VHT4SS_MCS5;
+	case	MPT_RATE_VHT4SS_MCS6:
+		return MGN_VHT4SS_MCS6;
+	case	MPT_RATE_VHT4SS_MCS7:
+		return MGN_VHT4SS_MCS7;
+	case	MPT_RATE_VHT4SS_MCS8:
+		return MGN_VHT4SS_MCS8;
+	case	MPT_RATE_VHT4SS_MCS9:
+		return MGN_VHT4SS_MCS9;
+
+	case	MPT_RATE_LAST:	/* fully automatiMGN_VHT2SS_MCS1;	 */
+	default:
+		RTW_INFO("<===mpt_to_mgnt_rate(), Invalid Rate: %d!!\n", MptRateIdx);
+		return 0x0;
+	}
+}
+
+
+u8 HwRateToMPTRate(u8 rate)
+{
+	u8	ret_rate = MGN_1M;
+
+	switch (rate) {
+	case DESC_RATE1M:
+		ret_rate = MPT_RATE_1M;
+		break;
+	case DESC_RATE2M:
+		ret_rate = MPT_RATE_2M;
+		break;
+	case DESC_RATE5_5M:
+		ret_rate = MPT_RATE_55M;
+		break;
+	case DESC_RATE11M:
+		ret_rate = MPT_RATE_11M;
+		break;
+	case DESC_RATE6M:
+		ret_rate = MPT_RATE_6M;
+		break;
+	case DESC_RATE9M:
+		ret_rate = MPT_RATE_9M;
+		break;
+	case DESC_RATE12M:
+		ret_rate = MPT_RATE_12M;
+		break;
+	case DESC_RATE18M:
+		ret_rate = MPT_RATE_18M;
+		break;
+	case DESC_RATE24M:
+		ret_rate = MPT_RATE_24M;
+		break;
+	case DESC_RATE36M:
+		ret_rate = MPT_RATE_36M;
+		break;
+	case DESC_RATE48M:
+		ret_rate = MPT_RATE_48M;
+		break;
+	case DESC_RATE54M:
+		ret_rate = MPT_RATE_54M;
+		break;
+	case DESC_RATEMCS0:
+		ret_rate = MPT_RATE_MCS0;
+		break;
+	case DESC_RATEMCS1:
+		ret_rate = MPT_RATE_MCS1;
+		break;
+	case DESC_RATEMCS2:
+		ret_rate = MPT_RATE_MCS2;
+		break;
+	case DESC_RATEMCS3:
+		ret_rate = MPT_RATE_MCS3;
+		break;
+	case DESC_RATEMCS4:
+		ret_rate = MPT_RATE_MCS4;
+		break;
+	case DESC_RATEMCS5:
+		ret_rate = MPT_RATE_MCS5;
+		break;
+	case DESC_RATEMCS6:
+		ret_rate = MPT_RATE_MCS6;
+		break;
+	case DESC_RATEMCS7:
+		ret_rate = MPT_RATE_MCS7;
+		break;
+	case DESC_RATEMCS8:
+		ret_rate = MPT_RATE_MCS8;
+		break;
+	case DESC_RATEMCS9:
+		ret_rate = MPT_RATE_MCS9;
+		break;
+	case DESC_RATEMCS10:
+		ret_rate = MPT_RATE_MCS10;
+		break;
+	case DESC_RATEMCS11:
+		ret_rate = MPT_RATE_MCS11;
+		break;
+	case DESC_RATEMCS12:
+		ret_rate = MPT_RATE_MCS12;
+		break;
+	case DESC_RATEMCS13:
+		ret_rate = MPT_RATE_MCS13;
+		break;
+	case DESC_RATEMCS14:
+		ret_rate = MPT_RATE_MCS14;
+		break;
+	case DESC_RATEMCS15:
+		ret_rate = MPT_RATE_MCS15;
+		break;
+	case DESC_RATEMCS16:
+		ret_rate = MPT_RATE_MCS16;
+		break;
+	case DESC_RATEMCS17:
+		ret_rate = MPT_RATE_MCS17;
+		break;
+	case DESC_RATEMCS18:
+		ret_rate = MPT_RATE_MCS18;
+		break;
+	case DESC_RATEMCS19:
+		ret_rate = MPT_RATE_MCS19;
+		break;
+	case DESC_RATEMCS20:
+		ret_rate = MPT_RATE_MCS20;
+		break;
+	case DESC_RATEMCS21:
+		ret_rate = MPT_RATE_MCS21;
+		break;
+	case DESC_RATEMCS22:
+		ret_rate = MPT_RATE_MCS22;
+		break;
+	case DESC_RATEMCS23:
+		ret_rate = MPT_RATE_MCS23;
+		break;
+	case DESC_RATEMCS24:
+		ret_rate = MPT_RATE_MCS24;
+		break;
+	case DESC_RATEMCS25:
+		ret_rate = MPT_RATE_MCS25;
+		break;
+	case DESC_RATEMCS26:
+		ret_rate = MPT_RATE_MCS26;
+		break;
+	case DESC_RATEMCS27:
+		ret_rate = MPT_RATE_MCS27;
+		break;
+	case DESC_RATEMCS28:
+		ret_rate = MPT_RATE_MCS28;
+		break;
+	case DESC_RATEMCS29:
+		ret_rate = MPT_RATE_MCS29;
+		break;
+	case DESC_RATEMCS30:
+		ret_rate = MPT_RATE_MCS30;
+		break;
+	case DESC_RATEMCS31:
+		ret_rate = MPT_RATE_MCS31;
+		break;
+	case DESC_RATEVHTSS1MCS0:
+		ret_rate = MPT_RATE_VHT1SS_MCS0;
+		break;
+	case DESC_RATEVHTSS1MCS1:
+		ret_rate = MPT_RATE_VHT1SS_MCS1;
+		break;
+	case DESC_RATEVHTSS1MCS2:
+		ret_rate = MPT_RATE_VHT1SS_MCS2;
+		break;
+	case DESC_RATEVHTSS1MCS3:
+		ret_rate = MPT_RATE_VHT1SS_MCS3;
+		break;
+	case DESC_RATEVHTSS1MCS4:
+		ret_rate = MPT_RATE_VHT1SS_MCS4;
+		break;
+	case DESC_RATEVHTSS1MCS5:
+		ret_rate = MPT_RATE_VHT1SS_MCS5;
+		break;
+	case DESC_RATEVHTSS1MCS6:
+		ret_rate = MPT_RATE_VHT1SS_MCS6;
+		break;
+	case DESC_RATEVHTSS1MCS7:
+		ret_rate = MPT_RATE_VHT1SS_MCS7;
+		break;
+	case DESC_RATEVHTSS1MCS8:
+		ret_rate = MPT_RATE_VHT1SS_MCS8;
+		break;
+	case DESC_RATEVHTSS1MCS9:
+		ret_rate = MPT_RATE_VHT1SS_MCS9;
+		break;
+	case DESC_RATEVHTSS2MCS0:
+		ret_rate = MPT_RATE_VHT2SS_MCS0;
+		break;
+	case DESC_RATEVHTSS2MCS1:
+		ret_rate = MPT_RATE_VHT2SS_MCS1;
+		break;
+	case DESC_RATEVHTSS2MCS2:
+		ret_rate = MPT_RATE_VHT2SS_MCS2;
+		break;
+	case DESC_RATEVHTSS2MCS3:
+		ret_rate = MPT_RATE_VHT2SS_MCS3;
+		break;
+	case DESC_RATEVHTSS2MCS4:
+		ret_rate = MPT_RATE_VHT2SS_MCS4;
+		break;
+	case DESC_RATEVHTSS2MCS5:
+		ret_rate = MPT_RATE_VHT2SS_MCS5;
+		break;
+	case DESC_RATEVHTSS2MCS6:
+		ret_rate = MPT_RATE_VHT2SS_MCS6;
+		break;
+	case DESC_RATEVHTSS2MCS7:
+		ret_rate = MPT_RATE_VHT2SS_MCS7;
+		break;
+	case DESC_RATEVHTSS2MCS8:
+		ret_rate = MPT_RATE_VHT2SS_MCS8;
+		break;
+	case DESC_RATEVHTSS2MCS9:
+		ret_rate = MPT_RATE_VHT2SS_MCS9;
+		break;
+	case DESC_RATEVHTSS3MCS0:
+		ret_rate = MPT_RATE_VHT3SS_MCS0;
+		break;
+	case DESC_RATEVHTSS3MCS1:
+		ret_rate = MPT_RATE_VHT3SS_MCS1;
+		break;
+	case DESC_RATEVHTSS3MCS2:
+		ret_rate = MPT_RATE_VHT3SS_MCS2;
+		break;
+	case DESC_RATEVHTSS3MCS3:
+		ret_rate = MPT_RATE_VHT3SS_MCS3;
+		break;
+	case DESC_RATEVHTSS3MCS4:
+		ret_rate = MPT_RATE_VHT3SS_MCS4;
+		break;
+	case DESC_RATEVHTSS3MCS5:
+		ret_rate = MPT_RATE_VHT3SS_MCS5;
+		break;
+	case DESC_RATEVHTSS3MCS6:
+		ret_rate = MPT_RATE_VHT3SS_MCS6;
+		break;
+	case DESC_RATEVHTSS3MCS7:
+		ret_rate = MPT_RATE_VHT3SS_MCS7;
+		break;
+	case DESC_RATEVHTSS3MCS8:
+		ret_rate = MPT_RATE_VHT3SS_MCS8;
+		break;
+	case DESC_RATEVHTSS3MCS9:
+		ret_rate = MPT_RATE_VHT3SS_MCS9;
+		break;
+	case DESC_RATEVHTSS4MCS0:
+		ret_rate = MPT_RATE_VHT4SS_MCS0;
+		break;
+	case DESC_RATEVHTSS4MCS1:
+		ret_rate = MPT_RATE_VHT4SS_MCS1;
+		break;
+	case DESC_RATEVHTSS4MCS2:
+		ret_rate = MPT_RATE_VHT4SS_MCS2;
+		break;
+	case DESC_RATEVHTSS4MCS3:
+		ret_rate = MPT_RATE_VHT4SS_MCS3;
+		break;
+	case DESC_RATEVHTSS4MCS4:
+		ret_rate = MPT_RATE_VHT4SS_MCS4;
+		break;
+	case DESC_RATEVHTSS4MCS5:
+		ret_rate = MPT_RATE_VHT4SS_MCS5;
+		break;
+	case DESC_RATEVHTSS4MCS6:
+		ret_rate = MPT_RATE_VHT4SS_MCS6;
+		break;
+	case DESC_RATEVHTSS4MCS7:
+		ret_rate = MPT_RATE_VHT4SS_MCS7;
+		break;
+	case DESC_RATEVHTSS4MCS8:
+		ret_rate = MPT_RATE_VHT4SS_MCS8;
+		break;
+	case DESC_RATEVHTSS4MCS9:
+		ret_rate = MPT_RATE_VHT4SS_MCS9;
+		break;
+
+	default:
+		RTW_INFO("hw_rate_to_m_rate(): Non supported Rate [%x]!!!\n", rate);
+		break;
+	}
+	return ret_rate;
+}
+
+u8 rtw_mpRateParseFunc(PADAPTER pAdapter, u8 *targetStr)
+{
+	u16 i = 0;
+	u8 *rateindex_Array[] = { "1M", "2M", "5.5M", "11M", "6M", "9M", "12M", "18M", "24M", "36M", "48M", "54M",
+		"HTMCS0", "HTMCS1", "HTMCS2", "HTMCS3", "HTMCS4", "HTMCS5", "HTMCS6", "HTMCS7",
+		"HTMCS8", "HTMCS9", "HTMCS10", "HTMCS11", "HTMCS12", "HTMCS13", "HTMCS14", "HTMCS15",
+		"HTMCS16", "HTMCS17", "HTMCS18", "HTMCS19", "HTMCS20", "HTMCS21", "HTMCS22", "HTMCS23",
+		"HTMCS24", "HTMCS25", "HTMCS26", "HTMCS27", "HTMCS28", "HTMCS29", "HTMCS30", "HTMCS31",
+		"VHT1MCS0", "VHT1MCS1", "VHT1MCS2", "VHT1MCS3", "VHT1MCS4", "VHT1MCS5", "VHT1MCS6", "VHT1MCS7", "VHT1MCS8", "VHT1MCS9",
+		"VHT2MCS0", "VHT2MCS1", "VHT2MCS2", "VHT2MCS3", "VHT2MCS4", "VHT2MCS5", "VHT2MCS6", "VHT2MCS7", "VHT2MCS8", "VHT2MCS9",
+		"VHT3MCS0", "VHT3MCS1", "VHT3MCS2", "VHT3MCS3", "VHT3MCS4", "VHT3MCS5", "VHT3MCS6", "VHT3MCS7", "VHT3MCS8", "VHT3MCS9",
+		"VHT4MCS0", "VHT4MCS1", "VHT4MCS2", "VHT4MCS3", "VHT4MCS4", "VHT4MCS5", "VHT4MCS6", "VHT4MCS7", "VHT4MCS8", "VHT4MCS9"
+				};
+
+	for (i = 0; i <= 83; i++) {
+		if (strcmp(targetStr, rateindex_Array[i]) == 0) {
+			RTW_INFO("%s , index = %d\n", __func__ , i);
+			return i;
+		}
+	}
+
+	RTW_INFO("%s ,please input a Data RATE String as:", __func__);
+	for (i = 0; i <= 83; i++) {
+		RTW_INFO("%s ", rateindex_Array[i]);
+		if (i % 10 == 0)
+			RTW_INFO("\n");
+	}
+	return _FAIL;
+}
+
+u32 mpt_ProQueryCalTxPower(
+	PADAPTER	pAdapter,
+	u8		RfPath
+)
+{
+
+	HAL_DATA_TYPE	*pHalData	= GET_HAL_DATA(pAdapter);
+	PMPT_CONTEXT		pMptCtx = &(pAdapter->mppriv.mpt_ctx);
+
+	u32			TxPower = 1;
+	u8			rate = 0;
+	struct txpwr_idx_comp tic;
+	u8 mgn_rate = mpt_to_mgnt_rate(pMptCtx->mpt_rate_index);
+
+	TxPower = rtw_hal_get_tx_power_index(pAdapter, RfPath, mgn_rate, pHalData->current_channel_bw, pHalData->current_channel, &tic);
+
+	RTW_INFO("bw=%d, ch=%d, rate=%d, txPower:%u = %u + (%d=%d:%d) + (%d) + (%d)\n",
+		pHalData->current_channel_bw, pHalData->current_channel, mgn_rate
+		, TxPower, tic.base, (tic.by_rate > tic.limit ? tic.limit : tic.by_rate), tic.by_rate, tic.limit, tic.tpt, tic.ebias);
+
+	pAdapter->mppriv.txpoweridx = (u8)TxPower;
+	pMptCtx->TxPwrLevel[ODM_RF_PATH_A] = (u8)TxPower;
+	pMptCtx->TxPwrLevel[ODM_RF_PATH_B] = (u8)TxPower;
+	pMptCtx->TxPwrLevel[ODM_RF_PATH_C] = (u8)TxPower;
+	pMptCtx->TxPwrLevel[ODM_RF_PATH_D]  = (u8)TxPower;
+	hal_mpt_SetTxPower(pAdapter);
+
+	return TxPower;
+}
+
+#ifdef CONFIG_MP_VHT_HW_TX_MODE
+static inline void dump_buf(u8 *buf, u32 len)
+{
+	u32 i;
+
+	RTW_INFO("-----------------Len %d----------------\n", len);
+	for (i = 0; i < len; i++)
+		RTW_INFO("%2.2x-", *(buf + i));
+	RTW_INFO("\n");
+}
+
+void ByteToBit(
+	u8	*out,
+	bool	*in,
+	u8	in_size)
+{
+	u8 i = 0, j = 0;
+
+	for (i = 0; i < in_size; i++) {
+		for (j = 0; j < 8; j++) {
+			if (in[8 * i + j])
+				out[i] |= (1 << j);
+		}
+	}
+}
+
+
+void CRC16_generator(
+	bool *out,
+	bool *in,
+	u8 in_size
+)
+{
+	u8 i = 0;
+	bool temp = 0, reg[] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
+
+	for (i = 0; i < in_size; i++) {/* take one's complement and bit reverse*/
+		temp = in[i] ^ reg[15];
+		reg[15]	= reg[14];
+		reg[14]	= reg[13];
+		reg[13]	= reg[12];
+		reg[12]	= reg[11];
+		reg[11]	= reg[10];
+		reg[10]	= reg[9];
+		reg[9]	= reg[8];
+		reg[8]	= reg[7];
+
+		reg[7]	= reg[6];
+		reg[6]	= reg[5];
+		reg[5]	= reg[4];
+		reg[4]	= reg[3];
+		reg[3]	= reg[2];
+		reg[2]	= reg[1];
+		reg[1]	= reg[0];
+		reg[12]	= reg[12] ^ temp;
+		reg[5]	= reg[5] ^ temp;
+		reg[0]	= temp;
+	}
+	for (i = 0; i < 16; i++)	/* take one's complement and bit reverse*/
+		out[i] = 1 - reg[15 - i];
+}
+
+
+
+/*========================================
+	SFD		SIGNAL	SERVICE	LENGTH	CRC
+	16 bit	8 bit	8 bit	16 bit	16 bit
+========================================*/
+void CCK_generator(
+	PRT_PMAC_TX_INFO	pPMacTxInfo,
+	PRT_PMAC_PKT_INFO	pPMacPktInfo
+)
+{
+	double	ratio = 0;
+	bool	crc16_in[32] = {0}, crc16_out[16] = {0};
+	bool LengthExtBit;
+	double LengthExact;
+	double LengthPSDU;
+	u8 i;
+	u32 PacketLength = pPMacTxInfo->PacketLength;
+
+	if (pPMacTxInfo->bSPreamble)
+		pPMacTxInfo->SFD = 0x05CF;
+	else
+		pPMacTxInfo->SFD = 0xF3A0;
+
+	switch (pPMacPktInfo->MCS) {
+	case 0:
+		pPMacTxInfo->SignalField = 0xA;
+		ratio = 8;
+		/*CRC16_in(1,0:7)=[0 1 0 1 0 0 0 0]*/
+		crc16_in[1] = crc16_in[3] = 1;
+		break;
+	case 1:
+		pPMacTxInfo->SignalField = 0x14;
+		ratio = 4;
+		/*CRC16_in(1,0:7)=[0 0 1 0 1 0 0 0];*/
+		crc16_in[2] = crc16_in[4] = 1;
+		break;
+	case 2:
+		pPMacTxInfo->SignalField = 0x37;
+		ratio = 8.0 / 5.5;
+		/*CRC16_in(1,0:7)=[1 1 1 0 1 1 0 0];*/
+		crc16_in[0] = crc16_in[1] = crc16_in[2] = crc16_in[4] = crc16_in[5] = 1;
+		break;
+	case 3:
+		pPMacTxInfo->SignalField = 0x6E;
+		ratio = 8.0 / 11.0;
+		/*CRC16_in(1,0:7)=[0 1 1 1 0 1 1 0];*/
+		crc16_in[1] = crc16_in[2] = crc16_in[3] = crc16_in[5] = crc16_in[6] = 1;
+		break;
+	}
+
+	LengthExact = PacketLength * ratio;
+	LengthPSDU = ceil(LengthExact);
+
+	if ((pPMacPktInfo->MCS == 3) &&
+	    ((LengthPSDU - LengthExact) >= 0.727 || (LengthPSDU - LengthExact) <= -0.727))
+		LengthExtBit = 1;
+	else
+		LengthExtBit = 0;
+
+
+	pPMacTxInfo->LENGTH = (u32)LengthPSDU;
+	/* CRC16_in(1,16:31) = LengthPSDU[0:15]*/
+	for (i = 0; i < 16; i++)
+		crc16_in[i + 16] = (pPMacTxInfo->LENGTH >> i) & 0x1;
+
+	if (LengthExtBit == 0) {
+		pPMacTxInfo->ServiceField = 0x0;
+		/* CRC16_in(1,8:15) = [0 0 0 0 0 0 0 0];*/
+	} else {
+		pPMacTxInfo->ServiceField = 0x80;
+		/*CRC16_in(1,8:15)=[0 0 0 0 0 0 0 1];*/
+		crc16_in[15] = 1;
+	}
+
+	CRC16_generator(crc16_out, crc16_in, 32);
+
+	memset(pPMacTxInfo->CRC16, 0, 2);
+	ByteToBit(pPMacTxInfo->CRC16, crc16_out, 2);
+
+}
+
+
+void PMAC_Get_Pkt_Param(
+	PRT_PMAC_TX_INFO	pPMacTxInfo,
+	PRT_PMAC_PKT_INFO	pPMacPktInfo)
+{
+
+	u8		TX_RATE_HEX = 0, MCS = 0;
+	u8		TX_RATE = pPMacTxInfo->TX_RATE;
+
+	/*	TX_RATE & Nss	*/
+	if (MPT_IS_2SS_RATE(TX_RATE))
+		pPMacPktInfo->Nss = 2;
+	else if (MPT_IS_3SS_RATE(TX_RATE))
+		pPMacPktInfo->Nss = 3;
+	else if (MPT_IS_4SS_RATE(TX_RATE))
+		pPMacPktInfo->Nss = 4;
+	else
+		pPMacPktInfo->Nss = 1;
+
+	RTW_INFO("PMacTxInfo.Nss =%d\n", pPMacPktInfo->Nss);
+
+	/*	MCS & TX_RATE_HEX*/
+	if (MPT_IS_CCK_RATE(TX_RATE)) {
+		switch (TX_RATE) {
+		case MPT_RATE_1M:
+			TX_RATE_HEX = MCS = 0;
+			break;
+		case MPT_RATE_2M:
+			TX_RATE_HEX = MCS = 1;
+			break;
+		case MPT_RATE_55M:
+			TX_RATE_HEX = MCS = 2;
+			break;
+		case MPT_RATE_11M:
+			TX_RATE_HEX = MCS = 3;
+			break;
+		}
+	} else if (MPT_IS_OFDM_RATE(TX_RATE)) {
+		MCS = TX_RATE - MPT_RATE_6M;
+		TX_RATE_HEX = MCS + 4;
+	} else if (MPT_IS_HT_RATE(TX_RATE)) {
+		MCS = TX_RATE - MPT_RATE_MCS0;
+		TX_RATE_HEX = MCS + 12;
+	} else if (MPT_IS_VHT_RATE(TX_RATE)) {
+		TX_RATE_HEX = TX_RATE - MPT_RATE_VHT1SS_MCS0 + 44;
+
+		if (MPT_IS_VHT_2S_RATE(TX_RATE))
+			MCS = TX_RATE - MPT_RATE_VHT2SS_MCS0;
+		else if (MPT_IS_VHT_3S_RATE(TX_RATE))
+			MCS = TX_RATE - MPT_RATE_VHT3SS_MCS0;
+		else if (MPT_IS_VHT_4S_RATE(TX_RATE))
+			MCS = TX_RATE - MPT_RATE_VHT4SS_MCS0;
+		else
+			MCS = TX_RATE - MPT_RATE_VHT1SS_MCS0;
+	}
+
+	pPMacPktInfo->MCS = MCS;
+	pPMacTxInfo->TX_RATE_HEX = TX_RATE_HEX;
+
+	RTW_INFO(" MCS=%d, TX_RATE_HEX =0x%x\n", MCS, pPMacTxInfo->TX_RATE_HEX);
+	/*	mSTBC & Nsts*/
+	pPMacPktInfo->Nsts = pPMacPktInfo->Nss;
+	if (pPMacTxInfo->bSTBC) {
+		if (pPMacPktInfo->Nss == 1) {
+			pPMacTxInfo->m_STBC = 2;
+			pPMacPktInfo->Nsts = pPMacPktInfo->Nss * 2;
+		} else
+			pPMacTxInfo->m_STBC = 1;
+	} else
+		pPMacTxInfo->m_STBC = 1;
+}
+
+
+u32 LDPC_parameter_generator(
+	u32 N_pld_int,
+	u32 N_CBPSS,
+	u32 N_SS,
+	u32 R,
+	u32 m_STBC,
+	u32 N_TCB_int
+)
+{
+	double	CR = 0.;
+	double	N_pld = (double)N_pld_int;
+	double	N_TCB = (double)N_TCB_int;
+	double	N_CW = 0., N_shrt = 0., N_spcw = 0., N_fshrt = 0.;
+	double	L_LDPC = 0., K_LDPC = 0., L_LDPC_info = 0.;
+	double	N_punc = 0., N_ppcw = 0., N_fpunc = 0., N_rep = 0., N_rpcw = 0., N_frep = 0.;
+	double	R_eff = 0.;
+	u32	VHTSIGA2B3  = 0;/* extra symbol from VHT-SIG-A2 Bit 3*/
+
+	if (R == 0)
+		CR	= 0.5;
+	else if (R == 1)
+		CR = 2. / 3.;
+	else if (R == 2)
+		CR = 3. / 4.;
+	else if (R == 3)
+		CR = 5. / 6.;
+
+	if (N_TCB <= 648.) {
+		N_CW	= 1.;
+		if (N_TCB >= N_pld + 912.*(1. - CR))
+			L_LDPC	= 1296.;
+		else
+			L_LDPC	= 648.;
+	} else if (N_TCB <= 1296.) {
+		N_CW	= 1.;
+		if (N_TCB >= (double)N_pld + 1464.*(1. - CR))
+			L_LDPC	= 1944.;
+		else
+			L_LDPC	= 1296.;
+	} else if	(N_TCB <= 1944.) {
+		N_CW	= 1.;
+		L_LDPC	= 1944.;
+	} else if (N_TCB <= 2592.) {
+		N_CW	= 2.;
+		if (N_TCB >= N_pld + 2916.*(1. - CR))
+			L_LDPC	= 1944.;
+		else
+			L_LDPC	= 1296.;
+	} else {
+		N_CW = ceil(N_pld / 1944. / CR);
+		L_LDPC	= 1944.;
+	}
+	/*	Number of information bits per CW*/
+	K_LDPC = L_LDPC * CR;
+	/*	Number of shortening bits					max(0, (N_CW * L_LDPC * R) - N_pld)*/
+	N_shrt = (N_CW * K_LDPC - N_pld) > 0. ? (N_CW * K_LDPC - N_pld) : 0.;
+	/*	Number of shortening bits per CW			N_spcw = rtfloor(N_shrt/N_CW)*/
+	N_spcw = rtfloor(N_shrt / N_CW);
+	/*	The first N_fshrt CWs shorten 1 bit more*/
+	N_fshrt = (double)((int)N_shrt % (int)N_CW);
+	/*	Number of data bits for the last N_CW-N_fshrt CWs*/
+	L_LDPC_info = K_LDPC - N_spcw;
+	/*	Number of puncturing bits*/
+	N_punc = (N_CW * L_LDPC - N_TCB - N_shrt) > 0. ? (N_CW * L_LDPC - N_TCB - N_shrt) : 0.;
+	if (((N_punc > .1 * N_CW * L_LDPC * (1. - CR)) && (N_shrt < 1.2 * N_punc * CR / (1. - CR))) ||
+	    (N_punc > 0.3 * N_CW * L_LDPC * (1. - CR))) {
+		/*cout << "*** N_TCB and N_punc are Recomputed ***" << endl;*/
+		VHTSIGA2B3 = 1;
+		N_TCB += (double)N_CBPSS * N_SS * m_STBC;
+		N_punc = (N_CW * L_LDPC - N_TCB - N_shrt) > 0. ? (N_CW * L_LDPC - N_TCB - N_shrt) : 0.;
+	} else
+		VHTSIGA2B3 = 0;
+
+	return VHTSIGA2B3;
+}	/* function end of LDPC_parameter_generator */
+
+/*========================================
+	Data field of PPDU
+	Get N_sym and SIGA2BB3
+========================================*/
+void PMAC_Nsym_generator(
+	PRT_PMAC_TX_INFO	pPMacTxInfo,
+	PRT_PMAC_PKT_INFO	pPMacPktInfo)
+{
+	u32	SIGA2B3 = 0;
+	u8	TX_RATE = pPMacTxInfo->TX_RATE;
+
+	u32 R, R_list[10] = {0, 0, 2, 0, 2, 1, 2, 3, 2, 3};
+	double CR = 0;
+	u32 N_SD, N_BPSC_list[10] = {1, 2, 2, 4, 4, 6, 6, 6, 8, 8};
+	u32 N_BPSC = 0, N_CBPS = 0, N_DBPS = 0, N_ES = 0, N_SYM = 0, N_pld = 0, N_TCB = 0;
+	int D_R = 0;
+
+	RTW_INFO("TX_RATE = %d\n", TX_RATE);
+	/*	N_SD*/
+	if (pPMacTxInfo->BandWidth == 0)
+		N_SD = 52;
+	else if (pPMacTxInfo->BandWidth == 1)
+		N_SD = 108;
+	else
+		N_SD = 234;
+
+	if (MPT_IS_HT_RATE(TX_RATE)) {
+		u8 MCS_temp;
+
+		if (pPMacPktInfo->MCS > 23)
+			MCS_temp = pPMacPktInfo->MCS - 24;
+		else if (pPMacPktInfo->MCS > 15)
+			MCS_temp = pPMacPktInfo->MCS - 16;
+		else if (pPMacPktInfo->MCS > 7)
+			MCS_temp = pPMacPktInfo->MCS - 8;
+		else
+			MCS_temp = pPMacPktInfo->MCS;
+
+		R = R_list[MCS_temp];
+
+		switch (R) {
+		case 0:
+			CR = .5;
+			break;
+		case 1:
+			CR = 2. / 3.;
+			break;
+		case 2:
+			CR = 3. / 4.;
+			break;
+		case 3:
+			CR = 5. / 6.;
+			break;
+		}
+
+		N_BPSC = N_BPSC_list[MCS_temp];
+		N_CBPS = N_BPSC * N_SD * pPMacPktInfo->Nss;
+		N_DBPS = (u32)((double)N_CBPS * CR);
+
+		if (pPMacTxInfo->bLDPC == false) {
+			N_ES = (u32)ceil((double)(N_DBPS * pPMacPktInfo->Nss) / 4. / 300.);
+			RTW_INFO("N_ES = %d\n", N_ES);
+
+			/*	N_SYM = m_STBC* (8*length+16+6*N_ES) / (m_STBC*N_DBPS)*/
+			N_SYM = pPMacTxInfo->m_STBC * (u32)ceil((double)(pPMacTxInfo->PacketLength * 8 + 16 + N_ES * 6) /
+					(double)(N_DBPS * pPMacTxInfo->m_STBC));
+
+		} else {
+			N_ES = 1;
+			/*	N_pld = length * 8 + 16*/
+			N_pld = pPMacTxInfo->PacketLength * 8 + 16;
+			RTW_INFO("N_pld = %d\n", N_pld);
+			N_SYM = pPMacTxInfo->m_STBC * (u32)ceil((double)(N_pld) /
+					(double)(N_DBPS * pPMacTxInfo->m_STBC));
+			RTW_INFO("N_SYM = %d\n", N_SYM);
+			/*	N_avbits = N_CBPS *m_STBC *(N_pld/N_CBPS*R*m_STBC)*/
+			N_TCB = N_CBPS * N_SYM;
+			RTW_INFO("N_TCB = %d\n", N_TCB);
+			SIGA2B3 = LDPC_parameter_generator(N_pld, N_CBPS, pPMacPktInfo->Nss, R, pPMacTxInfo->m_STBC, N_TCB);
+			RTW_INFO("SIGA2B3 = %d\n", SIGA2B3);
+			N_SYM = N_SYM + SIGA2B3 * pPMacTxInfo->m_STBC;
+			RTW_INFO("N_SYM = %d\n", N_SYM);
+		}
+	} else if (MPT_IS_VHT_RATE(TX_RATE)) {
+		R = R_list[pPMacPktInfo->MCS];
+
+		switch (R) {
+		case 0:
+			CR = .5;
+			break;
+		case 1:
+			CR = 2. / 3.;
+			break;
+		case 2:
+			CR = 3. / 4.;
+			break;
+		case 3:
+			CR = 5. / 6.;
+			break;
+		}
+		N_BPSC = N_BPSC_list[pPMacPktInfo->MCS];
+		N_CBPS = N_BPSC * N_SD * pPMacPktInfo->Nss;
+		N_DBPS = (u32)((double)N_CBPS * CR);
+		if (pPMacTxInfo->bLDPC == false) {
+			if (pPMacTxInfo->bSGI)
+				N_ES = (u32)ceil((double)(N_DBPS) / 3.6 / 600.);
+			else
+				N_ES = (u32)ceil((double)(N_DBPS) / 4. / 600.);
+			/*	N_SYM = m_STBC* (8*length+16+6*N_ES) / (m_STBC*N_DBPS)*/
+			N_SYM = pPMacTxInfo->m_STBC * (u32)ceil((double)(pPMacTxInfo->PacketLength * 8 + 16 + N_ES * 6) / (double)(N_DBPS * pPMacTxInfo->m_STBC));
+			SIGA2B3 = 0;
+		} else {
+			N_ES = 1;
+			/*	N_SYM = m_STBC* (8*length+N_service) / (m_STBC*N_DBPS)*/
+			N_SYM = pPMacTxInfo->m_STBC * (u32)ceil((double)(pPMacTxInfo->PacketLength * 8 + 16) / (double)(N_DBPS * pPMacTxInfo->m_STBC));
+			/*	N_avbits = N_sys_init * N_CBPS*/
+			N_TCB = N_CBPS * N_SYM;
+			/*	N_pld = N_sys_init * N_DBPS*/
+			N_pld = N_SYM * N_DBPS;
+			SIGA2B3 = LDPC_parameter_generator(N_pld, N_CBPS, pPMacPktInfo->Nss, R, pPMacTxInfo->m_STBC, N_TCB);
+			N_SYM = N_SYM + SIGA2B3 * pPMacTxInfo->m_STBC;
+		}
+
+		switch (R) {
+		case 0:
+			D_R = 2;
+			break;
+		case 1:
+			D_R = 3;
+			break;
+		case 2:
+			D_R = 4;
+			break;
+		case 3:
+			D_R = 6;
+			break;
+		}
+
+		if (((N_CBPS / N_ES) % D_R) != 0) {
+			RTW_INFO("MCS= %d is not supported when Nss=%d and BW= %d !!\n",  pPMacPktInfo->MCS, pPMacPktInfo->Nss, pPMacTxInfo->BandWidth);
+			return;
+		}
+
+		RTW_INFO("MCS= %d Nss=%d and BW= %d !!\n",  pPMacPktInfo->MCS, pPMacPktInfo->Nss, pPMacTxInfo->BandWidth);
+	}
+
+	pPMacPktInfo->N_sym = N_SYM;
+	pPMacPktInfo->SIGA2B3 = SIGA2B3;
+}
+
+/*========================================
+	L-SIG	Rate	R	Length	P	Tail
+			4b		1b	12b		1b	6b
+========================================*/
+
+void L_SIG_generator(
+	u32	N_SYM,		/* Max: 750*/
+	PRT_PMAC_TX_INFO	pPMacTxInfo,
+	PRT_PMAC_PKT_INFO	pPMacPktInfo)
+{
+	u8	sig_bi[24] = {0};	/* 24 BIT*/
+	u32	mode, LENGTH;
+	int i;
+
+	if (MPT_IS_OFDM_RATE(pPMacTxInfo->TX_RATE)) {
+		mode = pPMacPktInfo->MCS;
+		LENGTH = pPMacTxInfo->PacketLength;
+	} else {
+		u8	N_LTF;
+		double	T_data;
+		u32	OFDM_symbol;
+
+		mode = 0;
+
+		/*	Table 20-13 Num of HT-DLTFs request*/
+		if (pPMacPktInfo->Nsts <= 2)
+			N_LTF = pPMacPktInfo->Nsts;
+		else
+			N_LTF = 4;
+
+		if (pPMacTxInfo->bSGI)
+			T_data = 3.6;
+		else
+			T_data = 4.0;
+
+		/*(L-SIG, HT-SIG, HT-STF, HT-LTF....HT-LTF, Data)*/
+		if (MPT_IS_VHT_RATE(pPMacTxInfo->TX_RATE))
+			OFDM_symbol = (u32)ceil((double)(8 + 4 + N_LTF * 4 + N_SYM * T_data + 4) / 4.);
+		else
+			OFDM_symbol = (u32)ceil((double)(8 + 4 + N_LTF * 4 + N_SYM * T_data) / 4.);
+
+		RTW_INFO("%s , OFDM_symbol =%d\n", __func__, OFDM_symbol);
+		LENGTH = OFDM_symbol * 3 - 3;
+		RTW_INFO("%s , LENGTH =%d\n", __func__, LENGTH);
+
+	}
+	/*	Rate Field*/
+	switch (mode) {
+	case	0:
+		sig_bi[0] = 1;
+		sig_bi[1] = 1;
+		sig_bi[2] = 0;
+		sig_bi[3] = 1;
+		break;
+	case	1:
+		sig_bi[0] = 1;
+		sig_bi[1] = 1;
+		sig_bi[2] = 1;
+		sig_bi[3] = 1;
+		break;
+	case	2:
+		sig_bi[0] = 0;
+		sig_bi[1] = 1;
+		sig_bi[2] = 0;
+		sig_bi[3] = 1;
+		break;
+	case	3:
+		sig_bi[0] = 0;
+		sig_bi[1] = 1;
+		sig_bi[2] = 1;
+		sig_bi[3] = 1;
+		break;
+	case	4:
+		sig_bi[0] = 1;
+		sig_bi[1] = 0;
+		sig_bi[2] = 0;
+		sig_bi[3] = 1;
+		break;
+	case	5:
+		sig_bi[0] = 1;
+		sig_bi[1] = 0;
+		sig_bi[2] = 1;
+		sig_bi[3] = 1;
+		break;
+	case	6:
+		sig_bi[0] = 0;
+		sig_bi[1] = 0;
+		sig_bi[2] = 0;
+		sig_bi[3] = 1;
+		break;
+	case	7:
+		sig_bi[0] = 0;
+		sig_bi[1] = 0;
+		sig_bi[2] = 1;
+		sig_bi[3] = 1;
+		break;
+	}
+	/*Reserved bit*/
+	sig_bi[4] = 0;
+
+	/*	Length Field*/
+	for (i = 0; i < 12; i++)
+		sig_bi[i + 5] = (LENGTH >> i) & 1;
+
+	/* Parity Bit*/
+	sig_bi[17] = 0;
+	for (i = 0; i < 17; i++)
+		sig_bi[17] = sig_bi[17] + sig_bi[i];
+
+	sig_bi[17] %= 2;
+
+	/*	Tail Field*/
+	for (i = 18; i < 24; i++)
+		sig_bi[i] = 0;
+
+	/* dump_buf(sig_bi,24);*/
+	memset(pPMacTxInfo->LSIG, 0, 3);
+	ByteToBit(pPMacTxInfo->LSIG, (bool *)sig_bi, 3);
+}
+
+
+void CRC8_generator(
+	bool	*out,
+	bool	*in,
+	u8	in_size
+)
+{
+	u8 i = 0;
+	bool temp = 0, reg[] = {1, 1, 1, 1, 1, 1, 1, 1};
+
+	for (i = 0; i < in_size; i++) { /* take one's complement and bit reverse*/
+		temp = in[i] ^ reg[7];
+		reg[7]	= reg[6];
+		reg[6]	= reg[5];
+		reg[5]	= reg[4];
+		reg[4]	= reg[3];
+		reg[3]	= reg[2];
+		reg[2]	= reg[1] ^ temp;
+		reg[1]	= reg[0] ^ temp;
+		reg[0]	= temp;
+	}
+	for (i = 0; i < 8; i++)/* take one's complement and bit reverse*/
+		out[i] = reg[7 - i] ^ 1;
+}
+
+/*/================================================================================
+	HT-SIG1	MCS	CW	Length		24BIT + 24BIT
+			7b	1b	16b
+	HT-SIG2	Smoothing	Not sounding	Rsvd		AGG	STBC	FEC	SGI	N_ELTF	CRC	Tail
+			1b			1b			1b		1b	2b		1b	1b	2b		8b	6b
+================================================================================*/
+void HT_SIG_generator(
+	PRT_PMAC_TX_INFO	pPMacTxInfo,
+	PRT_PMAC_PKT_INFO	pPMacPktInfo
+)
+{
+	u32 i;
+	bool sig_bi[48] = {0}, crc8[8] = {0};
+	/*	MCS Field*/
+	for (i = 0; i < 7; i++)
+		sig_bi[i] = (pPMacPktInfo->MCS >> i) & 0x1;
+	/*	Packet BW Setting*/
+	sig_bi[7] = pPMacTxInfo->BandWidth;
+	/*	HT-Length Field*/
+	for (i = 0; i < 16; i++)
+		sig_bi[i + 8] = (pPMacTxInfo->PacketLength >> i) & 0x1;
+	/*	Smoothing;	1->allow smoothing*/
+	sig_bi[24] = 1;
+	/*Not Sounding*/
+	sig_bi[25] = 1 - pPMacTxInfo->NDP_sound;
+	/*Reserved bit*/
+	sig_bi[26] = 1;
+	/*/Aggregate*/
+	sig_bi[27] = 0;
+	/*STBC Field*/
+	if (pPMacTxInfo->bSTBC) {
+		sig_bi[28] = 1;
+		sig_bi[29] = 0;
+	} else {
+		sig_bi[28] = 0;
+		sig_bi[29] = 0;
+	}
+	/*Advance Coding,	0: BCC, 1: LDPC*/
+	sig_bi[30] = pPMacTxInfo->bLDPC;
+	/* Short GI*/
+	sig_bi[31] = pPMacTxInfo->bSGI;
+	/* N_ELTFs*/
+	if (pPMacTxInfo->NDP_sound == false) {
+		sig_bi[32]	= 0;
+		sig_bi[33]	= 0;
+	} else {
+		int	N_ELTF = pPMacTxInfo->Ntx - pPMacPktInfo->Nss;
+
+		for (i = 0; i < 2; i++)
+			sig_bi[32 + i] = (N_ELTF >> i) % 2;
+	}
+	/*	CRC-8*/
+	CRC8_generator(crc8, sig_bi, 34);
+
+	for (i = 0; i < 8; i++)
+		sig_bi[34 + i] = crc8[i];
+
+	/*Tail*/
+	for (i = 42; i < 48; i++)
+		sig_bi[i] = 0;
+
+	memset(pPMacTxInfo->HT_SIG, 0, 6);
+	ByteToBit(pPMacTxInfo->HT_SIG, sig_bi, 6);
+}
+
+
+/*======================================================================================
+	VHT-SIG-A1
+	BW	Reserved	STBC	G_ID	SU_Nsts	P_AID	TXOP_PS_NOT_ALLOW	Reserved
+	2b	1b			1b		6b	3b	9b		1b		2b					1b
+	VHT-SIG-A2
+	SGI	SGI_Nsym	SU/MU coding	LDPC_Extra	SU_NCS	Beamformed	Reserved	CRC	Tail
+	1b	1b			1b				1b			4b		1b			1b			8b	6b
+======================================================================================*/
+void VHT_SIG_A_generator(
+	PRT_PMAC_TX_INFO	pPMacTxInfo,
+	PRT_PMAC_PKT_INFO	pPMacPktInfo)
+{
+	u32 i;
+	bool sig_bi[48], crc8[8];
+
+	memset(sig_bi, 0, 48);
+	memset(crc8, 0, 8);
+
+	/*	BW Setting*/
+	for (i = 0; i < 2; i++)
+		sig_bi[i] = (pPMacTxInfo->BandWidth >> i) & 0x1;
+	/* Reserved Bit*/
+	sig_bi[2] = 1;
+	/*STBC Field*/
+	sig_bi[3] = pPMacTxInfo->bSTBC;
+	/*Group ID: Single User->A value of 0 or 63 indicates an SU PPDU. */
+	for (i = 0; i < 6; i++)
+		sig_bi[4 + i] = 0;
+	/*	N_STS/Partial AID*/
+	for (i = 0; i < 12; i++) {
+		if (i < 3)
+			sig_bi[10 + i] = ((pPMacPktInfo->Nsts - 1) >> i) & 0x1;
+		else
+			sig_bi[10 + i] = 0;
+	}
+	/*TXOP_PS_NOT_ALLPWED*/
+	sig_bi[22]	= 0;
+	/*Reserved Bits*/
+	sig_bi[23]	= 1;
+	/*Short GI*/
+	sig_bi[24] = pPMacTxInfo->bSGI;
+	if (pPMacTxInfo->bSGI > 0 && (pPMacPktInfo->N_sym % 10) == 9)
+		sig_bi[25] = 1;
+	else
+		sig_bi[25] = 0;
+	/* SU/MU[0] Coding*/
+	sig_bi[26] = pPMacTxInfo->bLDPC;	/*	0:BCC, 1:LDPC		*/
+	sig_bi[27] = pPMacPktInfo->SIGA2B3;	/*/	Record Extra OFDM Symols is added or not when LDPC is used*/
+	/*SU MCS/MU[1-3] Coding*/
+	for (i = 0; i < 4; i++)
+		sig_bi[28 + i] = (pPMacPktInfo->MCS >> i) & 0x1;
+	/*SU Beamform */
+	sig_bi[32] = 0;	/*packet.TXBF_en;*/
+	/*Reserved Bit*/
+	sig_bi[33] = 1;
+	/*CRC-8*/
+	CRC8_generator(crc8, sig_bi, 34);
+	for (i = 0; i < 8; i++)
+		sig_bi[34 + i]	= crc8[i];
+	/*Tail*/
+	for (i = 42; i < 48; i++)
+		sig_bi[i] = 0;
+
+	memset(pPMacTxInfo->VHT_SIG_A, 0, 6);
+	ByteToBit(pPMacTxInfo->VHT_SIG_A, sig_bi, 6);
+}
+
+/*======================================================================================
+	VHT-SIG-B
+	Length				Resesrved	Trail
+	17/19/21 BIT		3/2/2 BIT	6b
+======================================================================================*/
+void VHT_SIG_B_generator(
+	PRT_PMAC_TX_INFO	pPMacTxInfo)
+{
+	bool sig_bi[32], crc8_bi[8];
+	u32 i, len, res, tail = 6, total_len, crc8_in_len;
+	u32 sigb_len;
+
+	memset(sig_bi, 0, 32);
+	memset(crc8_bi, 0, 8);
+
+	/*Sounding Packet*/
+	if (pPMacTxInfo->NDP_sound == 1) {
+		if (pPMacTxInfo->BandWidth == 0) {
+			bool sigb_temp[26] = {0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0};
+
+			memcpy(sig_bi, sigb_temp, 26);
+		} else if (pPMacTxInfo->BandWidth == 1) {
+			bool sigb_temp[27] = {1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0};
+
+			memcpy(sig_bi, sigb_temp, 27);
+		} else if (pPMacTxInfo->BandWidth == 2) {
+			bool sigb_temp[29] = {0, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0};
+
+			memcpy(sig_bi, sigb_temp, 29);
+		}
+	} else {	/* Not NDP Sounding*/
+		bool *sigb_temp[29] = {0};
+
+		if (pPMacTxInfo->BandWidth == 0) {
+			len = 17;
+			res = 3;
+		} else if (pPMacTxInfo->BandWidth == 1) {
+			len = 19;
+			res = 2;
+		} else if (pPMacTxInfo->BandWidth == 2) {
+			len	= 21;
+			res	= 2;
+		} else {
+			len	= 21;
+			res	= 2;
+		}
+		total_len = len + res + tail;
+		crc8_in_len = len + res;
+
+		/*Length Field*/
+		sigb_len = (pPMacTxInfo->PacketLength + 3) >> 2;
+
+		for (i = 0; i < len; i++)
+			sig_bi[i] = (sigb_len >> i) & 0x1;
+		/*Reserved Field*/
+		for (i = 0; i < res; i++)
+			sig_bi[len + i] = 1;
+		/* CRC-8*/
+		CRC8_generator(crc8_bi, sig_bi, crc8_in_len);
+
+		/* Tail */
+		for (i = 0; i < tail; i++)
+			sig_bi[len + res + i] = 0;
+	}
+
+	memset(pPMacTxInfo->VHT_SIG_B, 0, 4);
+	ByteToBit(pPMacTxInfo->VHT_SIG_B, sig_bi, 4);
+
+	pPMacTxInfo->VHT_SIG_B_CRC = 0;
+	ByteToBit(&(pPMacTxInfo->VHT_SIG_B_CRC), crc8_bi, 1);
+}
+
+/*=======================
+ VHT Delimiter
+=======================*/
+void VHT_Delimiter_generator(
+	PRT_PMAC_TX_INFO	pPMacTxInfo
+)
+{
+	bool sig_bi[32] = {0}, crc8[8] = {0};
+	u32 crc8_in_len = 16;
+	u32 PacketLength = pPMacTxInfo->PacketLength;
+	int j;
+
+	/* Delimiter[0]: EOF*/
+	sig_bi[0] = 1;
+	/* Delimiter[1]: Reserved*/
+	sig_bi[1] = 0;
+	/* Delimiter[3:2]: MPDU Length High*/
+	sig_bi[2] = ((PacketLength - 4) >> 12) % 2;
+	sig_bi[3] = ((PacketLength - 4) >> 13) % 2;
+	/* Delimiter[15:4]: MPDU Length Low*/
+	for (j = 4; j < 16; j++)
+		sig_bi[j] = ((PacketLength - 4) >> (j - 4)) % 2;
+	CRC8_generator(crc8, sig_bi, crc8_in_len);
+	for (j = 16; j < 24; j++) /* Delimiter[23:16]: CRC 8*/
+		sig_bi[j] = crc8[j - 16];
+	for (j = 24; j < 32; j++) /* Delimiter[31:24]: Signature ('4E' in Hex, 78 in Dec)*/
+		sig_bi[j]	= (78 >> (j - 24)) % 2;
+
+	memset(pPMacTxInfo->VHT_Delimiter, 0, 4);
+	ByteToBit(pPMacTxInfo->VHT_Delimiter, sig_bi, 4);
+}
+
+#endif
+#endif
diff --git a/drivers/staging/rtl8188eu/core/rtw_odm.c b/drivers/staging/rtl8188eu/core/rtw_odm.c
new file mode 100644
index 000000000000..922a7b96db16
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_odm.c
@@ -0,0 +1,430 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+
+#include <rtw_odm.h>
+#include <hal_data.h>
+
+
+
+/* set ODM_CMNINFO_IC_TYPE based on chip_type */
+void rtw_odm_init_ic_type(_adapter *adapter)
+{
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+	struct PHY_DM_STRUCT *odm = &hal_data->odmpriv;
+	u32 ic_type = chip_type_to_odm_ic_type(rtw_get_chip_type(adapter));
+
+	rtw_warn_on(!ic_type);
+
+	odm_cmn_info_init(odm, ODM_CMNINFO_IC_TYPE, ic_type);
+}
+
+inline void rtw_odm_set_force_igi_lb(_adapter *adapter, u8 lb)
+{
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+
+	hal_data->u1ForcedIgiLb = lb;
+}
+
+inline u8 rtw_odm_get_force_igi_lb(_adapter *adapter)
+{
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+
+	return hal_data->u1ForcedIgiLb;
+}
+
+static void rtw_odm_adaptivity_ver_msg(void *sel, _adapter *adapter)
+{
+	RTW_PRINT_SEL(sel, "ADAPTIVITY_VERSION "ADAPTIVITY_VERSION"\n");
+}
+
+#define RTW_ADAPTIVITY_EN_DISABLE 0
+#define RTW_ADAPTIVITY_EN_ENABLE 1
+
+static void rtw_odm_adaptivity_en_msg(void *sel, _adapter *adapter)
+{
+	struct registry_priv *regsty = &adapter->registrypriv;
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+	struct PHY_DM_STRUCT *odm = &hal_data->odmpriv;
+
+	RTW_PRINT_SEL(sel, "RTW_ADAPTIVITY_EN_");
+
+	if (regsty->adaptivity_en == RTW_ADAPTIVITY_EN_DISABLE)
+		_RTW_PRINT_SEL(sel, "DISABLE\n");
+	else if (regsty->adaptivity_en == RTW_ADAPTIVITY_EN_ENABLE)
+		_RTW_PRINT_SEL(sel, "ENABLE\n");
+	else
+		_RTW_PRINT_SEL(sel, "INVALID\n");
+}
+
+#define RTW_ADAPTIVITY_MODE_NORMAL 0
+#define RTW_ADAPTIVITY_MODE_CARRIER_SENSE 1
+
+static void rtw_odm_adaptivity_mode_msg(void *sel, _adapter *adapter)
+{
+	struct registry_priv *regsty = &adapter->registrypriv;
+
+	RTW_PRINT_SEL(sel, "RTW_ADAPTIVITY_MODE_");
+
+	if (regsty->adaptivity_mode == RTW_ADAPTIVITY_MODE_NORMAL)
+		_RTW_PRINT_SEL(sel, "NORMAL\n");
+	else if (regsty->adaptivity_mode == RTW_ADAPTIVITY_MODE_CARRIER_SENSE)
+		_RTW_PRINT_SEL(sel, "CARRIER_SENSE\n");
+	else
+		_RTW_PRINT_SEL(sel, "INVALID\n");
+}
+
+#define RTW_ADAPTIVITY_DML_DISABLE 0
+#define RTW_ADAPTIVITY_DML_ENABLE 1
+
+static void rtw_odm_adaptivity_dml_msg(void *sel, _adapter *adapter)
+{
+	struct registry_priv *regsty = &adapter->registrypriv;
+
+	RTW_PRINT_SEL(sel, "RTW_ADAPTIVITY_DML_");
+
+	if (regsty->adaptivity_dml == RTW_ADAPTIVITY_DML_DISABLE)
+		_RTW_PRINT_SEL(sel, "DISABLE\n");
+	else if (regsty->adaptivity_dml == RTW_ADAPTIVITY_DML_ENABLE)
+		_RTW_PRINT_SEL(sel, "ENABLE\n");
+	else
+		_RTW_PRINT_SEL(sel, "INVALID\n");
+}
+
+static void rtw_odm_adaptivity_dc_backoff_msg(void *sel, _adapter *adapter)
+{
+	struct registry_priv *regsty = &adapter->registrypriv;
+
+	RTW_PRINT_SEL(sel, "RTW_ADAPTIVITY_DC_BACKOFF:%u\n", regsty->adaptivity_dc_backoff);
+}
+
+void rtw_odm_adaptivity_config_msg(void *sel, _adapter *adapter)
+{
+	rtw_odm_adaptivity_ver_msg(sel, adapter);
+	rtw_odm_adaptivity_en_msg(sel, adapter);
+	rtw_odm_adaptivity_mode_msg(sel, adapter);
+	rtw_odm_adaptivity_dml_msg(sel, adapter);
+	rtw_odm_adaptivity_dc_backoff_msg(sel, adapter);
+}
+
+bool rtw_odm_adaptivity_needed(_adapter *adapter)
+{
+	struct registry_priv *regsty = &adapter->registrypriv;
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	bool ret = false;
+
+	if (regsty->adaptivity_en == RTW_ADAPTIVITY_EN_ENABLE)
+		ret = true;
+
+	return ret;
+}
+
+void rtw_odm_adaptivity_parm_msg(void *sel, _adapter *adapter)
+{
+	HAL_DATA_TYPE *pHalData = GET_HAL_DATA(adapter);
+	struct PHY_DM_STRUCT *odm = &pHalData->odmpriv;
+
+	rtw_odm_adaptivity_config_msg(sel, adapter);
+
+	RTW_PRINT_SEL(sel, "%10s %16s %16s %22s %12s\n"
+		, "th_l2h_ini", "th_edcca_hl_diff", "th_l2h_ini_mode2", "th_edcca_hl_diff_mode2", "edcca_enable");
+	RTW_PRINT_SEL(sel, "0x%-8x %-16d 0x%-14x %-22d %-12d\n"
+		, (u8)odm->th_l2h_ini
+		, odm->th_edcca_hl_diff
+		, (u8)odm->th_l2h_ini_mode2
+		, odm->th_edcca_hl_diff_mode2
+		, odm->edcca_enable
+	);
+
+	RTW_PRINT_SEL(sel, "%15s %9s\n", "AdapEnableState", "Adap_Flag");
+	RTW_PRINT_SEL(sel, "%-15x %-9x\n"
+		, odm->adaptivity_enable
+		, odm->adaptivity_flag
+	);
+}
+
+void rtw_odm_adaptivity_parm_set(_adapter *adapter, s8 th_l2h_ini, s8 th_edcca_hl_diff, s8 th_l2h_ini_mode2, s8 th_edcca_hl_diff_mode2, u8 edcca_enable)
+{
+	HAL_DATA_TYPE *pHalData = GET_HAL_DATA(adapter);
+	struct PHY_DM_STRUCT *odm = &pHalData->odmpriv;
+
+	odm->th_l2h_ini = th_l2h_ini;
+	odm->th_edcca_hl_diff = th_edcca_hl_diff;
+	odm->th_l2h_ini_mode2 = th_l2h_ini_mode2;
+	odm->th_edcca_hl_diff_mode2 = th_edcca_hl_diff_mode2;
+	odm->edcca_enable = edcca_enable;
+}
+
+void rtw_odm_get_perpkt_rssi(void *sel, _adapter *adapter)
+{
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+	struct PHY_DM_STRUCT *odm = &(hal_data->odmpriv);
+
+	RTW_PRINT_SEL(sel, "rx_rate = %s, RSSI_A = %d(%%), RSSI_B = %d(%%)\n",
+		      HDATA_RATE(odm->rx_rate), odm->RSSI_A, odm->RSSI_B);
+}
+
+
+void rtw_odm_acquirespinlock(_adapter *adapter,	enum rt_spinlock_type type)
+{
+	PHAL_DATA_TYPE	pHalData = GET_HAL_DATA(adapter);
+	unsigned long irqL;
+
+	switch (type) {
+	case RT_IQK_SPINLOCK:
+		_enter_critical_bh(&pHalData->IQKSpinLock, &irqL);
+	default:
+		break;
+	}
+}
+
+void rtw_odm_releasespinlock(_adapter *adapter,	enum rt_spinlock_type type)
+{
+	PHAL_DATA_TYPE	pHalData = GET_HAL_DATA(adapter);
+	unsigned long irqL;
+
+	switch (type) {
+	case RT_IQK_SPINLOCK:
+		_exit_critical_bh(&pHalData->IQKSpinLock, &irqL);
+	default:
+		break;
+	}
+}
+
+inline u8 rtw_odm_get_dfs_domain(_adapter *adapter)
+{
+#ifdef CONFIG_DFS_MASTER
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+	struct PHY_DM_STRUCT *pDM_Odm = &(hal_data->odmpriv);
+
+	return pDM_Odm->dfs_region_domain;
+#else
+	return PHYDM_DFS_DOMAIN_UNKNOWN;
+#endif
+}
+
+inline u8 rtw_odm_dfs_domain_unknown(_adapter *adapter)
+{
+#ifdef CONFIG_DFS_MASTER
+	return rtw_odm_get_dfs_domain(adapter) == PHYDM_DFS_DOMAIN_UNKNOWN;
+#else
+	return 1;
+#endif
+}
+
+#ifdef CONFIG_DFS_MASTER
+inline void rtw_odm_radar_detect_reset(_adapter *adapter)
+{
+	phydm_radar_detect_reset(GET_ODM(adapter));
+}
+
+inline void rtw_odm_radar_detect_disable(_adapter *adapter)
+{
+	phydm_radar_detect_disable(GET_ODM(adapter));
+}
+
+/* called after ch, bw is set */
+inline void rtw_odm_radar_detect_enable(_adapter *adapter)
+{
+	phydm_radar_detect_enable(GET_ODM(adapter));
+}
+
+inline bool rtw_odm_radar_detect(_adapter *adapter)
+{
+	return phydm_radar_detect(GET_ODM(adapter));
+}
+#endif /* CONFIG_DFS_MASTER */
+
+void rtw_odm_parse_rx_phy_status_chinfo(union recv_frame *rframe, u8 *phys)
+{
+#ifndef DBG_RX_PHYSTATUS_CHINFO
+#define DBG_RX_PHYSTATUS_CHINFO 0
+#endif
+
+#if (ODM_PHY_STATUS_NEW_TYPE_SUPPORT == 1)
+	_adapter *adapter = rframe->u.hdr.adapter;
+	struct PHY_DM_STRUCT *phydm = GET_ODM(adapter);
+	struct rx_pkt_attrib *attrib = &rframe->u.hdr.attrib;
+	u8 *wlanhdr = get_recvframe_data(rframe);
+
+	if (phydm->support_ic_type & ODM_IC_PHY_STATUE_NEW_TYPE) {
+		/*
+		* 8723D:
+		* type_0(CCK)
+		*     l_rxsc
+		*         is filled with primary channel SC, not real rxsc.
+		*         0:LSC, 1:USC
+		* type_1(OFDM)
+		*     rf_mode
+		*         RF bandwidth when RX
+		*     l_rxsc(legacy), ht_rxsc
+		*         see below RXSC N-series
+		* type_2(Not used)
+		*/
+		/*
+		* 8821C, 8822B:
+		* type_0(CCK)
+		*     l_rxsc
+		*         is filled with primary channel SC, not real rxsc.
+		*         0:LSC, 1:USC
+		* type_1(OFDM)
+		*     rf_mode
+		*         RF bandwidth when RX
+		*     l_rxsc(legacy), ht_rxsc
+		*         see below RXSC AC-series
+		* type_2(Not used)
+		*/
+
+		if ((*phys & 0xf) == 0) {
+			struct _phy_status_rpt_jaguar2_type0 *phys_t0 = (struct _phy_status_rpt_jaguar2_type0 *)phys;
+
+			if (DBG_RX_PHYSTATUS_CHINFO) {
+				RTW_PRINT("phys_t%u ta="MAC_FMT" %s, %s(band:%u, ch:%u, l_rxsc:%u)\n"
+					, *phys & 0xf
+					, MAC_ARG(get_ta(wlanhdr))
+					, is_broadcast_mac_addr(get_ra(wlanhdr)) ? "BC" : is_multicast_mac_addr(get_ra(wlanhdr)) ? "MC" : "UC"
+					, HDATA_RATE(attrib->data_rate)
+					, phys_t0->band, phys_t0->channel, phys_t0->rxsc
+				);
+			}
+
+		} else if ((*phys & 0xf) == 1) {
+			struct _phy_status_rpt_jaguar2_type1 *phys_t1 = (struct _phy_status_rpt_jaguar2_type1 *)phys;
+			u8 rxsc = (attrib->data_rate > DESC_RATE11M && attrib->data_rate < DESC_RATEMCS0) ? phys_t1->l_rxsc : phys_t1->ht_rxsc;
+			u8 pkt_cch = 0;
+			u8 pkt_bw = CHANNEL_WIDTH_20;
+
+			#if	ODM_IC_11N_SERIES_SUPPORT
+			if (phydm->support_ic_type & ODM_IC_11N_SERIES) {
+				/* RXSC N-series */
+				#define RXSC_DUP	0
+				#define RXSC_LSC	1
+				#define RXSC_USC	2
+				#define RXSC_40M	3
+
+				static const s8 cch_offset_by_rxsc[4] = {0, -2, 2, 0};
+
+				if (phys_t1->rf_mode == 0) {
+					pkt_cch = phys_t1->channel;
+					pkt_bw = CHANNEL_WIDTH_20;
+				} else if (phys_t1->rf_mode == 1) {
+					if (rxsc == RXSC_LSC || rxsc == RXSC_USC) {
+						pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc];
+						pkt_bw = CHANNEL_WIDTH_20;
+					} else if (rxsc == RXSC_40M) {
+						pkt_cch = phys_t1->channel;
+						pkt_bw = CHANNEL_WIDTH_40;
+					}
+				} else
+					rtw_warn_on(1);
+
+				goto type1_end;
+			}
+			#endif /* ODM_IC_11N_SERIES_SUPPORT */
+
+			#if	ODM_IC_11AC_SERIES_SUPPORT
+			if (phydm->support_ic_type & ODM_IC_11AC_SERIES) {
+				/* RXSC AC-series */
+				#define RXSC_DUP			0 /* 0: RX from all SC of current rf_mode */
+
+				#define RXSC_LL20M_OF_160M	8 /* 1~8: RX from 20MHz SC */
+				#define RXSC_L20M_OF_160M	6
+				#define RXSC_L20M_OF_80M	4
+				#define RXSC_L20M_OF_40M	2
+				#define RXSC_U20M_OF_40M	1
+				#define RXSC_U20M_OF_80M	3
+				#define RXSC_U20M_OF_160M	5
+				#define RXSC_UU20M_OF_160M	7
+
+				#define RXSC_L40M_OF_160M	12 /* 9~12: RX from 40MHz SC */
+				#define RXSC_L40M_OF_80M	10
+				#define RXSC_U40M_OF_80M	9
+				#define RXSC_U40M_OF_160M	11
+
+				#define RXSC_L80M_OF_160M	14 /* 13~14: RX from 80MHz SC */
+				#define RXSC_U80M_OF_160M	13
+
+				static const s8 cch_offset_by_rxsc[15] = {0, 2, -2, 6, -6, 10, -10, 14, -14, 4, -4, 12, -12, 8, -8};
+
+				if (phys_t1->rf_mode > 3) {
+					/* invalid rf_mode */
+					rtw_warn_on(1);
+					goto type1_end;
+				}
+
+				if (phys_t1->rf_mode == 0) {
+					/* RF 20MHz */
+					pkt_cch = phys_t1->channel;
+					pkt_bw = CHANNEL_WIDTH_20;
+					goto type1_end;
+				}
+
+				if (rxsc == 0) {
+					/* RF and RX with same BW */
+					if (attrib->data_rate >= DESC_RATEMCS0) {
+						pkt_cch = phys_t1->channel;
+						pkt_bw = phys_t1->rf_mode;
+					}
+					goto type1_end;
+				}
+
+				if ((phys_t1->rf_mode == 1 && rxsc >= 1 && rxsc <= 2) /* RF 40MHz, RX 20MHz */
+					|| (phys_t1->rf_mode == 2 && rxsc >= 1 && rxsc <= 4) /* RF 80MHz, RX 20MHz */
+					|| (phys_t1->rf_mode == 3 && rxsc >= 1 && rxsc <= 8) /* RF 160MHz, RX 20MHz */
+				) {
+					pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc];
+					pkt_bw = CHANNEL_WIDTH_20;
+				} else if ((phys_t1->rf_mode == 2 && rxsc >= 9 && rxsc <= 10) /* RF 80MHz, RX 40MHz */
+					|| (phys_t1->rf_mode == 3 && rxsc >= 9 && rxsc <= 12) /* RF 160MHz, RX 40MHz */
+				) {
+					if (attrib->data_rate >= DESC_RATEMCS0) {
+						pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc];
+						pkt_bw = CHANNEL_WIDTH_40;
+					}
+				} else if ((phys_t1->rf_mode == 3 && rxsc >= 13 && rxsc <= 14) /* RF 160MHz, RX 80MHz */
+				) {
+					if (attrib->data_rate >= DESC_RATEMCS0) {
+						pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc];
+						pkt_bw = CHANNEL_WIDTH_80;
+					}
+				} else
+					rtw_warn_on(1);
+
+			}
+			#endif /* ODM_IC_11AC_SERIES_SUPPORT */
+
+type1_end:
+			if (DBG_RX_PHYSTATUS_CHINFO) {
+				RTW_PRINT("phys_t%u ta="MAC_FMT" %s, %s(band:%u, ch:%u, rf_mode:%u, l_rxsc:%u, ht_rxsc:%u) => %u,%u\n"
+					, *phys & 0xf
+					, MAC_ARG(get_ta(wlanhdr))
+					, is_broadcast_mac_addr(get_ra(wlanhdr)) ? "BC" : is_multicast_mac_addr(get_ra(wlanhdr)) ? "MC" : "UC"
+					, HDATA_RATE(attrib->data_rate)
+					, phys_t1->band, phys_t1->channel, phys_t1->rf_mode, phys_t1->l_rxsc, phys_t1->ht_rxsc
+					, pkt_cch, pkt_bw
+				);
+			}
+
+			/* for now, only return cneter channel of 20MHz packet */
+			if (pkt_cch && pkt_bw == CHANNEL_WIDTH_20)
+				attrib->ch = pkt_cch;
+
+		} else {
+			struct _phy_status_rpt_jaguar2_type2 *phys_t2 = (struct _phy_status_rpt_jaguar2_type2 *)phys;
+
+			if (DBG_RX_PHYSTATUS_CHINFO) {
+				RTW_PRINT("phys_t%u ta="MAC_FMT" %s, %s(band:%u, ch:%u, l_rxsc:%u, ht_rxsc:%u)\n"
+					, *phys & 0xf
+					, MAC_ARG(get_ta(wlanhdr))
+					, is_broadcast_mac_addr(get_ra(wlanhdr)) ? "BC" : is_multicast_mac_addr(get_ra(wlanhdr)) ? "MC" : "UC"
+					, HDATA_RATE(attrib->data_rate)
+					, phys_t2->band, phys_t2->channel, phys_t2->l_rxsc, phys_t2->ht_rxsc
+				);
+			}
+		}
+	}
+#endif /* (ODM_PHY_STATUS_NEW_TYPE_SUPPORT == 1) */
+
+}
+
diff --git a/drivers/staging/rtl8188eu/core/rtw_p2p.c b/drivers/staging/rtl8188eu/core/rtw_p2p.c
new file mode 100644
index 000000000000..b3941354165a
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_p2p.c
@@ -0,0 +1,5351 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_P2P_C_
+
+#include <drv_types.h>
+
+#ifdef CONFIG_P2P
+
+void p2p_concurrent_handler(_adapter *padapter);
+
+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 int is_any_client_associated(_adapter *padapter)
+{
+	return padapter->stapriv.asoc_list_cnt ? true : false;
+}
+
+static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	unsigned long irqL;
+	_list	*phead, *plist;
+	u32 len = 0;
+	u16 attr_len = 0;
+	u8 tmplen, *pdata_attr, *pstart, *pcur;
+	struct sta_info *psta = NULL;
+	_adapter *padapter = pwdinfo->padapter;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	pdata_attr = rtw_zmalloc(MAX_P2P_IE_LEN);
+
+	if (NULL == pdata_attr) {
+		RTW_INFO("%s pdata_attr malloc failed\n", __func__);
+		goto _exit;
+	}
+
+	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);
+
+	rtw_mfree(pdata_attr, MAX_P2P_IE_LEN);
+
+_exit:
+	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;
+	__le16 *fctrl;
+	_adapter *padapter = pwdinfo->padapter;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	unsigned char category = RTW_WLAN_CATEGORY_P2P;/* P2P action frame	 */
+	__be32	p2poui = cpu_to_be32(P2POUI);
+	u8	oui_subtype = P2P_GO_DISC_REQUEST;
+	u8	dialogToken = 0;
+
+	RTW_INFO("[%s]\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	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++;
+	set_frame_sub_type(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;
+	__le16 *fctrl;
+	_adapter *padapter = pwdinfo->padapter;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_DEVDISC_RESP;
+	u8 p2pie[8] = { 0x00 };
+	u32 p2pielen = 0;
+
+	RTW_INFO("[%s]\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	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++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	/* Build P2P public action frame header */
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+
+	/* Build P2P IE */
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/* P2P_ATTR_STATUS */
+	p2pielen += rtw_set_p2p_attr_content(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+
+}
+
+static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8 *raddr, u8 *frame_body, u16 config_method)
+{
+	_adapter *padapter = pwdinfo->padapter;
+	unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
+	u8			action = P2P_PUB_ACTION_ACTION;
+	u8			dialogToken = frame_body[7];	/*	The Dialog Token of provisioning discovery request frame. */
+	__be32			p2poui = cpu_to_be32(P2POUI);
+	u8			oui_subtype = P2P_PROVISION_DISC_RESP;
+	u8			wpsie[100] = { 0x00 };
+	u8			wpsielen = 0;
+#ifdef CONFIG_WFD
+	u32					wfdielen = 0;
+#endif
+
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	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, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, adapter_mac_addr(padapter), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
+
+	wpsielen = 0;
+	/*	WPS OUI */
+	/* *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI ); */
+	RTW_PUT_BE32(wpsie, WPSOUI);
+	wpsielen += 4;
+
+	/*	Config Method */
+	/*	Type: */
+	/* *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD ); */
+	RTW_PUT_BE16(wpsie + wpsielen, WPS_ATTR_CONF_METHOD);
+	wpsielen += 2;
+
+	/*	Length: */
+	/* *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 ); */
+	RTW_PUT_BE16(wpsie + wpsielen, 0x0002);
+	wpsielen += 2;
+
+	/*	Value: */
+	/* *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( config_method ); */
+	RTW_PUT_BE16(wpsie + wpsielen, config_method);
+	wpsielen += 2;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen);
+
+#ifdef CONFIG_WFD
+	wfdielen = build_provdisc_resp_wfd_ie(pwdinfo, pframe);
+	pframe += wfdielen;
+	pattrib->pktlen += wfdielen;
+#endif
+
+	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;
+	__le16 *fctrl;
+	_adapter *padapter = pwdinfo->padapter;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	unsigned char category = RTW_WLAN_CATEGORY_P2P;/* P2P action frame	 */
+	__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;
+
+	RTW_INFO("[%s]\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	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++;
+	set_frame_sub_type(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: */
+
+	/* go_add_noa_attr(pwdinfo); */
+
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
+
+
+	return len;
+
+}
+
+#ifdef CONFIG_WFD
+u32 build_beacon_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u16 val16 = 0;
+	u32 len = 0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	/*	WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110812 */
+	/*	According to the WFD Specification, the beacon frame should contain 4 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID */
+	/*	3. Coupled Sink Information */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+
+	if (P2P_ROLE_GO == pwdinfo->role) {
+		if (is_any_client_associated(pwdinfo->padapter)) {
+			/*	WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) */
+			val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD;
+			RTW_PUT_BE16(wfdie + wfdielen, val16);
+		} else {
+			/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
+			val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
+			RTW_PUT_BE16(wfdie + wfdielen, val16);
+		}
+
+	} else {
+		/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */
+		val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
+		RTW_PUT_BE16(wfdie + wfdielen, val16);
+	}
+
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+	rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+
+u32 build_probe_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u16 val16 = 0;
+	u32 len = 0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	/*	WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110812 */
+	/*	According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID */
+	/*	3. Coupled Sink Information */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+
+	if (1 == pwdinfo->wfd_tdls_enable) {
+		/*	WFD primary sink + available for WFD session + WiFi TDLS mode + WSC ( WFD Service Discovery )	 */
+		val16 = pwfd_info->wfd_device_type |
+			WFD_DEVINFO_SESSION_AVAIL |
+			WFD_DEVINFO_WSD |
+			WFD_DEVINFO_PC_TDLS;
+		RTW_PUT_BE16(wfdie + wfdielen, val16);
+	} else {
+		/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSC ( WFD Service Discovery )	 */
+		val16 = pwfd_info->wfd_device_type |
+			WFD_DEVINFO_SESSION_AVAIL |
+			WFD_DEVINFO_WSD;
+		RTW_PUT_BE16(wfdie + wfdielen, val16);
+	}
+
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+	rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+
+u32 build_probe_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 tunneled)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len = 0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	/*	WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110812 */
+	/*	According to the WFD Specification, the probe response frame should contain 4 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID */
+	/*	3. Coupled Sink Information */
+	/*	4. WFD Session Information */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+	/*	WFD primary sink + available for WFD session + WiFi Direct mode */
+
+	if (pwdinfo->session_available) {
+		if (P2P_ROLE_GO == pwdinfo->role) {
+			if (is_any_client_associated(pwdinfo->padapter)) {
+				if (pwdinfo->wfd_tdls_enable) {
+					/*	TDLS mode + WSD ( WFD Service Discovery ) */
+					RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
+				} else {
+					/*	WiFi Direct mode + WSD ( WFD Service Discovery ) */
+					RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
+				}
+			} else {
+				if (pwdinfo->wfd_tdls_enable) {
+					/*	available for WFD session + TDLS mode + WSD ( WFD Service Discovery ) */
+					RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
+				} else {
+					/*	available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */
+					RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
+				}
+			}
+		} else {
+			if (pwdinfo->wfd_tdls_enable) {
+				/*	available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */
+				RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
+			} else {
+
+				/*	available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */
+				RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
+			}
+		}
+	} else {
+		if (pwdinfo->wfd_tdls_enable)
+			RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT);
+		else
+			RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT);
+
+	}
+
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		/*	WFD Session Information ATTR */
+		/*	Type: */
+		wfdie[wfdielen++] = WFD_ATTR_SESSION_INFO;
+
+		/*	Length: */
+		/*	Note: In the WFD specification, the size of length field is 2. */
+		RTW_PUT_BE16(wfdie + wfdielen, 0x0000);
+		wfdielen += 2;
+
+		/*	Todo: to add the list of WFD device info descriptor in WFD group. */
+
+	}
+#ifdef CONFIG_CONCURRENT_MODE
+#ifdef CONFIG_TDLS
+	{
+		int i;
+		_adapter *iface = NULL;
+		struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			iface = dvobj->padapters[i];
+			if ((iface) && rtw_is_adapter_up(iface)) {
+				if (iface == padapter)
+					continue;
+
+				if ((tunneled == 0) && (iface->wdinfo.wfd_tdls_enable == 1)) {
+					/*	Alternative MAC Address ATTR
+						Type:					*/
+					wfdie[wfdielen++] = WFD_ATTR_ALTER_MAC;
+
+					/*	Length:
+						Note: In the WFD specification, the size of length field is 2.*/
+					RTW_PUT_BE16(wfdie + wfdielen,  ETH_ALEN);
+					wfdielen += 2;
+
+					/*	Value:
+						Alternative MAC Address*/
+					memcpy(wfdie + wfdielen, adapter_mac_addr(iface), ETH_ALEN);
+					wfdielen += ETH_ALEN;
+				}
+			}
+		}
+	}
+
+#endif /* CONFIG_TDLS*/
+#endif /* CONFIG_CONCURRENT_MODE */
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+
+u32 build_assoc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u16 val16 = 0;
+	u32 len = 0, wfdielen = 0;
+	_adapter					*padapter = NULL;
+	struct mlme_priv			*pmlmepriv = NULL;
+	struct wifi_display_info		*pwfd_info = NULL;
+
+	padapter = pwdinfo->padapter;
+	pmlmepriv = &padapter->mlmepriv;
+	pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
+		goto exit;
+
+	/* WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110812 */
+	/*	According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID */
+	/*	3. Coupled Sink Information */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */
+	val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
+	RTW_PUT_BE16(wfdie + wfdielen, val16);
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+	rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+
+u32 build_assoc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len = 0, wfdielen = 0;
+	u16 val16 = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	/*	WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110812 */
+	/*	According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID */
+	/*	3. Coupled Sink Information */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */
+	val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
+	RTW_PUT_BE16(wfdie + wfdielen, val16);
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+	rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+
+u32 build_nego_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len = 0, wfdielen = 0;
+	u16 val16 = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	/*	WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110825 */
+	/*	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID ( Optional ) */
+	/*	3. Local IP Adress ( Optional ) */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+	/*	WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available */
+	val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL;
+	RTW_PUT_BE16(wfdie + wfdielen, val16);
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+	rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+
+u32 build_nego_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len = 0, wfdielen = 0;
+	u16 val16 = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	/*	WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110825 */
+	/*	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID ( Optional ) */
+	/*	3. Local IP Adress ( Optional ) */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+	/*	WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available */
+	val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL;
+	RTW_PUT_BE16(wfdie + wfdielen, val16);
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+
+	rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+
+u32 build_nego_confirm_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len = 0, wfdielen = 0;
+	u16 val16 = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	/*	WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110825 */
+	/*	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID ( Optional ) */
+	/*	3. Local IP Adress ( Optional ) */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+	/*	WFD primary sink + WiFi Direct mode + WSD ( WFD Service Discovery ) + WFD Session Available */
+	val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL;
+	RTW_PUT_BE16(wfdie + wfdielen, val16);
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+
+	pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+
+u32 build_invitation_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len = 0, wfdielen = 0;
+	u16 val16 = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	/*	WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110825 */
+	/*	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID ( Optional ) */
+	/*	3. Local IP Adress ( Optional ) */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */
+	val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
+	RTW_PUT_BE16(wfdie + wfdielen, val16);
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+	if (P2P_ROLE_GO == pwdinfo->role) {
+		/*	WFD Session Information ATTR */
+		/*	Type: */
+		wfdie[wfdielen++] = WFD_ATTR_SESSION_INFO;
+
+		/*	Length: */
+		/*	Note: In the WFD specification, the size of length field is 2. */
+		RTW_PUT_BE16(wfdie + wfdielen, 0x0000);
+		wfdielen += 2;
+
+		/*	Todo: to add the list of WFD device info descriptor in WFD group. */
+
+	}
+
+	rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+
+u32 build_invitation_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u16 val16 = 0;
+	u32 len = 0, wfdielen = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	/*	WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110825 */
+	/*	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID ( Optional ) */
+	/*	3. Local IP Adress ( Optional ) */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */
+	val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
+	RTW_PUT_BE16(wfdie + wfdielen, val16);
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+	if (P2P_ROLE_GO == pwdinfo->role) {
+		/*	WFD Session Information ATTR */
+		/*	Type: */
+		wfdie[wfdielen++] = WFD_ATTR_SESSION_INFO;
+
+		/*	Length: */
+		/*	Note: In the WFD specification, the size of length field is 2. */
+		RTW_PUT_BE16(wfdie + wfdielen, 0x0000);
+		wfdielen += 2;
+
+		/*	Todo: to add the list of WFD device info descriptor in WFD group. */
+
+	}
+
+	rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+
+u32 build_provdisc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len = 0, wfdielen = 0;
+	u16 val16 = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	/*	WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110825 */
+	/*	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID ( Optional ) */
+	/*	3. Local IP Adress ( Optional ) */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */
+	val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
+	RTW_PUT_BE16(wfdie + wfdielen, val16);
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+
+	rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+
+u32 build_provdisc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u32 len = 0, wfdielen = 0;
+	u16 val16 = 0;
+	_adapter *padapter = pwdinfo->padapter;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->wdinfo.wfd_info;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+	/*	WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/*	WFA WFD v1.0 */
+
+	/*	Commented by Albert 20110825 */
+	/*	According to the WFD Specification, the provision discovery response frame should contain 3 WFD attributes */
+	/*	1. WFD Device Information */
+	/*	2. Associated BSSID ( Optional ) */
+	/*	3. Local IP Adress ( Optional ) */
+
+
+	/*	WFD Device Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value1: */
+	/*	WFD device information */
+	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD ( WFD Service Discovery ) */
+	val16 = pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD;
+	RTW_PUT_BE16(wfdie + wfdielen, val16);
+	wfdielen += 2;
+
+	/*	Value2: */
+	/*	Session Management Control Port */
+	/*	Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->rtsp_ctrlport);
+	wfdielen += 2;
+
+	/*	Value3: */
+	/*	WFD Device Maximum Throughput */
+	/*	300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/*	Associated BSSID ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	wfdielen += ETH_ALEN;
+
+	/*	Coupled Sink Information ATTR */
+	/*	Type: */
+	wfdie[wfdielen++] = WFD_ATTR_COUPLED_SINK_INFO;
+
+	/*	Length: */
+	/*	Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0007);
+	wfdielen += 2;
+
+	/*	Value: */
+	/*	Coupled Sink Status bitmap */
+	/*	Not coupled/available for Coupling */
+	wfdie[wfdielen++] = 0;
+	/* MAC Addr. */
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+	wfdie[wfdielen++] = 0;
+
+	rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
+
+exit:
+	return len;
+}
+#endif /* CONFIG_WFD */
+
+u32 build_probe_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
+{
+	u8 p2pie[MAX_P2P_IE_LEN] = { 0x00 };
+	u32 len = 0, p2pielen = 0;
+#ifdef CONFIG_INTEL_WIDI
+	struct mlme_priv *pmlmepriv = &(pwdinfo->padapter->mlmepriv);
+	u8 zero_array_check[L2SDTA_SERVICE_VE_LEN] = { 0x00 };
+	u8 widi_version = 0, i = 0;
+
+	if (!memcmp(pmlmepriv->sa_ext, zero_array_check, L2SDTA_SERVICE_VE_LEN) == false)
+		widi_version = 35;
+	else if (pmlmepriv->num_p2p_sdt != 0)
+		widi_version = 40;
+#endif /* CONFIG_INTEL_WIDI */
+
+	/*	P2P OUI */
+	p2pielen = 0;
+	p2pie[p2pielen++] = 0x50;
+	p2pie[p2pielen++] = 0x6F;
+	p2pie[p2pielen++] = 0x9A;
+	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
+
+	/*	Commented by Albert 20100907 */
+	/*	According to the P2P Specification, the probe response frame should contain 5 P2P attributes */
+	/*	1. P2P Capability */
+	/*	2. Extended Listen Timing */
+	/*	3. Notice of Absence ( NOA )	( Only GO needs this ) */
+	/*	4. Device Info */
+	/*	5. Group Info	( Only GO need this ) */
+
+	/*	P2P Capability ATTR */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
+
+	/*	Length: */
+	/* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 ); */
+	RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Device Capability Bitmap, 1 byte */
+	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
+
+	/*	Group Capability Bitmap, 1 byte */
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		p2pie[p2pielen] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS);
+
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
+			p2pie[p2pielen] |= P2P_GRPCAP_GROUP_FORMATION;
+
+		p2pielen++;
+	} else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
+		/*	Group Capability Bitmap, 1 byte */
+		if (pwdinfo->persistent_supported)
+			p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
+		else
+			p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
+	}
+
+	/*	Extended Listen Timing ATTR */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
+
+	/*	Length: */
+	/* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 ); */
+	RTW_PUT_LE16(p2pie + p2pielen, 0x0004);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	Availability Period */
+	/* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF ); */
+	RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
+	p2pielen += 2;
+
+	/*	Availability Interval */
+	/* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF ); */
+	RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
+	p2pielen += 2;
+
+
+	/* Notice of Absence ATTR */
+	/*	Type:  */
+	/*	Length: */
+	/*	Value: */
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
+		/* go_add_noa_attr(pwdinfo); */
+	}
+
+	/*	Device Info ATTR */
+	/*	Type: */
+	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
+
+	/*	Length: */
+	/*	21->P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)  */
+	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
+	/* *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len ); */
+#ifdef CONFIG_INTEL_WIDI
+	if (widi_version == 35)
+		RTW_PUT_LE16(p2pie + p2pielen, 21 + 8 + pwdinfo->device_name_len);
+	else if (widi_version == 40)
+		RTW_PUT_LE16(p2pie + p2pielen, 21 + 8 * pmlmepriv->num_p2p_sdt + pwdinfo->device_name_len);
+	else
+#endif /* CONFIG_INTEL_WIDI */
+		RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	/*	P2P Device Address */
+	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;
+
+#ifdef CONFIG_INTEL_WIDI
+	if (widi_version == 40) {
+		/*	Primary Device Type */
+		/*	Category ID */
+		/* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA ); */
+		RTW_PUT_BE16(p2pie + p2pielen, pmlmepriv->p2p_pdt_cid);
+		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, pmlmepriv->p2p_pdt_scid);
+		p2pielen += 2;
+	} else
+#endif /* CONFIG_INTEL_WIDI */
+	{
+		/*	Primary Device Type */
+		/*	Category ID */
+		/* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA ); */
+		RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_MULIT_MEDIA);
+		p2pielen += 2;
+
+		/*	OUI */
+		/* *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI ); */
+		RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
+		p2pielen += 4;
+
+		/*	Sub Category ID */
+		/* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER ); */
+		RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_MEDIA_SERVER);
+		p2pielen += 2;
+	}
+
+	/*	Number of Secondary Device Types */
+#ifdef CONFIG_INTEL_WIDI
+	if (widi_version == 35) {
+		p2pie[p2pielen++] = 0x01;
+
+		RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_DISPLAYS);
+		p2pielen += 2;
+
+		RTW_PUT_BE32(p2pie + p2pielen, INTEL_DEV_TYPE_OUI);
+		p2pielen += 4;
+
+		RTW_PUT_BE16(p2pie + p2pielen, P2P_SCID_WIDI_CONSUMER_SINK);
+		p2pielen += 2;
+	} else if (widi_version == 40) {
+		p2pie[p2pielen++] = pmlmepriv->num_p2p_sdt;
+		for (; i < pmlmepriv->num_p2p_sdt; i++) {
+			RTW_PUT_BE16(p2pie + p2pielen, pmlmepriv->p2p_sdt_cid[i]);
+			p2pielen += 2;
+
+			RTW_PUT_BE32(p2pie + p2pielen, INTEL_DEV_TYPE_OUI);
+			p2pielen += 4;
+
+			RTW_PUT_BE16(p2pie + p2pielen, pmlmepriv->p2p_sdt_scid[i]);
+			p2pielen += 2;
+		}
+	} else
+#endif /* CONFIG_INTEL_WIDI */
+		p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
+
+	/*	Device Name */
+	/*	Type: */
+	/* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME ); */
+	RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
+	p2pielen += 2;
+
+	/*	Length: */
+	/* *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len ); */
+	RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
+	p2pielen += 2;
+
+	/*	Value: */
+	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) && !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 = get_frame_sub_type(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) {
+		RTW_INFO("[%s] P2P IE not Found!!\n", __func__);
+		status_code =  P2P_STATUS_FAIL_INVALID_PARAM;
+	} else
+		RTW_INFO("[%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)) {
+			RTW_INFO("[%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)) {
+			RTW_INFO("[%s] Got P2P DEVICE INFO Attr!!\n", __func__);
+			pattr_content = pbuf = rtw_zmalloc(attr_contentlen);
+			if (pattr_content) {
+				u8 num_of_secdev_type;
+				u16 dev_name_len;
+
+
+				rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO , pattr_content, (uint *)&attr_contentlen);
+
+				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);
+				}
+
+
+				/* dev_name_len = attr_contentlen - ETH_ALEN - 2 - 8 - 1 - (num_of_secdev_type*8); */
+				psta->dev_name_len = 0;
+				if (WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(__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);
+				}
+
+				rtw_mfree(pbuf, attr_contentlen);
+
+			}
+
+		}
+
+		/* Get the next P2P IE */
+		p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+
+	}
+
+	return status_code;
+
+}
+
+u32 process_p2p_devdisc_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	u8 *frame_body;
+	u8 status, dialogToken;
+	struct sta_info *psta = NULL;
+	_adapter *padapter = pwdinfo->padapter;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *p2p_ie;
+	u32	p2p_ielen = 0;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	dialogToken = frame_body[7];
+	status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
+
+	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 (!memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) &&
+			    !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;
+					_list	*phead, *plist;
+
+					_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+					phead = &pstapriv->asoc_list;
+					plist = get_next(phead);
+
+					/* look up sta asoc_queue */
+					while ((rtw_end_of_queue_search(phead, plist)) == false) {
+						psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+
+						plist = get_next(plist);
+
+						if (psta->is_p2p_device && (psta->dev_cap & P2P_DEVCAP_CLIENT_DISCOVERABILITY) &&
+						    !memcmp(psta->dev_addr, dev_addr, ETH_ALEN)) {
+
+							/* _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL); */
+							/* issue GO Discoverability Request */
+							issue_group_disc_req(pwdinfo, psta->hwaddr);
+							/* _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL); */
+
+							status = P2P_STATUS_SUCCESS;
+
+							break;
+						} else
+							status = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
+
+					}
+					_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
+
+				} else
+					status = P2P_STATUS_FAIL_INVALID_PARAM;
+
+			} else
+				status = P2P_STATUS_FAIL_INVALID_PARAM;
+
+		}
+
+	}
+
+
+	/* issue Device Discoverability Response */
+	issue_p2p_devdisc_resp(pwdinfo, get_addr2_ptr(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, get_addr2_ptr(pframe), frame_body, uconfig_method);
+		}
+	}
+	RTW_INFO("[%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_check_peer_oper_ch(struct mlme_ext_priv *pmlmeext, u8 ch)
+{
+	u8 i = 0;
+
+	for (i = 0; i < pmlmeext->max_chan_nums; i++) {
+		if (pmlmeext->channel_set[i].ChannelNum == ch)
+			return _SUCCESS;
+	}
+
+	return _FAIL;
+}
+
+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)
+{
+	_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;
+#ifdef CONFIG_WFD
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+#endif /* CONFIG_TDLS	 */
+#endif /* CONFIG_WFD */
+	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 (!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 {
+		RTW_INFO("[%s] WPS IE not Found!!\n", __func__);
+		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+		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) {
+		RTW_INFO("[%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[100] = { 0x00 };
+		uint	ch_cnt = 0;
+		u8	peer_ch_list[100] = { 0x00 };
+		u8	peer_ch_num = 0;
+		u8	ch_list_inclusioned[100] = { 0x00 };
+		u8	ch_num_inclusioned = 0;
+		u16	cap_attr;
+		u8 listen_ch_attr[5] = { 0x00 };
+		__le16 le_tmp;
+
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING);
+
+		/* Check P2P Capability ATTR */
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8 *)&le_tmp, (uint *)&attr_contentlen)) {
+			cap_attr = le16_to_cpu(le_tmp);
+
+#if defined(CONFIG_WFD) && defined(CONFIG_TDLS)
+			if (!(cap_attr & P2P_GRPCAP_INTRABSS))
+				ptdlsinfo->ap_prohibited = true;
+#endif /* defined(CONFIG_WFD) && defined(CONFIG_TDLS) */
+		}
+
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT , &attr_content, &attr_contentlen)) {
+			RTW_INFO("[%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);
+			}
+		}
+
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_LISTEN_CH, (u8 *)listen_ch_attr, (uint *) &attr_contentlen) && attr_contentlen == 5)
+			pwdinfo->nego_req_info.peer_ch = listen_ch_attr[4];
+
+		RTW_INFO(FUNC_ADPT_FMT" listen channel :%u\n", FUNC_ADPT_ARG(padapter), pwdinfo->nego_req_info.peer_ch);
+
+		attr_contentlen = 0;
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENDED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) {
+			if (attr_contentlen != ETH_ALEN)
+				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) {
+				RTW_INFO("[%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)) {
+#ifdef CONFIG_CONCURRENT_MODE
+					if (rtw_mi_check_status(padapter, MI_LINKED)
+					    && padapter->registrypriv.full_ch_in_p2p_handshake == 0) {
+						RTW_INFO("[%s] desired channel NOT Found!\n", __func__);
+						result = P2P_STATUS_FAIL_NO_COMMON_CH;
+						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+						break;
+					} else
+#endif /* CONFIG_CONCURRENT_MODE */
+					{
+						u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
+						attr_contentlen = 0;
+
+						if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
+							peer_operating_ch = operatingch_info[4];
+
+						if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
+							ch_list_inclusioned, ch_num_inclusioned)) {
+							/**
+							 *	Change our operating channel as peer's for compatibility.
+							 */
+							pwdinfo->operating_channel = peer_operating_ch;
+							RTW_INFO("[%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];
+							RTW_INFO("[%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);
+	}
+
+	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;
+	}
+
+#ifdef CONFIG_WFD
+	rtw_process_wfd_ies(padapter, pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, __func__);
+#endif
+
+	return result ;
+}
+
+u8 process_p2p_group_negotation_resp(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	_adapter *padapter = pwdinfo->padapter;
+	u8	result = P2P_STATUS_SUCCESS;
+	u32	p2p_ielen, wps_ielen;
+	u8 *ies;
+	u32 ies_len;
+	u8 *p2p_ie;
+#ifdef CONFIG_WFD
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+#endif /* CONFIG_TDLS	 */
+#endif /* CONFIG_WFD */
+
+	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+	/*	Be able to know which one is the P2P GO and which one is P2P client. */
+
+	if (rtw_get_wps_ie(ies, ies_len, NULL, &wps_ielen)) {
+
+	} else {
+		RTW_INFO("[%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 };
+		uint	ch_cnt = 0;
+		u8	ch_content[100] = { 0x00 };
+		u8	groupid[38];
+		u16	cap_attr;
+		u8	peer_ch_list[100] = { 0x00 };
+		u8	peer_ch_num = 0;
+		u8	ch_list_inclusioned[100] = { 0x00 };
+		u8	ch_num_inclusioned = 0;
+		__le16 le_tmp;
+
+		while (p2p_ie) {	/*	Found the P2P IE. */
+			/* Check P2P Capability ATTR */
+			if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8 *)&le_tmp, (uint *)&attr_contentlen)) {
+				cap_attr = le16_to_cpu(le_tmp);
+#ifdef CONFIG_TDLS
+				if (!(cap_attr & P2P_GRPCAP_INTRABSS))
+					ptdlsinfo->ap_prohibited = true;
+#endif /* CONFIG_TDLS */
+			}
+
+			rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
+			if (attr_contentlen == 1) {
+				RTW_INFO("[%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_INTENDED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) {
+				if (attr_contentlen != ETH_ALEN)
+					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)) {
+				RTW_INFO("[%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)) {
+				RTW_INFO("[%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)) {
+				RTW_INFO("[%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) {
+					RTW_INFO("[%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)) {
+#ifdef CONFIG_CONCURRENT_MODE
+						if (rtw_mi_check_status(padapter, MI_LINKED)
+						    && padapter->registrypriv.full_ch_in_p2p_handshake == 0) {
+							RTW_INFO("[%s] desired channel NOT Found!\n", __func__);
+							result = P2P_STATUS_FAIL_NO_COMMON_CH;
+							rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+							break;
+						} else
+#endif /* CONFIG_CONCURRENT_MODE */
+						{
+							u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
+							attr_contentlen = 0;
+
+							if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
+								peer_operating_ch = operatingch_info[4];
+
+							if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
+								ch_list_inclusioned, ch_num_inclusioned)) {
+								/**
+								 *	Change our operating channel as peer's for compatibility.
+								 */
+								pwdinfo->operating_channel = peer_operating_ch;
+								RTW_INFO("[%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];
+								RTW_INFO("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
+							}
+						}
+
+					}
+				}
+
+			} else
+				RTW_INFO("[%s] channel list attribute not found!\n", __func__);
+
+			/*	Try to get the group id information if peer is GO */
+			attr_contentlen = 0;
+			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);
+		}
+
+	}
+
+#ifdef CONFIG_WFD
+	rtw_process_wfd_ies(padapter, pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, __func__);
+#endif
+
+	return result ;
+
+}
+
+u8 process_p2p_group_negotation_confirm(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
+{
+	_adapter *padapter = pwdinfo->padapter;
+	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) {
+			RTW_INFO("[%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);
+				}
+
+#ifdef CONFIG_CONCURRENT_MODE
+				if (rtw_mi_check_status(padapter, MI_LINKED)
+				    && padapter->registrypriv.full_ch_in_p2p_handshake == 0) {
+					/*	Switch back to the AP channel soon. */
+					_set_timer(&pwdinfo->ap_p2p_switch_timer, 100);
+				}
+#endif
+			} else {
+				rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
+				break;
+			}
+		}
+
+		/*	Try to get the group id information */
+		attr_contentlen = 0;
+		memset(groupid, 0x00, 38);
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) {
+			RTW_INFO("[%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)) {
+			RTW_INFO("[%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, get_addr2_ptr(pframe), status, dialogToken);
+
+	return true;
+}
+
+static void find_phase_handler(_adapter	*padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	NDIS_802_11_SSID	ssid;
+	unsigned long				irqL;
+	u8					_status = 0;
+
+
+	memset((unsigned char *)&ssid, 0, sizeof(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);
+	_status = rtw_sitesurvey_cmd(padapter, &ssid, 1, NULL, 0);
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+
+
+}
+
+static void restore_p2p_state_handler(_adapter	*padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (rtw_mi_check_status(padapter, MI_LINKED)) {
+		u8 union_ch = rtw_mi_get_union_chan(padapter);
+		u8 union_bw = rtw_mi_get_union_bw(padapter);
+		u8 union_offset = rtw_mi_get_union_offset(padapter);
+
+		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP)) {
+			set_channel_bwmode(padapter, union_ch, union_offset, union_bw);
+			rtw_mi_buddy_issue_nulldata(padapter, NULL, 0, 3, 500);
+		}
+	}
+#endif
+
+	rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+
+	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
+#ifdef CONFIG_CONCURRENT_MODE
+		p2p_concurrent_handler(padapter);
+#else
+		/*	In the P2P client mode, the driver should not switch back to its listen channel */
+		/*	because this P2P client should stay at the operating channel of P2P GO. */
+		set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+#endif
+	}
+}
+
+static void pre_tx_invitereq_handler(_adapter	*padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	u8	val8 = 1;
+
+	set_channel_bwmode(padapter, pwdinfo->invitereq_info.peer_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, 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);
+
+}
+
+static void pre_tx_provdisc_handler(_adapter	*padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	u8	val8 = 1;
+
+	set_channel_bwmode(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, 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);
+
+}
+
+static void pre_tx_negoreq_handler(_adapter	*padapter)
+{
+	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
+	u8	val8 = 1;
+
+	set_channel_bwmode(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+	issue_probereq_p2p(padapter , NULL);
+	/* WIN Phone only accept unicast probe request when nego back */
+	issue_probereq_p2p(padapter , pwdinfo->nego_req_info.peerDevAddr);
+	_set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+
+}
+
+#ifdef CONFIG_CONCURRENT_MODE
+void p2p_concurrent_handler(_adapter	*padapter)
+{
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8					val8;
+
+	if (rtw_mi_check_status(padapter, MI_LINKED)) {
+		u8 union_ch = rtw_mi_get_union_chan(padapter);
+		u8 union_bw = rtw_mi_get_union_bw(padapter);
+		u8 union_offset = rtw_mi_get_union_offset(padapter);
+
+		pwdinfo->operating_channel = union_ch;
+
+		if (pwdinfo->driver_interface == DRIVER_CFG80211) {
+			RTW_INFO("%s, switch ch back to union_ch=%d\n", __func__, union_ch);
+			set_channel_bwmode(padapter, union_ch, union_offset, union_bw);
+
+			rtw_mi_buddy_issue_nulldata(padapter, NULL, 0, 3, 500);
+
+		} else if (pwdinfo->driver_interface == DRIVER_WEXT) {
+			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) {
+				/*	Now, the driver stays on the AP's channel. */
+				/*	If the pwdinfo->ext_listen_period = 0, that means the P2P listen state is not available on listen channel. */
+				if (pwdinfo->ext_listen_period > 0) {
+					RTW_INFO("[%s] P2P_STATE_IDLE, ext_listen_period = %d\n", __func__, pwdinfo->ext_listen_period);
+
+					if (union_ch != pwdinfo->listen_channel) {
+						/*	Will switch to listen channel so that need to send the NULL data with PW bit to AP. */
+						rtw_mi_buddy_issue_nulldata(padapter, NULL, 1, 3, 500);
+						set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+					}
+
+					rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
+
+					if (!rtw_mi_check_mlmeinfo_state(padapter, WIFI_FW_AP_STATE)) {
+						val8 = 1;
+						rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+					}
+					/*	Todo: To check the value of pwdinfo->ext_listen_period is equal to 0 or not. */
+					_set_timer(&pwdinfo->ap_p2p_switch_timer, pwdinfo->ext_listen_period);
+				}
+			} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN) ||
+				rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL) ||
+				(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) && pwdinfo->nego_req_info.benable == false) ||
+				rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ)) {
+				/*	Now, the driver is in the listen state of P2P mode. */
+				RTW_INFO("[%s] P2P_STATE_IDLE, ext_listen_interval = %d\n", __func__, pwdinfo->ext_listen_interval);
+
+				/*	Commented by Albert 2012/11/01 */
+				/*	If the AP's channel is the same as the listen channel, we should still be in the listen state */
+				/*	Other P2P device is still able to find this device out even this device is in the AP's channel. */
+				/*	So, configure this device to be able to receive the probe request frame and set it to listen state. */
+				if (union_ch != pwdinfo->listen_channel) {
+
+					set_channel_bwmode(padapter, union_ch, union_offset, union_bw);
+					if (!rtw_mi_check_status(padapter, MI_AP_MODE)) {
+						val8 = 0;
+						rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+					}
+					rtw_p2p_set_state(pwdinfo, P2P_STATE_IDLE);
+					rtw_mi_buddy_issue_nulldata(padapter, NULL, 0, 3, 500);
+				}
+
+				/*	Todo: To check the value of pwdinfo->ext_listen_interval is equal to 0 or not. */
+				_set_timer(&pwdinfo->ap_p2p_switch_timer, pwdinfo->ext_listen_interval);
+			} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_OK)) {
+				/*	The driver had finished the P2P handshake successfully. */
+				val8 = 0;
+				rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+				set_channel_bwmode(padapter, union_ch, union_offset, union_bw);
+				rtw_mi_buddy_issue_nulldata(padapter, NULL, 0, 3, 500);
+			} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
+				val8 = 1;
+				set_channel_bwmode(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+				rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+				issue_probereq_p2p(padapter, NULL);
+				_set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+			} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) && pwdinfo->nego_req_info.benable) {
+				val8 = 1;
+				set_channel_bwmode(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+				rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+				issue_probereq_p2p(padapter, NULL);
+				_set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT);
+			} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ) && pwdinfo->invitereq_info.benable) {
+				/*
+				val8 = 1;
+				set_channel_bwmode(padapter, , HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+				rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+				issue_probereq_p2p(padapter, NULL);
+				_set_timer( &pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT );
+				*/
+			}
+		}
+	} else {
+		/* In p2p+softap. When in P2P_STATE_GONEGO_OK, not back to listen channel.*/
+		if (!rtw_p2p_chk_state(pwdinfo , P2P_STATE_GONEGO_OK) || padapter->registrypriv.full_ch_in_p2p_handshake == 0)
+			set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+		else
+			RTW_INFO("%s, buddy not linked, go nego ok, not back to listen channel\n", __func__);
+	}
+
+}
+#endif
+
+#ifdef CONFIG_IOCTL_CFG80211
+static int ro_ch_handler(_adapter *adapter, u8 *buf)
+{
+	/* TODO: move remain on channel logical here */
+	return H2C_SUCCESS;
+}
+
+static int cancel_ro_ch_handler(_adapter *padapter, u8 *buf)
+{
+	int ret = H2C_SUCCESS;
+	struct p2p_roch_parm *roch_parm = (struct p2p_roch_parm *)buf;
+	struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(padapter);
+	struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
+	struct wireless_dev *wdev;
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+	u8 ch, bw, offset;
+
+	_enter_critical_mutex(&pwdev_priv->roch_mutex, NULL);
+
+	if (rtw_cfg80211_get_is_roch(padapter) != true)
+		goto exit;
+
+	if (roch_parm->wdev && roch_parm->cookie) {
+		if (pcfg80211_wdinfo->ro_ch_wdev != roch_parm->wdev) {
+			RTW_WARN(FUNC_ADPT_FMT" ongoing wdev:%p, wdev:%p\n"
+				, FUNC_ADPT_ARG(padapter), pcfg80211_wdinfo->ro_ch_wdev, roch_parm->wdev);
+			rtw_warn_on(1);
+		}
+
+		if (pcfg80211_wdinfo->remain_on_ch_cookie != roch_parm->cookie) {
+			RTW_WARN(FUNC_ADPT_FMT" ongoing cookie:0x%llx, cookie:0x%llx\n"
+				, FUNC_ADPT_ARG(padapter), pcfg80211_wdinfo->remain_on_ch_cookie, roch_parm->cookie);
+			rtw_warn_on(1);
+		}
+	}
+
+	if (rtw_mi_get_ch_setting_union(padapter, &ch, &bw, &offset) != 0) {
+		if (0)
+			RTW_INFO(FUNC_ADPT_FMT" back to linked/linking union - ch:%u, bw:%u, offset:%u\n",
+				 FUNC_ADPT_ARG(padapter), ch, bw, offset);
+	} else if (adapter_wdev_data(padapter)->p2p_enabled && pwdinfo->listen_channel) {
+		ch = pwdinfo->listen_channel;
+		bw = CHANNEL_WIDTH_20;
+		offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+		if (0)
+			RTW_INFO(FUNC_ADPT_FMT" back to listen ch - ch:%u, bw:%u, offset:%u\n",
+				 FUNC_ADPT_ARG(padapter), ch, bw, offset);
+	} else {
+		ch = pcfg80211_wdinfo->restore_channel;
+		bw = CHANNEL_WIDTH_20;
+		offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+		if (0)
+			RTW_INFO(FUNC_ADPT_FMT" back to restore ch - ch:%u, bw:%u, offset:%u\n",
+				 FUNC_ADPT_ARG(padapter), ch, bw, offset);
+	}
+
+	set_channel_bwmode(padapter, ch, offset, bw);
+
+	rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
+#ifdef CONFIG_DEBUG_CFG80211
+	RTW_INFO("%s, role=%d, p2p_state=%d\n", __func__, rtw_p2p_role(pwdinfo), rtw_p2p_state(pwdinfo));
+#endif
+
+	wdev = pcfg80211_wdinfo->ro_ch_wdev;
+
+	rtw_cfg80211_set_is_roch(padapter, false);
+	pcfg80211_wdinfo->ro_ch_wdev = NULL;
+	pcfg80211_wdinfo->last_ro_ch_time = jiffies;
+
+	rtw_cfg80211_remain_on_channel_expired(wdev
+		, pcfg80211_wdinfo->remain_on_ch_cookie
+		, &pcfg80211_wdinfo->remain_on_ch_channel
+		, pcfg80211_wdinfo->remain_on_ch_type, GFP_KERNEL);
+
+	RTW_INFO("cfg80211_remain_on_channel_expired cookie:0x%llx\n"
+		, pcfg80211_wdinfo->remain_on_ch_cookie);
+
+#ifdef CONFIG_BT_COEXIST
+	rtw_btcoex_ScanNotify(padapter, false);
+#endif
+
+exit:
+	_exit_critical_mutex(&pwdev_priv->roch_mutex, NULL);
+
+	return ret;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+static void ro_ch_timer_process (void *FunctionContext)
+#else
+static void ro_ch_timer_process(struct timer_list *t)
+#endif
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_adapter *adapter = (_adapter *)FunctionContext;
+#else
+	_adapter *adapter = from_timer(adapter, t, cfg80211_wdinfo.remain_on_ch_timer);
+#endif
+
+	p2p_cancel_roch_cmd(adapter, 0, NULL, 0);
+}
+
+static void rtw_change_p2pie_op_ch(_adapter *padapter, const u8 *frame_body, u32 len, u8 ch)
+{
+	u8 *ies, *p2p_ie;
+	u32 ies_len, p2p_ielen;
+
+#ifdef CONFIG_MCC_MODE
+	if (MCC_EN(padapter))
+		return;
+#endif /* CONFIG_MCC_MODE */
+
+	ies = (u8 *)(frame_body + _PUBLIC_ACTION_IE_OFFSET_);
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+	p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+
+	while (p2p_ie) {
+		u32	attr_contentlen = 0;
+		u8 *pattr = NULL;
+
+		/* Check P2P_ATTR_OPERATING_CH */
+		attr_contentlen = 0;
+		pattr = NULL;
+		pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, (uint *)&attr_contentlen);
+		if (pattr != NULL)
+			*(pattr + 4) = ch;
+
+		/* 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);
+	}
+}
+
+static void rtw_change_p2pie_ch_list(_adapter *padapter, const u8 *frame_body, u32 len, u8 ch)
+{
+	u8 *ies, *p2p_ie;
+	u32 ies_len, p2p_ielen;
+
+#ifdef CONFIG_MCC_MODE
+	if (MCC_EN(padapter))
+		return;
+#endif /* CONFIG_MCC_MODE */
+
+	ies = (u8 *)(frame_body + _PUBLIC_ACTION_IE_OFFSET_);
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+	p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+
+	while (p2p_ie) {
+		u32	attr_contentlen = 0;
+		u8 *pattr = NULL;
+
+		/* Check P2P_ATTR_CH_LIST */
+		pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, (uint *)&attr_contentlen);
+		if (pattr != NULL) {
+			int i;
+			u32 num_of_ch;
+			u8 *pattr_temp = pattr + 3 ;
+
+			attr_contentlen -= 3;
+
+			while (attr_contentlen > 0) {
+				num_of_ch = *(pattr_temp + 1);
+
+				for (i = 0; i < num_of_ch; i++)
+					*(pattr_temp + 2 + i) = ch;
+
+				pattr_temp += (2 + num_of_ch);
+				attr_contentlen -= (2 + num_of_ch);
+			}
+		}
+
+		/* 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);
+	}
+}
+
+static bool rtw_chk_p2pie_ch_list_with_buddy(_adapter *padapter, const u8 *frame_body, u32 len)
+{
+	bool fit = false;
+#ifdef CONFIG_CONCURRENT_MODE
+	u8 *ies, *p2p_ie;
+	u32 ies_len, p2p_ielen;
+	u8 union_ch = rtw_mi_get_union_chan(padapter);
+
+	ies = (u8 *)(frame_body + _PUBLIC_ACTION_IE_OFFSET_);
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+	p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+
+	while (p2p_ie) {
+		u32	attr_contentlen = 0;
+		u8 *pattr = NULL;
+
+		/* Check P2P_ATTR_CH_LIST */
+		pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, (uint *)&attr_contentlen);
+		if (pattr != NULL) {
+			int i;
+			u32 num_of_ch;
+			u8 *pattr_temp = pattr + 3 ;
+
+			attr_contentlen -= 3;
+
+			while (attr_contentlen > 0) {
+				num_of_ch = *(pattr_temp + 1);
+
+				for (i = 0; i < num_of_ch; i++) {
+					if (*(pattr_temp + 2 + i) == union_ch) {
+						RTW_INFO(FUNC_ADPT_FMT" ch_list fit buddy_ch:%u\n", FUNC_ADPT_ARG(padapter), union_ch);
+						fit = true;
+						break;
+					}
+				}
+
+				pattr_temp += (2 + num_of_ch);
+				attr_contentlen -= (2 + num_of_ch);
+			}
+		}
+
+		/* Get the next P2P IE */
+		p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+	}
+#endif
+	return fit;
+}
+
+static bool rtw_chk_p2pie_op_ch_with_buddy(_adapter *padapter, const u8 *frame_body, u32 len)
+{
+	bool fit = false;
+#ifdef CONFIG_CONCURRENT_MODE
+	u8 *ies, *p2p_ie;
+	u32 ies_len, p2p_ielen;
+	u8 union_ch = rtw_mi_get_union_chan(padapter);
+
+	ies = (u8 *)(frame_body + _PUBLIC_ACTION_IE_OFFSET_);
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+	p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+
+	while (p2p_ie) {
+		u32	attr_contentlen = 0;
+		u8 *pattr = NULL;
+
+		/* Check P2P_ATTR_OPERATING_CH */
+		attr_contentlen = 0;
+		pattr = NULL;
+		pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, (uint *)&attr_contentlen);
+		if (pattr != NULL) {
+			if (*(pattr + 4) == union_ch) {
+				RTW_INFO(FUNC_ADPT_FMT" op_ch fit buddy_ch:%u\n", FUNC_ADPT_ARG(padapter), union_ch);
+				fit = true;
+				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);
+	}
+#endif
+	return fit;
+}
+
+static void rtw_cfg80211_adjust_p2pie_channel(_adapter *padapter, const u8 *frame_body, u32 len)
+{
+#ifdef CONFIG_CONCURRENT_MODE
+	u8 *ies, *p2p_ie;
+	u32 ies_len, p2p_ielen;
+	u8 union_ch = rtw_mi_get_union_chan(padapter);
+
+#ifdef CONFIG_MCC_MODE
+	if (MCC_EN(padapter))
+		return;
+#endif /* CONFIG_MCC_MODE */
+
+	ies = (u8 *)(frame_body + _PUBLIC_ACTION_IE_OFFSET_);
+	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
+
+	p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+
+	while (p2p_ie) {
+		u32	attr_contentlen = 0;
+		u8 *pattr = NULL;
+
+		/* Check P2P_ATTR_CH_LIST */
+		pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, (uint *)&attr_contentlen);
+		if (pattr != NULL) {
+			int i;
+			u32 num_of_ch;
+			u8 *pattr_temp = pattr + 3 ;
+
+			attr_contentlen -= 3;
+
+			while (attr_contentlen > 0) {
+				num_of_ch = *(pattr_temp + 1);
+
+				for (i = 0; i < num_of_ch; i++) {
+					if (*(pattr_temp + 2 + i) && *(pattr_temp + 2 + i) != union_ch) {
+						#ifdef RTW_SINGLE_WIPHY
+						RTW_ERR("replace ch_list:%u with:%u\n", *(pattr_temp + 2 + i), union_ch);
+						#endif
+						*(pattr_temp + 2 + i) = union_ch; /*forcing to the same channel*/
+					}
+				}
+
+				pattr_temp += (2 + num_of_ch);
+				attr_contentlen -= (2 + num_of_ch);
+			}
+		}
+
+		/* Check P2P_ATTR_OPERATING_CH */
+		attr_contentlen = 0;
+		pattr = NULL;
+		pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, (uint *)&attr_contentlen);
+		if (pattr != NULL) {
+			if (*(pattr + 4) && *(pattr + 4) != union_ch) {
+				#ifdef RTW_SINGLE_WIPHY
+				RTW_ERR("replace op_ch:%u with:%u\n", *(pattr + 4), union_ch);
+				#endif
+				*(pattr + 4) = union_ch; /*forcing to the same channel	*/
+			}
+		}
+
+		/* Get the next P2P IE */
+		p2p_ie = rtw_get_p2p_ie(p2p_ie + p2p_ielen, ies_len - (p2p_ie - ies + p2p_ielen), NULL, &p2p_ielen);
+
+	}
+
+#endif
+}
+
+#ifdef CONFIG_WFD
+static u32 rtw_xframe_build_wfd_ie(struct xmit_frame *xframe)
+{
+	_adapter *adapter = xframe->padapter;
+	struct wifidirect_info *wdinfo = &adapter->wdinfo;
+	u8 *frame = xframe->buf_addr + TXDESC_OFFSET;
+	u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 *frame_tail = frame + xframe->attrib.pktlen;
+	u8 category, action, OUI_Subtype, dialogToken = 0;
+	u32	wfdielen = 0;
+
+	category = frame_body[0];
+	if (category == RTW_WLAN_CATEGORY_PUBLIC) {
+		action = frame_body[1];
+		if (action == ACT_PUBLIC_VENDOR
+		    && !memcmp(frame_body + 2, P2P_OUI, 4)
+		   ) {
+			OUI_Subtype = frame_body[6];
+			dialogToken = frame_body[7];
+
+			switch (OUI_Subtype) {
+			case P2P_GO_NEGO_REQ:
+				wfdielen = build_nego_req_wfd_ie(wdinfo, frame_tail);
+				break;
+			case P2P_GO_NEGO_RESP:
+				wfdielen = build_nego_resp_wfd_ie(wdinfo, frame_tail);
+				break;
+			case P2P_GO_NEGO_CONF:
+				wfdielen = build_nego_confirm_wfd_ie(wdinfo, frame_tail);
+				break;
+			case P2P_INVIT_REQ:
+				wfdielen = build_invitation_req_wfd_ie(wdinfo, frame_tail);
+				break;
+			case P2P_INVIT_RESP:
+				wfdielen = build_invitation_resp_wfd_ie(wdinfo, frame_tail);
+				break;
+			case P2P_PROVISION_DISC_REQ:
+				wfdielen = build_provdisc_req_wfd_ie(wdinfo, frame_tail);
+				break;
+			case P2P_PROVISION_DISC_RESP:
+				wfdielen = build_provdisc_resp_wfd_ie(wdinfo, frame_tail);
+				break;
+			case P2P_DEVDISC_REQ:
+			case P2P_DEVDISC_RESP:
+			default:
+				break;
+			}
+
+		}
+	} else if (category == RTW_WLAN_CATEGORY_P2P) {
+		OUI_Subtype = frame_body[5];
+		dialogToken = frame_body[6];
+
+#ifdef CONFIG_DEBUG_CFG80211
+		RTW_INFO("ACTION_CATEGORY_P2P: OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n"
+			, cpu_to_be32(*((u32 *)(frame_body + 1))), OUI_Subtype, dialogToken);
+#endif
+
+		switch (OUI_Subtype) {
+		case P2P_NOTICE_OF_ABSENCE:
+			break;
+		case P2P_PRESENCE_REQUEST:
+			break;
+		case P2P_PRESENCE_RESPONSE:
+			break;
+		case P2P_GO_DISC_REQUEST:
+			break;
+		default:
+			break;
+		}
+	} else
+		RTW_INFO("%s, action frame category=%d\n", __func__, category);
+
+	xframe->attrib.pktlen += wfdielen;
+
+	return wfdielen;
+}
+#endif /* CONFIG_WFD */
+
+static bool rtw_xframe_del_wfd_ie(struct xmit_frame *xframe)
+{
+#define DBG_XFRAME_DEL_WFD_IE 0
+
+	_adapter *adapter = xframe->padapter;
+	u8 *frame = xframe->buf_addr + TXDESC_OFFSET;
+	u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 *frame_tail = frame + xframe->attrib.pktlen;
+	u8 category, action, OUI_Subtype;
+	u8 *ies = NULL;
+	uint ies_len_ori = 0;
+	uint ies_len = 0;
+
+	category = frame_body[0];
+	if (category == RTW_WLAN_CATEGORY_PUBLIC) {
+		action = frame_body[1];
+		if (action == ACT_PUBLIC_VENDOR
+		    && !memcmp(frame_body + 2, P2P_OUI, 4)
+		   ) {
+			OUI_Subtype = frame_body[6];
+
+			switch (OUI_Subtype) {
+			case P2P_GO_NEGO_REQ:
+			case P2P_GO_NEGO_RESP:
+			case P2P_GO_NEGO_CONF:
+			case P2P_INVIT_REQ:
+			case P2P_INVIT_RESP:
+			case P2P_PROVISION_DISC_REQ:
+			case P2P_PROVISION_DISC_RESP:
+				ies = frame_body + 8;
+				ies_len_ori = frame_tail - (frame_body + 8);
+				break;
+			}
+		}
+	}
+
+	if (ies && ies_len_ori) {
+		ies_len = rtw_del_wfd_ie(ies, ies_len_ori, DBG_XFRAME_DEL_WFD_IE ? __func__ : NULL);
+		xframe->attrib.pktlen -= (ies_len_ori - ies_len);
+	}
+
+	return ies_len_ori != ies_len;
+}
+
+/*
+* rtw_xframe_chk_wfd_ie -
+*
+*/
+void rtw_xframe_chk_wfd_ie(struct xmit_frame *xframe)
+{
+	_adapter *adapter = xframe->padapter;
+	u8 *frame = xframe->buf_addr + TXDESC_OFFSET;
+	u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 *frame_tail = frame + xframe->attrib.pktlen;
+
+	struct wifidirect_info *wdinfo = &adapter->wdinfo;
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	u8 build = 0;
+	u8 del = 0;
+
+	if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
+		del = 1;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (wdinfo->wfd_info->wfd_enable)
+#endif
+		del = build = 1;
+
+	if (del)
+		rtw_xframe_del_wfd_ie(xframe);
+
+#ifdef CONFIG_WFD
+	if (build)
+		rtw_xframe_build_wfd_ie(xframe);
+#endif
+}
+
+static u8 *dump_p2p_attr_ch_list(u8 *p2p_ie, uint p2p_ielen, u8 *buf, u32 buf_len)
+{
+	uint attr_contentlen = 0;
+	u8 *pattr = NULL;
+	int w_sz = 0;
+	u8 ch_cnt = 0;
+	u8 ch_list[40];
+	bool continuous = false;
+
+	pattr = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, NULL, &attr_contentlen);
+	if (pattr != NULL) {
+		int i, j;
+		u32 num_of_ch;
+		u8 *pattr_temp = pattr + 3 ;
+
+		attr_contentlen -= 3;
+
+		memset(ch_list, 0, 40);
+
+		while (attr_contentlen > 0) {
+			num_of_ch = *(pattr_temp + 1);
+
+			for (i = 0; i < num_of_ch; i++) {
+				for (j = 0; j < ch_cnt; j++) {
+					if (ch_list[j] == *(pattr_temp + 2 + i))
+						break;
+				}
+				if (j >= ch_cnt)
+					ch_list[ch_cnt++] = *(pattr_temp + 2 + i);
+
+			}
+
+			pattr_temp += (2 + num_of_ch);
+			attr_contentlen -= (2 + num_of_ch);
+		}
+
+		for (j = 0; j < ch_cnt; j++) {
+			if (j == 0)
+				w_sz += snprintf(buf + w_sz, buf_len - w_sz, "%u", ch_list[j]);
+			else if (ch_list[j] - ch_list[j - 1] != 1)
+				w_sz += snprintf(buf + w_sz, buf_len - w_sz, ", %u", ch_list[j]);
+			else if (j != ch_cnt - 1 && ch_list[j + 1] - ch_list[j] == 1) {
+				/* empty */
+			} else
+				w_sz += snprintf(buf + w_sz, buf_len - w_sz, "-%u", ch_list[j]);
+		}
+	}
+	return buf;
+}
+
+/*
+ * return true if requester is GO, false if responder is GO
+ */
+static bool rtw_p2p_nego_intent_compare(u8 req, u8 resp)
+{
+	if (req >> 1 == resp >> 1)
+		return  req & 0x01 ? true : false;
+	else if (req >> 1 > resp >> 1)
+		return true;
+	else
+		return false;
+}
+
+int rtw_p2p_check_frames(_adapter *padapter, const u8 *buf, u32 len, u8 tx)
+{
+	int is_p2p_frame = (-1);
+	unsigned char	*frame_body;
+	u8 category, action, OUI_Subtype, dialogToken = 0;
+	u8 *p2p_ie = NULL;
+	uint p2p_ielen = 0;
+	struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(padapter);
+	int status = -1;
+	u8 ch_list_buf[128] = {'\0'};
+	int op_ch = -1;
+	int listen_ch = -1;
+	u8 intent = 0;
+	u8 *iaddr = NULL;
+	u8 *gbssid = NULL;
+
+	frame_body = (unsigned char *)(buf + sizeof(struct rtw_ieee80211_hdr_3addr));
+	category = frame_body[0];
+	/* just for check */
+	if (category == RTW_WLAN_CATEGORY_PUBLIC) {
+		action = frame_body[1];
+		if (action == ACT_PUBLIC_VENDOR
+			&& !memcmp(frame_body + 2, P2P_OUI, 4)
+		) {
+			OUI_Subtype = frame_body[6];
+			dialogToken = frame_body[7];
+			is_p2p_frame = OUI_Subtype;
+
+			#ifdef CONFIG_DEBUG_CFG80211
+			RTW_INFO("ACTION_CATEGORY_PUBLIC: ACT_PUBLIC_VENDOR, OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n",
+				cpu_to_be32(*((u32 *)(frame_body + 2))), OUI_Subtype, dialogToken);
+			#endif
+
+			p2p_ie = rtw_get_p2p_ie(
+				(u8 *)buf + sizeof(struct rtw_ieee80211_hdr_3addr) + _PUBLIC_ACTION_IE_OFFSET_
+				, len - sizeof(struct rtw_ieee80211_hdr_3addr) - _PUBLIC_ACTION_IE_OFFSET_
+				, NULL, &p2p_ielen);
+
+			switch (OUI_Subtype) { /* OUI Subtype */
+				u8 *cont;
+				uint cont_len;
+			case P2P_GO_NEGO_REQ: {
+				struct rtw_wdev_nego_info *nego_info = &pwdev_priv->nego_info;
+
+				if (tx) {
+					#ifdef CONFIG_DRV_ISSUE_PROV_REQ /* IOT FOR S2 */
+					if (pwdev_priv->provdisc_req_issued == false)
+						rtw_cfg80211_issue_p2p_provision_request(padapter, buf, len);
+					#endif /* CONFIG_DRV_ISSUE_PROV_REQ */
+
+					/* pwdev_priv->provdisc_req_issued = false; */
+
+					#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
+					if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
+						rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr));
+					#endif
+				}
+
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len);
+				if (cont)
+					op_ch = *(cont + 4);
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_LISTEN_CH, NULL, &cont_len);
+				if (cont)
+					listen_ch = *(cont + 4);
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, NULL, &cont_len);
+				if (cont)
+					intent = *cont;
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENDED_IF_ADDR, NULL, &cont_len);
+				if (cont && cont_len == 6)
+					iaddr = cont;
+
+				if (nego_info->token != dialogToken)
+					rtw_wdev_nego_info_init(nego_info);
+
+				memcpy(nego_info->peer_mac, tx ? GetAddr1Ptr(buf) : get_addr2_ptr(buf), ETH_ALEN);
+				if (iaddr)
+					memcpy(tx ? nego_info->iface_addr : nego_info->peer_iface_addr, iaddr, ETH_ALEN);
+				nego_info->active = tx ? 1 : 0;
+				nego_info->token = dialogToken;
+				nego_info->req_op_ch = op_ch;
+				nego_info->req_listen_ch = listen_ch;
+				nego_info->req_intent = intent;
+				nego_info->state = 0;
+
+				dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128);
+				RTW_INFO("RTW_%s:P2P_GO_NEGO_REQ, dialogToken=%d, intent:%u%s, listen_ch:%d, op_ch:%d, ch_list:%s"
+					, (tx) ? "Tx" : "Rx" , dialogToken , (intent >> 1) , intent & 0x1 ? "+" : "-" , listen_ch , op_ch , ch_list_buf);
+				if (iaddr)
+					_RTW_INFO(", iaddr:"MAC_FMT, MAC_ARG(iaddr));
+				_RTW_INFO("\n");
+
+				if (!tx) {
+					#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
+					if (rtw_mi_check_status(padapter, MI_LINKED)
+					    && rtw_chk_p2pie_ch_list_with_buddy(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)) == false
+					    && padapter->registrypriv.full_ch_in_p2p_handshake == 0) {
+						RTW_INFO(FUNC_ADPT_FMT" ch_list has no intersect with buddy\n", FUNC_ADPT_ARG(padapter));
+						rtw_change_p2pie_ch_list(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr), 0);
+					}
+					#endif
+				}
+
+				break;
+			}
+			case P2P_GO_NEGO_RESP: {
+				struct rtw_wdev_nego_info *nego_info = &pwdev_priv->nego_info;
+
+				if (tx) {
+					#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
+					if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
+						rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr));
+					#endif
+				}
+
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len);
+				if (cont)
+					op_ch = *(cont + 4);
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, NULL, &cont_len);
+				if (cont)
+					intent = *cont;
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
+				if (cont)
+					status = *cont;
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENDED_IF_ADDR, NULL, &cont_len);
+				if (cont && cont_len == 6)
+					iaddr = cont;
+
+				if (nego_info->token == dialogToken && nego_info->state == 0
+					&& !memcmp(nego_info->peer_mac, tx ? GetAddr1Ptr(buf) : get_addr2_ptr(buf), ETH_ALEN)
+				) {
+					if (iaddr)
+						memcpy(tx ? nego_info->iface_addr : nego_info->peer_iface_addr, iaddr, ETH_ALEN);
+					nego_info->status = (status == -1) ? 0xff : status;
+					nego_info->rsp_op_ch = op_ch;
+					nego_info->rsp_intent = intent;
+					nego_info->state = 1;
+					if (status != 0)
+						nego_info->token = 0; /* init */
+				}
+
+				dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128);
+				RTW_INFO("RTW_%s:P2P_GO_NEGO_RESP, dialogToken=%d, intent:%u%s, status:%d, op_ch:%d, ch_list:%s"
+					, (tx) ? "Tx" : "Rx", dialogToken, (intent >> 1), intent & 0x1 ? "+" : "-", status, op_ch, ch_list_buf);
+				if (iaddr)
+					_RTW_INFO(", iaddr:"MAC_FMT, MAC_ARG(iaddr));
+				_RTW_INFO("\n");
+
+				if (!tx) {
+					pwdev_priv->provdisc_req_issued = false;
+					#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
+					if (rtw_mi_check_status(padapter, MI_LINKED)
+					    && rtw_chk_p2pie_ch_list_with_buddy(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)) == false
+					    && padapter->registrypriv.full_ch_in_p2p_handshake == 0) {
+						RTW_INFO(FUNC_ADPT_FMT" ch_list has no intersect with buddy\n", FUNC_ADPT_ARG(padapter));
+						rtw_change_p2pie_ch_list(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr), 0);
+					}
+					#endif
+				}
+
+				break;
+			}
+			case P2P_GO_NEGO_CONF: {
+				struct rtw_wdev_nego_info *nego_info = &pwdev_priv->nego_info;
+				bool is_go = false;
+
+				if (tx) {
+					#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
+					if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
+						rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr));
+					#endif
+				}
+
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len);
+				if (cont)
+					op_ch = *(cont + 4);
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
+				if (cont)
+					status = *cont;
+
+				if (nego_info->token == dialogToken && nego_info->state == 1
+				    && !memcmp(nego_info->peer_mac, tx ? GetAddr1Ptr(buf) : get_addr2_ptr(buf), ETH_ALEN)
+				   ) {
+					nego_info->status = (status == -1) ? 0xff : status;
+					nego_info->conf_op_ch = (op_ch == -1) ? 0 : op_ch;
+					nego_info->state = 2;
+
+					if (status == 0) {
+						if (rtw_p2p_nego_intent_compare(nego_info->req_intent, nego_info->rsp_intent) ^ !tx)
+							is_go = true;
+					}
+
+					nego_info->token = 0; /* init */
+				}
+
+				dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128);
+				RTW_INFO("RTW_%s:P2P_GO_NEGO_CONF, dialogToken=%d, status:%d, op_ch:%d, ch_list:%s\n"
+					, (tx) ? "Tx" : "Rx", dialogToken, status, op_ch, ch_list_buf);
+
+				if (!tx) {
+				}
+
+				break;
+			}
+			case P2P_INVIT_REQ: {
+				struct rtw_wdev_invit_info *invit_info = &pwdev_priv->invit_info;
+				int flags = -1;
+
+				if (tx) {
+					#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
+					if (rtw_mi_check_status(padapter, MI_LINKED)
+					    && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
+						rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr));
+					#endif
+				}
+
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, NULL, &cont_len);
+				if (cont)
+					flags = *cont;
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len);
+				if (cont)
+					op_ch = *(cont + 4);
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_BSSID, NULL, &cont_len);
+				if (cont && cont_len == 6)
+					gbssid = cont;
+
+				if (invit_info->token != dialogToken)
+					rtw_wdev_invit_info_init(invit_info);
+
+				memcpy(invit_info->peer_mac, tx ? GetAddr1Ptr(buf) : get_addr2_ptr(buf), ETH_ALEN);
+				if (gbssid)
+					memcpy(invit_info->group_bssid, gbssid, ETH_ALEN);
+				invit_info->active = tx ? 1 : 0;
+				invit_info->token = dialogToken;
+				invit_info->flags = (flags == -1) ? 0x0 : flags;
+				invit_info->req_op_ch = op_ch;
+				invit_info->state = 0;
+
+				dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128);
+				RTW_INFO("RTW_%s:P2P_INVIT_REQ, dialogToken=%d, flags:0x%02x, op_ch:%d, ch_list:%s"
+					, (tx) ? "Tx" : "Rx", dialogToken, flags, op_ch, ch_list_buf);
+				if (gbssid)
+					_RTW_INFO(", gbssid:"MAC_FMT, MAC_ARG(gbssid));
+				_RTW_INFO("\n");
+
+				if (!tx) {
+					#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
+					if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0) {
+						if (op_ch != -1 && rtw_chk_p2pie_op_ch_with_buddy(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)) == false) {
+							RTW_INFO(FUNC_ADPT_FMT" op_ch:%u has no intersect with buddy\n", FUNC_ADPT_ARG(padapter), op_ch);
+							rtw_change_p2pie_ch_list(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr), 0);
+						} else if (rtw_chk_p2pie_ch_list_with_buddy(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr)) == false) {
+							RTW_INFO(FUNC_ADPT_FMT" ch_list has no intersect with buddy\n", FUNC_ADPT_ARG(padapter));
+							rtw_change_p2pie_ch_list(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr), 0);
+						}
+					}
+					#endif
+				}
+
+				break;
+			}
+			case P2P_INVIT_RESP: {
+				struct rtw_wdev_invit_info *invit_info = &pwdev_priv->invit_info;
+
+				if (tx) {
+					#if defined(CONFIG_CONCURRENT_MODE) && defined(CONFIG_CFG80211_ONECHANNEL_UNDER_CONCURRENT)
+					if (rtw_mi_check_status(padapter, MI_LINKED) && padapter->registrypriv.full_ch_in_p2p_handshake == 0)
+						rtw_cfg80211_adjust_p2pie_channel(padapter, frame_body, len - sizeof(struct rtw_ieee80211_hdr_3addr));
+					#endif
+				}
+
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
+				if (cont) {
+					#ifdef CONFIG_P2P_INVITE_IOT
+					if (tx && *cont == 7) {
+						RTW_INFO("TX_P2P_INVITE_RESP, status is no common channel, change to unknown group\n");
+						*cont = 8; /* unknow group status */
+					}
+					#endif /* CONFIG_P2P_INVITE_IOT */
+					status = *cont;
+				}
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len);
+				if (cont)
+					op_ch = *(cont + 4);
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_BSSID, NULL, &cont_len);
+				if (cont && cont_len == 6)
+					gbssid = cont;
+
+				if (invit_info->token == dialogToken && invit_info->state == 0
+				    && !memcmp(invit_info->peer_mac, tx ? GetAddr1Ptr(buf) : get_addr2_ptr(buf), ETH_ALEN)
+				   ) {
+					invit_info->status = (status == -1) ? 0xff : status;
+					invit_info->rsp_op_ch = op_ch;
+					invit_info->state = 1;
+					invit_info->token = 0; /* init */
+				}
+
+				dump_p2p_attr_ch_list(p2p_ie, p2p_ielen, ch_list_buf, 128);
+				RTW_INFO("RTW_%s:P2P_INVIT_RESP, dialogToken=%d, status:%d, op_ch:%d, ch_list:%s"
+					, (tx) ? "Tx" : "Rx", dialogToken, status, op_ch, ch_list_buf);
+				if (gbssid)
+					_RTW_INFO(", gbssid:"MAC_FMT, MAC_ARG(gbssid));
+				_RTW_INFO("\n");
+
+				if (!tx) {
+				}
+
+				break;
+			}
+			case P2P_DEVDISC_REQ:
+				RTW_INFO("RTW_%s:P2P_DEVDISC_REQ, dialogToken=%d\n", (tx) ? "Tx" : "Rx", dialogToken);
+				break;
+			case P2P_DEVDISC_RESP:
+				cont = rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
+				RTW_INFO("RTW_%s:P2P_DEVDISC_RESP, dialogToken=%d, status:%d\n", (tx) ? "Tx" : "Rx", dialogToken, cont ? *cont : -1);
+				break;
+			case P2P_PROVISION_DISC_REQ: {
+				size_t frame_body_len = len - sizeof(struct rtw_ieee80211_hdr_3addr);
+				u8 *p2p_ie;
+				uint p2p_ielen = 0;
+				uint contentlen = 0;
+
+				RTW_INFO("RTW_%s:P2P_PROVISION_DISC_REQ, dialogToken=%d\n", (tx) ? "Tx" : "Rx", dialogToken);
+
+				/* if(tx) */
+				{
+					pwdev_priv->provdisc_req_issued = false;
+
+					p2p_ie = rtw_get_p2p_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, frame_body_len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen);
+					if (p2p_ie) {
+
+						if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, NULL, &contentlen)) {
+							pwdev_priv->provdisc_req_issued = false;/* case: p2p_client join p2p GO */
+						} else {
+							#ifdef CONFIG_DEBUG_CFG80211
+							RTW_INFO("provdisc_req_issued is true\n");
+							#endif /*CONFIG_DEBUG_CFG80211*/
+							pwdev_priv->provdisc_req_issued = true;/* case: p2p_devices connection before Nego req. */
+						}
+
+					}
+				}
+			}
+			break;
+			case P2P_PROVISION_DISC_RESP:
+				RTW_INFO("RTW_%s:P2P_PROVISION_DISC_RESP, dialogToken=%d\n", (tx) ? "Tx" : "Rx", dialogToken);
+				break;
+			default:
+				RTW_INFO("RTW_%s:OUI_Subtype=%d, dialogToken=%d\n", (tx) ? "Tx" : "Rx", OUI_Subtype, dialogToken);
+				break;
+			}
+
+		}
+
+	} else if (category == RTW_WLAN_CATEGORY_P2P) {
+		OUI_Subtype = frame_body[5];
+		dialogToken = frame_body[6];
+
+		#ifdef CONFIG_DEBUG_CFG80211
+		RTW_INFO("ACTION_CATEGORY_P2P: OUI=0x%x, OUI_Subtype=%d, dialogToken=%d\n",
+			cpu_to_be32(*((u32 *)(frame_body + 1))), OUI_Subtype, dialogToken);
+		#endif
+
+		is_p2p_frame = OUI_Subtype;
+
+		switch (OUI_Subtype) {
+		case P2P_NOTICE_OF_ABSENCE:
+			RTW_INFO("RTW_%s:P2P_NOTICE_OF_ABSENCE, dialogToken=%d\n", (tx) ? "TX" : "RX", dialogToken);
+			break;
+		case P2P_PRESENCE_REQUEST:
+			RTW_INFO("RTW_%s:P2P_PRESENCE_REQUEST, dialogToken=%d\n", (tx) ? "TX" : "RX", dialogToken);
+			break;
+		case P2P_PRESENCE_RESPONSE:
+			RTW_INFO("RTW_%s:P2P_PRESENCE_RESPONSE, dialogToken=%d\n", (tx) ? "TX" : "RX", dialogToken);
+			break;
+		case P2P_GO_DISC_REQUEST:
+			RTW_INFO("RTW_%s:P2P_GO_DISC_REQUEST, dialogToken=%d\n", (tx) ? "TX" : "RX", dialogToken);
+			break;
+		default:
+			RTW_INFO("RTW_%s:OUI_Subtype=%d, dialogToken=%d\n", (tx) ? "TX" : "RX", OUI_Subtype, dialogToken);
+			break;
+		}
+
+	} else
+		RTW_INFO("RTW_%s:action frame category=%d\n", (tx) ? "TX" : "RX", category);
+
+	return is_p2p_frame;
+}
+
+void rtw_init_cfg80211_wifidirect_info(_adapter	*padapter)
+{
+	struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
+
+	memset(pcfg80211_wdinfo, 0x00, sizeof(struct cfg80211_wifidirect_info));
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_init_timer(&pcfg80211_wdinfo->remain_on_ch_timer, padapter->pnetdev, ro_ch_timer_process, padapter);
+#else
+	timer_setup(&pcfg80211_wdinfo->remain_on_ch_timer, ro_ch_timer_process, 0);
+#endif
+}
+#endif /* CONFIG_IOCTL_CFG80211	 */
+
+s32 p2p_protocol_wk_hdl(_adapter *padapter, int intCmdType, u8 *buf)
+{
+	int ret = H2C_SUCCESS;
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+	switch (intCmdType) {
+	case P2P_FIND_PHASE_WK:
+		find_phase_handler(padapter);
+		break;
+
+	case P2P_RESTORE_STATE_WK:
+		restore_p2p_state_handler(padapter);
+		break;
+
+	case P2P_PRE_TX_PROVDISC_PROCESS_WK:
+#ifdef CONFIG_CONCURRENT_MODE
+		if (rtw_mi_check_status(padapter, MI_LINKED))
+			p2p_concurrent_handler(padapter);
+		else
+			pre_tx_provdisc_handler(padapter);
+#else
+		pre_tx_provdisc_handler(padapter);
+#endif
+		break;
+
+	case P2P_PRE_TX_INVITEREQ_PROCESS_WK:
+#ifdef CONFIG_CONCURRENT_MODE
+		if (rtw_mi_check_status(padapter, MI_LINKED))
+			p2p_concurrent_handler(padapter);
+		else
+			pre_tx_invitereq_handler(padapter);
+#else
+		pre_tx_invitereq_handler(padapter);
+#endif
+		break;
+
+	case P2P_PRE_TX_NEGOREQ_PROCESS_WK:
+#ifdef CONFIG_CONCURRENT_MODE
+		if (rtw_mi_check_status(padapter, MI_LINKED))
+			p2p_concurrent_handler(padapter);
+		else
+			pre_tx_negoreq_handler(padapter);
+#else
+		pre_tx_negoreq_handler(padapter);
+#endif
+		break;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	case P2P_AP_P2P_CH_SWITCH_PROCESS_WK:
+		p2p_concurrent_handler(padapter);
+		break;
+#endif
+
+#ifdef CONFIG_IOCTL_CFG80211
+	case P2P_RO_CH_WK:
+		ret = ro_ch_handler(padapter, buf);
+		break;
+	case P2P_CANCEL_RO_CH_WK:
+		ret = cancel_ro_ch_handler(padapter, buf);
+		break;
+#endif
+
+	default:
+		rtw_warn_on(1);
+		break;
+	}
+
+	return ret;
+}
+
+int process_p2p_cross_connect_ie(PADAPTER padapter, u8 *IEs, u32 IELength)
+{
+	int ret = true;
+	u8 *ies;
+	u32 ies_len;
+	u8 *p2p_ie;
+	u32	p2p_ielen = 0;
+	u8	p2p_attr[MAX_P2P_IE_LEN] = { 0x00 };/* NoA length should be n*(13) + 2 */
+	u32	attr_contentlen = 0;
+
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+
+	if (IELength <= _BEACON_IE_OFFSET_)
+		return ret;
+
+	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) {
+		/* Get P2P Manageability IE. */
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_MANAGEABILITY, p2p_attr, &attr_contentlen)) {
+			if ((p2p_attr[0] & (BIT(0) | BIT(1))) == 0x01)
+				ret = false;
+			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);
+	}
+
+	return ret;
+}
+
+#ifdef CONFIG_P2P_PS
+void process_p2p_ps_ie(PADAPTER padapter, u8 *IEs, u32 IELength)
+{
+	u8 *ies;
+	u32 ies_len;
+	u8 *p2p_ie;
+	u32	p2p_ielen = 0;
+	u8	noa_attr[MAX_P2P_IE_LEN] = { 0x00 };/* NoA length should be n*(13) + 2 */
+	u32	attr_contentlen = 0;
+
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	u8	find_p2p = false, find_p2p_ps = false;
+	u8	noa_offset, noa_num, noa_index;
+
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+#ifdef CONFIG_CONCURRENT_MODE
+#ifndef CONFIG_FW_MULTI_PORT_SUPPORT
+	if (padapter->hw_port != HW_PORT0)
+		return;
+#endif
+#endif
+	if (IELength <= _BEACON_IE_OFFSET_)
+		return;
+
+	ies = IEs + _BEACON_IE_OFFSET_;
+	ies_len = IELength - _BEACON_IE_OFFSET_;
+
+	p2p_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &p2p_ielen);
+
+	while (p2p_ie) {
+		find_p2p = true;
+		/* Get Notice of Absence IE. */
+		if (rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_NOA, noa_attr, &attr_contentlen)) {
+			find_p2p_ps = true;
+			noa_index = noa_attr[0];
+
+			if ((pwdinfo->p2p_ps_mode == P2P_PS_NONE) ||
+			    (noa_index != pwdinfo->noa_index)) { /* if index change, driver should reconfigure related setting. */
+				pwdinfo->noa_index = noa_index;
+				pwdinfo->opp_ps = noa_attr[1] >> 7;
+				pwdinfo->ctwindow = noa_attr[1] & 0x7F;
+
+				noa_offset = 2;
+				noa_num = 0;
+				/* NoA length should be n*(13) + 2 */
+				if (attr_contentlen > 2) {
+					while (noa_offset < attr_contentlen) {
+						/* 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 (adapter_to_pwrctl(padapter)->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 == false))
+			p2p_ps_wk_cmd(padapter, P2P_PS_DISABLE, 1);
+	}
+
+}
+
+void p2p_ps_wk_hdl(_adapter *padapter, u8 p2p_ps_state)
+{
+	struct pwrctrl_priv		*pwrpriv = adapter_to_pwrctl(padapter);
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+
+
+	/* 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 (pwrpriv->bFwCurrentInPSMode) {
+			if (pwrpriv->smart_ps == 0) {
+				pwrpriv->smart_ps = 2;
+				rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&(pwrpriv->pwr_mode)));
+			}
+		}
+		break;
+	case P2P_PS_ENABLE:
+		if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
+#ifdef CONFIG_MCC_MODE
+			if (MCC_EN(padapter)) {
+				if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_DOING_MCC)) {
+					RTW_INFO("P2P PS enble under MCC\n");
+					rtw_warn_on(1);
+				}
+
+			}
+#endif /* CONFIG_MCC_MODE */
+			pwdinfo->p2p_ps_state = p2p_ps_state;
+
+			if (pwdinfo->ctwindow > 0) {
+				if (pwrpriv->smart_ps != 0) {
+					pwrpriv->smart_ps = 0;
+					RTW_INFO("%s(): Enter CTW, change SmartPS\n", __func__);
+					rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&(pwrpriv->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;
+	}
+
+}
+
+u8 p2p_ps_wk_cmd(_adapter *padapter, u8 p2p_ps_state, u8 enqueue)
+{
+	struct cmd_obj	*ph2c;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
+	u8	res = _SUCCESS;
+
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)
+#ifdef CONFIG_CONCURRENT_MODE
+#ifndef CONFIG_FW_MULTI_PORT_SUPPORT
+	    || (padapter->hw_port != HW_PORT0)
+#endif
+#endif
+	   )
+		return res;
+
+	if (enqueue) {
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+		if (pdrvextra_cmd_parm == NULL) {
+			rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm->ec_id = P2P_PS_WK_CID;
+		pdrvextra_cmd_parm->type = p2p_ps_state;
+		pdrvextra_cmd_parm->size = 0;
+		pdrvextra_cmd_parm->pbuf = NULL;
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+		res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+	} else
+		p2p_ps_wk_hdl(padapter, p2p_ps_state);
+
+exit:
+
+
+	return res;
+
+}
+#endif /* CONFIG_P2P_PS */
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+static void reset_ch_sitesurvey_timer_process (void *FunctionContext)
+#else
+static void reset_ch_sitesurvey_timer_process(struct timer_list *t)
+#endif
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_adapter *adapter = (_adapter *)FunctionContext;
+#else
+	_adapter *adapter = from_timer(adapter, t, wdinfo.reset_ch_sitesurvey);
+#endif
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	RTW_INFO("[%s] In\n", __func__);
+	/*	Reset the operation channel information */
+	pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
+#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
+	pwdinfo->rx_invitereq_info.operation_ch[1] = 0;
+	pwdinfo->rx_invitereq_info.operation_ch[2] = 0;
+	pwdinfo->rx_invitereq_info.operation_ch[3] = 0;
+#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */
+	pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+static void reset_ch_sitesurvey_timer_process2 (void *FunctionContext)
+#else
+static void reset_ch_sitesurvey_timer_process2(struct timer_list *t)
+#endif
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_adapter *adapter = (_adapter *)FunctionContext;
+#else
+	_adapter *adapter = from_timer(adapter, t, wdinfo.reset_ch_sitesurvey2);
+#endif
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	RTW_INFO("[%s] In\n", __func__);
+	/*	Reset the operation channel information */
+	pwdinfo->p2p_info.operation_ch[0] = 0;
+#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
+	pwdinfo->p2p_info.operation_ch[1] = 0;
+	pwdinfo->p2p_info.operation_ch[2] = 0;
+	pwdinfo->p2p_info.operation_ch[3] = 0;
+#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */
+	pwdinfo->p2p_info.scan_op_ch_only = 0;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+static void restore_p2p_state_timer_process (void *FunctionContext)
+#else
+static void restore_p2p_state_timer_process(struct timer_list *t)
+#endif
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_adapter *adapter = (_adapter *)FunctionContext;
+#else
+	_adapter *adapter = from_timer(adapter, t,
+					     wdinfo.restore_p2p_state_timer);
+#endif
+	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);
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+static void pre_tx_scan_timer_process (void *FunctionContext)
+#else
+static void pre_tx_scan_timer_process(struct timer_list *t)
+#endif
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_adapter *adapter = (_adapter *) FunctionContext;
+#else
+	_adapter *adapter = from_timer(adapter, t, wdinfo.pre_tx_scan_timer);
+#endif
+	struct	wifidirect_info				*pwdinfo = &adapter->wdinfo;
+	unsigned long							irqL;
+	struct mlme_priv					*pmlmepriv = &adapter->mlmepriv;
+	u8								_status = 0;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	_enter_critical_bh(&pmlmepriv->lock, &irqL);
+
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
+		if (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
+		RTW_INFO("[%s] p2p_state is %d, ignore!!\n", __func__, rtw_p2p_state(pwdinfo));
+
+	_exit_critical_bh(&pmlmepriv->lock, &irqL);
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+static void find_phase_timer_process (void *FunctionContext)
+#else
+static void find_phase_timer_process(struct timer_list *t)
+#endif
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_adapter *adapter = (_adapter *)FunctionContext;
+#else
+	_adapter *adapter = from_timer(adapter, t, wdinfo.find_phase_timer);
+#endif
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+	adapter->wdinfo.find_phase_state_exchange_cnt++;
+
+	p2p_protocol_wk_cmd(adapter, P2P_FIND_PHASE_WK);
+}
+
+#ifdef CONFIG_CONCURRENT_MODE
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+void ap_p2p_switch_timer_process(void *FunctionContext)
+#else
+void ap_p2p_switch_timer_process(struct timer_list *t)
+#endif
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_adapter *adapter = (_adapter *)FunctionContext;
+#else
+	_adapter *adapter = from_timer(adapter, t, wdinfo.ap_p2p_switch_timer);
+#endif
+	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
+#ifdef CONFIG_IOCTL_CFG80211
+	struct rtw_wdev_priv *pwdev_priv = adapter_wdev_data(adapter);
+#endif
+
+	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	ATOMIC_SET(&pwdev_priv->switch_ch_to, 1);
+#endif
+
+	p2p_protocol_wk_cmd(adapter, P2P_AP_P2P_CH_SWITCH_PROCESS_WK);
+}
+#endif
+
+void reset_global_wifidirect_info(_adapter *padapter)
+{
+	struct wifidirect_info	*pwdinfo;
+
+	pwdinfo = &padapter->wdinfo;
+	pwdinfo->persistent_supported = 0;
+	pwdinfo->session_available = true;
+	rtw_tdls_wfd_enable(padapter, 0);
+	pwdinfo->wfd_tdls_weaksec = true;
+}
+
+#ifdef CONFIG_WFD
+int rtw_init_wifi_display_info(_adapter *padapter)
+{
+	int	res = _SUCCESS;
+	struct wifi_display_info *pwfd_info = &padapter->wfd_info;
+
+	/* Used in P2P and TDLS */
+	pwfd_info->init_rtsp_ctrlport = 554;
+#ifdef CONFIG_IOCTL_CFG80211
+	pwfd_info->rtsp_ctrlport = 0;
+#else
+	pwfd_info->rtsp_ctrlport = pwfd_info->init_rtsp_ctrlport; /* set non-zero value for legacy wfd */
+#endif
+	pwfd_info->tdls_rtsp_ctrlport = 0;
+	pwfd_info->peer_rtsp_ctrlport = 0;	/*	Reset to 0 */
+	pwfd_info->wfd_enable = false;
+	pwfd_info->wfd_device_type = WFD_DEVINFO_PSINK;
+	pwfd_info->scan_result_type = SCAN_RESULT_P2P_ONLY;
+
+	/* Used in P2P */
+	pwfd_info->peer_session_avail = true;
+	pwfd_info->wfd_pc = false;
+
+	/* Used in TDLS */
+	memset(pwfd_info->ip_address, 0x00, 4);
+	memset(pwfd_info->peer_ip_address, 0x00, 4);
+	return res;
+
+}
+
+inline void rtw_wfd_enable(_adapter *adapter, bool on)
+{
+	struct wifi_display_info *wfdinfo = &adapter->wfd_info;
+
+	if (on) {
+		wfdinfo->rtsp_ctrlport = wfdinfo->init_rtsp_ctrlport;
+		wfdinfo->wfd_enable = true;
+
+	} else {
+		wfdinfo->wfd_enable = false;
+		wfdinfo->rtsp_ctrlport = 0;
+	}
+}
+
+inline void rtw_wfd_set_ctrl_port(_adapter *adapter, u16 port)
+{
+	struct wifi_display_info *wfdinfo = &adapter->wfd_info;
+
+	wfdinfo->init_rtsp_ctrlport = port;
+	if (wfdinfo->wfd_enable)
+		wfdinfo->rtsp_ctrlport = port;
+	if (adapter->wdinfo.wfd_tdls_enable == 1)
+		wfdinfo->tdls_rtsp_ctrlport = port;
+}
+
+inline void rtw_tdls_wfd_enable(_adapter *adapter, bool on)
+{
+	struct wifi_display_info *wfdinfo = &adapter->wfd_info;
+
+	if (on) {
+		wfdinfo->tdls_rtsp_ctrlport = wfdinfo->init_rtsp_ctrlport;
+		adapter->wdinfo.wfd_tdls_enable = 1;
+
+	} else {
+		adapter->wdinfo.wfd_tdls_enable = 0;
+		wfdinfo->tdls_rtsp_ctrlport = 0;
+	}
+}
+
+u32 rtw_append_beacon_wfd_ie(_adapter *adapter, u8 *pbuf)
+{
+	struct wifidirect_info *wdinfo = &adapter->wdinfo;
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	u8 build_ie_by_self = 0;
+	u32 len = 0;
+
+	if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (wdinfo->wfd_info->wfd_enable)
+#endif
+		build_ie_by_self = 1;
+
+	if (build_ie_by_self)
+		len = build_beacon_wfd_ie(wdinfo, pbuf);
+#ifdef CONFIG_IOCTL_CFG80211
+	else if (mlme->wfd_beacon_ie && mlme->wfd_beacon_ie_len > 0) {
+		len = mlme->wfd_beacon_ie_len;
+		memcpy(pbuf, mlme->wfd_beacon_ie, len);
+	}
+#endif
+
+exit:
+	return len;
+}
+
+u32 rtw_append_probe_req_wfd_ie(_adapter *adapter, u8 *pbuf)
+{
+	struct wifidirect_info *wdinfo = &adapter->wdinfo;
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	u8 build_ie_by_self = 0;
+	u32 len = 0;
+
+	if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (wdinfo->wfd_info->wfd_enable)
+#endif
+		build_ie_by_self = 1;
+
+	if (build_ie_by_self)
+		len = build_probe_req_wfd_ie(wdinfo, pbuf);
+#ifdef CONFIG_IOCTL_CFG80211
+	else if (mlme->wfd_probe_req_ie && mlme->wfd_probe_req_ie_len > 0) {
+		len = mlme->wfd_probe_req_ie_len;
+		memcpy(pbuf, mlme->wfd_probe_req_ie, len);
+	}
+#endif
+
+exit:
+	return len;
+}
+
+u32 rtw_append_probe_resp_wfd_ie(_adapter *adapter, u8 *pbuf)
+{
+	struct wifidirect_info *wdinfo = &adapter->wdinfo;
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	u8 build_ie_by_self = 0;
+	u32 len = 0;
+
+	if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (wdinfo->wfd_info->wfd_enable)
+#endif
+		build_ie_by_self = 1;
+
+	if (build_ie_by_self)
+		len = build_probe_resp_wfd_ie(wdinfo, pbuf, 0);
+#ifdef CONFIG_IOCTL_CFG80211
+	else if (mlme->wfd_probe_resp_ie && mlme->wfd_probe_resp_ie_len > 0) {
+		len = mlme->wfd_probe_resp_ie_len;
+		memcpy(pbuf, mlme->wfd_probe_resp_ie, len);
+	}
+#endif
+
+exit:
+	return len;
+}
+
+u32 rtw_append_assoc_req_wfd_ie(_adapter *adapter, u8 *pbuf)
+{
+	struct wifidirect_info *wdinfo = &adapter->wdinfo;
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	u8 build_ie_by_self = 0;
+	u32 len = 0;
+
+	if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (wdinfo->wfd_info->wfd_enable)
+#endif
+		build_ie_by_self = 1;
+
+	if (build_ie_by_self)
+		len = build_assoc_req_wfd_ie(wdinfo, pbuf);
+#ifdef CONFIG_IOCTL_CFG80211
+	else if (mlme->wfd_assoc_req_ie && mlme->wfd_assoc_req_ie_len > 0) {
+		len = mlme->wfd_assoc_req_ie_len;
+		memcpy(pbuf, mlme->wfd_assoc_req_ie, len);
+	}
+#endif
+
+exit:
+	return len;
+}
+
+u32 rtw_append_assoc_resp_wfd_ie(_adapter *adapter, u8 *pbuf)
+{
+	struct wifidirect_info *wdinfo = &adapter->wdinfo;
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	u8 build_ie_by_self = 0;
+	u32 len = 0;
+
+	if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
+		goto exit;
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (wdinfo->wfd_info->wfd_enable)
+#endif
+		build_ie_by_self = 1;
+
+	if (build_ie_by_self)
+		len = build_assoc_resp_wfd_ie(wdinfo, pbuf);
+#ifdef CONFIG_IOCTL_CFG80211
+	else if (mlme->wfd_assoc_resp_ie && mlme->wfd_assoc_resp_ie_len > 0) {
+		len = mlme->wfd_assoc_resp_ie_len;
+		memcpy(pbuf, mlme->wfd_assoc_resp_ie, len);
+	}
+#endif
+
+exit:
+	return len;
+}
+
+#endif /* CONFIG_WFD */
+
+void rtw_init_wifidirect_timers(_adapter *padapter)
+{
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_init_timer(&pwdinfo->find_phase_timer, padapter->pnetdev, find_phase_timer_process, padapter);
+	_init_timer(&pwdinfo->restore_p2p_state_timer, padapter->pnetdev, restore_p2p_state_timer_process, padapter);
+	_init_timer(&pwdinfo->pre_tx_scan_timer, padapter->pnetdev, pre_tx_scan_timer_process, padapter);
+	_init_timer(&pwdinfo->reset_ch_sitesurvey, padapter->pnetdev, reset_ch_sitesurvey_timer_process, padapter);
+	_init_timer(&pwdinfo->reset_ch_sitesurvey2, padapter->pnetdev, reset_ch_sitesurvey_timer_process2, padapter);
+#ifdef CONFIG_CONCURRENT_MODE
+	_init_timer(&pwdinfo->ap_p2p_switch_timer, padapter->pnetdev, ap_p2p_switch_timer_process, padapter);
+#endif
+#else
+	timer_setup(&pwdinfo->find_phase_timer, find_phase_timer_process, 0);
+	timer_setup(&pwdinfo->restore_p2p_state_timer,
+		    restore_p2p_state_timer_process, 0);
+	timer_setup(&pwdinfo->pre_tx_scan_timer, pre_tx_scan_timer_process, 0);
+	timer_setup(&pwdinfo->reset_ch_sitesurvey,
+		    reset_ch_sitesurvey_timer_process, 0);
+	timer_setup(&pwdinfo->reset_ch_sitesurvey2,
+		    reset_ch_sitesurvey_timer_process2, 0);
+#ifdef CONFIG_CONCURRENT_MODE
+	timer_setup(&pwdinfo->ap_p2p_switch_timer,
+		    ap_p2p_switch_timer_process, 0);
+#endif
+#endif
+}
+
+void rtw_init_wifidirect_addrs(_adapter *padapter, u8 *dev_addr, u8 *iface_addr)
+{
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
+
+	/*init device&interface address */
+	if (dev_addr)
+		memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN);
+	if (iface_addr)
+		memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN);
+#endif
+}
+
+void init_wifidirect_info(_adapter *padapter, enum P2P_ROLE role)
+{
+	struct wifidirect_info	*pwdinfo;
+#ifdef CONFIG_WFD
+	struct wifi_display_info	*pwfd_info = &padapter->wfd_info;
+#endif
+	u8 union_ch = 0;
+	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. */
+
+	if (role != P2P_ROLE_DISABLE
+		&& pwdinfo->driver_interface != DRIVER_CFG80211
+	) {
+		#ifdef CONFIG_CONCURRENT_MODE
+		if (rtw_mi_check_status(padapter, MI_LINKED))
+			union_ch = rtw_mi_get_union_chan(padapter);
+
+		if (union_ch != 0 &&
+			(union_ch == 1 || union_ch == 6 || union_ch == 11)
+		) {
+			/* Use the AP's channel as the listen channel */
+			/* This will avoid the channel switch between AP's channel and listen channel */
+			pwdinfo->listen_channel = union_ch;
+		} else
+		#endif /* CONFIG_CONCURRENT_MODE */
+		{
+			/* Use the channel 11 as the listen channel */
+			pwdinfo->listen_channel = 11;
+		}
+	}
+
+	if (role == P2P_ROLE_DEVICE) {
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
+#ifdef CONFIG_CONCURRENT_MODE
+		if (rtw_mi_check_status(padapter, MI_LINKED))
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_IDLE);
+		else
+#endif
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
+
+		pwdinfo->intent = 1;
+		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_LISTEN);
+	} else if (role == P2P_ROLE_CLIENT) {
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+		pwdinfo->intent = 1;
+		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+	} else if (role == P2P_ROLE_GO) {
+		rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
+		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
+		pwdinfo->intent = 15;
+		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
+	}
+
+	/*	Use the OFDM rate in the P2P probe response frame. ( 6(B), 9(B), 12, 18, 24, 36, 48, 54 )	 */
+	pwdinfo->support_rate[0] = 0x8c;	/*	6(B) */
+	pwdinfo->support_rate[1] = 0x92;	/*	9(B) */
+	pwdinfo->support_rate[2] = 0x18;	/*	12 */
+	pwdinfo->support_rate[3] = 0x24;	/*	18 */
+	pwdinfo->support_rate[4] = 0x30;	/*	24 */
+	pwdinfo->support_rate[5] = 0x48;	/*	36 */
+	pwdinfo->support_rate[6] = 0x60;	/*	48 */
+	pwdinfo->support_rate[7] = 0x6c;	/*	54 */
+
+	memcpy((void *) pwdinfo->p2p_wildcard_ssid, "DIRECT-", 7);
+
+	memset(pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN);
+	pwdinfo->device_name_len = 0;
+
+	memset(&pwdinfo->invitereq_info, 0x00, sizeof(struct tx_invite_req_info));
+	pwdinfo->invitereq_info.token = 3;	/*	Token used for P2P invitation request frame. */
+
+	memset(&pwdinfo->inviteresp_info, 0x00, sizeof(struct tx_invite_resp_info));
+	pwdinfo->inviteresp_info.token = 0;
+
+	pwdinfo->profileindex = 0;
+	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)((jiffies % 3) + 1);
+	/* RTW_INFO( "[%s] listen_dwell time is %d00ms\n", __func__, pwdinfo->listen_dwell ); */
+
+	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;
+
+	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;
+
+	memset(pwdinfo->nego_ssid, 0x00, WLAN_SSID_MAXLEN);
+	pwdinfo->nego_ssidlen = 0;
+
+	pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO;
+#ifdef CONFIG_WFD
+	pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY  | WPS_CONFIG_METHOD_PBC;
+	pwdinfo->wfd_info = pwfd_info;
+#else
+	pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD;
+#endif /* CONFIG_WFD */
+	pwdinfo->channel_list_attr_len = 0;
+	memset(pwdinfo->channel_list_attr, 0x00, 100);
+
+	memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, 0x00, 4);
+	memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, '0', 3);
+	memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
+#ifdef CONFIG_CONCURRENT_MODE
+#ifdef CONFIG_IOCTL_CFG80211
+	pwdinfo->ext_listen_interval = 1000; /* The interval to be available with legacy AP during p2p0-find/scan */
+	pwdinfo->ext_listen_period = 3000; /* The time period to be available for P2P during nego */
+#else /* !CONFIG_IOCTL_CFG80211 */
+	/* pwdinfo->ext_listen_interval = 3000; */
+	/* pwdinfo->ext_listen_period = 400; */
+	pwdinfo->ext_listen_interval = 1000;
+	pwdinfo->ext_listen_period = 1000;
+#endif /* !CONFIG_IOCTL_CFG80211 */
+#endif
+
+	/* Commented by Kurt 20130319
+	 * For WiDi purpose: Use CFG80211 interface but controled WFD/RDS frame by driver itself. */
+#ifdef CONFIG_IOCTL_CFG80211
+	pwdinfo->driver_interface = DRIVER_CFG80211;
+#else
+	pwdinfo->driver_interface = DRIVER_WEXT;
+#endif /* CONFIG_IOCTL_CFG80211 */
+
+	pwdinfo->wfd_tdls_enable = 0;
+	memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
+	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 */
+#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
+	pwdinfo->rx_invitereq_info.operation_ch[2] = 0;
+	pwdinfo->rx_invitereq_info.operation_ch[3] = 0;
+	pwdinfo->rx_invitereq_info.operation_ch[4] = 0;
+#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */
+	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 */
+#ifdef CONFIG_P2P_OP_CHK_SOCIAL_CH
+	pwdinfo->p2p_info.operation_ch[2] = 0;
+	pwdinfo->p2p_info.operation_ch[3] = 0;
+	pwdinfo->p2p_info.operation_ch[4] = 0;
+#endif /* CONFIG_P2P_OP_CHK_SOCIAL_CH */
+	pwdinfo->p2p_info.scan_op_ch_only = 0;
+}
+
+#ifdef CONFIG_DBG_P2P
+
+/**
+ * rtw_p2p_role_txt - Get the p2p role name as a text string
+ * @role: P2P role
+ * Returns: The state name as a printable text string
+ */
+const char *rtw_p2p_role_txt(enum P2P_ROLE role)
+{
+	switch (role) {
+	case P2P_ROLE_DISABLE:
+		return "P2P_ROLE_DISABLE";
+	case P2P_ROLE_DEVICE:
+		return "P2P_ROLE_DEVICE";
+	case P2P_ROLE_CLIENT:
+		return "P2P_ROLE_CLIENT";
+	case P2P_ROLE_GO:
+		return "P2P_ROLE_GO";
+	default:
+		return "UNKNOWN";
+	}
+}
+
+/**
+ * rtw_p2p_state_txt - Get the p2p state name as a text string
+ * @state: P2P state
+ * Returns: The state name as a printable text string
+ */
+const char *rtw_p2p_state_txt(enum P2P_STATE state)
+{
+	switch (state) {
+	case P2P_STATE_NONE:
+		return "P2P_STATE_NONE";
+	case P2P_STATE_IDLE:
+		return "P2P_STATE_IDLE";
+	case P2P_STATE_LISTEN:
+		return "P2P_STATE_LISTEN";
+	case P2P_STATE_SCAN:
+		return "P2P_STATE_SCAN";
+	case P2P_STATE_FIND_PHASE_LISTEN:
+		return "P2P_STATE_FIND_PHASE_LISTEN";
+	case P2P_STATE_FIND_PHASE_SEARCH:
+		return "P2P_STATE_FIND_PHASE_SEARCH";
+	case P2P_STATE_TX_PROVISION_DIS_REQ:
+		return "P2P_STATE_TX_PROVISION_DIS_REQ";
+	case P2P_STATE_RX_PROVISION_DIS_RSP:
+		return "P2P_STATE_RX_PROVISION_DIS_RSP";
+	case P2P_STATE_RX_PROVISION_DIS_REQ:
+		return "P2P_STATE_RX_PROVISION_DIS_REQ";
+	case P2P_STATE_GONEGO_ING:
+		return "P2P_STATE_GONEGO_ING";
+	case P2P_STATE_GONEGO_OK:
+		return "P2P_STATE_GONEGO_OK";
+	case P2P_STATE_GONEGO_FAIL:
+		return "P2P_STATE_GONEGO_FAIL";
+	case P2P_STATE_RECV_INVITE_REQ_MATCH:
+		return "P2P_STATE_RECV_INVITE_REQ_MATCH";
+	case P2P_STATE_PROVISIONING_ING:
+		return "P2P_STATE_PROVISIONING_ING";
+	case P2P_STATE_PROVISIONING_DONE:
+		return "P2P_STATE_PROVISIONING_DONE";
+	case P2P_STATE_TX_INVITE_REQ:
+		return "P2P_STATE_TX_INVITE_REQ";
+	case P2P_STATE_RX_INVITE_RESP_OK:
+		return "P2P_STATE_RX_INVITE_RESP_OK";
+	case P2P_STATE_RECV_INVITE_REQ_DISMATCH:
+		return "P2P_STATE_RECV_INVITE_REQ_DISMATCH";
+	case P2P_STATE_RECV_INVITE_REQ_GO:
+		return "P2P_STATE_RECV_INVITE_REQ_GO";
+	case P2P_STATE_RECV_INVITE_REQ_JOIN:
+		return "P2P_STATE_RECV_INVITE_REQ_JOIN";
+	case P2P_STATE_RX_INVITE_RESP_FAIL:
+		return "P2P_STATE_RX_INVITE_RESP_FAIL";
+	case P2P_STATE_RX_INFOR_NOREADY:
+		return "P2P_STATE_RX_INFOR_NOREADY";
+	case P2P_STATE_TX_INFOR_NOREADY:
+		return "P2P_STATE_TX_INFOR_NOREADY";
+	default:
+		return "UNKNOWN";
+	}
+}
+
+void dbg_rtw_p2p_set_state(struct wifidirect_info *wdinfo, enum P2P_STATE state, const char *caller, int line)
+{
+	if (!_rtw_p2p_chk_state(wdinfo, state)) {
+		enum P2P_STATE old_state = _rtw_p2p_state(wdinfo);
+		_rtw_p2p_set_state(wdinfo, state);
+		RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_state from %s to %s\n", caller, line
+			, rtw_p2p_state_txt(old_state), rtw_p2p_state_txt(_rtw_p2p_state(wdinfo))
+			);
+	} else {
+		RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_state to same state %s\n", caller, line
+			 , rtw_p2p_state_txt(_rtw_p2p_state(wdinfo))
+			);
+	}
+}
+void dbg_rtw_p2p_set_pre_state(struct wifidirect_info *wdinfo, enum P2P_STATE state, const char *caller, int line)
+{
+	if (_rtw_p2p_pre_state(wdinfo) != state) {
+		enum P2P_STATE old_state = _rtw_p2p_pre_state(wdinfo);
+		_rtw_p2p_set_pre_state(wdinfo, state);
+		RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_pre_state from %s to %s\n", caller, line
+			, rtw_p2p_state_txt(old_state), rtw_p2p_state_txt(_rtw_p2p_pre_state(wdinfo))
+			);
+	} else {
+		RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_pre_state to same state %s\n", caller, line
+			 , rtw_p2p_state_txt(_rtw_p2p_pre_state(wdinfo))
+			);
+	}
+}
+
+void dbg_rtw_p2p_set_role(struct wifidirect_info *wdinfo, enum P2P_ROLE role, const char *caller, int line)
+{
+	if (wdinfo->role != role) {
+		enum P2P_ROLE old_role = wdinfo->role;
+		_rtw_p2p_set_role(wdinfo, role);
+		RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_role from %s to %s\n", caller, line
+			, rtw_p2p_role_txt(old_role), rtw_p2p_role_txt(wdinfo->role)
+			);
+	} else {
+		RTW_INFO("[CONFIG_DBG_P2P]%s:%d set_role to same role %s\n", caller, line
+			 , rtw_p2p_role_txt(wdinfo->role)
+			);
+	}
+}
+#endif /* CONFIG_DBG_P2P */
+
+
+int rtw_p2p_enable(_adapter *padapter, enum P2P_ROLE role)
+{
+	int ret = _SUCCESS;
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+
+	if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT || role == P2P_ROLE_GO) {
+		u8 channel, ch_offset;
+		u16 bwmode;
+
+#if defined(CONFIG_CONCURRENT_MODE) && (!defined(RTW_P2P_GROUP_INTERFACE) || !RTW_P2P_GROUP_INTERFACE)
+		/*	Commented by Albert 2011/12/30 */
+		/*	The driver just supports 1 P2P group operation. */
+		/*	So, this function will do nothing if the buddy adapter had enabled the P2P function. */
+		/*if(!rtw_p2p_chk_state(pbuddy_wdinfo, P2P_STATE_NONE))
+			return ret;*/
+		/*The buddy adapter had enabled the P2P function.*/
+		if (rtw_mi_buddy_stay_in_p2p_mode(padapter))
+			return ret;
+#endif /* CONFIG_CONCURRENT_MODE */
+
+		/* leave IPS/Autosuspend */
+		if (_FAIL == rtw_pwr_wakeup(padapter)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		/*	Added by Albert 2011/03/22 */
+		/*	In the P2P mode, the driver should not support the b mode. */
+		/*	So, the Tx packet shouldn't use the CCK rate */
+		#ifdef CONFIG_IOCTL_CFG80211
+		if (rtw_cfg80211_iface_has_p2p_group_cap(padapter))
+		#endif
+			update_tx_basic_rate(padapter, WIRELESS_11AGN);
+
+		/* Enable P2P function */
+		init_wifidirect_info(padapter, role);
+
+		#ifdef CONFIG_IOCTL_CFG80211
+		if (padapter->wdinfo.driver_interface == DRIVER_CFG80211)
+			adapter_wdev_data(padapter)->p2p_enabled = true;
+		#endif
+
+		rtw_hal_set_odm_var(padapter, HAL_ODM_P2P_STATE, NULL, true);
+#ifdef CONFIG_WFD
+		if (hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+			rtw_hal_set_odm_var(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, true);
+#endif
+
+	} else if (role == P2P_ROLE_DISABLE) {
+#ifdef CONFIG_INTEL_WIDI
+		if (padapter->mlmepriv.p2p_reject_disable)
+			return ret;
+#endif /* CONFIG_INTEL_WIDI */
+
+		#ifdef CONFIG_IOCTL_CFG80211
+		if (padapter->wdinfo.driver_interface == DRIVER_CFG80211)
+			adapter_wdev_data(padapter)->p2p_enabled = false;
+		#endif
+
+		pwdinfo->listen_channel = 0;
+
+		/* 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);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+			reset_ch_sitesurvey_timer_process(padapter);
+			reset_ch_sitesurvey_timer_process2(padapter);
+#else
+			reset_ch_sitesurvey_timer_process(&padapter->wdinfo.reset_ch_sitesurvey);
+			reset_ch_sitesurvey_timer_process(&padapter->wdinfo.reset_ch_sitesurvey2);
+#endif
+#ifdef CONFIG_CONCURRENT_MODE
+			_cancel_timer_ex(&pwdinfo->ap_p2p_switch_timer);
+#endif
+			rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
+			rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_NONE);
+			rtw_p2p_set_role(pwdinfo, P2P_ROLE_DISABLE);
+			memset(&pwdinfo->rx_prov_disc_info, 0x00, sizeof(struct rx_provdisc_req_info));
+
+			/* Remove profiles in wifidirect_info structure. */
+			memset(&pwdinfo->profileinfo[0], 0x00, sizeof(struct profile_info) * P2P_MAX_PERSISTENT_GROUP_NUM);
+			pwdinfo->profileindex = 0;
+		}
+
+		rtw_hal_set_odm_var(padapter, HAL_ODM_P2P_STATE, NULL, false);
+#ifdef CONFIG_WFD
+		if (hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+			rtw_hal_set_odm_var(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, false);
+#endif
+
+		if (_FAIL == rtw_pwr_wakeup(padapter)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		/* Restore to initial setting. */
+		update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
+
+#ifdef CONFIG_INTEL_WIDI
+		rtw_reset_widi_info(padapter);
+#endif /* CONFIG_INTEL_WIDI */
+
+		/* For WiDi purpose. */
+#ifdef CONFIG_IOCTL_CFG80211
+		pwdinfo->driver_interface = DRIVER_CFG80211;
+#else
+		pwdinfo->driver_interface = DRIVER_WEXT;
+#endif /* CONFIG_IOCTL_CFG80211 */
+
+	}
+
+exit:
+	return ret;
+}
+
+#endif /* CONFIG_P2P */
diff --git a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
new file mode 100644
index 000000000000..5a9426bb15cb
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
@@ -0,0 +1,2480 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_PWRCTRL_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+#include <hal_com_h2c.h>
+
+int rtw_fw_ps_state(PADAPTER padapter)
+{
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	int ret = _FAIL, dont_care = 0;
+	u16 fw_ps_state = 0;
+	u32 start_time;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct registry_priv  *registry_par = &padapter->registrypriv;
+
+	if (registry_par->check_fw_ps != 1)
+		return _SUCCESS;
+
+	_enter_pwrlock(&pwrpriv->check_32k_lock);
+
+	if (RTW_CANNOT_RUN(padapter)) {
+		RTW_INFO("%s: bSurpriseRemoved=%s , hw_init_completed=%d, bDriverStopped=%s\n", __func__
+			 , rtw_is_surprise_removed(padapter) ? "True" : "False"
+			 , rtw_get_hw_init_completed(padapter)
+			 , rtw_is_drv_stopped(padapter) ? "True" : "False");
+		goto exit_fw_ps_state;
+	}
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_REQ_FW_PS, (u8 *)&dont_care);
+	{
+		/* 4. if 0x88[7]=1, driver set cmd to leave LPS/IPS. */
+		/* Else, hw will keep in active mode. */
+		/* debug info: */
+		/* 0x88[7] = 32kpermission, */
+		/* 0x88[6:0] = current_ps_state */
+		/* 0x89[7:0] = last_rpwm */
+
+		rtw_hal_get_hwreg(padapter, HW_VAR_FW_PS_STATE, (u8 *)&fw_ps_state);
+
+		if ((fw_ps_state & 0x80) == 0)
+			ret = _SUCCESS;
+		else {
+			pdbgpriv->dbg_poll_fail_cnt++;
+			RTW_INFO("%s: fw_ps_state=%04x\n", __func__, fw_ps_state);
+		}
+	}
+
+
+exit_fw_ps_state:
+	_exit_pwrlock(&pwrpriv->check_32k_lock);
+	return ret;
+}
+
+#ifdef CONFIG_IPS
+void _ips_enter(_adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+	pwrpriv->bips_processing = true;
+
+	/* syn ips_mode with request */
+	pwrpriv->ips_mode = pwrpriv->ips_mode_req;
+
+	pwrpriv->ips_enter_cnts++;
+	RTW_INFO("==>ips_enter cnts:%d\n", pwrpriv->ips_enter_cnts);
+
+	if (rf_off == pwrpriv->change_rfpwrstate) {
+		pwrpriv->bpower_saving = true;
+		RTW_INFO("nolinked power save enter\n");
+
+		if (pwrpriv->ips_mode == IPS_LEVEL_2)
+			pwrpriv->bkeepfwalive = true;
+
+		rtw_ips_pwr_down(padapter);
+		pwrpriv->rf_pwrstate = rf_off;
+	}
+	pwrpriv->bips_processing = false;
+
+}
+
+void ips_enter(_adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+
+#ifdef CONFIG_BT_COEXIST
+	rtw_btcoex_IpsNotify(padapter, pwrpriv->ips_mode_req);
+#endif /* CONFIG_BT_COEXIST */
+
+	_enter_pwrlock(&pwrpriv->lock);
+	_ips_enter(padapter);
+	_exit_pwrlock(&pwrpriv->lock);
+}
+
+int _ips_leave(_adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	int result = _SUCCESS;
+
+	if ((pwrpriv->rf_pwrstate == rf_off) && (!pwrpriv->bips_processing)) {
+		pwrpriv->bips_processing = true;
+		pwrpriv->change_rfpwrstate = rf_on;
+		pwrpriv->ips_leave_cnts++;
+		RTW_INFO("==>ips_leave cnts:%d\n", pwrpriv->ips_leave_cnts);
+
+		result = rtw_ips_pwr_up(padapter);
+		if (result == _SUCCESS)
+			pwrpriv->rf_pwrstate = rf_on;
+		RTW_INFO("nolinked power save leave\n");
+
+		RTW_INFO("==> ips_leave.....LED(0x%08x)...\n", rtw_read32(padapter, 0x4c));
+		pwrpriv->bips_processing = false;
+
+		pwrpriv->bkeepfwalive = false;
+		pwrpriv->bpower_saving = false;
+	}
+
+	return result;
+}
+
+int ips_leave(_adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	int ret;
+
+	if (!is_primary_adapter(padapter))
+		return _SUCCESS;
+
+	_enter_pwrlock(&pwrpriv->lock);
+	ret = _ips_leave(padapter);
+#ifdef DBG_CHECK_FW_PS_STATE
+	if (rtw_fw_ps_state(padapter) == _FAIL) {
+		RTW_INFO("ips leave doesn't leave 32k\n");
+		pdbgpriv->dbg_leave_ips_fail_cnt++;
+	}
+#endif /* DBG_CHECK_FW_PS_STATE */
+	_exit_pwrlock(&pwrpriv->lock);
+
+	if (_SUCCESS == ret)
+		odm_dm_reset(&GET_HAL_DATA(padapter)->odmpriv);
+
+#ifdef CONFIG_BT_COEXIST
+	if (_SUCCESS == ret)
+		rtw_btcoex_IpsNotify(padapter, IPS_NONE);
+#endif /* CONFIG_BT_COEXIST */
+
+	return ret;
+}
+#endif /* CONFIG_IPS */
+
+#ifdef CONFIG_AUTOSUSPEND
+	extern void autosuspend_enter(_adapter *padapter);
+	extern int autoresume_enter(_adapter *padapter);
+#endif
+
+static bool rtw_pwr_unassociated_idle(_adapter *adapter)
+{
+	u8 i;
+	_adapter *iface;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct xmit_priv *pxmit_priv = &adapter->xmitpriv;
+	struct mlme_priv *pmlmepriv;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo;
+#ifdef CONFIG_IOCTL_CFG80211
+	struct cfg80211_wifidirect_info *pcfg80211_wdinfo;
+#endif
+#endif
+
+	bool ret = false;
+
+	if (adapter_to_pwrctl(adapter)->bpower_saving == true) {
+		/* RTW_INFO("%s: already in LPS or IPS mode\n", __func__); */
+		goto exit;
+	}
+
+	if (adapter_to_pwrctl(adapter)->ips_deny_time >= jiffies) {
+		/* RTW_INFO("%s ips_deny_time\n", __func__); */
+		goto exit;
+	}
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		if ((iface) && rtw_is_adapter_up(iface)) {
+			pmlmepriv = &(iface->mlmepriv);
+#ifdef CONFIG_P2P
+			pwdinfo = &(iface->wdinfo);
+#ifdef CONFIG_IOCTL_CFG80211
+			pcfg80211_wdinfo = &iface->cfg80211_wdinfo;
+#endif
+#endif
+			if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE | WIFI_SITE_MONITOR)
+			    || check_fwstate(pmlmepriv, WIFI_UNDER_LINKING | WIFI_UNDER_WPS)
+			    || check_fwstate(pmlmepriv, WIFI_AP_STATE)
+			    || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE)
+#if defined(CONFIG_P2P) && defined(CONFIG_IOCTL_CFG80211)
+			    || rtw_cfg80211_get_is_roch(iface) == true
+#elif defined(CONFIG_P2P)
+			    || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)
+			    || rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN)
+#endif
+#if defined(CONFIG_P2P) && defined(CONFIG_IOCTL_CFG80211)
+			    || rtw_get_passing_time_ms(pcfg80211_wdinfo->last_ro_ch_time) < 3000
+#endif
+			   )
+				goto exit;
+
+		}
+	}
+
+#if (MP_DRIVER == 1)
+	if (adapter->registrypriv.mp_mode == 1)
+		goto exit;
+#endif
+
+#ifdef CONFIG_INTEL_PROXIM
+	if (adapter->proximity.proxim_on == true)
+		return;
+#endif
+
+	if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF ||
+	    pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) {
+		RTW_PRINT("There are some pkts to transmit\n");
+		RTW_PRINT("free_xmitbuf_cnt: %d, free_xmit_extbuf_cnt: %d\n",
+			pxmit_priv->free_xmitbuf_cnt, pxmit_priv->free_xmit_extbuf_cnt);
+		goto exit;
+	}
+
+	ret = true;
+
+exit:
+	return ret;
+}
+
+
+/*
+ * ATTENTION:
+ *	rtw_ps_processor() doesn't handle LPS.
+ */
+void rtw_ps_processor(_adapter *padapter)
+{
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_P2P */
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+#ifdef SUPPORT_HW_RFOFF_DETECTED
+	rt_rf_power_state rfpwrstate;
+#endif /* SUPPORT_HW_RFOFF_DETECTED */
+	u32 ps_deny = 0;
+
+	_enter_pwrlock(&adapter_to_pwrctl(padapter)->lock);
+	ps_deny = rtw_ps_deny_get(padapter);
+	_exit_pwrlock(&adapter_to_pwrctl(padapter)->lock);
+	if (ps_deny != 0) {
+		RTW_INFO(FUNC_ADPT_FMT ": ps_deny=0x%08X, skip power save!\n",
+			 FUNC_ADPT_ARG(padapter), ps_deny);
+		goto exit;
+	}
+
+	if (pwrpriv->bInSuspend == true) { /* system suspend or autosuspend */
+		pdbgpriv->dbg_ps_insuspend_cnt++;
+		RTW_INFO("%s, pwrpriv->bInSuspend == true ignore this process\n", __func__);
+		return;
+	}
+
+	pwrpriv->ps_processing = true;
+
+#ifdef SUPPORT_HW_RFOFF_DETECTED
+	if (pwrpriv->bips_processing == true)
+		goto exit;
+
+	/* RTW_INFO("==> fw report state(0x%x)\n",rtw_read8(padapter,0x1ca));	 */
+	if (pwrpriv->bHWPwrPindetect) {
+#ifdef CONFIG_AUTOSUSPEND
+		if (padapter->registrypriv.usbss_enable) {
+			if (pwrpriv->rf_pwrstate == rf_on) {
+				if (padapter->net_closed == true)
+					pwrpriv->ps_flag = true;
+
+				rfpwrstate = RfOnOffDetect(padapter);
+				RTW_INFO("@@@@- #1  %s==> rfstate:%s\n", __func__, (rfpwrstate == rf_on) ? "rf_on" : "rf_off");
+				if (rfpwrstate != pwrpriv->rf_pwrstate) {
+					if (rfpwrstate == rf_off) {
+						pwrpriv->change_rfpwrstate = rf_off;
+
+						pwrpriv->bkeepfwalive = true;
+						pwrpriv->brfoffbyhw = true;
+
+						autosuspend_enter(padapter);
+					}
+				}
+			}
+		} else
+#endif /* CONFIG_AUTOSUSPEND */
+		{
+			rfpwrstate = RfOnOffDetect(padapter);
+			RTW_INFO("@@@@- #2  %s==> rfstate:%s\n", __func__, (rfpwrstate == rf_on) ? "rf_on" : "rf_off");
+
+			if (rfpwrstate != pwrpriv->rf_pwrstate) {
+				if (rfpwrstate == rf_off) {
+					pwrpriv->change_rfpwrstate = rf_off;
+					pwrpriv->brfoffbyhw = true;
+					rtw_hw_suspend(padapter);
+				} else {
+					pwrpriv->change_rfpwrstate = rf_on;
+					rtw_hw_resume(padapter);
+				}
+				RTW_INFO("current rf_pwrstate(%s)\n", (pwrpriv->rf_pwrstate == rf_off) ? "rf_off" : "rf_on");
+			}
+		}
+		pwrpriv->pwr_state_check_cnts++;
+	}
+#endif /* SUPPORT_HW_RFOFF_DETECTED */
+
+	if (pwrpriv->ips_mode_req == IPS_NONE)
+		goto exit;
+
+	if (rtw_pwr_unassociated_idle(padapter) == false)
+		goto exit;
+
+	if ((pwrpriv->rf_pwrstate == rf_on) && ((pwrpriv->pwr_state_check_cnts % 4) == 0)) {
+		RTW_INFO("==>%s .fw_state(%x)\n", __func__, get_fwstate(pmlmepriv));
+#if defined(CONFIG_BT_COEXIST) && defined (CONFIG_AUTOSUSPEND)
+#else
+		pwrpriv->change_rfpwrstate = rf_off;
+#endif
+#ifdef CONFIG_AUTOSUSPEND
+		if (padapter->registrypriv.usbss_enable) {
+			if (pwrpriv->bHWPwrPindetect)
+				pwrpriv->bkeepfwalive = true;
+
+			if (padapter->net_closed == true)
+				pwrpriv->ps_flag = true;
+
+#if defined(CONFIG_BT_COEXIST) && defined (CONFIG_AUTOSUSPEND)
+			if (pwrpriv->bInternalAutoSuspend)
+				RTW_INFO("<==%s .pwrpriv->bInternalAutoSuspend)(%x)\n", __func__, pwrpriv->bInternalAutoSuspend);
+			else {
+				pwrpriv->change_rfpwrstate = rf_off;
+				RTW_INFO("<==%s .pwrpriv->bInternalAutoSuspend)(%x) call autosuspend_enter\n", __func__, pwrpriv->bInternalAutoSuspend);
+				autosuspend_enter(padapter);
+			}
+#else
+			autosuspend_enter(padapter);
+#endif	/* if defined (CONFIG_BT_COEXIST)&& defined (CONFIG_AUTOSUSPEND) */
+		} else if (pwrpriv->bHWPwrPindetect) {
+		} else
+#endif /* CONFIG_AUTOSUSPEND */
+		{
+#if defined(CONFIG_BT_COEXIST) && defined (CONFIG_AUTOSUSPEND)
+			pwrpriv->change_rfpwrstate = rf_off;
+#endif	/* defined (CONFIG_BT_COEXIST)&& defined (CONFIG_AUTOSUSPEND) */
+
+#ifdef CONFIG_IPS
+			ips_enter(padapter);
+#endif
+		}
+	}
+exit:
+#ifndef CONFIG_IPS_CHECK_IN_WD
+	rtw_set_pwr_state_check_timer(pwrpriv);
+#endif
+	pwrpriv->ps_processing = false;
+	return;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+static void pwr_state_check_handler(RTW_TIMER_HDL_ARGS)
+#else
+static void pwr_state_check_handler(struct timer_list *t)
+#endif
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_adapter *padapter = (_adapter *)FunctionContext;
+#else
+	struct pwrctrl_priv *pwrpriv = from_timer(pwrpriv, t, pwr_state_check_timer);
+	_adapter *padapter = pwrpriv->padapter;
+#endif
+
+	rtw_ps_cmd(padapter);
+}
+
+#ifdef CONFIG_LPS
+void	traffic_check_for_leave_lps(PADAPTER padapter, u8 tx, u32 tx_packets)
+{
+#ifdef CONFIG_CHECK_LEAVE_LPS
+	static u32 start_time = 0;
+	static u32 xmit_cnt = 0;
+	u8	bLeaveLPS = false;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+
+
+
+	if (tx) { /* from tx */
+		xmit_cnt += tx_packets;
+
+		if (start_time == 0)
+			start_time = jiffies;
+
+		if (rtw_get_passing_time_ms(start_time) > 2000) { /* 2 sec == watch dog timer */
+			if (xmit_cnt > 8) {
+				if ((adapter_to_pwrctl(padapter)->bLeisurePs)
+				    && (adapter_to_pwrctl(padapter)->pwr_mode != PS_MODE_ACTIVE)
+#ifdef CONFIG_BT_COEXIST
+				    && (rtw_btcoex_IsBtControlLps(padapter) == false)
+#endif
+				   ) {
+					/* RTW_INFO("leave lps via Tx = %d\n", xmit_cnt);			 */
+					bLeaveLPS = true;
+				}
+			}
+
+			start_time = jiffies;
+			xmit_cnt = 0;
+		}
+
+	} else { /* from rx path */
+		if (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 4/*2*/) {
+			if ((adapter_to_pwrctl(padapter)->bLeisurePs)
+			    && (adapter_to_pwrctl(padapter)->pwr_mode != PS_MODE_ACTIVE)
+#ifdef CONFIG_BT_COEXIST
+			    && (rtw_btcoex_IsBtControlLps(padapter) == false)
+#endif
+			   ) {
+				/* RTW_INFO("leave lps via Rx = %d\n", pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod);	 */
+				bLeaveLPS = true;
+			}
+		}
+	}
+
+	if (bLeaveLPS) {
+		/* RTW_INFO("leave lps via %s, Tx = %d, Rx = %d\n", tx?"Tx":"Rx", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod,pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod);	 */
+		/* rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1); */
+		rtw_lps_ctrl_wk_cmd(padapter, tx ? LPS_CTRL_TX_TRAFFIC_LEAVE : LPS_CTRL_RX_TRAFFIC_LEAVE, tx ? 0 : 1);
+	}
+#endif /* CONFIG_CHECK_LEAVE_LPS */
+}
+
+/*
+ * Description:
+ *	This function MUST be called under power lock protect
+ *
+ * Parameters
+ *	padapter
+ *	pslv			power state level, only could be PS_STATE_S0 ~ PS_STATE_S4
+ *
+ */
+void rtw_set_rpwm(PADAPTER padapter, u8 pslv)
+{
+	u8	rpwm;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+#ifdef CONFIG_DETECT_CPWM_BY_POLLING
+	u8 cpwm_orig;
+#endif /* CONFIG_DETECT_CPWM_BY_POLLING */
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	pslv = PS_STATE(pslv);
+
+#ifdef CONFIG_LPS_RPWM_TIMER
+	if (pwrpriv->brpwmtimeout == true)
+		RTW_INFO("%s: RPWM timeout, force to set RPWM(0x%02X) again!\n", __func__, pslv);
+	else
+#endif /* CONFIG_LPS_RPWM_TIMER */
+	{
+		if ((pwrpriv->rpwm == pslv)
+#ifdef CONFIG_LPS_LCLK
+		    || ((pwrpriv->rpwm >= PS_STATE_S2) && (pslv >= PS_STATE_S2))
+#endif
+		   ) {
+			return;
+		}
+	}
+
+	if (rtw_is_surprise_removed(padapter) ||
+	    (!rtw_is_hw_init_completed(padapter))) {
+
+		pwrpriv->cpwm = PS_STATE_S4;
+
+		return;
+	}
+
+	if (rtw_is_drv_stopped(padapter)) {
+
+		if (pslv < PS_STATE_S2) {
+			return;
+		}
+	}
+
+	rpwm = pslv | pwrpriv->tog;
+#ifdef CONFIG_LPS_LCLK
+	/* only when from PS_STATE S0/S1 to S2 and higher needs ACK */
+	if ((pwrpriv->cpwm < PS_STATE_S2) && (pslv >= PS_STATE_S2))
+		rpwm |= PS_ACK;
+#endif
+
+	pwrpriv->rpwm = pslv;
+
+#ifdef CONFIG_DETECT_CPWM_BY_POLLING
+	cpwm_orig = 0;
+	if (rpwm & PS_ACK)
+		rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_orig);
+#endif
+
+#if defined(CONFIG_LPS_RPWM_TIMER) && !defined(CONFIG_DETECT_CPWM_BY_POLLING)
+	if (rpwm & PS_ACK)
+		_set_timer(&pwrpriv->pwr_rpwm_timer, LPS_RPWM_WAIT_MS);
+#endif /* CONFIG_LPS_RPWM_TIMER & !CONFIG_DETECT_CPWM_BY_POLLING */
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm));
+
+	pwrpriv->tog += 0x80;
+
+#ifdef CONFIG_LPS_LCLK
+	/* No LPS 32K, No Ack */
+	if (rpwm & PS_ACK) {
+#ifdef CONFIG_DETECT_CPWM_BY_POLLING
+		u32 start_time;
+		u8 cpwm_now;
+		u8 poll_cnt = 0;
+
+		start_time = jiffies;
+
+		/* polling cpwm */
+		do {
+			rtw_msleep_os(1);
+			poll_cnt++;
+			cpwm_now = 0;
+			rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_now);
+			if ((cpwm_orig ^ cpwm_now) & 0x80) {
+				pwrpriv->cpwm = PS_STATE_S4;
+				pwrpriv->cpwm_tog = cpwm_now & PS_TOGGLE;
+#ifdef DBG_CHECK_FW_PS_STATE
+				RTW_INFO("%s: polling cpwm OK! poll_cnt=%d, cpwm_orig=%02x, cpwm_now=%02x , 0x100=0x%x\n"
+					, __func__, poll_cnt, cpwm_orig, cpwm_now, rtw_read8(padapter, REG_CR));
+				if (rtw_fw_ps_state(padapter) == _FAIL) {
+					RTW_INFO("leave 32k but fw state in 32k\n");
+					pdbgpriv->dbg_rpwm_toogle_cnt++;
+				}
+#endif /* DBG_CHECK_FW_PS_STATE */
+				break;
+			}
+
+			if (rtw_get_passing_time_ms(start_time) > LPS_RPWM_WAIT_MS) {
+				RTW_INFO("%s: polling cpwm timeout! poll_cnt=%d, cpwm_orig=%02x, cpwm_now=%02x\n", __func__, poll_cnt, cpwm_orig, cpwm_now);
+#ifdef DBG_CHECK_FW_PS_STATE
+				if (rtw_fw_ps_state(padapter) == _FAIL) {
+					RTW_INFO("rpwm timeout and fw ps state in 32k\n");
+					pdbgpriv->dbg_rpwm_timeout_fail_cnt++;
+				}
+#endif /* DBG_CHECK_FW_PS_STATE */
+#ifdef CONFIG_LPS_RPWM_TIMER
+				_set_timer(&pwrpriv->pwr_rpwm_timer, 1);
+#endif /* CONFIG_LPS_RPWM_TIMER */
+				break;
+			}
+		} while (1);
+#endif /* CONFIG_DETECT_CPWM_BY_POLLING */
+	} else
+#endif /* CONFIG_LPS_LCLK */
+	{
+		pwrpriv->cpwm = pslv;
+	}
+
+}
+
+static u8 PS_RDY_CHECK(_adapter *padapter)
+{
+	u32 curr_time, delta_time;
+	struct pwrctrl_priv	*pwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+#ifdef CONFIG_P2P
+	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
+#ifdef CONFIG_IOCTL_CFG80211
+	struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
+#endif /* CONFIG_IOCTL_CFG80211 */
+#endif /* CONFIG_P2P */
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+	if (pwrpriv->bInSuspend && pwrpriv->wowlan_mode)
+		return true;
+	else if (pwrpriv->bInSuspend && pwrpriv->wowlan_ap_mode)
+		return true;
+	else if (pwrpriv->bInSuspend)
+		return false;
+#else
+	if (pwrpriv->bInSuspend)
+		return false;
+#endif
+
+	curr_time = jiffies;
+
+	delta_time = curr_time - pwrpriv->DelayLPSLastTimeStamp;
+
+	if (delta_time < LPS_DELAY_TIME)
+		return false;
+
+	if (check_fwstate(pmlmepriv, WIFI_SITE_MONITOR)
+	    || check_fwstate(pmlmepriv, WIFI_UNDER_LINKING | WIFI_UNDER_WPS)
+	    || check_fwstate(pmlmepriv, WIFI_AP_STATE)
+	    || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE)
+#if defined(CONFIG_P2P) && defined(CONFIG_IOCTL_CFG80211)
+	    || rtw_cfg80211_get_is_roch(padapter) == true
+#endif
+	    || rtw_is_scan_deny(padapter)
+#ifdef CONFIG_TDLS
+	    /* TDLS link is established. */
+	    || (padapter->tdlsinfo.link_established == true)
+#endif /* CONFIG_TDLS		 */
+	   )
+		return false;
+
+	if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && (padapter->securitypriv.binstallGrpkey == false)) {
+		RTW_INFO("Group handshake still in progress !!!\n");
+		return false;
+	}
+
+#ifdef CONFIG_IOCTL_CFG80211
+	if (!rtw_cfg80211_pwr_mgmt(padapter))
+		return false;
+#endif
+
+	return true;
+}
+
+#if defined(CONFIG_FWLPS_IN_IPS)
+void rtw_set_fw_in_ips_mode(PADAPTER padapter, u8 enable)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	int cnt = 0;
+	u32 start_time;
+	u8 val8 = 0;
+	u8 cpwm_orig = 0, cpwm_now = 0;
+	u8 parm[H2C_INACTIVE_PS_LEN] = {0};
+
+	if (padapter->netif_up == false) {
+		RTW_INFO("%s: ERROR, netif is down\n", __func__);
+		return;
+	}
+
+	/* u8 cmd_param; */ /* BIT0:enable, BIT1:NoConnect32k */
+	if (enable) {
+#ifdef CONFIG_BT_COEXIST
+		rtw_btcoex_IpsNotify(padapter, pwrpriv->ips_mode_req);
+#endif
+		/* Enter IPS */
+		RTW_INFO("%s: issue H2C to FW when entering IPS\n", __func__);
+
+		parm[0] = 0x1;/* suggest by Isaac.Hsu*/
+#ifdef CONFIG_PNO_SUPPORT
+		if (pwrpriv->pno_inited) {
+			parm[1] = pwrpriv->pnlo_info->fast_scan_iterations;
+			parm[2] = pwrpriv->pnlo_info->slow_scan_period;
+		}
+#endif
+
+		rtw_hal_fill_h2c_cmd(padapter, /* H2C_FWLPS_IN_IPS_, */
+				     H2C_INACTIVE_PS_,
+				     H2C_INACTIVE_PS_LEN, parm);
+		/* poll 0x1cc to make sure H2C command already finished by FW; MAC_0x1cc=0 means H2C done by FW. */
+		do {
+			val8 = rtw_read8(padapter, REG_HMETFR);
+			cnt++;
+			RTW_INFO("%s  polling REG_HMETFR=0x%x, cnt=%d\n",
+				 __func__, val8, cnt);
+			rtw_mdelay_os(10);
+		} while (cnt < 100 && (val8 != 0));
+
+#ifdef CONFIG_LPS_LCLK
+		/* H2C done, enter 32k */
+		if (val8 == 0) {
+			/* ser rpwm to enter 32k */
+			val8 = rtw_read8(padapter, SDIO_LOCAL_BASE | SDIO_REG_HRPWM1);
+			RTW_INFO("%s: read rpwm=%02x\n", __func__, val8);
+			val8 += 0x80;
+			val8 |= BIT(0);
+			rtw_write8(padapter, SDIO_LOCAL_BASE | SDIO_REG_HRPWM1, val8);
+			RTW_INFO("%s: write rpwm=%02x\n", __func__, val8);
+			adapter_to_pwrctl(padapter)->tog = (val8 + 0x80) & 0x80;
+			cnt = val8 = 0;
+			if (parm[1] == 0 || parm[2] == 0) {
+				do {
+					val8 = rtw_read8(padapter, REG_CR);
+					cnt++;
+					RTW_INFO("%s  polling 0x100=0x%x, cnt=%d\n",
+						 __func__, val8, cnt);
+					RTW_INFO("%s 0x08:%02x, 0x03:%02x\n",
+						 __func__,
+						 rtw_read8(padapter, 0x08),
+						 rtw_read8(padapter, 0x03));
+					rtw_mdelay_os(10);
+				} while (cnt < 20 && (val8 != 0xEA));
+			}
+		}
+#endif
+	} else {
+		/* Leave IPS */
+		RTW_INFO("%s: Leaving IPS in FWLPS state\n", __func__);
+
+#ifdef CONFIG_LPS_LCLK
+		/* for polling cpwm */
+		cpwm_orig = 0;
+		rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_orig);
+
+		/* ser rpwm */
+		val8 = rtw_read8(padapter, SDIO_LOCAL_BASE | SDIO_REG_HRPWM1);
+		val8 &= 0x80;
+		val8 += 0x80;
+		val8 |= BIT(6);
+		rtw_write8(padapter, SDIO_LOCAL_BASE | SDIO_REG_HRPWM1, val8);
+		RTW_INFO("%s: write rpwm=%02x\n", __func__, val8);
+		adapter_to_pwrctl(padapter)->tog = (val8 + 0x80) & 0x80;
+
+		/* do polling cpwm */
+		start_time = jiffies;
+		do {
+
+			rtw_mdelay_os(1);
+
+			rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_now);
+			if ((cpwm_orig ^ cpwm_now) & 0x80)
+				break;
+
+			if (rtw_get_passing_time_ms(start_time) > 100) {
+				RTW_INFO("%s: polling cpwm timeout when leaving IPS in FWLPS state\n", __func__);
+				break;
+			}
+		} while (1);
+
+#endif
+		parm[0] = 0x0;
+		parm[1] = 0x0;
+		parm[2] = 0x0;
+		rtw_hal_fill_h2c_cmd(padapter, H2C_INACTIVE_PS_,
+				     H2C_INACTIVE_PS_LEN, parm);
+#ifdef CONFIG_BT_COEXIST
+		rtw_btcoex_IpsNotify(padapter, IPS_NONE);
+#endif
+	}
+}
+#endif /* CONFIG_PNO_SUPPORT */
+
+void rtw_set_ps_mode(PADAPTER padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode, const char *msg)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_P2P */
+#ifdef CONFIG_TDLS
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	unsigned long irqL;
+	int i, j;
+	_list	*plist, *phead;
+	struct sta_info *ptdls_sta;
+#endif /* CONFIG_TDLS */
+#ifdef CONFIG_LPS_PG
+	u8 lps_pg_hdl_id = 0;
+#endif
+
+
+
+	if (ps_mode > PM_Card_Disable) {
+		return;
+	}
+
+	if (pwrpriv->pwr_mode == ps_mode) {
+		if (PS_MODE_ACTIVE == ps_mode)
+			return;
+
+#ifndef CONFIG_BT_COEXIST
+		if ((pwrpriv->smart_ps == smart_ps) &&
+		    (pwrpriv->bcn_ant_mode == bcn_ant_mode))
+			return;
+#endif /* !CONFIG_BT_COEXIST */
+	}
+
+#ifdef CONFIG_FW_MULTI_PORT_SUPPORT
+	if (PS_MODE_ACTIVE != ps_mode) {
+		rtw_set_ps_rsvd_page(padapter);
+		rtw_set_default_port_id(padapter);
+	}
+#endif
+
+#ifdef CONFIG_LPS_PG
+	if ((PS_MODE_ACTIVE != ps_mode) && (pwrpriv->blpspg_info_up)) {
+		/*rtw_hal_set_lps_pg_info(padapter);*/
+		lps_pg_hdl_id = LPS_PG_INFO_CFG;
+		rtw_hal_set_hwreg(padapter, HW_VAR_LPS_PG_HANDLE, (u8 *)(&lps_pg_hdl_id));
+	}
+#endif
+
+#ifdef CONFIG_LPS_LCLK
+	_enter_pwrlock(&pwrpriv->lock);
+#endif
+
+	/* if(pwrpriv->pwr_mode == PS_MODE_ACTIVE) */
+	if (ps_mode == PS_MODE_ACTIVE) {
+		if (1
+#ifdef CONFIG_BT_COEXIST
+		    && (((rtw_btcoex_IsBtControlLps(padapter) == false)
+#ifdef CONFIG_P2P_PS
+			 && (pwdinfo->opp_ps == 0)
+#endif /* CONFIG_P2P_PS */
+			)
+			|| ((rtw_btcoex_IsBtControlLps(padapter) == true)
+			    && (rtw_btcoex_IsLpsOn(padapter) == false))
+		       )
+#else /* !CONFIG_BT_COEXIST */
+#ifdef CONFIG_P2P_PS
+		    && (pwdinfo->opp_ps == 0)
+#endif /* CONFIG_P2P_PS */
+#endif /* !CONFIG_BT_COEXIST */
+		   ) {
+			RTW_INFO(FUNC_ADPT_FMT" Leave 802.11 power save - %s\n",
+				 FUNC_ADPT_ARG(padapter), msg);
+
+			if (pwrpriv->lps_leave_cnts < UINT_MAX)
+				pwrpriv->lps_leave_cnts++;
+			else
+				pwrpriv->lps_leave_cnts = 0;
+#ifdef CONFIG_TDLS
+			for (i = 0; i < NUM_STA; i++) {
+				phead = &(pstapriv->sta_hash[i]);
+				plist = get_next(phead);
+
+				while ((rtw_end_of_queue_search(phead, plist)) == false) {
+					ptdls_sta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+					if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)
+						issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta->hwaddr, 0, 0, 0);
+					plist = get_next(plist);
+				}
+			}
+#endif /* CONFIG_TDLS */
+
+			pwrpriv->pwr_mode = ps_mode;
+			rtw_set_rpwm(padapter, PS_STATE_S4);
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN) || defined(CONFIG_P2P_WOWLAN)
+			if (pwrpriv->wowlan_mode == true ||
+			    pwrpriv->wowlan_ap_mode == true ||
+			    pwrpriv->wowlan_p2p_mode == true) {
+				u32 start_time, delay_ms;
+				u8 val8;
+				delay_ms = 20;
+				start_time = jiffies;
+				do {
+					rtw_hal_get_hwreg(padapter, HW_VAR_SYS_CLKR, &val8);
+					if (!(val8 & BIT(4))) { /* 0x08 bit4 =1 --> in 32k, bit4 = 0 --> leave 32k */
+						pwrpriv->cpwm = PS_STATE_S4;
+						break;
+					}
+					if (rtw_get_passing_time_ms(start_time) > delay_ms) {
+						RTW_INFO("%s: Wait for FW 32K leave more than %u ms!!!\n",
+							__func__, delay_ms);
+						pdbgpriv->dbg_wow_leave_ps_fail_cnt++;
+						break;
+					}
+					rtw_usleep_os(100);
+				} while (1);
+			}
+#endif
+			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+
+#ifdef CONFIG_LPS_POFF
+			rtw_hal_set_hwreg(padapter, HW_VAR_LPS_POFF_SET_MODE,
+					  (u8 *)(&ps_mode));
+#endif /*CONFIG_LPS_POFF*/
+
+			pwrpriv->bFwCurrentInPSMode = false;
+
+#ifdef CONFIG_BT_COEXIST
+			rtw_btcoex_LpsNotify(padapter, ps_mode);
+#endif /* CONFIG_BT_COEXIST */
+		}
+	} else {
+		if ((PS_RDY_CHECK(padapter) && check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE))
+#ifdef CONFIG_BT_COEXIST
+		    || ((rtw_btcoex_IsBtControlLps(padapter) == true)
+			&& (rtw_btcoex_IsLpsOn(padapter) == true))
+#endif
+#ifdef CONFIG_P2P_WOWLAN
+		    || (pwrpriv->wowlan_p2p_mode)
+#endif /* CONFIG_P2P_WOWLAN */
+		   ) {
+			u8 pslv;
+
+			RTW_INFO(FUNC_ADPT_FMT" Enter 802.11 power save - %s\n",
+				 FUNC_ADPT_ARG(padapter), msg);
+
+			if (pwrpriv->lps_enter_cnts < UINT_MAX)
+				pwrpriv->lps_enter_cnts++;
+			else
+				pwrpriv->lps_enter_cnts = 0;
+#ifdef CONFIG_TDLS
+			for (i = 0; i < NUM_STA; i++) {
+				phead = &(pstapriv->sta_hash[i]);
+				plist = get_next(phead);
+
+				while ((rtw_end_of_queue_search(phead, plist)) == false) {
+					ptdls_sta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+					if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)
+						issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta->hwaddr, 1, 0, 0);
+					plist = get_next(plist);
+				}
+			}
+#endif /* CONFIG_TDLS */
+
+#ifdef CONFIG_BT_COEXIST
+			rtw_btcoex_LpsNotify(padapter, ps_mode);
+#endif /* CONFIG_BT_COEXIST */
+
+#ifdef CONFIG_LPS_POFF
+			rtw_hal_set_hwreg(padapter, HW_VAR_LPS_POFF_SET_MODE,
+					  (u8 *)(&ps_mode));
+#endif /*CONFIG_LPS_POFF*/
+
+			pwrpriv->bFwCurrentInPSMode = true;
+			pwrpriv->pwr_mode = ps_mode;
+			pwrpriv->smart_ps = smart_ps;
+			pwrpriv->bcn_ant_mode = bcn_ant_mode;
+			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+
+#ifdef CONFIG_P2P_PS
+			/* Set CTWindow after LPS */
+			if (pwdinfo->opp_ps == 1)
+				p2p_ps_wk_cmd(padapter, P2P_PS_ENABLE, 0);
+#endif /* CONFIG_P2P_PS */
+
+			pslv = PS_STATE_S2;
+#ifdef CONFIG_LPS_LCLK
+			if (pwrpriv->alives == 0)
+				pslv = PS_STATE_S0;
+#endif /* CONFIG_LPS_LCLK */
+
+#ifdef CONFIG_BT_COEXIST
+			if ((rtw_btcoex_IsBtDisabled(padapter) == false)
+			    && (rtw_btcoex_IsBtControlLps(padapter) == true)) {
+				u8 val8;
+
+				val8 = rtw_btcoex_LpsVal(padapter);
+				if (val8 & BIT(4))
+					pslv = PS_STATE_S2;
+
+			}
+#endif /* CONFIG_BT_COEXIST */
+
+			rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+#ifdef CONFIG_LPS_LCLK
+	_exit_pwrlock(&pwrpriv->lock);
+#endif
+
+}
+
+/*
+ * Return:
+ *	0:	Leave OK
+ *	-1:	Timeout
+ *	-2:	Other error
+ */
+s32 LPS_RF_ON_check(PADAPTER padapter, u32 delay_ms)
+{
+	u32 start_time;
+	u8 bAwake = false;
+	s32 err = 0;
+
+
+	start_time = jiffies;
+	while (1) {
+		rtw_hal_get_hwreg(padapter, HW_VAR_FWLPS_RF_ON, &bAwake);
+		if (bAwake)
+			break;
+
+		if (rtw_is_surprise_removed(padapter)) {
+			err = -2;
+			RTW_INFO("%s: device surprise removed!!\n", __func__);
+			break;
+		}
+
+		if (rtw_get_passing_time_ms(start_time) > delay_ms) {
+			err = -1;
+			RTW_INFO("%s: Wait for FW LPS leave more than %u ms!!!\n", __func__, delay_ms);
+			break;
+		}
+		rtw_usleep_os(100);
+	}
+
+	return err;
+}
+
+/*
+ *	Description:
+ *		Enter the leisure power save mode.
+ *   */
+void LPS_Enter(PADAPTER padapter, const char *msg)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct pwrctrl_priv	*pwrpriv = dvobj_to_pwrctl(dvobj);
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	int n_assoc_iface = 0;
+	int i;
+	char buf[32] = {0};
+
+
+	/*	RTW_INFO("+LeisurePSEnter\n"); */
+	if (false == padapter->bFWReady)
+		return;
+
+#ifdef CONFIG_BT_COEXIST
+	if (rtw_btcoex_IsBtControlLps(padapter) == true)
+		return;
+#endif
+
+	/* Skip lps enter request if number of assocated adapters is not 1 */
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		if (check_fwstate(&(dvobj->padapters[i]->mlmepriv), WIFI_ASOC_STATE))
+			n_assoc_iface++;
+	}
+	if (n_assoc_iface != 1)
+		return;
+
+#ifndef CONFIG_FW_MULTI_PORT_SUPPORT
+	/* Skip lps enter request for adapter not port0 */
+	if (get_hw_port(padapter) != HW_PORT0)
+		return;
+#endif
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		if (PS_RDY_CHECK(dvobj->padapters[i]) == false)
+			return;
+	}
+
+#ifdef CONFIG_P2P_PS
+	if (padapter->wdinfo.p2p_ps_mode == P2P_PS_NOA) {
+		return;/* supporting p2p client ps NOA via H2C_8723B_P2P_PS_OFFLOAD */
+	}
+#endif /* CONFIG_P2P_PS */
+
+	if (pwrpriv->bLeisurePs) {
+		/* Idle for a while if we connect to AP a while ago. */
+		if (pwrpriv->LpsIdleCount >= 2) { /* 4 Sec */
+			if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
+				sprintf(buf, "WIFI-%s", msg);
+				pwrpriv->bpower_saving = true;
+				rtw_set_ps_mode(padapter, pwrpriv->power_mgnt, padapter->registrypriv.smart_ps, 0, buf);
+			}
+		} else
+			pwrpriv->LpsIdleCount++;
+	}
+
+	/*	RTW_INFO("-LeisurePSEnter\n"); */
+
+}
+
+/*
+ *	Description:
+ *		Leave the leisure power save mode.
+ *   */
+void LPS_Leave(PADAPTER padapter, const char *msg)
+{
+#define LPS_LEAVE_TIMEOUT_MS 100
+
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct pwrctrl_priv	*pwrpriv = dvobj_to_pwrctl(dvobj);
+	u32 start_time;
+	u8 bAwake = false;
+	char buf[32] = {0};
+	struct debug_priv *pdbgpriv = &dvobj->drv_dbg;
+
+
+	/*	RTW_INFO("+LeisurePSLeave\n"); */
+
+#ifdef CONFIG_BT_COEXIST
+	if (rtw_btcoex_IsBtControlLps(padapter) == true)
+		return;
+#endif
+
+	if (pwrpriv->bLeisurePs) {
+		if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) {
+			sprintf(buf, "WIFI-%s", msg);
+			rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, buf);
+
+			if (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+				LPS_RF_ON_check(padapter, LPS_LEAVE_TIMEOUT_MS);
+		}
+	}
+
+	pwrpriv->bpower_saving = false;
+#ifdef DBG_CHECK_FW_PS_STATE
+	if (rtw_fw_ps_state(padapter) == _FAIL) {
+		RTW_INFO("leave lps, fw in 32k\n");
+		pdbgpriv->dbg_leave_lps_fail_cnt++;
+	}
+#endif /* DBG_CHECK_FW_PS_STATE
+ * 	RTW_INFO("-LeisurePSLeave\n"); */
+
+}
+#endif
+
+void LeaveAllPowerSaveModeDirect(PADAPTER Adapter)
+{
+	PADAPTER pri_padapter = GET_PRIMARY_ADAPTER(Adapter);
+	struct mlme_priv	*pmlmepriv = &(Adapter->mlmepriv);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(Adapter);
+	struct dvobj_priv *psdpriv = Adapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+#ifndef CONFIG_DETECT_CPWM_BY_POLLING
+	u8 cpwm_orig, cpwm_now;
+	u32 start_time;
+#endif /* CONFIG_DETECT_CPWM_BY_POLLING */
+
+
+	RTW_INFO("%s.....\n", __func__);
+
+	if (rtw_is_surprise_removed(Adapter)) {
+		RTW_INFO(FUNC_ADPT_FMT ": bSurpriseRemoved=true Skip!\n", FUNC_ADPT_ARG(Adapter));
+		return;
+	}
+
+	if (rtw_mi_check_status(Adapter, MI_LINKED)) { /*connect*/
+
+		if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
+			RTW_INFO("%s: Driver Already Leave LPS\n", __func__);
+			return;
+		}
+
+#ifdef CONFIG_LPS_LCLK
+		_enter_pwrlock(&pwrpriv->lock);
+
+#ifndef CONFIG_DETECT_CPWM_BY_POLLING
+		cpwm_orig = 0;
+		rtw_hal_get_hwreg(Adapter, HW_VAR_CPWM, &cpwm_orig);
+#endif /* CONFIG_DETECT_CPWM_BY_POLLING */
+		rtw_set_rpwm(Adapter, PS_STATE_S4);
+
+#ifndef CONFIG_DETECT_CPWM_BY_POLLING
+
+		start_time = jiffies;
+
+		/* polling cpwm */
+		do {
+			rtw_mdelay_os(1);
+
+			rtw_hal_get_hwreg(Adapter, HW_VAR_CPWM, &cpwm_now);
+			if ((cpwm_orig ^ cpwm_now) & 0x80) {
+				pwrpriv->cpwm = PS_STATE_S4;
+				pwrpriv->cpwm_tog = cpwm_now & PS_TOGGLE;
+#ifdef DBG_CHECK_FW_PS_STATE
+				RTW_INFO("%s: polling cpwm OK! cpwm_orig=%02x, cpwm_now=%02x, 0x100=0x%x\n"
+					, __func__, cpwm_orig, cpwm_now, rtw_read8(Adapter, REG_CR));
+				if (rtw_fw_ps_state(Adapter) == _FAIL) {
+					RTW_INFO("%s: leave 32k but fw state in 32k\n", __func__);
+					pdbgpriv->dbg_rpwm_toogle_cnt++;
+				}
+#endif /* DBG_CHECK_FW_PS_STATE */
+				break;
+			}
+
+			if (rtw_get_passing_time_ms(start_time) > LPS_RPWM_WAIT_MS) {
+				RTW_INFO("%s: polling cpwm timeout! cpwm_orig=%02x, cpwm_now=%02x\n", __func__, cpwm_orig, cpwm_now);
+#ifdef DBG_CHECK_FW_PS_STATE
+				if (rtw_fw_ps_state(Adapter) == _FAIL) {
+					RTW_INFO("rpwm timeout and fw ps state in 32k\n");
+					pdbgpriv->dbg_rpwm_timeout_fail_cnt++;
+				}
+#endif /* DBG_CHECK_FW_PS_STATE */
+				break;
+			}
+		} while (1);
+#endif /* CONFIG_DETECT_CPWM_BY_POLLING */
+
+		_exit_pwrlock(&pwrpriv->lock);
+#endif
+
+#ifdef CONFIG_P2P_PS
+		p2p_ps_wk_cmd(pri_padapter, P2P_PS_DISABLE, 0);
+#endif /* CONFIG_P2P_PS */
+
+#ifdef CONFIG_LPS
+		rtw_lps_ctrl_wk_cmd(pri_padapter, LPS_CTRL_LEAVE, 0);
+#endif
+	} else {
+		if (pwrpriv->rf_pwrstate == rf_off) {
+#ifdef CONFIG_AUTOSUSPEND
+			if (Adapter->registrypriv.usbss_enable) {
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35))
+				usb_disable_autosuspend(adapter_to_dvobj(Adapter)->pusbdev);
+#elif (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 22) && LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 34))
+				adapter_to_dvobj(Adapter)->pusbdev->autosuspend_disabled = Adapter->bDisableAutosuspend;/* autosuspend disabled by the user */
+#endif
+			} else
+#endif
+			{
+#ifdef CONFIG_IPS
+				if (false == ips_leave(pri_padapter))
+					RTW_INFO("======> ips_leave fail.............\n");
+#endif
+			}
+		}
+	}
+
+}
+
+/*
+ * Description: Leave all power save mode: LPS, FwLPS, IPS if needed.
+ * Move code to function by tynli. 2010.03.26.
+ *   */
+void LeaveAllPowerSaveMode(PADAPTER Adapter)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(Adapter);
+	struct mlme_priv	*pmlmepriv = &(Adapter->mlmepriv);
+	u8	enqueue = 0;
+	int n_assoc_iface = 0;
+	int i;
+
+
+	/* RTW_INFO("%s.....\n",__func__); */
+
+	if (false == Adapter->bup) {
+		RTW_INFO(FUNC_ADPT_FMT ": bup=%d Skip!\n",
+			 FUNC_ADPT_ARG(Adapter), Adapter->bup);
+		return;
+	}
+
+	if (rtw_is_surprise_removed(Adapter)) {
+		RTW_INFO(FUNC_ADPT_FMT ": bSurpriseRemoved=true Skip!\n", FUNC_ADPT_ARG(Adapter));
+		return;
+	}
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		if (check_fwstate(&(dvobj->padapters[i]->mlmepriv), WIFI_ASOC_STATE))
+			n_assoc_iface++;
+	}
+
+	if (n_assoc_iface) {
+		/* connect */
+#ifdef CONFIG_LPS_LCLK
+		enqueue = 1;
+#endif
+
+#ifdef CONFIG_P2P_PS
+		p2p_ps_wk_cmd(Adapter, P2P_PS_DISABLE, enqueue);
+#endif /* CONFIG_P2P_PS */
+
+#ifdef CONFIG_LPS
+		rtw_lps_ctrl_wk_cmd(Adapter, LPS_CTRL_LEAVE, enqueue);
+#endif
+
+#ifdef CONFIG_LPS_LCLK
+		LPS_Leave_check(Adapter);
+#endif
+	} else {
+		if (adapter_to_pwrctl(Adapter)->rf_pwrstate == rf_off) {
+#ifdef CONFIG_AUTOSUSPEND
+			if (Adapter->registrypriv.usbss_enable) {
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35))
+				usb_disable_autosuspend(adapter_to_dvobj(Adapter)->pusbdev);
+#elif (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 22) && LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 34))
+				adapter_to_dvobj(Adapter)->pusbdev->autosuspend_disabled = Adapter->bDisableAutosuspend;/* autosuspend disabled by the user */
+#endif
+			} else
+#endif
+			{
+#if defined(CONFIG_FWLPS_IN_IPS) || defined(CONFIG_SWLPS_IN_IPS) || (defined(CONFIG_PLATFORM_SPRD))
+#ifdef CONFIG_IPS
+				if (false == ips_leave(Adapter))
+					RTW_INFO("======> ips_leave fail.............\n");
+#endif
+#endif /* CONFIG_SWLPS_IN_IPS || (CONFIG_PLATFORM_SPRD) */
+			}
+		}
+	}
+
+}
+
+#ifdef CONFIG_LPS_LCLK
+void LPS_Leave_check(
+	PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrpriv;
+	u32	start_time;
+	u8	bReady;
+
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	bReady = false;
+	start_time = jiffies;
+
+	rtw_yield_os();
+
+	while (1) {
+		_enter_pwrlock(&pwrpriv->lock);
+
+		if (rtw_is_surprise_removed(padapter)
+		    || (!rtw_is_hw_init_completed(padapter))
+		    || rtw_is_drv_stopped(padapter)
+		    || (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+		   )
+			bReady = true;
+
+		_exit_pwrlock(&pwrpriv->lock);
+
+		if (bReady)
+			break;
+
+		if (rtw_get_passing_time_ms(start_time) > 100) {
+			RTW_INFO("Wait for cpwm event  than 100 ms!!!\n");
+			break;
+		}
+		rtw_msleep_os(1);
+	}
+
+}
+
+/*
+ * Caller:ISR handler...
+ *
+ * This will be called when CPWM interrupt is up.
+ *
+ * using to update cpwn of drv; and drv willl make a decision to up or down pwr level
+ */
+void cpwm_int_hdl(
+	PADAPTER padapter,
+	struct reportpwrstate_parm *preportpwrstate)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+	if (!padapter)
+		goto exit;
+
+	if (RTW_CANNOT_RUN(padapter))
+		goto exit;
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+	_enter_pwrlock(&pwrpriv->lock);
+
+#ifdef CONFIG_LPS_RPWM_TIMER
+	if (pwrpriv->rpwm < PS_STATE_S2) {
+		RTW_INFO("%s: Redundant CPWM Int. RPWM=0x%02X CPWM=0x%02x\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+		_exit_pwrlock(&pwrpriv->lock);
+		goto exit;
+	}
+#endif /* CONFIG_LPS_RPWM_TIMER */
+
+	pwrpriv->cpwm = PS_STATE(preportpwrstate->state);
+	pwrpriv->cpwm_tog = preportpwrstate->state & PS_TOGGLE;
+
+	if (pwrpriv->cpwm >= PS_STATE_S2) {
+		if (pwrpriv->alives & CMD_ALIVE)
+			up(&padapter->cmdpriv.cmd_queue_sema);
+
+		if (pwrpriv->alives & XMIT_ALIVE)
+			up(&padapter->xmitpriv.xmit_sema);
+	}
+
+	_exit_pwrlock(&pwrpriv->lock);
+
+exit:
+	return;
+}
+
+static void cpwm_event_callback(struct work_struct *work)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(work, struct pwrctrl_priv, cpwm_event);
+	struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv);
+	_adapter *adapter = dvobj_get_primary_adapter(dvobj);
+	struct reportpwrstate_parm report;
+
+	/* RTW_INFO("%s\n",__func__); */
+
+	report.state = PS_STATE_S2;
+	cpwm_int_hdl(adapter, &report);
+}
+
+static void dma_event_callback(struct work_struct *work)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(work, struct pwrctrl_priv, dma_event);
+	struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv);
+	_adapter *adapter = dvobj_get_primary_adapter(dvobj);
+
+	rtw_unregister_tx_alive(adapter);
+}
+
+#ifdef CONFIG_LPS_RPWM_TIMER
+static void rpwmtimeout_workitem_callback(struct work_struct *work)
+{
+	PADAPTER padapter;
+	struct dvobj_priv *dvobj;
+	struct pwrctrl_priv *pwrpriv;
+
+
+	pwrpriv = container_of(work, struct pwrctrl_priv, rpwmtimeoutwi);
+	dvobj = pwrctl_to_dvobj(pwrpriv);
+	padapter = dvobj_get_primary_adapter(dvobj);
+	/*	RTW_INFO("+%s: rpwm=0x%02X cpwm=0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); */
+
+	if (!padapter)
+		return;
+
+	if (RTW_CANNOT_RUN(padapter))
+		return;
+
+	_enter_pwrlock(&pwrpriv->lock);
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
+		RTW_INFO("%s: rpwm=0x%02X cpwm=0x%02X CPWM done!\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+		goto exit;
+	}
+	_exit_pwrlock(&pwrpriv->lock);
+
+	if (rtw_read8(padapter, 0x100) != 0xEA) {
+		struct reportpwrstate_parm report;
+
+		report.state = PS_STATE_S2;
+		RTW_INFO("\n%s: FW already leave 32K!\n\n", __func__);
+		cpwm_int_hdl(padapter, &report);
+		return;
+	}
+
+	_enter_pwrlock(&pwrpriv->lock);
+
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
+		RTW_INFO("%s: cpwm=%d, nothing to do!\n", __func__, pwrpriv->cpwm);
+		goto exit;
+	}
+	pwrpriv->brpwmtimeout = true;
+	rtw_set_rpwm(padapter, pwrpriv->rpwm);
+	pwrpriv->brpwmtimeout = false;
+
+exit:
+	_exit_pwrlock(&pwrpriv->lock);
+}
+
+/*
+ * This function is a timer handler, can't do any IO in it.
+ */
+static void pwr_rpwm_timeout_handler(void *FunctionContext)
+{
+	PADAPTER padapter;
+	struct pwrctrl_priv *pwrpriv;
+
+
+	padapter = (PADAPTER)FunctionContext;
+	pwrpriv = adapter_to_pwrctl(padapter);
+	if (!padapter)
+		return;
+
+	if (RTW_CANNOT_RUN(padapter))
+		return;
+
+	RTW_INFO("+%s: rpwm=0x%02X cpwm=0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
+		RTW_INFO("+%s: cpwm=%d, nothing to do!\n", __func__, pwrpriv->cpwm);
+		return;
+	}
+
+	_set_workitem(&pwrpriv->rpwmtimeoutwi);
+}
+#endif /* CONFIG_LPS_RPWM_TIMER */
+
+__inline static void register_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag)
+{
+	pwrctrl->alives |= tag;
+}
+
+__inline static void unregister_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag)
+{
+	pwrctrl->alives &= ~tag;
+}
+
+
+/*
+ * Description:
+ *	Check if the fw_pwrstate is okay for I/O.
+ *	If not (cpwm is less than S2), then the sub-routine
+ *	will raise the cpwm to be greater than or equal to S2.
+ *
+ *	Calling Context: Passive
+ *
+ *	Constraint:
+ *		1. this function will request pwrctrl->lock
+ *
+ * Return Value:
+ *	_SUCCESS	hardware is ready for I/O
+ *	_FAIL		can't I/O right now
+ */
+s32 rtw_register_task_alive(PADAPTER padapter, u32 task)
+{
+	s32 res;
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+
+	res = _SUCCESS;
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S2;
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, task);
+
+	if (pwrctrl->bFwCurrentInPSMode == true) {
+
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+#ifdef CONFIG_DETECT_CPWM_BY_POLLING
+	if (_FAIL == res) {
+		if (pwrctrl->cpwm >= PS_STATE_S2)
+			res = _SUCCESS;
+	}
+#endif /* CONFIG_DETECT_CPWM_BY_POLLING */
+
+
+	return res;
+}
+
+/*
+ * Description:
+ *	If task is done, call this func. to power down firmware again.
+ *
+ *	Constraint:
+ *		1. this function will request pwrctrl->lock
+ *
+ * Return Value:
+ *	none
+ */
+void rtw_unregister_task_alive(PADAPTER padapter, u32 task)
+{
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S0;
+
+#ifdef CONFIG_BT_COEXIST
+	if ((rtw_btcoex_IsBtDisabled(padapter) == false)
+	    && (rtw_btcoex_IsBtControlLps(padapter) == true)) {
+		u8 val8;
+
+		val8 = rtw_btcoex_LpsVal(padapter);
+		if (val8 & BIT(4))
+			pslv = PS_STATE_S2;
+
+	}
+#endif /* CONFIG_BT_COEXIST */
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, task);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
+	    && (pwrctrl->bFwCurrentInPSMode == true)) {
+
+		if (pwrctrl->cpwm > pslv) {
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+}
+
+/*
+ * Caller: rtw_xmit_thread
+ *
+ * Check if the fw_pwrstate is okay for xmit.
+ * If not (cpwm is less than S3), then the sub-routine
+ * will raise the cpwm to be greater than or equal to S3.
+ *
+ * Calling Context: Passive
+ *
+ * Return Value:
+ *	 _SUCCESS	rtw_xmit_thread can write fifo/txcmd afterwards.
+ *	 _FAIL		rtw_xmit_thread can not do anything.
+ */
+s32 rtw_register_tx_alive(PADAPTER padapter)
+{
+	s32 res;
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+
+	res = _SUCCESS;
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S2;
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, XMIT_ALIVE);
+
+	if (pwrctrl->bFwCurrentInPSMode == true) {
+
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+#ifdef CONFIG_DETECT_CPWM_BY_POLLING
+	if (_FAIL == res) {
+		if (pwrctrl->cpwm >= PS_STATE_S2)
+			res = _SUCCESS;
+	}
+#endif /* CONFIG_DETECT_CPWM_BY_POLLING */
+
+
+	return res;
+}
+
+/*
+ * Caller: rtw_cmd_thread
+ *
+ * Check if the fw_pwrstate is okay for issuing cmd.
+ * If not (cpwm should be is less than S2), then the sub-routine
+ * will raise the cpwm to be greater than or equal to S2.
+ *
+ * Calling Context: Passive
+ *
+ * Return Value:
+ *	_SUCCESS	rtw_cmd_thread can issue cmds to firmware afterwards.
+ *	_FAIL		rtw_cmd_thread can not do anything.
+ */
+s32 rtw_register_cmd_alive(PADAPTER padapter)
+{
+	s32 res;
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+
+	res = _SUCCESS;
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S2;
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, CMD_ALIVE);
+
+	if (pwrctrl->bFwCurrentInPSMode == true) {
+
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+#ifdef CONFIG_DETECT_CPWM_BY_POLLING
+	if (_FAIL == res) {
+		if (pwrctrl->cpwm >= PS_STATE_S2)
+			res = _SUCCESS;
+	}
+#endif /* CONFIG_DETECT_CPWM_BY_POLLING */
+
+
+	return res;
+}
+
+/*
+ * Caller: rx_isr
+ *
+ * Calling Context: Dispatch/ISR
+ *
+ * Return Value:
+ *	_SUCCESS
+ *	_FAIL
+ */
+s32 rtw_register_rx_alive(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, RECV_ALIVE);
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+
+	return _SUCCESS;
+}
+
+/*
+ * Caller: evt_isr or evt_thread
+ *
+ * Calling Context: Dispatch/ISR or Passive
+ *
+ * Return Value:
+ *	_SUCCESS
+ *	_FAIL
+ */
+s32 rtw_register_evt_alive(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, EVT_ALIVE);
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+
+	return _SUCCESS;
+}
+
+/*
+ * Caller: ISR
+ *
+ * If ISR's txdone,
+ * No more pkts for TX,
+ * Then driver shall call this fun. to power down firmware again.
+ */
+void rtw_unregister_tx_alive(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+	_adapter *iface;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	u8 pslv, i;
+
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S0;
+
+#ifdef CONFIG_BT_COEXIST
+	if ((rtw_btcoex_IsBtDisabled(padapter) == false)
+	    && (rtw_btcoex_IsBtControlLps(padapter) == true)) {
+		u8 val8;
+
+		val8 = rtw_btcoex_LpsVal(padapter);
+		if (val8 & BIT(4))
+			pslv = PS_STATE_S2;
+
+	}
+#endif /* CONFIG_BT_COEXIST */
+
+#ifdef CONFIG_P2P_PS
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		if ((iface) && rtw_is_adapter_up(iface)) {
+			if (iface->wdinfo.p2p_ps_mode > P2P_PS_NONE) {
+				pslv = PS_STATE_S2;
+				break;
+			}
+		}
+	}
+#endif
+	_enter_pwrlock(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, XMIT_ALIVE);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
+	    && (pwrctrl->bFwCurrentInPSMode == true)) {
+
+		if (pwrctrl->cpwm > pslv) {
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+}
+
+/*
+ * Caller: ISR
+ *
+ * If all commands have been done,
+ * and no more command to do,
+ * then driver shall call this fun. to power down firmware again.
+ */
+void rtw_unregister_cmd_alive(PADAPTER padapter)
+{
+	_adapter *iface;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv, i;
+
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S0;
+
+#ifdef CONFIG_BT_COEXIST
+	if ((rtw_btcoex_IsBtDisabled(padapter) == false)
+	    && (rtw_btcoex_IsBtControlLps(padapter) == true)) {
+		u8 val8;
+
+		val8 = rtw_btcoex_LpsVal(padapter);
+		if (val8 & BIT(4))
+			pslv = PS_STATE_S2;
+
+	}
+#endif /* CONFIG_BT_COEXIST */
+
+#ifdef CONFIG_P2P_PS
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		if ((iface) && rtw_is_adapter_up(iface)) {
+			if (iface->wdinfo.p2p_ps_mode > P2P_PS_NONE) {
+				pslv = PS_STATE_S2;
+				break;
+			}
+		}
+	}
+#endif
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, CMD_ALIVE);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
+	    && (pwrctrl->bFwCurrentInPSMode == true)) {
+
+		if (pwrctrl->cpwm > pslv) {
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+}
+
+/*
+ * Caller: ISR
+ */
+void rtw_unregister_rx_alive(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+
+	_enter_pwrlock(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, RECV_ALIVE);
+
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+}
+
+void rtw_unregister_evt_alive(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+
+	unregister_task_alive(pwrctrl, EVT_ALIVE);
+
+
+	_exit_pwrlock(&pwrctrl->lock);
+
+}
+#endif	/* CONFIG_LPS_LCLK */
+
+#ifdef CONFIG_RESUME_IN_WORKQUEUE
+	static void resume_workitem_callback(struct work_struct *work);
+#endif /* CONFIG_RESUME_IN_WORKQUEUE */
+
+void rtw_init_pwrctrl_priv(PADAPTER padapter)
+{
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	u8 val8 = 0;
+
+#if defined(CONFIG_CONCURRENT_MODE)
+	if (padapter->adapter_type != PRIMARY_ADAPTER)
+		return;
+#endif
+	_init_pwrlock(&pwrctrlpriv->lock);
+	_init_pwrlock(&pwrctrlpriv->check_32k_lock);
+	pwrctrlpriv->rf_pwrstate = rf_on;
+	pwrctrlpriv->ips_enter_cnts = 0;
+	pwrctrlpriv->ips_leave_cnts = 0;
+	pwrctrlpriv->lps_enter_cnts = 0;
+	pwrctrlpriv->lps_leave_cnts = 0;
+	pwrctrlpriv->bips_processing = false;
+
+	pwrctrlpriv->ips_mode = padapter->registrypriv.ips_mode;
+	pwrctrlpriv->ips_mode_req = padapter->registrypriv.ips_mode;
+
+	pwrctrlpriv->pwr_state_check_interval = RTW_PWR_STATE_CHK_INTERVAL;
+	pwrctrlpriv->pwr_state_check_cnts = 0;
+	pwrctrlpriv->bInternalAutoSuspend = false;
+	pwrctrlpriv->bInSuspend = false;
+	pwrctrlpriv->bkeepfwalive = false;
+	pwrctrlpriv->padapter = padapter;
+
+#ifdef CONFIG_AUTOSUSPEND
+#ifdef SUPPORT_HW_RFOFF_DETECTED
+	pwrctrlpriv->pwr_state_check_interval = (pwrctrlpriv->bHWPwrPindetect) ? 1000 : 2000;
+#endif
+#endif
+
+	pwrctrlpriv->LpsIdleCount = 0;
+
+#ifdef CONFIG_LPS_PG
+	pwrctrlpriv->lpspg_rsvd_page_locate = 0;
+#endif
+
+	/* pwrctrlpriv->FWCtrlPSMode =padapter->registrypriv.power_mgnt; */ /* PS_MODE_MIN; */
+	if (padapter->registrypriv.mp_mode == 1)
+		pwrctrlpriv->power_mgnt = PS_MODE_ACTIVE ;
+	else
+		pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt; /* PS_MODE_MIN; */
+	pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt) ? true : false;
+
+	pwrctrlpriv->bFwCurrentInPSMode = false;
+
+	pwrctrlpriv->rpwm = 0;
+	pwrctrlpriv->cpwm = PS_STATE_S4;
+
+	pwrctrlpriv->pwr_mode = PS_MODE_ACTIVE;
+	pwrctrlpriv->smart_ps = padapter->registrypriv.smart_ps;
+	pwrctrlpriv->bcn_ant_mode = 0;
+	pwrctrlpriv->dtim = 0;
+
+	pwrctrlpriv->tog = 0x80;
+
+#ifdef CONFIG_LPS_LCLK
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&pwrctrlpriv->rpwm));
+
+	_init_workitem(&pwrctrlpriv->cpwm_event, cpwm_event_callback, NULL);
+
+	_init_workitem(&pwrctrlpriv->dma_event, dma_event_callback, NULL);
+
+#ifdef CONFIG_LPS_RPWM_TIMER
+	pwrctrlpriv->brpwmtimeout = false;
+	_init_workitem(&pwrctrlpriv->rpwmtimeoutwi, rpwmtimeout_workitem_callback, NULL);
+	_init_timer(&pwrctrlpriv->pwr_rpwm_timer, padapter->pnetdev, pwr_rpwm_timeout_handler, padapter);
+#endif /* CONFIG_LPS_RPWM_TIMER */
+#endif /* CONFIG_LPS_LCLK */
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_init_timer(&pwrctrlpriv->pwr_state_check_timer, padapter->pnetdev, pwr_state_check_handler, padapter);
+#else
+	timer_setup(&pwrctrlpriv->pwr_state_check_timer, pwr_state_check_handler, 0);
+#endif
+
+	pwrctrlpriv->wowlan_mode = false;
+	pwrctrlpriv->wowlan_ap_mode = false;
+	pwrctrlpriv->wowlan_p2p_mode = false;
+	pwrctrlpriv->wowlan_last_wake_reason = 0;
+
+#ifdef CONFIG_RESUME_IN_WORKQUEUE
+	_init_workitem(&pwrctrlpriv->resume_work, resume_workitem_callback, NULL);
+	pwrctrlpriv->rtw_workqueue = create_singlethread_workqueue("rtw_workqueue");
+#endif /* CONFIG_RESUME_IN_WORKQUEUE */
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_ANDROID_POWER)
+	pwrctrlpriv->early_suspend.suspend = NULL;
+	rtw_register_early_suspend(pwrctrlpriv);
+#endif /* CONFIG_HAS_EARLYSUSPEND || CONFIG_ANDROID_POWER */
+
+#ifdef CONFIG_GPIO_WAKEUP
+	/*default low active*/
+	pwrctrlpriv->is_high_active = HIGH_ACTIVE;
+	val8 = (pwrctrlpriv->is_high_active == 0) ? 1 : 0;
+	rtw_hal_switch_gpio_wl_ctrl(padapter, WAKEUP_GPIO_IDX, true);
+	rtw_hal_set_output_gpio(padapter, WAKEUP_GPIO_IDX, val8);
+	RTW_INFO("%s: set GPIO_%d %d as default.\n",
+		 __func__, WAKEUP_GPIO_IDX, val8);
+#endif /* CONFIG_GPIO_WAKEUP */
+
+#ifdef CONFIG_WOWLAN
+	rtw_wow_pattern_sw_reset(padapter);
+	pwrctrlpriv->wowlan_in_resume = false;
+#ifdef CONFIG_PNO_SUPPORT
+	pwrctrlpriv->pno_inited = false;
+	pwrctrlpriv->pnlo_info = NULL;
+	pwrctrlpriv->pscan_info = NULL;
+	pwrctrlpriv->pno_ssid_list = NULL;
+#endif /* CONFIG_PNO_SUPPORT */
+#ifdef CONFIG_WOW_PATTERN_HW_CAM
+	_rtw_mutex_init(&pwrctrlpriv->wowlan_pattern_cam_mutex);
+#endif
+	pwrctrlpriv->wowlan_aoac_rpt_loc = 0;
+#endif /* CONFIG_WOWLAN */
+
+#ifdef CONFIG_LPS_POFF
+	rtw_hal_set_hwreg(padapter, HW_VAR_LPS_POFF_INIT, 0);
+#endif
+
+
+}
+
+void rtw_free_pwrctrl_priv(PADAPTER adapter)
+{
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(adapter);
+
+#if defined(CONFIG_CONCURRENT_MODE)
+	if (adapter->adapter_type != PRIMARY_ADAPTER)
+		return;
+#endif
+
+
+	/* memset((unsigned char *)pwrctrlpriv, 0, sizeof(struct pwrctrl_priv)); */
+
+
+#ifdef CONFIG_RESUME_IN_WORKQUEUE
+	if (pwrctrlpriv->rtw_workqueue) {
+		flush_workqueue(pwrctrlpriv->rtw_workqueue);
+		destroy_workqueue(pwrctrlpriv->rtw_workqueue);
+	}
+#endif
+
+#ifdef CONFIG_LPS_POFF
+	rtw_hal_set_hwreg(adapter, HW_VAR_LPS_POFF_DEINIT, 0);
+#endif
+
+#ifdef CONFIG_WOWLAN
+#ifdef CONFIG_PNO_SUPPORT
+	if (pwrctrlpriv->pnlo_info != NULL)
+		RTW_INFO("****** pnlo_info memory leak********\n");
+
+	if (pwrctrlpriv->pscan_info != NULL)
+		RTW_INFO("****** pscan_info memory leak********\n");
+
+	if (pwrctrlpriv->pno_ssid_list != NULL)
+		RTW_INFO("****** pno_ssid_list memory leak********\n");
+#endif
+#ifdef CONFIG_WOW_PATTERN_HW_CAM
+	_rtw_mutex_free(&pwrctrlpriv->wowlan_pattern_cam_mutex);
+#endif
+
+#endif /* CONFIG_WOWLAN */
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_ANDROID_POWER)
+	rtw_unregister_early_suspend(pwrctrlpriv);
+#endif /* CONFIG_HAS_EARLYSUSPEND || CONFIG_ANDROID_POWER */
+
+	_free_pwrlock(&pwrctrlpriv->lock);
+	_free_pwrlock(&pwrctrlpriv->check_32k_lock);
+
+}
+
+#ifdef CONFIG_RESUME_IN_WORKQUEUE
+extern int rtw_resume_process(_adapter *padapter);
+
+static void resume_workitem_callback(struct work_struct *work)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(work, struct pwrctrl_priv, resume_work);
+	struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv);
+	_adapter *adapter = dvobj_get_primary_adapter(dvobj);
+
+	RTW_INFO("%s\n", __func__);
+
+	rtw_resume_process(adapter);
+
+	rtw_resume_unlock_suspend();
+}
+
+void rtw_resume_in_workqueue(struct pwrctrl_priv *pwrpriv)
+{
+	/* accquire system's suspend lock preventing from falliing asleep while resume in workqueue */
+	/* rtw_lock_suspend(); */
+
+	rtw_resume_lock_suspend();
+
+	queue_work(pwrpriv->rtw_workqueue, &pwrpriv->resume_work);
+}
+#endif /* CONFIG_RESUME_IN_WORKQUEUE */
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_ANDROID_POWER)
+inline bool rtw_is_earlysuspend_registered(struct pwrctrl_priv *pwrpriv)
+{
+	return (pwrpriv->early_suspend.suspend) ? true : false;
+}
+
+inline bool rtw_is_do_late_resume(struct pwrctrl_priv *pwrpriv)
+{
+	return (pwrpriv->do_late_resume) ? true : false;
+}
+
+inline void rtw_set_do_late_resume(struct pwrctrl_priv *pwrpriv, bool enable)
+{
+	pwrpriv->do_late_resume = enable;
+}
+#endif
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+extern int rtw_resume_process(_adapter *padapter);
+static void rtw_early_suspend(struct early_suspend *h)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend);
+	RTW_INFO("%s\n", __func__);
+
+	rtw_set_do_late_resume(pwrpriv, false);
+}
+
+static void rtw_late_resume(struct early_suspend *h)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend);
+	struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv);
+	_adapter *adapter = dvobj_get_primary_adapter(dvobj);
+
+	RTW_INFO("%s\n", __func__);
+
+	if (pwrpriv->do_late_resume) {
+		rtw_set_do_late_resume(pwrpriv, false);
+		rtw_resume_process(adapter);
+	}
+}
+
+void rtw_register_early_suspend(struct pwrctrl_priv *pwrpriv)
+{
+	RTW_INFO("%s\n", __func__);
+
+	/* jeff: set the early suspend level before blank screen, so we wll do late resume after scree is lit */
+	pwrpriv->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN - 20;
+	pwrpriv->early_suspend.suspend = rtw_early_suspend;
+	pwrpriv->early_suspend.resume = rtw_late_resume;
+	register_early_suspend(&pwrpriv->early_suspend);
+
+
+}
+
+void rtw_unregister_early_suspend(struct pwrctrl_priv *pwrpriv)
+{
+	RTW_INFO("%s\n", __func__);
+
+	rtw_set_do_late_resume(pwrpriv, false);
+
+	if (pwrpriv->early_suspend.suspend)
+		unregister_early_suspend(&pwrpriv->early_suspend);
+
+	pwrpriv->early_suspend.suspend = NULL;
+	pwrpriv->early_suspend.resume = NULL;
+}
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+
+#ifdef CONFIG_ANDROID_POWER
+	extern int rtw_resume_process(PADAPTER padapter);
+static void rtw_early_suspend(android_early_suspend_t *h)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend);
+	RTW_INFO("%s\n", __func__);
+
+	rtw_set_do_late_resume(pwrpriv, false);
+}
+
+static void rtw_late_resume(android_early_suspend_t *h)
+{
+	struct pwrctrl_priv *pwrpriv = container_of(h, struct pwrctrl_priv, early_suspend);
+	struct dvobj_priv *dvobj = pwrctl_to_dvobj(pwrpriv);
+	_adapter *adapter = dvobj_get_primary_adapter(dvobj);
+
+	RTW_INFO("%s\n", __func__);
+	if (pwrpriv->do_late_resume) {
+		rtw_set_do_late_resume(pwrpriv, false);
+		rtw_resume_process(adapter);
+	}
+}
+
+void rtw_register_early_suspend(struct pwrctrl_priv *pwrpriv)
+{
+	RTW_INFO("%s\n", __func__);
+
+	/* jeff: set the early suspend level before blank screen, so we wll do late resume after scree is lit */
+	pwrpriv->early_suspend.level = ANDROID_EARLY_SUSPEND_LEVEL_BLANK_SCREEN - 20;
+	pwrpriv->early_suspend.suspend = rtw_early_suspend;
+	pwrpriv->early_suspend.resume = rtw_late_resume;
+	android_register_early_suspend(&pwrpriv->early_suspend);
+}
+
+void rtw_unregister_early_suspend(struct pwrctrl_priv *pwrpriv)
+{
+	RTW_INFO("%s\n", __func__);
+
+	rtw_set_do_late_resume(pwrpriv, false);
+
+	if (pwrpriv->early_suspend.suspend)
+		android_unregister_early_suspend(&pwrpriv->early_suspend);
+
+	pwrpriv->early_suspend.suspend = NULL;
+	pwrpriv->early_suspend.resume = NULL;
+}
+#endif /* CONFIG_ANDROID_POWER */
+
+u8 rtw_interface_ps_func(_adapter *padapter, HAL_INTF_PS_FUNC efunc_id, u8 *val)
+{
+	u8 bResult = true;
+	rtw_hal_intf_ps_func(padapter, efunc_id, val);
+
+	return bResult;
+}
+
+
+inline void rtw_set_ips_deny(_adapter *padapter, u32 ms)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	pwrpriv->ips_deny_time = jiffies + rtw_ms_to_systime(ms);
+}
+
+/*
+* rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend
+* @adapter: pointer to _adapter structure
+* @ips_deffer_ms: the ms wiil prevent from falling into IPS after wakeup
+* Return _SUCCESS or _FAIL
+*/
+
+int _rtw_pwr_wakeup(_adapter *padapter, u32 ips_deffer_ms, const char *caller)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj);
+	struct mlme_priv *pmlmepriv;
+	int ret = _SUCCESS;
+	int i;
+	u32 start = jiffies;
+
+	/* for LPS */
+	LeaveAllPowerSaveMode(padapter);
+
+	/* IPS still bound with primary adapter */
+	padapter = GET_PRIMARY_ADAPTER(padapter);
+	pmlmepriv = &padapter->mlmepriv;
+
+	if (pwrpriv->ips_deny_time < jiffies + rtw_ms_to_systime(ips_deffer_ms))
+		pwrpriv->ips_deny_time = jiffies + rtw_ms_to_systime(ips_deffer_ms);
+
+
+	if (pwrpriv->ps_processing) {
+		RTW_INFO("%s wait ps_processing...\n", __func__);
+		while (pwrpriv->ps_processing && rtw_get_passing_time_ms(start) <= 3000)
+			rtw_msleep_os(10);
+		if (pwrpriv->ps_processing)
+			RTW_INFO("%s wait ps_processing timeout\n", __func__);
+		else
+			RTW_INFO("%s wait ps_processing done\n", __func__);
+	}
+
+#ifdef DBG_CONFIG_ERROR_DETECT
+	if (rtw_hal_sreset_inprogress(padapter)) {
+		RTW_INFO("%s wait sreset_inprogress...\n", __func__);
+		while (rtw_hal_sreset_inprogress(padapter) && rtw_get_passing_time_ms(start) <= 4000)
+			rtw_msleep_os(10);
+		if (rtw_hal_sreset_inprogress(padapter))
+			RTW_INFO("%s wait sreset_inprogress timeout\n", __func__);
+		else
+			RTW_INFO("%s wait sreset_inprogress done\n", __func__);
+	}
+#endif
+
+	if (pwrpriv->bInternalAutoSuspend == false && pwrpriv->bInSuspend) {
+		RTW_INFO("%s wait bInSuspend...\n", __func__);
+		while (pwrpriv->bInSuspend
+		       && ((rtw_get_passing_time_ms(start) <= 3000 && !rtw_is_do_late_resume(pwrpriv))
+			|| (rtw_get_passing_time_ms(start) <= 500 && rtw_is_do_late_resume(pwrpriv)))
+		      )
+			rtw_msleep_os(10);
+		if (pwrpriv->bInSuspend)
+			RTW_INFO("%s wait bInSuspend timeout\n", __func__);
+		else
+			RTW_INFO("%s wait bInSuspend done\n", __func__);
+	}
+
+	/* System suspend is not allowed to wakeup */
+	if ((pwrpriv->bInternalAutoSuspend == false) && (pwrpriv->bInSuspend)) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* block??? */
+	if ((pwrpriv->bInternalAutoSuspend == true)  && (padapter->net_closed == true)) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* I think this should be check in IPS, LPS, autosuspend functions... */
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+#if defined(CONFIG_BT_COEXIST) && defined (CONFIG_AUTOSUSPEND)
+		if (pwrpriv->bInternalAutoSuspend) {
+			if (0 == pwrpriv->autopm_cnt) {
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 33))
+				if (usb_autopm_get_interface(adapter_to_dvobj(padapter)->pusbintf) < 0)
+					RTW_INFO("can't get autopm:\n");
+#elif (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 20))
+				usb_autopm_disable(adapter_to_dvobj(padapter)->pusbintf);
+#else
+				usb_autoresume_device(adapter_to_dvobj(padapter)->pusbdev, 1);
+#endif
+				pwrpriv->autopm_cnt++;
+			}
+#endif	/* #if defined (CONFIG_BT_COEXIST) && defined (CONFIG_AUTOSUSPEND) */
+			ret = _SUCCESS;
+			goto exit;
+#if defined(CONFIG_BT_COEXIST) && defined (CONFIG_AUTOSUSPEND)
+		}
+#endif	/* #if defined (CONFIG_BT_COEXIST) && defined (CONFIG_AUTOSUSPEND) */
+	}
+
+	if (rf_off == pwrpriv->rf_pwrstate) {
+#ifdef CONFIG_AUTOSUSPEND
+		if (pwrpriv->brfoffbyhw == true) {
+			RTW_INFO("hw still in rf_off state ...........\n");
+			ret = _FAIL;
+			goto exit;
+		} else if (padapter->registrypriv.usbss_enable) {
+			RTW_INFO("%s call autoresume_enter....\n", __func__);
+			if (_FAIL ==  autoresume_enter(padapter)) {
+				RTW_INFO("======> autoresume fail.............\n");
+				ret = _FAIL;
+				goto exit;
+			}
+		} else
+#endif
+		{
+#ifdef CONFIG_IPS
+			RTW_INFO("%s call ips_leave....\n", __func__);
+			if (_FAIL ==  ips_leave(padapter)) {
+				RTW_INFO("======> ips_leave fail.............\n");
+				ret = _FAIL;
+				goto exit;
+			}
+#endif
+		}
+	}
+
+	/* TODO: the following checking need to be merged... */
+	if (rtw_is_drv_stopped(padapter)
+	    || !padapter->bup
+	    || !rtw_is_hw_init_completed(padapter)
+	   ) {
+		RTW_INFO("%s: bDriverStopped=%s, bup=%d, hw_init_completed=%u\n"
+			 , caller
+			 , rtw_is_drv_stopped(padapter) ? "True" : "False"
+			 , padapter->bup
+			 , rtw_get_hw_init_completed(padapter));
+		ret = false;
+		goto exit;
+	}
+
+exit:
+	if (pwrpriv->ips_deny_time < jiffies + rtw_ms_to_systime(ips_deffer_ms))
+		pwrpriv->ips_deny_time = jiffies + rtw_ms_to_systime(ips_deffer_ms);
+	return ret;
+
+}
+
+int rtw_pm_set_lps(_adapter *padapter, u8 mode)
+{
+	int	ret = 0;
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	if (mode < PS_MODE_NUM) {
+		if (pwrctrlpriv->power_mgnt != mode) {
+			if (PS_MODE_ACTIVE == mode)
+				LeaveAllPowerSaveMode(padapter);
+			else
+				pwrctrlpriv->LpsIdleCount = 2;
+			pwrctrlpriv->power_mgnt = mode;
+			pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt) ? true : false;
+		}
+	} else
+		ret = -EINVAL;
+
+	return ret;
+}
+
+int rtw_pm_set_ips(_adapter *padapter, u8 mode)
+{
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	if (mode == IPS_NORMAL || mode == IPS_LEVEL_2) {
+		rtw_ips_mode_req(pwrctrlpriv, mode);
+		RTW_INFO("%s %s\n", __func__, mode == IPS_NORMAL ? "IPS_NORMAL" : "IPS_LEVEL_2");
+		return 0;
+	} else if (mode == IPS_NONE) {
+		rtw_ips_mode_req(pwrctrlpriv, mode);
+		RTW_INFO("%s %s\n", __func__, "IPS_NONE");
+		if (!rtw_is_surprise_removed(padapter) && (_FAIL == rtw_pwr_wakeup(padapter)))
+			return -EFAULT;
+	} else
+		return -EINVAL;
+	return 0;
+}
+
+/*
+ * ATTENTION:
+ *	This function will request pwrctrl LOCK!
+ */
+void rtw_ps_deny(PADAPTER padapter, PS_DENY_REASON reason)
+{
+	struct pwrctrl_priv *pwrpriv;
+	s32 ret;
+
+
+	/* 	RTW_INFO("+" FUNC_ADPT_FMT ": Request PS deny for %d (0x%08X)\n",
+	 *		FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	_enter_pwrlock(&pwrpriv->lock);
+	if (pwrpriv->ps_deny & BIT(reason)) {
+		RTW_INFO(FUNC_ADPT_FMT ": [WARNING] Reason %d had been set before!!\n",
+			 FUNC_ADPT_ARG(padapter), reason);
+	}
+	pwrpriv->ps_deny |= BIT(reason);
+	_exit_pwrlock(&pwrpriv->lock);
+
+	/* 	RTW_INFO("-" FUNC_ADPT_FMT ": Now PS deny for 0x%08X\n",
+	 *		FUNC_ADPT_ARG(padapter), pwrpriv->ps_deny); */
+}
+
+/*
+ * ATTENTION:
+ *	This function will request pwrctrl LOCK!
+ */
+void rtw_ps_deny_cancel(PADAPTER padapter, PS_DENY_REASON reason)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+
+	/* 	RTW_INFO("+" FUNC_ADPT_FMT ": Cancel PS deny for %d(0x%08X)\n",
+	 *		FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	_enter_pwrlock(&pwrpriv->lock);
+	if ((pwrpriv->ps_deny & BIT(reason)) == 0) {
+		RTW_INFO(FUNC_ADPT_FMT ": [ERROR] Reason %d had been canceled before!!\n",
+			 FUNC_ADPT_ARG(padapter), reason);
+	}
+	pwrpriv->ps_deny &= ~BIT(reason);
+	_exit_pwrlock(&pwrpriv->lock);
+
+	/* 	RTW_INFO("-" FUNC_ADPT_FMT ": Now PS deny for 0x%08X\n",
+	 *		FUNC_ADPT_ARG(padapter), pwrpriv->ps_deny); */
+}
+
+/*
+ * ATTENTION:
+ *	Before calling this function pwrctrl lock should be occupied already,
+ *	otherwise it may return incorrect value.
+ */
+u32 rtw_ps_deny_get(PADAPTER padapter)
+{
+	u32 deny;
+
+
+	deny = adapter_to_pwrctl(padapter)->ps_deny;
+
+	return deny;
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_recv.c b/drivers/staging/rtl8188eu/core/rtw_recv.c
new file mode 100755
index 000000000000..31507391476a
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_recv.c
@@ -0,0 +1,4191 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_RECV_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS);
+#else
+void rtw_signal_stat_timer_hdl(struct timer_list *t);
+#endif
+
+enum {
+	SIGNAL_STAT_CALC_PROFILE_0 = 0,
+	SIGNAL_STAT_CALC_PROFILE_1,
+	SIGNAL_STAT_CALC_PROFILE_MAX
+};
+
+static u8 signal_stat_calc_profile[SIGNAL_STAT_CALC_PROFILE_MAX][2] = {
+	{4, 1},	/* Profile 0 => pre_stat : curr_stat = 4 : 1 */
+	{3, 7}	/* Profile 1 => pre_stat : curr_stat = 3 : 7 */
+};
+
+#ifndef RTW_SIGNAL_STATE_CALC_PROFILE
+	#define RTW_SIGNAL_STATE_CALC_PROFILE SIGNAL_STAT_CALC_PROFILE_1
+#endif
+
+#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */
+
+void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv)
+{
+
+
+
+	memset((u8 *)psta_recvpriv, 0, sizeof(struct sta_recv_priv));
+
+	spin_lock_init(&psta_recvpriv->lock);
+
+	/* for(i=0; i<MAX_RX_NUMBLKS; i++) */
+	/*	_rtw_init_queue(&psta_recvpriv->blk_strms[i]); */
+
+	_rtw_init_queue(&psta_recvpriv->defrag_q);
+
+
+}
+
+sint _rtw_init_recv_priv(struct recv_priv *precvpriv, _adapter *padapter)
+{
+	sint i;
+
+	union recv_frame *precvframe;
+	sint	res = _SUCCESS;
+
+
+	/* We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */
+	/* memset((unsigned char *)precvpriv, 0, sizeof (struct  recv_priv)); */
+
+	spin_lock_init(&precvpriv->lock);
+
+#ifdef CONFIG_RECV_THREAD_MODE
+	sema_init(&precvpriv->recv_sema, 0);
+	sema_init(&precvpriv->terminate_recvthread_sema, 0);
+#endif
+
+	_rtw_init_queue(&precvpriv->free_recv_queue);
+	_rtw_init_queue(&precvpriv->recv_pending_queue);
+	_rtw_init_queue(&precvpriv->uc_swdec_pending_queue);
+
+	precvpriv->adapter = padapter;
+
+	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
+
+	precvpriv->sink_udpport = 0;
+	precvpriv->pre_rtp_rxseq = 0;
+	precvpriv->cur_rtp_rxseq = 0;
+
+#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+	precvpriv->store_law_data_flag = 1;
+#else
+	precvpriv->store_law_data_flag = 0;
+#endif
+
+	rtw_os_recv_resource_init(precvpriv, padapter);
+
+	precvpriv->pallocated_frame_buf = rtw_zvmalloc(NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ);
+
+	if (precvpriv->pallocated_frame_buf == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	/* memset(precvpriv->pallocated_frame_buf, 0, NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ); */
+
+	precvpriv->precv_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(precvpriv->pallocated_frame_buf), RXFRAME_ALIGN_SZ);
+	/* precvpriv->precv_frame_buf = precvpriv->pallocated_frame_buf + RXFRAME_ALIGN_SZ - */
+	/*						((SIZE_PTR) (precvpriv->pallocated_frame_buf) &(RXFRAME_ALIGN_SZ-1)); */
+
+	precvframe = (union recv_frame *) precvpriv->precv_frame_buf;
+
+
+	for (i = 0; i < NR_RECVFRAME ; i++) {
+		INIT_LIST_HEAD(&(precvframe->u.list));
+
+		list_add_tail(&(precvframe->u.list), &(precvpriv->free_recv_queue.queue));
+
+		res = rtw_os_recv_resource_alloc(padapter, precvframe);
+
+		precvframe->u.hdr.len = 0;
+
+		precvframe->u.hdr.adapter = padapter;
+		precvframe++;
+
+	}
+
+       ATOMIC_SET(&(precvpriv->rx_pending_cnt), 1);
+
+       sema_init(&precvpriv->allrxreturnevt, 0);
+
+	res = rtw_hal_init_recv_priv(padapter);
+
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_init_timer(&precvpriv->signal_stat_timer, padapter->pnetdev, RTW_TIMER_HDL_NAME(signal_stat), padapter);
+#else
+	timer_setup(&precvpriv->signal_stat_timer, rtw_signal_stat_timer_hdl, 0);
+#endif
+
+	precvpriv->signal_stat_sampling_interval = 2000; /* ms */
+	/* precvpriv->signal_stat_converging_constant = 5000; */ /* ms */
+
+	rtw_set_signal_stat_timer(precvpriv);
+#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */
+
+exit:
+
+
+	return res;
+
+}
+
+static void rtw_mfree_recv_priv_lock(struct recv_priv *precvpriv)
+{
+}
+
+void _rtw_free_recv_priv(struct recv_priv *precvpriv)
+{
+	_adapter	*padapter = precvpriv->adapter;
+
+
+	rtw_free_uc_swdec_pending_queue(padapter);
+
+	rtw_mfree_recv_priv_lock(precvpriv);
+
+	rtw_os_recv_resource_free(precvpriv);
+
+	if (precvpriv->pallocated_frame_buf)
+		rtw_vmfree(precvpriv->pallocated_frame_buf, NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ);
+
+	rtw_hal_free_recv_priv(padapter);
+
+
+}
+
+bool rtw_rframe_del_wfd_ie(union recv_frame *rframe, u8 ies_offset)
+{
+#define DBG_RFRAME_DEL_WFD_IE 0
+	u8 *ies = rframe->u.hdr.rx_data + sizeof(struct rtw_ieee80211_hdr_3addr) + ies_offset;
+	uint ies_len_ori = rframe->u.hdr.len - (ies - rframe->u.hdr.rx_data);
+	uint ies_len;
+
+	ies_len = rtw_del_wfd_ie(ies, ies_len_ori, DBG_RFRAME_DEL_WFD_IE ? __func__ : NULL);
+	rframe->u.hdr.len -= ies_len_ori - ies_len;
+
+	return ies_len_ori != ies_len;
+}
+
+union recv_frame *_rtw_alloc_recvframe(_queue *pfree_recv_queue)
+{
+
+	union recv_frame  *precvframe;
+	_list	*plist, *phead;
+	_adapter *padapter;
+	struct recv_priv *precvpriv;
+
+	if (_rtw_queue_empty(pfree_recv_queue))
+		precvframe = NULL;
+	else {
+		phead = get_list_head(pfree_recv_queue);
+
+		plist = get_next(phead);
+
+		precvframe = LIST_CONTAINOR(plist, union recv_frame, u);
+
+		list_del_init(&precvframe->u.hdr.list);
+		padapter = precvframe->u.hdr.adapter;
+		if (padapter != NULL) {
+			precvpriv = &padapter->recvpriv;
+			if (pfree_recv_queue == &precvpriv->free_recv_queue)
+				precvpriv->free_recvframe_cnt--;
+		}
+	}
+
+
+	return precvframe;
+
+}
+
+union recv_frame *rtw_alloc_recvframe(_queue *pfree_recv_queue)
+{
+	unsigned long irqL;
+	union recv_frame  *precvframe;
+
+	_enter_critical_bh(&pfree_recv_queue->lock, &irqL);
+
+	precvframe = _rtw_alloc_recvframe(pfree_recv_queue);
+
+	_exit_critical_bh(&pfree_recv_queue->lock, &irqL);
+
+	return precvframe;
+}
+
+void rtw_init_recvframe(union recv_frame *precvframe, struct recv_priv *precvpriv)
+{
+	/* Perry: This can be removed */
+	INIT_LIST_HEAD(&precvframe->u.hdr.list);
+
+	precvframe->u.hdr.len = 0;
+}
+
+int rtw_free_recvframe(union recv_frame *precvframe, _queue *pfree_recv_queue)
+{
+	unsigned long irqL;
+	_adapter *padapter = precvframe->u.hdr.adapter;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+
+#ifdef CONFIG_CONCURRENT_MODE
+	padapter = GET_PRIMARY_ADAPTER(padapter);
+	precvpriv = &padapter->recvpriv;
+	pfree_recv_queue = &precvpriv->free_recv_queue;
+	precvframe->u.hdr.adapter = padapter;
+#endif
+
+
+	rtw_os_free_recvframe(precvframe);
+
+
+	_enter_critical_bh(&pfree_recv_queue->lock, &irqL);
+
+	list_del_init(&(precvframe->u.hdr.list));
+
+	precvframe->u.hdr.len = 0;
+
+	list_add_tail(&(precvframe->u.hdr.list), get_list_head(pfree_recv_queue));
+
+	if (padapter != NULL) {
+		if (pfree_recv_queue == &precvpriv->free_recv_queue)
+			precvpriv->free_recvframe_cnt++;
+	}
+
+	_exit_critical_bh(&pfree_recv_queue->lock, &irqL);
+
+
+	return _SUCCESS;
+
+}
+
+
+
+
+sint _rtw_enqueue_recvframe(union recv_frame *precvframe, _queue *queue)
+{
+
+	_adapter *padapter = precvframe->u.hdr.adapter;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+
+	/* INIT_LIST_HEAD(&(precvframe->u.hdr.list)); */
+	list_del_init(&(precvframe->u.hdr.list));
+
+
+	list_add_tail(&(precvframe->u.hdr.list), get_list_head(queue));
+
+	if (padapter != NULL) {
+		if (queue == &precvpriv->free_recv_queue)
+			precvpriv->free_recvframe_cnt++;
+	}
+
+
+	return _SUCCESS;
+}
+
+sint rtw_enqueue_recvframe(union recv_frame *precvframe, _queue *queue)
+{
+	sint ret;
+	unsigned long irqL;
+
+	/* _spinlock(&pfree_recv_queue->lock); */
+	_enter_critical_bh(&queue->lock, &irqL);
+	ret = _rtw_enqueue_recvframe(precvframe, queue);
+	/* spin_unlock(&pfree_recv_queue->lock); */
+	_exit_critical_bh(&queue->lock, &irqL);
+
+	return ret;
+}
+
+/*
+sint	rtw_enqueue_recvframe(union recv_frame *precvframe, _queue *queue)
+{
+	return rtw_free_recvframe(precvframe, queue);
+}
+*/
+
+
+
+
+/*
+caller : defrag ; recvframe_chk_defrag in recv_thread  (passive)
+pframequeue: defrag_queue : will be accessed in recv_thread  (passive)
+
+using spinlock to protect
+
+*/
+
+void rtw_free_recvframe_queue(_queue *pframequeue,  _queue *pfree_recv_queue)
+{
+	union	recv_frame	*precvframe;
+	_list	*plist, *phead;
+
+	spin_lock(&pframequeue->lock);
+
+	phead = get_list_head(pframequeue);
+	plist = get_next(phead);
+
+	while (rtw_end_of_queue_search(phead, plist) == false) {
+		precvframe = LIST_CONTAINOR(plist, union recv_frame, u);
+
+		plist = get_next(plist);
+
+		/* list_del_init(&precvframe->u.hdr.list); */ /* will do this in rtw_free_recvframe() */
+
+		rtw_free_recvframe(precvframe, pfree_recv_queue);
+	}
+
+	spin_unlock(&pframequeue->lock);
+
+
+}
+
+u32 rtw_free_uc_swdec_pending_queue(_adapter *adapter)
+{
+	u32 cnt = 0;
+	union recv_frame *pending_frame;
+	while ((pending_frame = rtw_alloc_recvframe(&adapter->recvpriv.uc_swdec_pending_queue))) {
+		rtw_free_recvframe(pending_frame, &adapter->recvpriv.free_recv_queue);
+		cnt++;
+	}
+
+	if (cnt)
+		RTW_INFO(FUNC_ADPT_FMT" dequeue %d\n", FUNC_ADPT_ARG(adapter), cnt);
+
+	return cnt;
+}
+
+
+sint rtw_enqueue_recvbuf_to_head(struct recv_buf *precvbuf, _queue *queue)
+{
+	unsigned long irqL;
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	list_del_init(&precvbuf->list);
+	list_add(&precvbuf->list, get_list_head(queue));
+
+	_exit_critical_bh(&queue->lock, &irqL);
+	return _SUCCESS;
+}
+
+sint rtw_enqueue_recvbuf(struct recv_buf *precvbuf, _queue *queue)
+{
+	unsigned long irqL;
+
+	_enter_critical_ex(&queue->lock, &irqL);
+
+	list_del_init(&precvbuf->list);
+
+	list_add_tail(&precvbuf->list, get_list_head(queue));
+	_exit_critical_ex(&queue->lock, &irqL);
+	return _SUCCESS;
+}
+
+struct recv_buf *rtw_dequeue_recvbuf(_queue *queue)
+{
+	unsigned long irqL;
+	struct recv_buf *precvbuf;
+	_list	*plist, *phead;
+
+	_enter_critical_ex(&queue->lock, &irqL);
+
+	if (_rtw_queue_empty(queue))
+		precvbuf = NULL;
+	else {
+		phead = get_list_head(queue);
+
+		plist = get_next(phead);
+
+		precvbuf = LIST_CONTAINOR(plist, struct recv_buf, list);
+
+		list_del_init(&precvbuf->list);
+	}
+
+	_exit_critical_ex(&queue->lock, &irqL);
+
+	return precvbuf;
+}
+
+sint recvframe_chkmic(_adapter *adapter,  union recv_frame *precvframe);
+sint recvframe_chkmic(_adapter *adapter,  union recv_frame *precvframe)
+{
+
+	sint	i, res = _SUCCESS;
+	u32	datalen;
+	u8	miccode[8];
+	u8	bmic_err = false, brpt_micerror = true;
+	u8	*pframe, *payload, *pframemic;
+	u8	*mickey;
+	/* u8	*iv,rxdata_key_idx=0; */
+	struct	sta_info		*stainfo;
+	struct	rx_pkt_attrib	*prxattrib = &precvframe->u.hdr.attrib;
+	struct	security_priv	*psecuritypriv = &adapter->securitypriv;
+
+	struct mlme_ext_priv	*pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	stainfo = rtw_get_stainfo(&adapter->stapriv , &prxattrib->ta[0]);
+
+	if (prxattrib->encrypt == _TKIP_) {
+
+		/* calculate mic code */
+		if (stainfo != NULL) {
+			if (IS_MCAST(prxattrib->ra)) {
+				/* mickey=&psecuritypriv->dot118021XGrprxmickey.skey[0]; */
+				/* iv = precvframe->u.hdr.rx_data+prxattrib->hdrlen; */
+				/* rxdata_key_idx =( ((iv[3])>>6)&0x3) ; */
+				mickey = &psecuritypriv->dot118021XGrprxmickey[prxattrib->key_index].skey[0];
+
+				/* RTW_INFO("\n recvframe_chkmic: bcmc key psecuritypriv->dot118021XGrpKeyid(%d),pmlmeinfo->key_index(%d) ,recv key_id(%d)\n", */
+				/*								psecuritypriv->dot118021XGrpKeyid,pmlmeinfo->key_index,rxdata_key_idx); */
+
+				if (psecuritypriv->binstallGrpkey == false) {
+					res = _FAIL;
+					RTW_INFO("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n");
+					goto exit;
+				}
+			} else {
+				mickey = &stainfo->dot11tkiprxmickey.skey[0];
+			}
+
+			datalen = precvframe->u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len - prxattrib->icv_len - 8; /* icv_len included the mic code */
+			pframe = precvframe->u.hdr.rx_data;
+			payload = pframe + prxattrib->hdrlen + prxattrib->iv_len;
+
+
+			/* rtw_seccalctkipmic(&stainfo->dot11tkiprxmickey.skey[0],pframe,payload, datalen ,&miccode[0],(unsigned char)prxattrib->priority); */ /* care the length of the data */
+
+			rtw_seccalctkipmic(mickey, pframe, payload, datalen , &miccode[0], (unsigned char)prxattrib->priority); /* care the length of the data */
+
+			pframemic = payload + datalen;
+
+			bmic_err = false;
+
+			for (i = 0; i < 8; i++) {
+				if (miccode[i] != *(pframemic + i)) {
+					bmic_err = true;
+				}
+			}
+
+
+			if (bmic_err) {
+
+
+
+				/* double check key_index for some timing issue , */
+				/* cannot compare with psecuritypriv->dot118021XGrpKeyid also cause timing issue */
+				if ((IS_MCAST(prxattrib->ra))  && (prxattrib->key_index != pmlmeinfo->key_index))
+					brpt_micerror = false;
+
+				if ((prxattrib->bdecrypted) && (brpt_micerror == true)) {
+					rtw_handle_tkip_mic_err(adapter, stainfo, (u8)IS_MCAST(prxattrib->ra));
+					RTW_INFO(" mic error :prxattrib->bdecrypted=%d\n", prxattrib->bdecrypted);
+				} else {
+					RTW_INFO(" mic error :prxattrib->bdecrypted=%d\n", prxattrib->bdecrypted);
+				}
+
+				res = _FAIL;
+
+			} else {
+				/* mic checked ok */
+				if ((psecuritypriv->bcheck_grpkey == false) && (IS_MCAST(prxattrib->ra))) {
+					psecuritypriv->bcheck_grpkey = true;
+				}
+			}
+
+		}
+
+		recvframe_pull_tail(precvframe, 8);
+
+	}
+
+exit:
+
+
+	return res;
+
+}
+
+/*#define DBG_RX_SW_DECRYPTOR*/
+
+/* decrypt and set the ivlen,icvlen of the recv_frame */
+union recv_frame *decryptor(_adapter *padapter, union recv_frame *precv_frame);
+union recv_frame *decryptor(_adapter *padapter, union recv_frame *precv_frame)
+{
+
+	struct rx_pkt_attrib *prxattrib = &precv_frame->u.hdr.attrib;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	union recv_frame *return_packet = precv_frame;
+	u32	 res = _SUCCESS;
+
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt);
+
+
+	if (prxattrib->encrypt > 0) {
+		u8 *iv = precv_frame->u.hdr.rx_data + prxattrib->hdrlen;
+		prxattrib->key_index = (((iv[3]) >> 6) & 0x3) ;
+
+		if (prxattrib->key_index > WEP_KEYS) {
+			RTW_INFO("prxattrib->key_index(%d) > WEP_KEYS\n", prxattrib->key_index);
+
+			switch (prxattrib->encrypt) {
+			case _WEP40_:
+			case _WEP104_:
+				prxattrib->key_index = psecuritypriv->dot11PrivacyKeyIndex;
+				break;
+			case _TKIP_:
+			case _AES_:
+			default:
+				prxattrib->key_index = psecuritypriv->dot118021XGrpKeyid;
+				break;
+			}
+		}
+	}
+
+	if ((prxattrib->encrypt > 0) && ((prxattrib->bdecrypted == 0) || (psecuritypriv->sw_decrypt))) {
+
+#ifdef CONFIG_CONCURRENT_MODE
+		if (!IS_MCAST(prxattrib->ra)) /* bc/mc packets use sw decryption for concurrent mode */
+#endif
+			psecuritypriv->hw_decrypted = false;
+
+#ifdef DBG_RX_SW_DECRYPTOR
+		RTW_INFO(ADPT_FMT" - sec_type:%s DO SW decryption\n",
+			ADPT_ARG(padapter), security_type_str(prxattrib->encrypt));
+#endif
+
+#ifdef DBG_RX_DECRYPTOR
+		RTW_INFO("[%s] %d:prxstat->bdecrypted:%d,  prxattrib->encrypt:%d,  Setting psecuritypriv->hw_decrypted = %d\n",
+			 __func__,
+			 __LINE__,
+			 prxattrib->bdecrypted,
+			 prxattrib->encrypt,
+			 psecuritypriv->hw_decrypted);
+#endif
+
+		switch (prxattrib->encrypt) {
+		case _WEP40_:
+		case _WEP104_:
+			DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_wep);
+			rtw_wep_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		case _TKIP_:
+			DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_tkip);
+			res = rtw_tkip_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		case _AES_:
+			DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_aes);
+			res = rtw_aes_decrypt(padapter, (u8 *)precv_frame);
+			break;
+#ifdef CONFIG_WAPI_SUPPORT
+		case _SMS4_:
+			DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_wapi);
+			rtw_sms4_decrypt(padapter, (u8 *)precv_frame);
+			break;
+#endif
+		default:
+			break;
+		}
+	} else if (prxattrib->bdecrypted == 1 &&
+		   prxattrib->encrypt > 0 &&
+		   (psecuritypriv->busetkipkey == 1 ||
+		    prxattrib->encrypt != _TKIP_)) {
+			DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_hw);
+
+			psecuritypriv->hw_decrypted = true;
+#ifdef DBG_RX_DECRYPTOR
+			RTW_INFO("[%s] %d:prxstat->bdecrypted:%d,  prxattrib->encrypt:%d,  Setting psecuritypriv->hw_decrypted = %d\n",
+				 __func__,
+				 __LINE__,
+				 prxattrib->bdecrypted,
+				 prxattrib->encrypt,
+				 psecuritypriv->hw_decrypted);
+
+#endif
+	} else {
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_unknown);
+#ifdef DBG_RX_DECRYPTOR
+		RTW_INFO("[%s] %d:prxstat->bdecrypted:%d,  prxattrib->encrypt:%d,  Setting psecuritypriv->hw_decrypted = %d\n",
+			 __func__,
+			 __LINE__,
+			 prxattrib->bdecrypted,
+			 prxattrib->encrypt,
+			 psecuritypriv->hw_decrypted);
+#endif
+	}
+
+	if (res == _FAIL) {
+		rtw_free_recvframe(return_packet, &padapter->recvpriv.free_recv_queue);
+		return_packet = NULL;
+	} else {
+		prxattrib->bdecrypted = true;
+	}
+	return return_packet;
+}
+/* ###set the security information in the recv_frame */
+static union recv_frame *portctrl(_adapter *adapter, union recv_frame *precv_frame)
+{
+	u8 *psta_addr = NULL;
+	u8 *ptr;
+	uint  auth_alg;
+	struct recv_frame_hdr *pfhdr;
+	struct sta_info *psta;
+	struct sta_priv *pstapriv ;
+	union recv_frame *prtnframe;
+	u16	ether_type = 0;
+	u16  eapol_type = 0x888e;/* for Funia BD's WPA issue  */
+	struct rx_pkt_attrib *pattrib;
+	__be16 be_tmp;
+
+	pstapriv = &adapter->stapriv;
+
+	auth_alg = adapter->securitypriv.dot11AuthAlgrthm;
+
+	ptr = get_recvframe_data(precv_frame);
+	pfhdr = &precv_frame->u.hdr;
+	pattrib = &pfhdr->attrib;
+	psta_addr = pattrib->ta;
+
+	prtnframe = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, psta_addr);
+
+
+	if (auth_alg == dot11AuthAlgrthm_8021X) {
+		if ((psta != NULL) && (psta->ieee8021x_blocked)) {
+			/* blocked */
+			/* only accept EAPOL frame */
+
+			prtnframe = precv_frame;
+
+			/* get ether_type */
+			ptr = ptr + pfhdr->attrib.hdrlen + pfhdr->attrib.iv_len + LLC_HEADER_SIZE;
+			memcpy(&be_tmp, ptr, 2);
+			ether_type = ntohs(be_tmp);
+
+			if (ether_type == eapol_type)
+				prtnframe = precv_frame;
+			else {
+				/* free this frame */
+				rtw_free_recvframe(precv_frame, &adapter->recvpriv.free_recv_queue);
+				prtnframe = NULL;
+			}
+		} else {
+			/* allowed */
+			/* check decryption status, and decrypt the frame if needed */
+
+
+			prtnframe = precv_frame;
+			/* check is the EAPOL frame or not (Rekey) */
+			/* if(ether_type == eapol_type){ */
+			/* check Rekey */
+
+			/*	prtnframe=precv_frame; */
+			/* } */
+		}
+	} else
+		prtnframe = precv_frame;
+
+
+	return prtnframe;
+
+}
+
+sint recv_decache(union recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache);
+sint recv_decache(union recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache)
+{
+	sint tid = precv_frame->u.hdr.attrib.priority;
+
+	u16 seq_ctrl = ((le16_to_cpu(precv_frame->u.hdr.attrib.seq_num) & 0xffff) << 4) |
+		       (precv_frame->u.hdr.attrib.frag_num & 0xf);
+
+	if (tid > 15)
+		return _FAIL;
+
+	if (1) { /* if(bretry) */
+		if (seq_ctrl == prxcache->tid_rxseq[tid]) {
+			/* for non-AMPDU case	*/
+			precv_frame->u.hdr.psta->sta_stats.duplicate_cnt++;
+
+			if (precv_frame->u.hdr.psta->sta_stats.duplicate_cnt % 100 == 0)
+				RTW_INFO("%s: seq=%d\n", __func__, precv_frame->u.hdr.attrib.seq_num);
+
+			return _FAIL;
+		}
+	}
+
+	prxcache->tid_rxseq[tid] = seq_ctrl;
+
+	return _SUCCESS;
+}
+
+/* VALID_PN_CHK
+ * Return true when PN is legal, otherwise false.
+ * Legal PN:
+ *	1. If old PN is 0, any PN is legal
+ *	2. PN > old PN
+ */
+#define PN_LESS_CHK(a, b)	(((a-b) & 0x800000000000L) != 0)
+#define VALID_PN_CHK(new, old)	(((old) == 0) || PN_LESS_CHK(old, new))
+#define CCMPH_2_KEYID(ch)	(((ch) & 0x00000000c0000000L) >> 30)
+#define CCMPH_2_PN(ch)	((ch) & 0x000000000000ffffL) \
+				| (((ch) & 0xffffffff00000000L) >> 16)
+sint recv_ucast_pn_decache(union recv_frame *precv_frame, struct stainfo_rxcache *prxcache);
+sint recv_ucast_pn_decache(union recv_frame *precv_frame, struct stainfo_rxcache *prxcache)
+{
+	_adapter *padapter = precv_frame->u.hdr.adapter;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	u8 *pdata = precv_frame->u.hdr.rx_data;
+	u32 data_len = precv_frame->u.hdr.len;
+	sint tid = precv_frame->u.hdr.attrib.priority;
+	u64 tmp_iv_hdr = 0;
+	u64 curr_pn = 0, pkt_pn = 0;
+
+	if (tid > 15)
+		return _FAIL;
+
+	if (pattrib->encrypt == _AES_) {
+		tmp_iv_hdr = le64_to_cpu(*(__le64*)(pdata + pattrib->hdrlen));
+		pkt_pn = CCMPH_2_PN(tmp_iv_hdr);
+	
+		tmp_iv_hdr = le64_to_cpu(*(__le64*)prxcache->iv[tid]);
+		curr_pn = CCMPH_2_PN(tmp_iv_hdr);	
+
+		if (!VALID_PN_CHK(pkt_pn, curr_pn)) {
+			/* return _FAIL; */
+		} else
+			memcpy(prxcache->iv[tid], (pdata + pattrib->hdrlen), sizeof(prxcache->iv[tid]));
+	}
+
+	return _SUCCESS;
+}
+
+static sint recv_bcast_pn_decache(union recv_frame *precv_frame)
+{
+	_adapter *padapter = precv_frame->u.hdr.adapter;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	u8 *pdata = precv_frame->u.hdr.rx_data;
+	u32 data_len = precv_frame->u.hdr.len;
+	u64 tmp_iv_hdr = 0;
+	u64 curr_pn = 0, pkt_pn = 0;
+	u8 key_id;
+
+	if ((pattrib->encrypt == _AES_) &&
+		(check_fwstate(pmlmepriv, WIFI_STATION_STATE))) {		
+
+		tmp_iv_hdr = le64_to_cpu(*(__le64*)(pdata + pattrib->hdrlen));
+		key_id = CCMPH_2_KEYID(tmp_iv_hdr);
+		pkt_pn = CCMPH_2_PN(tmp_iv_hdr);
+	
+		curr_pn = le64_to_cpu(*(__le64*)psecuritypriv->iv_seq[key_id]);
+		curr_pn &= 0x0000ffffffffffffL;
+
+		if (!VALID_PN_CHK(pkt_pn, curr_pn))
+			return _FAIL;
+
+		*(__le64*)psecuritypriv->iv_seq[key_id] = cpu_to_le64(pkt_pn);
+	}
+
+	return _SUCCESS;
+}
+
+void process_pwrbit_data(_adapter *padapter, union recv_frame *precv_frame);
+void process_pwrbit_data(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_AP_MODE
+	unsigned char pwrbit;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, pattrib->src);
+
+	pwrbit = GetPwrMgt(ptr);
+
+	if (psta) {
+		if (pwrbit) {
+			if (!(psta->state & WIFI_SLEEP_STATE)) {
+				/* psta->state |= WIFI_SLEEP_STATE; */
+				/* pstapriv->sta_dz_bitmap |= BIT(psta->aid); */
+
+				stop_sta_xmit(padapter, psta);
+
+				/* RTW_INFO("to sleep, sta_dz_bitmap=%x\n", pstapriv->sta_dz_bitmap); */
+			}
+		} else {
+			if (psta->state & WIFI_SLEEP_STATE) {
+				/* psta->state ^= WIFI_SLEEP_STATE; */
+				/* pstapriv->sta_dz_bitmap &= ~BIT(psta->aid); */
+
+				wakeup_sta_to_xmit(padapter, psta);
+
+				/* RTW_INFO("to wakeup, sta_dz_bitmap=%x\n", pstapriv->sta_dz_bitmap); */
+			}
+		}
+
+	}
+
+#endif
+}
+
+void process_wmmps_data(_adapter *padapter, union recv_frame *precv_frame);
+void process_wmmps_data(_adapter *padapter, union recv_frame *precv_frame)
+{
+#ifdef CONFIG_AP_MODE
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, pattrib->src);
+
+	if (!psta)
+		return;
+
+#ifdef CONFIG_TDLS
+	if (!(psta->tdls_sta_state & TDLS_LINKED_STATE)) {
+#endif /* CONFIG_TDLS */
+
+		if (!psta->qos_option)
+			return;
+
+		if (!(psta->qos_info & 0xf))
+			return;
+
+#ifdef CONFIG_TDLS
+	}
+#endif /* CONFIG_TDLS		 */
+
+	if (psta->state & WIFI_SLEEP_STATE) {
+		u8 wmmps_ac = 0;
+
+		switch (pattrib->priority) {
+		case 1:
+		case 2:
+			wmmps_ac = psta->uapsd_bk & BIT(1);
+			break;
+		case 4:
+		case 5:
+			wmmps_ac = psta->uapsd_vi & BIT(1);
+			break;
+		case 6:
+		case 7:
+			wmmps_ac = psta->uapsd_vo & BIT(1);
+			break;
+		case 0:
+		case 3:
+		default:
+			wmmps_ac = psta->uapsd_be & BIT(1);
+			break;
+		}
+
+		if (wmmps_ac) {
+			if (psta->sleepq_ac_len > 0) {
+				/* process received triggered frame */
+				xmit_delivery_enabled_frames(padapter, psta);
+			} else {
+				/* issue one qos null frame with More data bit = 0 and the EOSP bit set (=1) */
+				issue_qos_nulldata(padapter, psta->hwaddr, (u16)pattrib->priority, 0, 0);
+			}
+		}
+
+	}
+
+
+#endif
+
+}
+
+#ifdef CONFIG_TDLS
+sint OnTDLS(_adapter *adapter, union recv_frame *precv_frame)
+{
+	struct rx_pkt_attrib	*pattrib = &precv_frame->u.hdr.attrib;
+	sint ret = _SUCCESS;
+	u8 *paction = get_recvframe_data(precv_frame);
+	u8 category_field = 1;
+#ifdef CONFIG_WFD
+	u8 WFA_OUI[3] = { 0x50, 0x6f, 0x9a };
+#endif /* CONFIG_WFD */
+	struct tdls_info *ptdlsinfo = &(adapter->tdlsinfo);
+
+	/* point to action field */
+	paction += pattrib->hdrlen
+		   + pattrib->iv_len
+		   + SNAP_SIZE
+		   + ETH_TYPE_LEN
+		   + PAYLOAD_TYPE_LEN
+		   + category_field;
+
+	RTW_INFO("[TDLS] Recv %s from "MAC_FMT" with SeqNum = %d\n", rtw_tdls_action_txt(*paction), MAC_ARG(pattrib->src), GetSequence(get_recvframe_data(precv_frame)));
+
+	if (hal_chk_wl_func(adapter, WL_FUNC_TDLS) == false) {
+		RTW_INFO("Ignore tdls frame since hal doesn't support tdls\n");
+		ret = _FAIL;
+		return ret;
+	}
+
+	if (ptdlsinfo->tdls_enable == false) {
+		RTW_INFO("recv tdls frame, "
+			 "but tdls haven't enabled\n");
+		ret = _FAIL;
+		return ret;
+	}
+
+	switch (*paction) {
+	case TDLS_SETUP_REQUEST:
+		ret = On_TDLS_Setup_Req(adapter, precv_frame);
+		break;
+	case TDLS_SETUP_RESPONSE:
+		ret = On_TDLS_Setup_Rsp(adapter, precv_frame);
+		break;
+	case TDLS_SETUP_CONFIRM:
+		ret = On_TDLS_Setup_Cfm(adapter, precv_frame);
+		break;
+	case TDLS_TEARDOWN:
+		ret = On_TDLS_Teardown(adapter, precv_frame);
+		break;
+	case TDLS_DISCOVERY_REQUEST:
+		ret = On_TDLS_Dis_Req(adapter, precv_frame);
+		break;
+	case TDLS_PEER_TRAFFIC_INDICATION:
+		ret = On_TDLS_Peer_Traffic_Indication(adapter, precv_frame);
+		break;
+	case TDLS_PEER_TRAFFIC_RESPONSE:
+		ret = On_TDLS_Peer_Traffic_Rsp(adapter, precv_frame);
+		break;
+#ifdef CONFIG_TDLS_CH_SW
+	case TDLS_CHANNEL_SWITCH_REQUEST:
+		ret = On_TDLS_Ch_Switch_Req(adapter, precv_frame);
+		break;
+	case TDLS_CHANNEL_SWITCH_RESPONSE:
+		ret = On_TDLS_Ch_Switch_Rsp(adapter, precv_frame);
+		break;
+#endif
+#ifdef CONFIG_WFD
+	/* First byte of WFA OUI */
+	case 0x50:
+		if (!memcmp(WFA_OUI, paction, 3)) {
+			/* Probe request frame */
+			if (*(paction + 3) == 0x04) {
+				/* WFDTDLS: for sigma test, do not setup direct link automatically */
+				ptdlsinfo->dev_discovered = true;
+				RTW_INFO("recv tunneled probe request frame\n");
+				issue_tunneled_probe_rsp(adapter, precv_frame);
+			}
+			/* Probe response frame */
+			if (*(paction + 3) == 0x05) {
+				/* WFDTDLS: for sigma test, do not setup direct link automatically */
+				ptdlsinfo->dev_discovered = true;
+				RTW_INFO("recv tunneled probe response frame\n");
+			}
+		}
+		break;
+#endif /* CONFIG_WFD */
+	default:
+		RTW_INFO("receive TDLS frame %d but not support\n", *paction);
+		ret = _FAIL;
+		break;
+	}
+
+exit:
+	return ret;
+
+}
+#endif /* CONFIG_TDLS */
+
+void count_rx_stats(_adapter *padapter, union recv_frame *prframe, struct sta_info *sta);
+void count_rx_stats(_adapter *padapter, union recv_frame *prframe, struct sta_info *sta)
+{
+	int	sz;
+	struct sta_info		*psta = NULL;
+	struct stainfo_stats	*pstats = NULL;
+	struct rx_pkt_attrib	*pattrib = &prframe->u.hdr.attrib;
+	struct recv_priv		*precvpriv = &padapter->recvpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	sz = get_recvframe_len(prframe);
+	precvpriv->rx_bytes += sz;
+
+	padapter->mlmepriv.LinkDetectInfo.NumRxOkInPeriod++;
+
+	if ((!MacAddr_isBcst(pattrib->dst)) && (!IS_MCAST(pattrib->dst)))
+		padapter->mlmepriv.LinkDetectInfo.NumRxUnicastOkInPeriod++;
+
+	if (sta)
+		psta = sta;
+	else
+		psta = prframe->u.hdr.psta;
+
+	if (psta) {
+		pstats = &psta->sta_stats;
+
+		pstats->rx_data_pkts++;
+		pstats->rx_bytes += sz;
+
+		pstats->rxratecnt[pattrib->data_rate]++;
+		/*record rx packets for every tid*/
+		pstats->rx_data_qos_pkts[pattrib->priority]++;
+
+#ifdef CONFIG_TDLS
+		if (psta->tdls_sta_state & TDLS_LINKED_STATE) {
+			struct sta_info *pap_sta = NULL;
+			pap_sta = rtw_get_stainfo(&padapter->stapriv, get_bssid(&padapter->mlmepriv));
+			if (pap_sta) {
+				pstats = &pap_sta->sta_stats;
+				pstats->rx_data_pkts++;
+				pstats->rx_bytes += sz;
+			}
+		}
+#endif /* CONFIG_TDLS */
+	}
+
+#ifdef CONFIG_CHECK_LEAVE_LPS
+	traffic_check_for_leave_lps(padapter, false, 0);
+#endif /* CONFIG_LPS */
+
+}
+
+sint sta2sta_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta
+);
+sint sta2sta_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta
+)
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	sint ret = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct	sta_priv		*pstapriv = &adapter->stapriv;
+	struct	mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = adapter_mac_addr(adapter);
+	u8 *sta_addr = NULL;
+	sint bmcast = IS_MCAST(pattrib->dst);
+
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &adapter->tdlsinfo;
+#ifdef CONFIG_TDLS_CH_SW
+	struct tdls_ch_switch *pchsw_info = &ptdlsinfo->chsw_info;
+#endif
+	struct sta_info *ptdls_sta = NULL;
+	u8 *psnap_type = ptr + pattrib->hdrlen + pattrib->iv_len + SNAP_SIZE;
+	/* frame body located after [+2]: ether-type, [+1]: payload type */
+	u8 *pframe_body = psnap_type + 2 + 1;
+#endif
+
+
+	/* RTW_INFO("[%s] %d, seqnum:%d\n", __func__, __LINE__, pattrib->seq_num); */
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) ||
+	    (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))) {
+
+		/* filter packets that SA is myself or multicast or broadcast */
+		if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN))	&& (!bmcast)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		    !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		    (memcmp(pattrib->bssid, mybssid, ETH_ALEN))) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		sta_addr = pattrib->src;
+
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+#ifdef CONFIG_TDLS
+
+		/* direct link data transfer */
+		if (ptdlsinfo->link_established) {
+			ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->src);
+			if (ptdls_sta == NULL) {
+				ret = _FAIL;
+				goto exit;
+			} else if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) {
+				/* filter packets that SA is myself or multicast or broadcast */
+				if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) {
+					ret = _FAIL;
+					goto exit;
+				}
+				/* da should be for me */
+				if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) {
+					ret = _FAIL;
+					goto exit;
+				}
+				/* check BSSID */
+				if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+				    !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+				    (memcmp(pattrib->bssid, mybssid, ETH_ALEN))) {
+					ret = _FAIL;
+					goto exit;
+				}
+
+#ifdef CONFIG_TDLS_CH_SW
+				if (ATOMIC_READ(&pchsw_info->chsw_on)) {
+					if (adapter->mlmeextpriv.cur_channel != rtw_get_oper_ch(adapter)) {
+						pchsw_info->ch_sw_state |= TDLS_PEER_AT_OFF_STATE;
+						if (!(pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE))
+							_cancel_timer_ex(&ptdls_sta->ch_sw_timer);
+						/* On_TDLS_Peer_Traffic_Rsp(adapter, precv_frame); */
+					}
+				}
+#endif
+
+				/* process UAPSD tdls sta */
+				process_pwrbit_data(adapter, precv_frame);
+
+				/* if NULL-frame, check pwrbit */
+				if ((get_frame_sub_type(ptr) & WIFI_DATA_NULL) == WIFI_DATA_NULL) {
+					/* NULL-frame with pwrbit=1, buffer_STA should buffer frames for sleep_STA */
+					if (GetPwrMgt(ptr)) {
+						/* it would be triggered when we are off channel and receiving NULL DATA */
+						/* we can confirm that peer STA is at off channel */
+						RTW_INFO("TDLS: recv peer null frame with pwr bit 1\n");
+						/* ptdls_sta->tdls_sta_state|=TDLS_PEER_SLEEP_STATE; */
+					}
+
+					/* TODO: Updated BSSID's seq. */
+					/* RTW_INFO("drop Null Data\n"); */
+					ptdls_sta->tdls_sta_state &= ~(TDLS_WAIT_PTR_STATE);
+					ret = _FAIL;
+					goto exit;
+				}
+
+				/* receive some of all TDLS management frames, process it at ON_TDLS */
+				if (!memcmp(psnap_type, SNAP_ETH_TYPE_TDLS, 2)) {
+					ret = OnTDLS(adapter, precv_frame);
+					goto exit;
+				}
+
+				if ((get_frame_sub_type(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE)
+					process_wmmps_data(adapter, precv_frame);
+
+				ptdls_sta->tdls_sta_state &= ~(TDLS_WAIT_PTR_STATE);
+
+			}
+
+			sta_addr = pattrib->src;
+
+		} else
+#endif /* CONFIG_TDLS */
+		{
+			/* For Station mode, sa and bssid should always be BSSID, and DA is my mac-address */
+			if (memcmp(pattrib->bssid, pattrib->src, ETH_ALEN)) {
+				ret = _FAIL;
+				goto exit;
+			}
+
+			sta_addr = pattrib->bssid;
+		}
+
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		if (bmcast) {
+			/* For AP mode, if DA == MCAST, then BSSID should be also MCAST */
+			if (!IS_MCAST(pattrib->bssid)) {
+				ret = _FAIL;
+				goto exit;
+			}
+		} else { /* not mc-frame */
+			/* For AP mode, if DA is non-MCAST, then it must be BSSID, and bssid == BSSID */
+			if (memcmp(pattrib->bssid, pattrib->dst, ETH_ALEN)) {
+				ret = _FAIL;
+				goto exit;
+			}
+
+			sta_addr = pattrib->src;
+		}
+
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+		memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->src, get_addr2_ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+		sta_addr = mybssid;
+	} else
+		ret  = _FAIL;
+
+
+
+	if (bmcast)
+		*psta = rtw_get_bcmc_stainfo(adapter);
+	else
+		*psta = rtw_get_stainfo(pstapriv, sta_addr); /* get ap_info */
+
+#ifdef CONFIG_TDLS
+	if (ptdls_sta != NULL)
+		*psta = ptdls_sta;
+#endif /* CONFIG_TDLS */
+
+	if (*psta == NULL) {
+#ifdef CONFIG_MP_INCLUDED
+		if (adapter->registrypriv.mp_mode == 1) {
+			if (check_fwstate(pmlmepriv, WIFI_MP_STATE))
+				adapter->mppriv.rx_pktloss++;
+		}
+#endif
+		ret = _FAIL;
+		goto exit;
+	}
+
+exit:
+	return ret;
+
+}
+
+sint ap2sta_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta);
+sint ap2sta_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta)
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	sint ret = _SUCCESS;
+	struct	sta_priv		*pstapriv = &adapter->stapriv;
+	struct	mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = adapter_mac_addr(adapter);
+	sint bmcast = IS_MCAST(pattrib->dst);
+
+
+	if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+	    && (check_fwstate(pmlmepriv, _FW_LINKED)
+		|| check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
+	   ) {
+
+		/* filter packets that SA is myself or multicast or broadcast */
+		if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) {
+#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME %s SA="MAC_FMT", myhwaddr="MAC_FMT"\n",
+				__func__, MAC_ARG(pattrib->src), MAC_ARG(myhwaddr));
+#endif
+			ret = _FAIL;
+			goto exit;
+		}
+
+		/* da should be for me */
+		if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) {
+#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME %s DA="MAC_FMT"\n", __func__, MAC_ARG(pattrib->dst));
+#endif
+			ret = _FAIL;
+			goto exit;
+		}
+
+
+		/* check BSSID */
+		if (!memcmp(pattrib->bssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		    !memcmp(mybssid, "\x0\x0\x0\x0\x0\x0", ETH_ALEN) ||
+		    (memcmp(pattrib->bssid, mybssid, ETH_ALEN))) {
+#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME %s BSSID="MAC_FMT", mybssid="MAC_FMT"\n",
+				__func__, MAC_ARG(pattrib->bssid), MAC_ARG(mybssid));
+#endif
+
+			if (!bmcast) {
+				RTW_INFO(ADPT_FMT" -issue_deauth to the nonassociated ap=" MAC_FMT " for the reason(7)\n", ADPT_ARG(adapter), MAC_ARG(pattrib->bssid));
+				issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+			}
+
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if (bmcast)
+			*psta = rtw_get_bcmc_stainfo(adapter);
+		else
+			*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get ap_info */
+
+		if (*psta == NULL) {
+#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME %s can't get psta under STATION_MODE ; drop pkt\n", __func__);
+#endif
+			ret = _FAIL;
+			goto exit;
+		}
+
+		/*if ((get_frame_sub_type(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) {
+		}
+		*/
+
+		if (get_frame_sub_type(ptr) & BIT(6)) {
+			/* No data, will not indicate to upper layer, temporily count it here */
+			count_rx_stats(adapter, precv_frame, *psta);
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+
+	} else if ((check_fwstate(pmlmepriv, WIFI_MP_STATE)) &&
+		   (check_fwstate(pmlmepriv, _FW_LINKED))) {
+		memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->src, get_addr2_ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+
+		*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get sta_info */
+		if (*psta == NULL) {
+#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME %s can't get psta under WIFI_MP_STATE ; drop pkt\n", __func__);
+#endif
+			ret = _FAIL;
+			goto exit;
+		}
+
+
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		/* Special case */
+		ret = RTW_RX_HANDLED;
+		goto exit;
+	} else {
+		if (!memcmp(myhwaddr, pattrib->dst, ETH_ALEN) && (!bmcast)) {
+			*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get sta_info */
+			if (*psta == NULL) {
+
+				/* for AP multicast issue , modify by yiwei */
+				static u32 send_issue_deauth_time = 0;
+
+				/* RTW_INFO("After send deauth , %u ms has elapsed.\n", rtw_get_passing_time_ms(send_issue_deauth_time)); */
+
+				if (rtw_get_passing_time_ms(send_issue_deauth_time) > 10000 || send_issue_deauth_time == 0) {
+					send_issue_deauth_time = jiffies;
+
+					RTW_INFO("issue_deauth to the ap=" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->bssid));
+
+					issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+				}
+			}
+		}
+
+		ret = _FAIL;
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME %s fw_state:0x%x\n", __func__, get_fwstate(pmlmepriv));
+#endif
+	}
+
+exit:
+
+
+	return ret;
+
+}
+
+sint sta2ap_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta);
+sint sta2ap_data_frame(
+	_adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta)
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct	sta_priv		*pstapriv = &adapter->stapriv;
+	struct	mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	unsigned char *mybssid  = get_bssid(pmlmepriv);
+	sint ret = _SUCCESS;
+
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		/* For AP mode, RA=BSSID, TX=STA(SRC_ADDR), A3=DST_ADDR */
+		if (memcmp(pattrib->bssid, mybssid, ETH_ALEN)) {
+			ret = _FAIL;
+			goto exit;
+		}
+
+		*psta = rtw_get_stainfo(pstapriv, pattrib->src);
+		if (*psta == NULL) {
+			#ifdef CONFIG_DFS_MASTER
+			struct rf_ctl_t *rfctl = adapter_to_rfctl(adapter);
+
+			/* prevent RX tasklet blocks cmd_thread */
+			if (rfctl->radar_detected == 1)
+				goto bypass_deauth7;
+			#endif
+
+			RTW_INFO("issue_deauth to sta=" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->src));
+
+			issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+
+#ifdef CONFIG_DFS_MASTER
+bypass_deauth7:
+#endif
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+
+		process_pwrbit_data(adapter, precv_frame);
+
+		if ((get_frame_sub_type(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE)
+			process_wmmps_data(adapter, precv_frame);
+
+		if (get_frame_sub_type(ptr) & BIT(6)) {
+			/* No data, will not indicate to upper layer, temporily count it here */
+			count_rx_stats(adapter, precv_frame, *psta);
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+	} else if ((check_fwstate(pmlmepriv, WIFI_MP_STATE)) &&
+		   (check_fwstate(pmlmepriv, _FW_LINKED))) {
+		/* RTW_INFO("%s ,in WIFI_MP_STATE\n",__func__); */
+		memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->src, get_addr2_ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+
+		*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /* get sta_info */
+		if (*psta == NULL) {
+#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME %s can't get psta under WIFI_MP_STATE ; drop pkt\n", __func__);
+#endif
+			ret = _FAIL;
+			goto exit;
+		}
+
+	} else {
+		u8 *myhwaddr = adapter_mac_addr(adapter);
+		if (memcmp(pattrib->ra, myhwaddr, ETH_ALEN)) {
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+		RTW_INFO("issue_deauth to sta=" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->src));
+		issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+		ret = RTW_RX_HANDLED;
+		goto exit;
+	}
+
+exit:
+
+
+	return ret;
+
+}
+
+sint validate_recv_ctrl_frame(_adapter *padapter, union recv_frame *precv_frame);
+sint validate_recv_ctrl_frame(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct sta_info *psta = NULL;
+	/* uint len = precv_frame->u.hdr.len; */
+
+	/* RTW_INFO("+validate_recv_ctrl_frame\n"); */
+
+	if (GetFrameType(pframe) != WIFI_CTRL_TYPE)
+		return _FAIL;
+
+	/* receive the frames that ra(a1) is my address */
+	if (memcmp(GetAddr1Ptr(pframe), adapter_mac_addr(padapter), ETH_ALEN))
+		return _FAIL;
+
+	psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(pframe));
+	if (psta == NULL)
+		return _FAIL;
+
+	/* for rx pkt statistics */
+	psta->sta_stats.rx_ctrl_pkts++;
+
+	/* only handle ps-poll */
+	if (get_frame_sub_type(pframe) == WIFI_PSPOLL) {
+#ifdef CONFIG_AP_MODE
+		u16 aid;
+		u8 wmmps_ac = 0;
+
+		aid = GetAid(pframe);
+		if (psta->aid != aid)
+			return _FAIL;
+
+		switch (pattrib->priority) {
+		case 1:
+		case 2:
+			wmmps_ac = psta->uapsd_bk & BIT(0);
+			break;
+		case 4:
+		case 5:
+			wmmps_ac = psta->uapsd_vi & BIT(0);
+			break;
+		case 6:
+		case 7:
+			wmmps_ac = psta->uapsd_vo & BIT(0);
+			break;
+		case 0:
+		case 3:
+		default:
+			wmmps_ac = psta->uapsd_be & BIT(0);
+			break;
+		}
+
+		if (wmmps_ac)
+			return _FAIL;
+
+		if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+			RTW_INFO("%s alive check-rx ps-poll\n", __func__);
+			psta->expire_to = pstapriv->expire_to;
+			psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+		}
+
+		if ((psta->state & WIFI_SLEEP_STATE) && (pstapriv->sta_dz_bitmap & BIT(psta->aid))) {
+			unsigned long irqL;
+			_list	*xmitframe_plist, *xmitframe_phead;
+			struct xmit_frame *pxmitframe = NULL;
+			struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+			/* _enter_critical_bh(&psta->sleep_q.lock, &irqL); */
+			_enter_critical_bh(&pxmitpriv->lock, &irqL);
+
+			xmitframe_phead = get_list_head(&psta->sleep_q);
+			xmitframe_plist = get_next(xmitframe_phead);
+
+			if ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == false) {
+				pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+				xmitframe_plist = get_next(xmitframe_plist);
+
+				list_del_init(&pxmitframe->list);
+
+				psta->sleepq_len--;
+
+				if (psta->sleepq_len > 0)
+					pxmitframe->attrib.mdata = 1;
+				else
+					pxmitframe->attrib.mdata = 0;
+
+				pxmitframe->attrib.triggered = 1;
+
+				/* RTW_INFO("handling ps-poll, q_len=%d, tim=%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
+
+				rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+				if (psta->sleepq_len == 0) {
+					pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+					/* RTW_INFO("after handling ps-poll, tim=%x\n", pstapriv->tim_bitmap); */
+
+					/* upate BCN for TIM IE */
+					/* update_BCNTIM(padapter);		 */
+					update_beacon(padapter, _TIM_IE_, NULL, true);
+				}
+
+				/* _exit_critical_bh(&psta->sleep_q.lock, &irqL); */
+				_exit_critical_bh(&pxmitpriv->lock, &irqL);
+
+			} else {
+				/* _exit_critical_bh(&psta->sleep_q.lock, &irqL); */
+				_exit_critical_bh(&pxmitpriv->lock, &irqL);
+
+				/* RTW_INFO("no buffered packets to xmit\n"); */
+				if (pstapriv->tim_bitmap & BIT(psta->aid)) {
+					if (psta->sleepq_len == 0) {
+						RTW_INFO("no buffered packets to xmit\n");
+
+						/* issue nulldata with More data bit = 0 to indicate we have no buffered packets */
+						issue_nulldata_in_interrupt(padapter, psta->hwaddr, 0);
+					} else {
+						RTW_INFO("error!psta->sleepq_len=%d\n", psta->sleepq_len);
+						psta->sleepq_len = 0;
+					}
+
+					pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+					/* upate BCN for TIM IE */
+					/* update_BCNTIM(padapter); */
+					update_beacon(padapter, _TIM_IE_, NULL, true);
+				}
+			}
+		}
+#endif /* CONFIG_AP_MODE */
+	} else if (get_frame_sub_type(pframe) == WIFI_NDPA) {
+#ifdef CONFIG_BEAMFORMING
+		beamforming_get_ndpa_frame(padapter, precv_frame);
+#endif/*CONFIG_BEAMFORMING*/
+	}
+
+	return _FAIL;
+
+}
+
+union recv_frame *recvframe_chk_defrag(PADAPTER padapter, union recv_frame *precv_frame);
+
+static sint validate_recv_mgnt_frame(PADAPTER padapter, union recv_frame *precv_frame)
+{
+	precv_frame = recvframe_chk_defrag(padapter, precv_frame);
+	if (precv_frame == NULL) {
+		return _SUCCESS;
+	}
+
+	{
+		/* for rx pkt statistics */
+		struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, get_addr2_ptr(precv_frame->u.hdr.rx_data));
+		if (psta) {
+			psta->sta_stats.rx_mgnt_pkts++;
+			if (get_frame_sub_type(precv_frame->u.hdr.rx_data) == WIFI_BEACON)
+				psta->sta_stats.rx_beacon_pkts++;
+			else if (get_frame_sub_type(precv_frame->u.hdr.rx_data) == WIFI_PROBEREQ)
+				psta->sta_stats.rx_probereq_pkts++;
+			else if (get_frame_sub_type(precv_frame->u.hdr.rx_data) == WIFI_PROBERSP) {
+				if (!memcmp(adapter_mac_addr(padapter), GetAddr1Ptr(precv_frame->u.hdr.rx_data), ETH_ALEN))
+					psta->sta_stats.rx_probersp_pkts++;
+				else if (is_broadcast_mac_addr(GetAddr1Ptr(precv_frame->u.hdr.rx_data))
+					|| is_multicast_mac_addr(GetAddr1Ptr(precv_frame->u.hdr.rx_data)))
+					psta->sta_stats.rx_probersp_bm_pkts++;
+				else
+					psta->sta_stats.rx_probersp_uo_pkts++;
+			}
+		}
+	}
+
+#ifdef CONFIG_INTEL_PROXIM
+	if (padapter->proximity.proxim_on) {
+		struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+		struct recv_stat *prxstat = (struct recv_stat *)  precv_frame->u.hdr.rx_head ;
+		u8 *pda, *psa, *pbssid, *ptr;
+		ptr = precv_frame->u.hdr.rx_data;
+		pda = get_da(ptr);
+		psa = get_sa(ptr);
+		pbssid = get_hdr_bssid(ptr);
+
+
+		memcpy(pattrib->dst, pda, ETH_ALEN);
+		memcpy(pattrib->src, psa, ETH_ALEN);
+
+		memcpy(pattrib->bssid, pbssid, ETH_ALEN);
+
+		switch (pattrib->to_fr_ds) {
+		case 0:
+			memcpy(pattrib->ra, pda, ETH_ALEN);
+			memcpy(pattrib->ta, psa, ETH_ALEN);
+			break;
+
+		case 1:
+			memcpy(pattrib->ra, pda, ETH_ALEN);
+			memcpy(pattrib->ta, pbssid, ETH_ALEN);
+			break;
+
+		case 2:
+			memcpy(pattrib->ra, pbssid, ETH_ALEN);
+			memcpy(pattrib->ta, psa, ETH_ALEN);
+			break;
+
+		case 3:
+			memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN);
+			memcpy(pattrib->ta, get_addr2_ptr(ptr), ETH_ALEN);
+			break;
+
+		default:
+			break;
+
+		}
+		pattrib->priority = 0;
+		pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 30 : 24;
+
+		padapter->proximity.proxim_rx(padapter, precv_frame);
+	}
+#endif
+	mgt_dispatcher(padapter, precv_frame);
+
+	return _SUCCESS;
+
+}
+
+sint validate_recv_data_frame(_adapter *adapter, union recv_frame *precv_frame);
+sint validate_recv_data_frame(_adapter *adapter, union recv_frame *precv_frame)
+{
+	u8 bretry;
+	u8 *psa, *pda, *pbssid;
+	struct sta_info *psta = NULL;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib	*pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv	*pstapriv = &adapter->stapriv;
+	struct security_priv	*psecuritypriv = &adapter->securitypriv;
+	sint ret = _SUCCESS;
+
+
+	bretry = GetRetry(ptr);
+	pda = get_da(ptr);
+	psa = get_sa(ptr);
+	pbssid = get_hdr_bssid(ptr);
+
+	if (pbssid == NULL) {
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME %s pbssid == NULL\n", __func__);
+#endif
+		ret = _FAIL;
+		goto exit;
+	}
+
+	memcpy(pattrib->dst, pda, ETH_ALEN);
+	memcpy(pattrib->src, psa, ETH_ALEN);
+
+	memcpy(pattrib->bssid, pbssid, ETH_ALEN);
+
+	switch (pattrib->to_fr_ds) {
+	case 0:
+		memcpy(pattrib->ra, pda, ETH_ALEN);
+		memcpy(pattrib->ta, psa, ETH_ALEN);
+		ret = sta2sta_data_frame(adapter, precv_frame, &psta);
+		break;
+
+	case 1:
+		memcpy(pattrib->ra, pda, ETH_ALEN);
+		memcpy(pattrib->ta, pbssid, ETH_ALEN);
+		ret = ap2sta_data_frame(adapter, precv_frame, &psta);
+		break;
+
+	case 2:
+		memcpy(pattrib->ra, pbssid, ETH_ALEN);
+		memcpy(pattrib->ta, psa, ETH_ALEN);
+		ret = sta2ap_data_frame(adapter, precv_frame, &psta);
+		break;
+
+	case 3:
+		memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->ta, get_addr2_ptr(ptr), ETH_ALEN);
+		ret = _FAIL;
+		break;
+
+	default:
+		ret = _FAIL;
+		break;
+
+	}
+
+	if (ret == _FAIL) {
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME %s case:%d, res:%d\n", __func__, pattrib->to_fr_ds, ret);
+#endif
+		goto exit;
+	} else if (ret == RTW_RX_HANDLED)
+		goto exit;
+
+
+	if (psta == NULL) {
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME %s psta == NULL\n", __func__);
+#endif
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* psta->rssi = prxcmd->rssi; */
+	/* psta->signal_quality= prxcmd->sq; */
+	precv_frame->u.hdr.psta = psta;
+
+
+	pattrib->amsdu = 0;
+	pattrib->ack_policy = 0;
+	/* parsing QC field */
+	if (pattrib->qos == 1) {
+		pattrib->priority = GetPriority((ptr + 24));
+		pattrib->ack_policy = GetAckpolicy((ptr + 24));
+		pattrib->amsdu = GetAMsdu((ptr + 24));
+		pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 32 : 26;
+
+		if (pattrib->priority != 0 && pattrib->priority != 3)
+			adapter->recvpriv.is_any_non_be_pkts = true;
+		else
+			adapter->recvpriv.is_any_non_be_pkts = false;
+	} else {
+		pattrib->priority = 0;
+		pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 30 : 24;
+	}
+
+
+	if (pattrib->order) /* HT-CTRL 11n */
+		pattrib->hdrlen += 4;
+
+	precv_frame->u.hdr.preorder_ctrl = &psta->recvreorder_ctrl[pattrib->priority];
+
+	/* decache, drop duplicate recv packets */
+	if (recv_decache(precv_frame, bretry, &psta->sta_recvpriv.rxcache) == _FAIL) {
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME %s recv_decache return _FAIL\n", __func__);
+#endif
+		ret = _FAIL;
+		goto exit;
+	}
+
+	if (!IS_MCAST(pattrib->ra)) {
+		if (recv_ucast_pn_decache(precv_frame, &psta->sta_recvpriv.rxcache) == _FAIL) {
+			#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME %s recv_ucast_pn_decache return _FAIL\n", __func__);
+			#endif
+			ret = _FAIL;
+			goto exit;
+		}		
+	} else {
+		if (recv_bcast_pn_decache(precv_frame) == _FAIL) {
+			#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME "FUNC_ADPT_FMT" recv_bcast_pn_decache _FAIL for invalid PN!\n"
+				, FUNC_ADPT_ARG(adapter));
+			#endif
+			ret = _FAIL;
+			goto exit;
+		}
+	}
+
+	if (pattrib->privacy) {
+
+
+#ifdef CONFIG_TDLS
+		if ((psta->tdls_sta_state & TDLS_LINKED_STATE) && (psta->dot118021XPrivacy == _AES_))
+			pattrib->encrypt = psta->dot118021XPrivacy;
+		else
+#endif /* CONFIG_TDLS */
+			GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, IS_MCAST(pattrib->ra));
+
+
+		SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt);
+	} else {
+		pattrib->encrypt = 0;
+		pattrib->iv_len = pattrib->icv_len = 0;
+	}
+
+exit:
+
+
+	return ret;
+}
+
+#ifdef CONFIG_IEEE80211W
+static sint validate_80211w_mgmt(_adapter *adapter, union recv_frame *precv_frame)
+{
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct sta_info	*psta;
+	struct sta_priv		*pstapriv = &adapter->stapriv;
+	u8 type;
+	u8 subtype;
+
+	type =  GetFrameType(ptr);
+	subtype = get_frame_sub_type(ptr); /* bit(7)~bit(2) */
+
+	if (adapter->securitypriv.binstallBIPkey) {
+		/* unicast management frame decrypt */
+		if (pattrib->privacy && !(IS_MCAST(GetAddr1Ptr(ptr))) &&
+		    (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC || subtype == WIFI_ACTION)) {
+			u8 *ppp, *mgmt_DATA;
+			u32 data_len = 0;
+			ppp = get_addr2_ptr(ptr);
+
+			pattrib->bdecrypted = 0;
+			pattrib->encrypt = _AES_;
+			pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+			/* set iv and icv length */
+			SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt);
+			memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN);
+			memcpy(pattrib->ta, get_addr2_ptr(ptr), ETH_ALEN);
+			/* actual management data frame body */
+			data_len = pattrib->pkt_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
+			mgmt_DATA = rtw_zmalloc(data_len);
+			if (mgmt_DATA == NULL) {
+				RTW_INFO("%s mgmt allocate fail  !!!!!!!!!\n", __func__);
+				goto validate_80211w_fail;
+			}
+			precv_frame = decryptor(adapter, precv_frame);
+			/* save actual management data frame body */
+			memcpy(mgmt_DATA, ptr + pattrib->hdrlen + pattrib->iv_len, data_len);
+			/* overwrite the iv field */
+			memcpy(ptr + pattrib->hdrlen, mgmt_DATA, data_len);
+			/* remove the iv and icv length */
+			pattrib->pkt_len = pattrib->pkt_len - pattrib->iv_len - pattrib->icv_len;
+			rtw_mfree(mgmt_DATA, data_len);
+			if (!precv_frame) {
+				RTW_INFO("%s mgmt descrypt fail  !!!!!!!!!\n", __func__);
+				goto validate_80211w_fail;
+			}
+		} else if (IS_MCAST(GetAddr1Ptr(ptr)) &&
+			(subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC)) {
+			sint BIP_ret = _SUCCESS;
+			/* verify BIP MME IE of broadcast/multicast de-auth/disassoc packet */
+			BIP_ret = rtw_BIP_verify(adapter, (u8 *)precv_frame);
+			if (BIP_ret == _FAIL) {
+				/* RTW_INFO("802.11w BIP verify fail\n"); */
+				goto validate_80211w_fail;
+			} else if (BIP_ret == RTW_RX_HANDLED) {
+				RTW_INFO("802.11w recv none protected packet\n");
+				/* drop pkt, don't issue sa query request */
+				/* issue_action_SA_Query(adapter, NULL, 0, 0, 0); */
+				goto validate_80211w_fail;
+			}
+		} /* 802.11w protect */
+		else {
+			psta = rtw_get_stainfo(pstapriv, get_addr2_ptr(ptr));
+
+			if (subtype == WIFI_ACTION && psta && psta->bpairwise_key_installed) {
+				/* according 802.11-2012 standard, these five types are not robust types */
+				if (ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_PUBLIC          &&
+				    ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_HT              &&
+				    ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_UNPROTECTED_WNM &&
+				    ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_SELF_PROTECTED  &&
+				    ptr[WLAN_HDR_A3_LEN] != RTW_WLAN_CATEGORY_P2P) {
+					RTW_INFO("action frame category=%d should robust\n", ptr[WLAN_HDR_A3_LEN]);
+					goto validate_80211w_fail;
+				}
+			} else if (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC) {
+				unsigned short	reason;
+				reason = le16_to_cpu(*(unsigned short *)(ptr + WLAN_HDR_A3_LEN));
+				RTW_INFO("802.11w recv none protected packet, reason=%d\n", reason);
+				if (reason == 6 || reason == 7) {
+					/* issue sa query request */
+					issue_action_SA_Query(adapter, NULL, 0, 0, IEEE80211W_RIGHT_KEY);
+				}
+				goto validate_80211w_fail;
+			}
+		}
+	}
+	return _SUCCESS;
+
+validate_80211w_fail:
+	return _FAIL;
+
+}
+#endif /* CONFIG_IEEE80211W */
+
+static inline void dump_rx_packet(u8 *ptr)
+{
+	int i;
+
+	RTW_INFO("#############################\n");
+	for (i = 0; i < 64; i = i + 8)
+		RTW_INFO("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n", *(ptr + i),
+			*(ptr + i + 1), *(ptr + i + 2) , *(ptr + i + 3) , *(ptr + i + 4), *(ptr + i + 5), *(ptr + i + 6), *(ptr + i + 7));
+	RTW_INFO("#############################\n");
+}
+
+sint validate_recv_frame(_adapter *adapter, union recv_frame *precv_frame);
+sint validate_recv_frame(_adapter *adapter, union recv_frame *precv_frame)
+{
+	/* shall check frame subtype, to / from ds, da, bssid */
+
+	/* then call check if rx seq/frag. duplicated. */
+
+	u8 type;
+	u8 subtype;
+	sint retval = _SUCCESS;
+
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	u8  ver = (unsigned char)(*ptr) & 0x3 ;
+#ifdef CONFIG_FIND_BEST_CHANNEL
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+#endif
+
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &adapter->tdlsinfo;
+#endif /* CONFIG_TDLS */
+#ifdef CONFIG_WAPI_SUPPORT
+	PRT_WAPI_T	pWapiInfo = &adapter->wapiInfo;
+	struct recv_frame_hdr *phdr = &precv_frame->u.hdr;
+	u8 wai_pkt = 0;
+	u16 sc;
+	u8	external_len = 0;
+#endif
+
+
+#ifdef CONFIG_FIND_BEST_CHANNEL
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		int ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, rtw_get_oper_ch(adapter));
+		if (ch_set_idx >= 0)
+			pmlmeext->channel_set[ch_set_idx].rx_count++;
+	}
+#endif
+
+#ifdef CONFIG_TDLS
+	if (ptdlsinfo->ch_sensing == 1 && ptdlsinfo->cur_channel != 0)
+		ptdlsinfo->collect_pkt_num[ptdlsinfo->cur_channel - 1]++;
+#endif /* CONFIG_TDLS */
+
+#ifdef RTK_DMP_PLATFORM
+	if (0) {
+		RTW_INFO("++\n");
+		{
+			int i;
+			for (i = 0; i < 64; i = i + 8)
+				RTW_INFO("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:", *(ptr + i),
+					*(ptr + i + 1), *(ptr + i + 2) , *(ptr + i + 3) , *(ptr + i + 4), *(ptr + i + 5), *(ptr + i + 6), *(ptr + i + 7));
+
+		}
+		RTW_INFO("--\n");
+	}
+#endif /* RTK_DMP_PLATFORM */
+
+	/* add version chk */
+	if (ver != 0) {
+		retval = _FAIL;
+		DBG_COUNTER(adapter->rx_logs.core_rx_pre_ver_err);
+		goto exit;
+	}
+
+	type =  GetFrameType(ptr);
+	subtype = get_frame_sub_type(ptr); /* bit(7)~bit(2) */
+
+	pattrib->to_fr_ds = get_tofr_ds(ptr);
+
+	pattrib->frag_num = GetFragNum(ptr);
+	pattrib->seq_num = cpu_to_le16(GetSequence(ptr));
+
+	pattrib->pw_save = GetPwrMgt(ptr);
+	pattrib->mfrag = GetMFrag(ptr);
+	pattrib->mdata = GetMData(ptr);
+	pattrib->privacy = GetPrivacy(ptr);
+	pattrib->order = GetOrder(ptr);
+#ifdef CONFIG_WAPI_SUPPORT
+	sc = (pattrib->seq_num << 4) | pattrib->frag_num;
+#endif
+
+	{
+		u8 bDumpRxPkt = 0;
+
+		rtw_hal_get_def_var(adapter, HAL_DEF_DBG_DUMP_RXPKT, &(bDumpRxPkt));
+		if (bDumpRxPkt == 1) /* dump all rx packets */
+			dump_rx_packet(ptr);
+		else if ((bDumpRxPkt == 2) && (type == WIFI_MGT_TYPE))
+			dump_rx_packet(ptr);
+		else if ((bDumpRxPkt == 3) && (type == WIFI_DATA_TYPE))
+			dump_rx_packet(ptr);
+	}
+	switch (type) {
+	case WIFI_MGT_TYPE: /* mgnt */
+		DBG_COUNTER(adapter->rx_logs.core_rx_pre_mgmt);
+#ifdef CONFIG_IEEE80211W
+		if (validate_80211w_mgmt(adapter, precv_frame) == _FAIL) {
+			retval = _FAIL;
+			DBG_COUNTER(adapter->rx_logs.core_rx_pre_mgmt_err_80211w);
+			break;
+		}
+#endif /* CONFIG_IEEE80211W */
+
+		retval = validate_recv_mgnt_frame(adapter, precv_frame);
+		if (retval == _FAIL) {
+			DBG_COUNTER(adapter->rx_logs.core_rx_pre_mgmt_err);
+		}
+		retval = _FAIL; /* only data frame return _SUCCESS */
+		break;
+	case WIFI_CTRL_TYPE: /* ctrl */
+		DBG_COUNTER(adapter->rx_logs.core_rx_pre_ctrl);
+		retval = validate_recv_ctrl_frame(adapter, precv_frame);
+		if (retval == _FAIL) {
+			DBG_COUNTER(adapter->rx_logs.core_rx_pre_ctrl_err);
+		}
+		retval = _FAIL; /* only data frame return _SUCCESS */
+		break;
+	case WIFI_DATA_TYPE: /* data */
+		DBG_COUNTER(adapter->rx_logs.core_rx_pre_data);
+#ifdef CONFIG_WAPI_SUPPORT
+		if (pattrib->qos)
+			external_len = 2;
+		else
+			external_len = 0;
+
+		wai_pkt = rtw_wapi_is_wai_packet(adapter, ptr);
+
+		phdr->bIsWaiPacket = wai_pkt;
+
+		if (wai_pkt != 0) {
+			if (sc != adapter->wapiInfo.wapiSeqnumAndFragNum)
+				adapter->wapiInfo.wapiSeqnumAndFragNum = sc;
+			else {
+				retval = _FAIL;
+				DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_wapi_seq_err);
+				break;
+			}
+		} else {
+
+			if (rtw_wapi_drop_for_key_absent(adapter, get_addr2_ptr(ptr))) {
+				retval = _FAIL;
+				WAPI_TRACE(WAPI_RX, "drop for key absent for rx\n");
+				DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_wapi_key_err);
+				break;
+			}
+		}
+
+#endif
+
+		pattrib->qos = (subtype & BIT(7)) ? 1 : 0;
+		retval = validate_recv_data_frame(adapter, precv_frame);
+		if (retval == _FAIL) {
+			struct recv_priv *precvpriv = &adapter->recvpriv;
+			precvpriv->rx_drop++;
+			DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_err);
+		} else if (retval == _SUCCESS) {
+#ifdef DBG_RX_DUMP_EAP
+			u8 bDumpRxPkt;
+			u16 eth_type;
+
+			/* dump eapol */
+			rtw_hal_get_def_var(adapter, HAL_DEF_DBG_DUMP_RXPKT, &(bDumpRxPkt));
+			/* get ether_type */
+			memcpy(&eth_type, ptr + pattrib->hdrlen + pattrib->iv_len + LLC_HEADER_SIZE, 2);
+			eth_type = ntohs((unsigned short) eth_type);
+			if ((bDumpRxPkt == 4) && (eth_type == 0x888e))
+				dump_rx_packet(ptr);
+#endif
+		} else
+			DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_handled);
+		break;
+	default:
+		DBG_COUNTER(adapter->rx_logs.core_rx_pre_unknown);
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME validate_recv_data_frame fail! type=0x%x\n", type);
+#endif
+		retval = _FAIL;
+		break;
+	}
+
+exit:
+
+
+	return retval;
+}
+
+
+/* remove the wlanhdr and add the eth_hdr */
+
+static sint wlanhdr_to_ethhdr(union recv_frame *precvframe)
+{
+	sint	rmv_len;
+	u16	eth_type, len;
+	u8	bsnaphdr;
+	u8	*psnap_type;
+	__be16 be_tmp;
+	struct ieee80211_snap_hdr	*psnap;
+
+	sint ret = _SUCCESS;
+	_adapter			*adapter = precvframe->u.hdr.adapter;
+	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+
+	u8	*ptr = get_recvframe_data(precvframe) ; /* point to frame_ctrl field */
+	struct rx_pkt_attrib *pattrib = &precvframe->u.hdr.attrib;
+
+
+	if (pattrib->encrypt)
+		recvframe_pull_tail(precvframe, pattrib->icv_len);
+
+	psnap = (struct ieee80211_snap_hdr *)(ptr + pattrib->hdrlen + pattrib->iv_len);
+	psnap_type = ptr + pattrib->hdrlen + pattrib->iv_len + SNAP_SIZE;
+	/* convert hdr + possible LLC headers into Ethernet header */
+	/* eth_type = (psnap_type[0] << 8) | psnap_type[1]; */
+	if ((!memcmp(psnap, rtw_rfc1042_header, SNAP_SIZE) &&
+	     (!memcmp(psnap_type, SNAP_ETH_TYPE_IPX, 2) == false) &&
+	     (!memcmp(psnap_type, SNAP_ETH_TYPE_APPLETALK_AARP, 2) == false)) ||
+	    /* eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) || */
+	    !memcmp(psnap, rtw_bridge_tunnel_header, SNAP_SIZE)) {
+		/* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
+		bsnaphdr = true;
+	} else {
+		/* Leave Ethernet header part of hdr and full payload */
+		bsnaphdr = false;
+	}
+
+	rmv_len = pattrib->hdrlen + pattrib->iv_len + (bsnaphdr ? SNAP_SIZE : 0);
+	len = precvframe->u.hdr.len - rmv_len;
+
+
+	memcpy(&be_tmp, ptr + rmv_len, 2);
+	eth_type = ntohs(be_tmp); /* pattrib->ether_type */
+	pattrib->eth_type = cpu_to_le16(eth_type);
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (0x8899 == pattrib->eth_type) {
+		struct sta_info *psta = precvframe->u.hdr.psta;
+
+		RTW_INFO("wlan rx: got eth_type=0x%x\n", pattrib->eth_type);
+
+		if (psta && psta->isrc && psta->pid > 0) {
+			u16 rx_pid;
+
+			rx_pid = *(u16 *)(ptr + rmv_len + 2);
+
+			RTW_INFO("wlan rx(pid=0x%x): sta("MAC_FMT") pid=0x%x\n",
+				 rx_pid, MAC_ARG(psta->hwaddr), psta->pid);
+
+			if (rx_pid == psta->pid) {
+				int i;
+				u16 len = *(u16 *)(ptr + rmv_len + 4);
+				/* u16 ctrl_type = *(u16*)(ptr+rmv_len+6); */
+
+				/* RTW_INFO("RC: len=0x%x, ctrl_type=0x%x\n", len, ctrl_type);  */
+				RTW_INFO("RC: len=0x%x\n", len);
+
+				for (i = 0; i < len; i++)
+					RTW_INFO("0x%x\n", *(ptr + rmv_len + 6 + i));
+				/* RTW_INFO("0x%x\n", *(ptr+rmv_len+8+i)); */
+
+				RTW_INFO("RC-end\n");
+			}
+		}
+	}
+#endif /* CONFIG_AUTO_AP_MODE */
+
+	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE))) {
+		ptr += rmv_len ;
+		*ptr = 0x87;
+		*(ptr + 1) = 0x12;
+
+		eth_type = 0x8712;
+		/* append rx status for mp test packets */
+		ptr = recvframe_pull(precvframe, (rmv_len - sizeof(struct ethhdr) + 2) - 24);
+		if (!ptr) {
+			ret = _FAIL;
+			goto exiting;
+		}
+		memcpy(ptr, get_rxmem(precvframe), 24);
+		ptr += 24;
+	} else {
+		ptr = recvframe_pull(precvframe, (rmv_len - sizeof(struct ethhdr) + (bsnaphdr ? 2 : 0)));
+		if (!ptr) {
+			ret = _FAIL;
+			goto exiting;
+		}
+	}
+
+	if (ptr) {
+		memcpy(ptr, pattrib->dst, ETH_ALEN);
+		memcpy(ptr + ETH_ALEN, pattrib->src, ETH_ALEN);
+
+		if (!bsnaphdr) {
+			be_tmp = htons(len);
+			memcpy(ptr + 12, &be_tmp, 2);
+		}
+	}
+
+exiting:
+	return ret;
+
+}
+
+/* perform defrag */
+static union recv_frame *recvframe_defrag(_adapter *adapter, _queue *defrag_q)
+{
+	_list	*plist, *phead;
+	u8	*data, wlanhdr_offset;
+	u8	curfragnum;
+	struct recv_frame_hdr *pfhdr, *pnfhdr;
+	union recv_frame *prframe, *pnextrframe;
+	_queue	*pfree_recv_queue;
+
+	curfragnum = 0;
+	pfree_recv_queue = &adapter->recvpriv.free_recv_queue;
+
+	phead = get_list_head(defrag_q);
+	plist = get_next(phead);
+	prframe = LIST_CONTAINOR(plist, union recv_frame, u);
+	pfhdr = &prframe->u.hdr;
+	list_del_init(&(prframe->u.list));
+
+	if (curfragnum != pfhdr->attrib.frag_num) {
+		/* the first fragment number must be 0 */
+		/* free the whole queue */
+		rtw_free_recvframe(prframe, pfree_recv_queue);
+		rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+
+		return NULL;
+	}
+	curfragnum++;
+
+	plist = get_list_head(defrag_q);
+
+	plist = get_next(plist);
+
+	data = get_recvframe_data(prframe);
+
+	while (rtw_end_of_queue_search(phead, plist) == false) {
+		pnextrframe = LIST_CONTAINOR(plist, union recv_frame , u);
+		pnfhdr = &pnextrframe->u.hdr;
+
+
+		/* check the fragment sequence  (2nd ~n fragment frame) */
+
+		if (curfragnum != pnfhdr->attrib.frag_num) {
+			/* the fragment number must be increasing  (after decache) */
+			/* release the defrag_q & prframe */
+			rtw_free_recvframe(prframe, pfree_recv_queue);
+			rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+			return NULL;
+		}
+
+		curfragnum++;
+
+		/* copy the 2nd~n fragment frame's payload to the first fragment */
+		/* get the 2nd~last fragment frame's payload */
+
+		wlanhdr_offset = pnfhdr->attrib.hdrlen + pnfhdr->attrib.iv_len;
+
+		recvframe_pull(pnextrframe, wlanhdr_offset);
+
+		/* append  to first fragment frame's tail (if privacy frame, pull the ICV) */
+		recvframe_pull_tail(prframe, pfhdr->attrib.icv_len);
+
+		/* memcpy */
+		memcpy(pfhdr->rx_tail, pnfhdr->rx_data, pnfhdr->len);
+
+		recvframe_put(prframe, cpu_to_le16(pnfhdr->len));
+
+		pfhdr->attrib.icv_len = pnfhdr->attrib.icv_len;
+		plist = get_next(plist);
+
+	};
+
+	/* free the defrag_q queue and return the prframe */
+	rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+
+	return prframe;
+}
+
+/* check if need to defrag, if needed queue the frame to defrag_q */
+union recv_frame *recvframe_chk_defrag(PADAPTER padapter, union recv_frame *precv_frame)
+{
+	u8	ismfrag;
+	u8	fragnum;
+	u8	*psta_addr;
+	struct recv_frame_hdr *pfhdr;
+	struct sta_info *psta;
+	struct sta_priv *pstapriv;
+	_list *phead;
+	union recv_frame *prtnframe = NULL;
+	_queue *pfree_recv_queue, *pdefrag_q;
+
+
+	pstapriv = &padapter->stapriv;
+
+	pfhdr = &precv_frame->u.hdr;
+
+	pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+	/* need to define struct of wlan header frame ctrl */
+	ismfrag = pfhdr->attrib.mfrag;
+	fragnum = pfhdr->attrib.frag_num;
+
+	psta_addr = pfhdr->attrib.ta;
+	psta = rtw_get_stainfo(pstapriv, psta_addr);
+	if (psta == NULL) {
+		u8 type = GetFrameType(pfhdr->rx_data);
+		if (type != WIFI_DATA_TYPE) {
+			psta = rtw_get_bcmc_stainfo(padapter);
+			pdefrag_q = &psta->sta_recvpriv.defrag_q;
+		} else
+			pdefrag_q = NULL;
+	} else
+		pdefrag_q = &psta->sta_recvpriv.defrag_q;
+
+	if ((ismfrag == 0) && (fragnum == 0)) {
+		prtnframe = precv_frame;/* isn't a fragment frame */
+	}
+
+	if (ismfrag == 1) {
+		/* 0~(n-1) fragment frame */
+		/* enqueue to defraf_g */
+		if (pdefrag_q != NULL) {
+			if (fragnum == 0) {
+				/* the first fragment */
+				if (_rtw_queue_empty(pdefrag_q) == false) {
+					/* free current defrag_q */
+					rtw_free_recvframe_queue(pdefrag_q, pfree_recv_queue);
+				}
+			}
+
+
+			/* Then enqueue the 0~(n-1) fragment into the defrag_q */
+
+			/* spin_lock(&pdefrag_q->lock); */
+			phead = get_list_head(pdefrag_q);
+			list_add_tail(&pfhdr->list, phead);
+			/* spin_unlock(&pdefrag_q->lock); */
+
+
+			prtnframe = NULL;
+
+		} else {
+			/* can't find this ta's defrag_queue, so free this recv_frame */
+			rtw_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+
+	}
+
+	if ((ismfrag == 0) && (fragnum != 0)) {
+		/* the last fragment frame */
+		/* enqueue the last fragment */
+		if (pdefrag_q != NULL) {
+			/* spin_lock(&pdefrag_q->lock); */
+			phead = get_list_head(pdefrag_q);
+			list_add_tail(&pfhdr->list, phead);
+			/* spin_unlock(&pdefrag_q->lock); */
+
+			/* call recvframe_defrag to defrag */
+			precv_frame = recvframe_defrag(padapter, pdefrag_q);
+			prtnframe = precv_frame;
+
+		} else {
+			/* can't find this ta's defrag_queue, so free this recv_frame */
+			rtw_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+
+	}
+
+
+	if ((prtnframe != NULL) && (prtnframe->u.hdr.attrib.privacy)) {
+		/* after defrag we must check tkip mic code */
+		if (recvframe_chkmic(padapter,  prtnframe) == _FAIL) {
+			rtw_free_recvframe(prtnframe, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+	}
+
+
+	return prtnframe;
+
+}
+
+static int amsdu_to_msdu(_adapter *padapter, union recv_frame *prframe)
+{
+	int	a_len, padding_len;
+	u16	nSubframe_Length;
+	u8	nr_subframes, i;
+	u8	*pdata;
+	_pkt *sub_pkt, *subframes[MAX_SUBFRAME_COUNT];
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	_queue *pfree_recv_queue = &(precvpriv->free_recv_queue);
+	int	ret = _SUCCESS;
+
+	nr_subframes = 0;
+
+	recvframe_pull(prframe, prframe->u.hdr.attrib.hdrlen);
+
+	if (prframe->u.hdr.attrib.iv_len > 0)
+		recvframe_pull(prframe, prframe->u.hdr.attrib.iv_len);
+
+	a_len = prframe->u.hdr.len;
+
+	pdata = prframe->u.hdr.rx_data;
+
+	while (a_len > ETH_HLEN) {
+
+		/* Offset 12 denote 2 mac address */
+		nSubframe_Length = RTW_GET_BE16(pdata + 12);
+
+		if (a_len < (ETHERNET_HEADER_SIZE + nSubframe_Length)) {
+			RTW_INFO("nRemain_Length is %d and nSubframe_Length is : %d\n", a_len, nSubframe_Length);
+			break;
+		}
+
+		sub_pkt = rtw_os_alloc_msdu_pkt(prframe, nSubframe_Length, pdata);
+		if (sub_pkt == NULL) {
+			RTW_INFO("%s(): allocate sub packet fail !!!\n", __func__);
+			break;
+		}
+
+		/* move the data point to data content */
+		pdata += ETH_HLEN;
+		a_len -= ETH_HLEN;
+
+		subframes[nr_subframes++] = sub_pkt;
+
+		if (nr_subframes >= MAX_SUBFRAME_COUNT) {
+			RTW_INFO("ParseSubframe(): Too many Subframes! Packets dropped!\n");
+			break;
+		}
+
+		pdata += nSubframe_Length;
+		a_len -= nSubframe_Length;
+		if (a_len != 0) {
+			padding_len = 4 - ((nSubframe_Length + ETH_HLEN) & (4 - 1));
+			if (padding_len == 4)
+				padding_len = 0;
+
+			if (a_len < padding_len) {
+				RTW_INFO("ParseSubframe(): a_len < padding_len !\n");
+				break;
+			}
+			pdata += padding_len;
+			a_len -= padding_len;
+		}
+	}
+
+	for (i = 0; i < nr_subframes; i++) {
+		sub_pkt = subframes[i];
+
+		/* Indicat the packets to upper layer */
+		if (sub_pkt)
+			rtw_os_recv_indicate_pkt(padapter, sub_pkt, &prframe->u.hdr.attrib);
+	}
+
+	prframe->u.hdr.len = 0;
+	rtw_free_recvframe(prframe, pfree_recv_queue);/* free this recv_frame */
+
+	return ret;
+}
+
+static int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, __le16 seq_num)
+{
+	PADAPTER padapter = preorder_ctrl->padapter;
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	u8	wsize = preorder_ctrl->wsize_b;
+	u16	wend = (preorder_ctrl->indicate_seq + wsize - 1) & 0xFFF; /* % 4096; */
+
+	/* Rx Reorder initialize condition. */
+	if (preorder_ctrl->indicate_seq == 0xFFFF) {
+		preorder_ctrl->indicate_seq = le16_to_cpu(seq_num);
+#ifdef DBG_RX_SEQ
+		RTW_INFO("DBG_RX_SEQ %s:%d init IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+			 preorder_ctrl->indicate_seq, seq_num);
+#endif
+	}
+	/* Drop out the packet which SeqNum is smaller than WinStart */
+	if (SN_LESS(le16_to_cpu(seq_num), preorder_ctrl->indicate_seq)) {
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("%s IndicateSeq: %d > NewSeq: %d\n", __func__,
+			 preorder_ctrl->indicate_seq, seq_num);
+#endif
+		return false;
+	}
+
+	/*  */
+	/* Sliding window manipulation. Conditions includes: */
+	/* 1. Incoming SeqNum is equal to WinStart =>Window shift 1 */
+	/* 2. Incoming SeqNum is larger than the WinEnd => Window shift N */
+	/*  */
+	if (SN_EQUAL(le16_to_cpu(seq_num), preorder_ctrl->indicate_seq)) {
+		preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+
+#ifdef DBG_RX_SEQ
+		RTW_INFO("DBG_RX_SEQ %s:%d SN_EQUAL IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+			 preorder_ctrl->indicate_seq, seq_num);
+#endif
+	} else if (SN_LESS(wend, le16_to_cpu(seq_num))) {
+		/* DbgPrint("CheckRxTsIndicateSeq(): Window Shift! IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */
+
+		/* boundary situation, when seq_num cross 0xFFF */
+		if (le16_to_cpu(seq_num) >= (wsize - 1))
+			preorder_ctrl->indicate_seq = le16_to_cpu(seq_num) + 1 - wsize;
+		else
+			preorder_ctrl->indicate_seq = 0xFFF - (wsize - (le16_to_cpu(seq_num) + 1)) + 1;
+		pdbgpriv->dbg_rx_ampdu_window_shift_cnt++;
+#ifdef DBG_RX_SEQ
+		RTW_INFO("DBG_RX_SEQ %s:%d SN_LESS(wend, seq_num) IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+			 preorder_ctrl->indicate_seq, le16_to_cpu(seq_num));
+#endif
+	}
+
+	/* DbgPrint("exit->check_indicate_seq(): IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */
+
+	return true;
+}
+
+static int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, union recv_frame *prframe)
+{
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	_queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+	_list	*phead, *plist;
+	union recv_frame *pnextrframe;
+	struct rx_pkt_attrib *pnextattrib;
+
+	/* DbgPrint("+enqueue_reorder_recvframe()\n"); */
+
+	/* _enter_critical_ex(&ppending_recvframe_queue->lock, &irql); */
+	/* spin_lock(&ppending_recvframe_queue->lock); */
+
+
+	phead = get_list_head(ppending_recvframe_queue);
+	plist = get_next(phead);
+
+	while (rtw_end_of_queue_search(phead, plist) == false) {
+		pnextrframe = LIST_CONTAINOR(plist, union recv_frame, u);
+		pnextattrib = &pnextrframe->u.hdr.attrib;
+
+		if (SN_LESS(le16_to_cpu(pnextattrib->seq_num), le16_to_cpu(pattrib->seq_num)))
+			plist = get_next(plist);
+		else if (SN_EQUAL(pnextattrib->seq_num, pattrib->seq_num)) {
+			/* Duplicate entry is found!! Do not insert current entry. */
+
+			/* _exit_critical_ex(&ppending_recvframe_queue->lock, &irql); */
+
+			return false;
+		} else
+			break;
+
+		/* DbgPrint("enqueue_reorder_recvframe():while\n"); */
+
+	}
+
+
+	/* _enter_critical_ex(&ppending_recvframe_queue->lock, &irql); */
+	/* spin_lock(&ppending_recvframe_queue->lock); */
+
+	list_del_init(&(prframe->u.hdr.list));
+
+	list_add_tail(&(prframe->u.hdr.list), plist);
+
+	/* spin_unlock(&ppending_recvframe_queue->lock); */
+	/* _exit_critical_ex(&ppending_recvframe_queue->lock, &irql); */
+
+
+	return true;
+
+}
+
+void recv_indicatepkts_pkt_loss_cnt(struct debug_priv *pdbgpriv, u64 prev_seq, u64 current_seq);
+void recv_indicatepkts_pkt_loss_cnt(struct debug_priv *pdbgpriv, u64 prev_seq, u64 current_seq)
+{
+	if (current_seq < prev_seq)
+		pdbgpriv->dbg_rx_ampdu_loss_count += (4096 + current_seq - prev_seq);
+
+	else
+		pdbgpriv->dbg_rx_ampdu_loss_count += (current_seq - prev_seq);
+}
+int recv_indicatepkts_in_order(_adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced);
+int recv_indicatepkts_in_order(_adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced)
+{
+	/* unsigned long irql; */
+	/* u8 bcancelled; */
+	_list	*phead, *plist;
+	union recv_frame *prframe;
+	struct rx_pkt_attrib *pattrib;
+	/* u8 index = 0; */
+	int bPktInBuf = false;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	_queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate_in_oder);
+
+	/* DbgPrint("+recv_indicatepkts_in_order\n"); */
+
+	/* _enter_critical_ex(&ppending_recvframe_queue->lock, &irql); */
+	/* spin_lock(&ppending_recvframe_queue->lock); */
+
+	phead =	get_list_head(ppending_recvframe_queue);
+	plist = get_next(phead);
+
+	/* Handling some condition for forced indicate case. */
+	if (bforced) {
+		pdbgpriv->dbg_rx_ampdu_forced_indicate_count++;
+		if (list_empty(phead)) {
+			/* _exit_critical_ex(&ppending_recvframe_queue->lock, &irql); */
+			/* spin_unlock(&ppending_recvframe_queue->lock); */
+			return true;
+		}
+
+		prframe = LIST_CONTAINOR(plist, union recv_frame, u);
+		pattrib = &prframe->u.hdr.attrib;
+
+#ifdef DBG_RX_SEQ
+		RTW_INFO("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+			 preorder_ctrl->indicate_seq, pattrib->seq_num);
+#endif
+		recv_indicatepkts_pkt_loss_cnt(pdbgpriv, preorder_ctrl->indicate_seq, le16_to_cpu(pattrib->seq_num));
+		preorder_ctrl->indicate_seq = le16_to_cpu(pattrib->seq_num);
+
+	}
+
+	/* Prepare indication list and indication. */
+	/* Check if there is any packet need indicate. */
+	while (!list_empty(phead)) {
+
+		prframe = LIST_CONTAINOR(plist, union recv_frame, u);
+		pattrib = &prframe->u.hdr.attrib;
+
+		if (!SN_LESS(preorder_ctrl->indicate_seq, le16_to_cpu(pattrib->seq_num))) {
+			plist = get_next(plist);
+			list_del_init(&(prframe->u.hdr.list));
+
+			if (SN_EQUAL(preorder_ctrl->indicate_seq, le16_to_cpu(pattrib->seq_num))) {
+				preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+#ifdef DBG_RX_SEQ
+				RTW_INFO("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+					preorder_ctrl->indicate_seq, pattrib->seq_num);
+#endif
+			}
+
+			/* indicate this recv_frame */
+			if (!pattrib->amsdu) {
+				if (!RTW_CANNOT_RUN(padapter))
+					rtw_recv_indicatepkt(padapter, prframe);/*indicate this recv_frame*/
+
+			} else if (pattrib->amsdu == 1) {
+				if (amsdu_to_msdu(padapter, prframe) != _SUCCESS)
+					rtw_free_recvframe(prframe, &precvpriv->free_recv_queue);
+			} else {
+				/* error condition; */
+			}
+
+
+			/* Update local variables. */
+			bPktInBuf = false;
+
+		} else {
+			bPktInBuf = true;
+			break;
+		}
+
+		/* DbgPrint("recv_indicatepkts_in_order():while\n"); */
+
+	}
+
+	return bPktInBuf;
+}
+
+static int recv_indicatepkt_reorder(_adapter *padapter, union recv_frame *prframe)
+{
+	unsigned long irql;
+	int retval = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct recv_reorder_ctrl *preorder_ctrl = prframe->u.hdr.preorder_ctrl;
+	_queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate_reoder);
+
+	if (!pattrib->amsdu) {
+		/* s1. */
+		retval = wlanhdr_to_ethhdr(prframe);
+		if (retval != _SUCCESS) {
+#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME %s wlanhdr_to_ethhdr error!\n", __func__);
+#endif
+			return retval;
+		}
+
+		/* if ((pattrib->qos!=1) || pattrib->priority!=0 || IS_MCAST(pattrib->ra) */
+		/*	|| (pattrib->eth_type==0x0806) || (pattrib->ack_policy!=0)) */
+		if (pattrib->qos != 1) {
+			if (!RTW_CANNOT_RUN(padapter)) {
+
+				rtw_recv_indicatepkt(padapter, prframe);
+				return _SUCCESS;
+
+			}
+
+#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME %s pattrib->qos !=1\n", __func__);
+#endif
+
+			return _FAIL;
+
+		}
+
+		if (preorder_ctrl->enable == false) {
+			/* indicate this recv_frame			 */
+			preorder_ctrl->indicate_seq = le16_to_cpu(pattrib->seq_num);
+#ifdef DBG_RX_SEQ
+			RTW_INFO("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+				 preorder_ctrl->indicate_seq, pattrib->seq_num);
+#endif
+
+			rtw_recv_indicatepkt(padapter, prframe);
+
+			preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) % 4096;
+#ifdef DBG_RX_SEQ
+			RTW_INFO("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+				 preorder_ctrl->indicate_seq, pattrib->seq_num);
+#endif
+
+			return _SUCCESS;
+		}
+
+#ifndef CONFIG_RECV_REORDERING_CTRL
+		/* indicate this recv_frame */
+		rtw_recv_indicatepkt(padapter, prframe);
+		return _SUCCESS;
+#endif
+
+	} else if (pattrib->amsdu == 1) { /* temp filter->means didn't support A-MSDUs in a A-MPDU */
+		if (preorder_ctrl->enable == false) {
+			preorder_ctrl->indicate_seq = le16_to_cpu(pattrib->seq_num);
+#ifdef DBG_RX_SEQ
+			RTW_INFO("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+				 preorder_ctrl->indicate_seq, pattrib->seq_num);
+#endif
+
+			retval = amsdu_to_msdu(padapter, prframe);
+
+			preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) % 4096;
+#ifdef DBG_RX_SEQ
+			RTW_INFO("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+				 preorder_ctrl->indicate_seq, pattrib->seq_num);
+#endif
+
+			if (retval != _SUCCESS) {
+#ifdef DBG_RX_DROP_FRAME
+				RTW_INFO("DBG_RX_DROP_FRAME %s amsdu_to_msdu fail\n", __func__);
+#endif
+			}
+
+			return retval;
+		}
+	} else {
+
+	}
+
+	_enter_critical_bh(&ppending_recvframe_queue->lock, &irql);
+
+
+	/* s2. check if winstart_b(indicate_seq) needs to been updated */
+	if (!check_indicate_seq(preorder_ctrl, pattrib->seq_num)) {
+		pdbgpriv->dbg_rx_ampdu_drop_count++;
+
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME %s check_indicate_seq fail\n", __func__);
+#endif
+		goto _err_exit;
+	}
+
+
+	/* s3. Insert all packet into Reorder Queue to maintain its ordering. */
+	if (!enqueue_reorder_recvframe(preorder_ctrl, prframe)) {
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME %s enqueue_reorder_recvframe fail\n", __func__);
+#endif
+		goto _err_exit;
+	}
+
+
+	/* s4. */
+	/* Indication process. */
+	/* After Packet dropping and Sliding Window shifting as above, we can now just indicate the packets */
+	/* with the SeqNum smaller than latest WinStart and buffer other packets. */
+	/*  */
+	/* For Rx Reorder condition: */
+	/* 1. All packets with SeqNum smaller than WinStart => Indicate */
+	/* 2. All packets with SeqNum larger than or equal to WinStart => Buffer it. */
+	/*  */
+
+	/* recv_indicatepkts_in_order(padapter, preorder_ctrl, true); */
+	if (recv_indicatepkts_in_order(padapter, preorder_ctrl, false)) {
+		if (!preorder_ctrl->bReorderWaiting) {
+			preorder_ctrl->bReorderWaiting = true;
+			_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+		}
+		_exit_critical_bh(&ppending_recvframe_queue->lock, &irql);
+	} else {
+		preorder_ctrl->bReorderWaiting = false;
+		_exit_critical_bh(&ppending_recvframe_queue->lock, &irql);
+		_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+	}
+
+
+_success_exit:
+
+	return _SUCCESS;
+
+_err_exit:
+
+	_exit_critical_bh(&ppending_recvframe_queue->lock, &irql);
+
+	return _FAIL;
+}
+
+
+void rtw_reordering_ctrl_timeout_handler(void *pcontext)
+{
+	unsigned long irql;
+	struct recv_reorder_ctrl *preorder_ctrl = (struct recv_reorder_ctrl *)pcontext;
+	_adapter *padapter = preorder_ctrl->padapter;
+	_queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+
+	if (RTW_CANNOT_RUN(padapter))
+		return;
+
+	/* RTW_INFO("+rtw_reordering_ctrl_timeout_handler()=>\n"); */
+
+	_enter_critical_bh(&ppending_recvframe_queue->lock, &irql);
+
+	if (preorder_ctrl)
+		preorder_ctrl->bReorderWaiting = false;
+
+	if (recv_indicatepkts_in_order(padapter, preorder_ctrl, true))
+		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+
+	_exit_critical_bh(&ppending_recvframe_queue->lock, &irql);
+
+}
+
+int process_recv_indicatepkts(_adapter *padapter, union recv_frame *prframe);
+int process_recv_indicatepkts(_adapter *padapter, union recv_frame *prframe)
+{
+	int retval = _SUCCESS;
+	/* struct recv_priv *precvpriv = &padapter->recvpriv; */
+	/* struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib; */
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+#ifdef CONFIG_TDLS
+	struct sta_info *psta = prframe->u.hdr.psta;
+#endif /* CONFIG_TDLS */
+	struct ht_priv	*phtpriv = &pmlmepriv->htpriv;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate);
+
+#ifdef CONFIG_TDLS
+	if ((phtpriv->ht_option) ||
+	    ((psta->tdls_sta_state & TDLS_LINKED_STATE) &&
+	     (psta->htpriv.ht_option) &&
+	     (psta->htpriv.ampdu_enable))) /* B/G/N Mode */
+#else
+	if (phtpriv->ht_option) /* B/G/N Mode */
+#endif /* CONFIG_TDLS */
+	{
+		/* prframe->u.hdr.preorder_ctrl = &precvpriv->recvreorder_ctrl[pattrib->priority]; */
+
+		if (recv_indicatepkt_reorder(padapter, prframe) != _SUCCESS) { /* including perform A-MPDU Rx Ordering Buffer Control */
+#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME %s recv_indicatepkt_reorder error!\n", __func__);
+#endif
+
+			if (!RTW_CANNOT_RUN(padapter))	{
+				retval = _FAIL;
+				return retval;
+			}
+		}
+	} else /* B/G mode */
+	{
+		retval = wlanhdr_to_ethhdr(prframe);
+		if (retval != _SUCCESS) {
+#ifdef DBG_RX_DROP_FRAME
+			RTW_INFO("DBG_RX_DROP_FRAME %s wlanhdr_to_ethhdr error!\n", __func__);
+#endif
+			return retval;
+		}
+
+		if (!RTW_CANNOT_RUN(padapter)) {
+			/* indicate this recv_frame */
+			rtw_recv_indicatepkt(padapter, prframe);
+		} else {
+
+			retval = _FAIL;
+			return retval;
+		}
+
+	}
+
+	return retval;
+
+}
+
+#ifdef CONFIG_MP_INCLUDED
+static int validate_mp_recv_frame(_adapter *adapter, union recv_frame *precv_frame)
+{
+	int ret = _SUCCESS;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	u8 type, subtype;
+	struct mp_priv *pmppriv = &adapter->mppriv;
+	struct mp_tx		*pmptx;
+	unsigned char	*sa , *da, *bs;
+
+	pmptx = &pmppriv->tx;
+
+	if (pmppriv->bloopback) {
+		if (!memcmp(ptr + 24, pmptx->buf + 24, precv_frame->u.hdr.len - 24) == false) {
+			RTW_INFO("Compare payload content Fail !!!\n");
+			ret = _FAIL;
+		}
+	}
+ 	if (pmppriv->bSetRxBssid) {
+
+		sa = get_addr2_ptr(ptr);
+		da = GetAddr1Ptr(ptr);
+		bs = GetAddr3Ptr(ptr);
+		type =	GetFrameType(ptr);
+		subtype = get_frame_sub_type(ptr); /* bit(7)~bit(2)  */
+
+		if (!memcmp(bs, adapter->mppriv.network_macaddr, ETH_ALEN) == false)
+			ret = _FAIL;
+
+		RTW_DBG("############ type:0x%02x subtype:0x%02x #################\n", type, subtype);
+		RTW_DBG("A2 sa %02X:%02X:%02X:%02X:%02X:%02X \n", *(sa) , *(sa + 1), *(sa+ 2), *(sa + 3), *(sa + 4), *(sa + 5));
+		RTW_DBG("A1 da %02X:%02X:%02X:%02X:%02X:%02X \n", *(da) , *(da + 1), *(da+ 2), *(da + 3), *(da + 4), *(da + 5));
+		RTW_DBG("A3 bs %02X:%02X:%02X:%02X:%02X:%02X \n --------------------------\n", *(bs) , *(bs + 1), *(bs+ 2), *(bs + 3), *(bs + 4), *(bs + 5));
+	}
+
+	if (!adapter->mppriv.bmac_filter)
+		return ret;
+
+	if (!memcmp(get_addr2_ptr(ptr), adapter->mppriv.mac_filter, ETH_ALEN) == false)
+		ret = _FAIL;
+
+	return ret;
+}
+
+static sint MPwlanhdr_to_ethhdr(union recv_frame *precvframe)
+{
+	sint	rmv_len;
+	u16 eth_type, len;
+	u8	bsnaphdr;
+	u8	*psnap_type;
+	u8 mcastheadermac[] = {0x01, 0x00, 0x5e};
+	__be16 be_tmp;
+	__le16 le_tmp;
+	struct ieee80211_snap_hdr	*psnap;
+
+	sint ret = _SUCCESS;
+	_adapter			*adapter = precvframe->u.hdr.adapter;
+	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+
+	u8	*ptr = get_recvframe_data(precvframe) ; /* point to frame_ctrl field */
+	struct rx_pkt_attrib *pattrib = &precvframe->u.hdr.attrib;
+
+
+	if (pattrib->encrypt)
+		recvframe_pull_tail(precvframe, pattrib->icv_len);
+
+	psnap = (struct ieee80211_snap_hdr *)(ptr + pattrib->hdrlen + pattrib->iv_len);
+	psnap_type = ptr + pattrib->hdrlen + pattrib->iv_len + SNAP_SIZE;
+	/* convert hdr + possible LLC headers into Ethernet header */
+	/* eth_type = (psnap_type[0] << 8) | psnap_type[1]; */
+	if ((!memcmp(psnap, rtw_rfc1042_header, SNAP_SIZE) &&
+	     (!memcmp(psnap_type, SNAP_ETH_TYPE_IPX, 2) == false) &&
+	     (!memcmp(psnap_type, SNAP_ETH_TYPE_APPLETALK_AARP, 2) == false)) ||
+	    /* eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) || */
+	    !memcmp(psnap, rtw_bridge_tunnel_header, SNAP_SIZE)) {
+		/* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
+		bsnaphdr = true;
+	} else {
+		/* Leave Ethernet header part of hdr and full payload */
+		bsnaphdr = false;
+	}
+
+	rmv_len = pattrib->hdrlen + pattrib->iv_len + (bsnaphdr ? SNAP_SIZE : 0);
+	len = precvframe->u.hdr.len - rmv_len;
+
+	memcpy(&be_tmp, ptr + rmv_len, 2);
+	eth_type = ntohs((__be16)be_tmp); /* pattrib->ether_type */
+	pattrib->eth_type = cpu_to_le16(eth_type);
+
+	ptr = recvframe_pull(precvframe, (rmv_len - sizeof(struct ethhdr) + (bsnaphdr ? 2 : 0)));
+
+	memcpy(ptr, pattrib->dst, ETH_ALEN);
+	memcpy(ptr + ETH_ALEN, pattrib->src, ETH_ALEN);
+
+	if (!bsnaphdr) {
+		be_tmp = htons(len);
+		memcpy(ptr + 12, &be_tmp, 2);
+	}
+
+
+	be_tmp = htons(le16_to_cpu(pattrib->seq_num));
+	/* RTW_INFO("wlan seq = %d ,seq_num =%x\n",len,pattrib->seq_num); */
+	memcpy(ptr + 12, &be_tmp, 2);
+	if (adapter->mppriv.bRTWSmbCfg) {
+		/* if(!memcmp(mcastheadermac, pattrib->dst, 3)) */ /* SimpleConfig Dest. */
+		/*			memcpy(ptr+ETH_ALEN, pattrib->bssid, ETH_ALEN); */
+
+		if (!memcmp(mcastheadermac, pattrib->bssid, 3)) /* SimpleConfig Dest. */
+			memcpy(ptr, pattrib->bssid, ETH_ALEN);
+
+	}
+
+
+	return ret;
+
+}
+
+
+static int mp_recv_frame(_adapter *padapter, union recv_frame *rframe)
+{
+	int ret = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &rframe->u.hdr.attrib;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+#ifdef CONFIG_MP_INCLUDED
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mp_priv *pmppriv = &padapter->mppriv;
+#endif /* CONFIG_MP_INCLUDED */
+	u8 type;
+	u8 *ptr = rframe->u.hdr.rx_data;
+	u8 *psa, *pda, *pbssid;
+	struct sta_info *psta = NULL;
+	DBG_COUNTER(padapter->rx_logs.core_rx_pre);
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED))) { /* &&(padapter->mppriv.check_mp_pkt == 0)) */
+		if (pattrib->crc_err == 1)
+			padapter->mppriv.rx_crcerrpktcount++;
+		else {
+			if (_SUCCESS == validate_mp_recv_frame(padapter, rframe))
+				padapter->mppriv.rx_pktcount++;
+			else
+				padapter->mppriv.rx_pktcount_filter_out++;
+		}
+
+		if (pmppriv->rx_bindicatePkt == false) {
+			ret = _FAIL;
+			rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+			goto exit;
+		} else {
+			type =	GetFrameType(ptr);
+			pattrib->to_fr_ds = get_tofr_ds(ptr);
+			pattrib->frag_num = GetFragNum(ptr);
+			pattrib->seq_num = cpu_to_le16(GetSequence(ptr));
+			pattrib->pw_save = GetPwrMgt(ptr);
+			pattrib->mfrag = GetMFrag(ptr);
+			pattrib->mdata = GetMData(ptr);
+			pattrib->privacy = GetPrivacy(ptr);
+			pattrib->order = GetOrder(ptr);
+
+			if (type == WIFI_DATA_TYPE) {
+				pda = get_da(ptr);
+				psa = get_sa(ptr);
+				pbssid = get_hdr_bssid(ptr);
+
+				memcpy(pattrib->dst, pda, ETH_ALEN);
+				memcpy(pattrib->src, psa, ETH_ALEN);
+				memcpy(pattrib->bssid, pbssid, ETH_ALEN);
+
+				switch (pattrib->to_fr_ds) {
+				case 0:
+					memcpy(pattrib->ra, pda, ETH_ALEN);
+					memcpy(pattrib->ta, psa, ETH_ALEN);
+					ret = sta2sta_data_frame(padapter, rframe, &psta);
+					break;
+
+				case 1:
+
+					memcpy(pattrib->ra, pda, ETH_ALEN);
+					memcpy(pattrib->ta, pbssid, ETH_ALEN);
+					ret = ap2sta_data_frame(padapter, rframe, &psta);
+
+					break;
+
+				case 2:
+					memcpy(pattrib->ra, pbssid, ETH_ALEN);
+					memcpy(pattrib->ta, psa, ETH_ALEN);
+					ret = sta2ap_data_frame(padapter, rframe, &psta);
+					break;
+
+				case 3:
+					memcpy(pattrib->ra, GetAddr1Ptr(ptr), ETH_ALEN);
+					memcpy(pattrib->ta, get_addr2_ptr(ptr), ETH_ALEN);
+					ret = _FAIL;
+					break;
+
+				default:
+					ret = _FAIL;
+					break;
+				}
+
+				ret = MPwlanhdr_to_ethhdr(rframe);
+
+				if (ret != _SUCCESS) {
+#ifdef DBG_RX_DROP_FRAME
+					RTW_INFO("DBG_RX_DROP_FRAME %s wlanhdr_to_ethhdr: drop pkt\n", __func__);
+#endif
+					rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+					ret = _FAIL;
+					goto exit;
+				}
+				if (!RTW_CANNOT_RUN(padapter)) {
+					/* indicate this recv_frame */
+					ret = rtw_recv_indicatepkt(padapter, rframe);
+					if (ret != _SUCCESS) {
+#ifdef DBG_RX_DROP_FRAME
+						RTW_INFO("DBG_RX_DROP_FRAME %s rtw_recv_indicatepkt fail!\n", __func__);
+#endif
+						rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+						ret = _FAIL;
+
+						goto exit;
+					}
+				} else {
+#ifdef DBG_RX_DROP_FRAME
+					RTW_INFO("DBG_RX_DROP_FRAME %s ecv_func:bDriverStopped(%s) OR bSurpriseRemoved(%s)\n", __func__,
+						rtw_is_drv_stopped(padapter) ? "True" : "False",
+						rtw_is_surprise_removed(padapter) ? "True" : "False");
+#endif
+					ret = _FAIL;
+					rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+					goto exit;
+				}
+
+			}
+		}
+
+	}
+
+	rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+	ret = _FAIL;
+
+exit:
+	return ret;
+
+}
+#endif
+
+static sint fill_radiotap_hdr(_adapter *padapter, union recv_frame *precvframe, u8 *buf)
+{
+#define CHAN2FREQ(a) ((a < 14) ? (2407+5*a) : (5000+5*a))
+
+#ifndef IEEE80211_RADIOTAP_RX_FLAGS
+#define IEEE80211_RADIOTAP_RX_FLAGS 14
+#endif
+
+#ifndef IEEE80211_RADIOTAP_MCS
+#define IEEE80211_RADIOTAP_MCS 19
+#endif
+#ifndef IEEE80211_RADIOTAP_VHT
+#define IEEE80211_RADIOTAP_VHT 21
+#endif
+
+#ifndef IEEE80211_RADIOTAP_F_BADFCS
+#define IEEE80211_RADIOTAP_F_BADFCS 0x40 /* bad FCS */
+#endif
+
+	sint ret = _SUCCESS;
+	_adapter			*adapter = precvframe->u.hdr.adapter;
+	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
+	struct rx_pkt_attrib *pattrib = &precvframe->u.hdr.attrib;
+
+	HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter);
+
+	__le16 tmp_16bit = 0;
+
+	u8 data_rate[] = {
+		2, 4, 11, 22, /* CCK */
+		12, 18, 24, 36, 48, 72, 93, 108, /* OFDM */
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, /* HT MCS index */
+		16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31,
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, /* VHT Nss 1 */
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, /* VHT Nss 2 */
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, /* VHT Nss 3 */
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, /* VHT Nss 4 */
+	};
+
+	_pkt *pskb = NULL;
+
+	struct ieee80211_radiotap_header *rtap_hdr = NULL;
+	u8 *ptr = NULL;
+
+	u8 hdr_buf[64] = {0};
+	u16 rt_len = 8;
+
+	/* create header */
+	rtap_hdr = (struct ieee80211_radiotap_header *)&hdr_buf[0];
+	rtap_hdr->it_version = PKTHDR_RADIOTAP_VERSION;
+
+	/* tsft */
+	if (pattrib->tsfl) {
+		rtap_hdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_TSFT);
+		memcpy(&hdr_buf[rt_len], &pattrib->tsfl, 8);
+		rt_len += 8;
+	}
+
+	/* flags */
+	rtap_hdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_FLAGS);
+	if (0)
+		hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_CFP;
+
+	if (0)
+		hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_SHORTPRE;
+
+	if ((pattrib->encrypt == 1) || (pattrib->encrypt == 5))
+		hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_WEP;
+
+	if (pattrib->mfrag)
+		hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_FRAG;
+
+	/* always append FCS */
+	hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_FCS;
+
+
+	if (0)
+		hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_DATAPAD;
+
+	if (pattrib->crc_err)
+		hdr_buf[rt_len] |= IEEE80211_RADIOTAP_F_BADFCS;
+
+	if (pattrib->sgi) {
+		/* Currently unspecified but used */
+		hdr_buf[rt_len] |= 0x80;
+	}
+	rt_len += 1;
+
+	/* rate */
+	if (pattrib->data_rate < 12) {
+		rtap_hdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_RATE);
+		if (pattrib->data_rate < 4) {
+			/* CCK */
+			hdr_buf[rt_len] = data_rate[pattrib->data_rate];
+		} else {
+			/* OFDM */
+			hdr_buf[rt_len] = data_rate[pattrib->data_rate];
+		}
+	}
+	rt_len += 1; /* force padding 1 byte for aligned */
+
+	/* channel */
+	tmp_16bit = 0;
+	rtap_hdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_CHANNEL);
+	tmp_16bit = cpu_to_le16(CHAN2FREQ(rtw_get_oper_ch(padapter)));
+	/*tmp_16bit = CHAN2FREQ(pHalData->current_channel);*/
+	memcpy(&hdr_buf[rt_len], &tmp_16bit, 2);
+	rt_len += 2;
+
+	/* channel flags */
+	tmp_16bit = 0;
+	if (pHalData->current_band_type == 0)
+		tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_2GHZ);
+	else
+		tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_5GHZ);
+
+	if (pattrib->data_rate < 12) {
+		if (pattrib->data_rate < 4) {
+			/* CCK */
+			tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_CCK);
+		} else {
+			/* OFDM */
+			tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_OFDM);
+		}
+	} else
+		tmp_16bit |= cpu_to_le16(IEEE80211_CHAN_DYN);
+	memcpy(&hdr_buf[rt_len], &tmp_16bit, 2);
+	rt_len += 2;
+
+	/* dBm Antenna Signal */
+	rtap_hdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_DBM_ANTSIGNAL);
+	hdr_buf[rt_len] = pattrib->phy_info.RecvSignalPower;
+	rt_len += 1;
+
+	/* Antenna */
+	rtap_hdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_ANTENNA);
+	hdr_buf[rt_len] = 0; /* pHalData->rf_type; */
+	rt_len += 1;
+
+	/* RX flags */
+	rtap_hdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_RX_FLAGS);
+	rt_len += 2;
+
+	/* MCS information */
+	if (pattrib->data_rate >= 12 && pattrib->data_rate < 44) {
+		rtap_hdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_MCS);
+		/* known, flag */
+		hdr_buf[rt_len] |= BIT1; /* MCS index known */
+
+		/* bandwidth */
+		hdr_buf[rt_len] |= BIT0;
+		hdr_buf[rt_len + 1] |= (pattrib->bw & 0x03);
+
+		/* guard interval */
+		hdr_buf[rt_len] |= BIT2;
+		hdr_buf[rt_len + 1] |= (pattrib->sgi & 0x01) << 2;
+
+		/* STBC */
+		hdr_buf[rt_len] |= BIT5;
+		hdr_buf[rt_len + 1] |= (pattrib->stbc & 0x03) << 5;
+
+		rt_len += 2;
+
+		/* MCS rate index */
+		hdr_buf[rt_len] = data_rate[pattrib->data_rate];
+		rt_len += 1;
+	}
+
+	/* VHT */
+	if (pattrib->data_rate >= 44 && pattrib->data_rate < 84) {
+		rtap_hdr->it_present |= cpu_to_le32((1 << IEEE80211_RADIOTAP_VHT));
+
+		/* known 16 bit, flag 8 bit */
+		tmp_16bit = 0;
+
+		/* Bandwidth */
+		tmp_16bit |= cpu_to_le16(BIT6);
+
+		/* Group ID */
+		tmp_16bit |= cpu_to_le16(BIT7);
+
+		/* Partial AID */
+		tmp_16bit |= cpu_to_le16(BIT8);
+
+		/* STBC */
+		tmp_16bit |= cpu_to_le16(BIT0);
+		hdr_buf[rt_len + 2] |= (pattrib->stbc & 0x01);
+
+		/* Guard interval */
+		tmp_16bit |= cpu_to_le16(BIT2);
+		hdr_buf[rt_len + 2] |= (pattrib->sgi & 0x01) << 2;
+
+		/* LDPC extra OFDM symbol */
+		tmp_16bit |= cpu_to_le16(BIT4);
+		hdr_buf[rt_len + 2] |= (pattrib->ldpc & 0x01) << 4;
+
+		memcpy(&hdr_buf[rt_len], &tmp_16bit, 2);
+		rt_len += 3;
+
+		/* bandwidth */
+		if (pattrib->bw == 0)
+			hdr_buf[rt_len] |= 0;
+		else if (pattrib->bw == 1)
+			hdr_buf[rt_len] |= 1;
+		else if (pattrib->bw == 2)
+			hdr_buf[rt_len] |= 4;
+		else if (pattrib->bw == 3)
+			hdr_buf[rt_len] |= 11;
+		rt_len += 1;
+
+		/* mcs_nss */
+		if (pattrib->data_rate >= 44 && pattrib->data_rate < 54) {
+			hdr_buf[rt_len] |= 1;
+			hdr_buf[rt_len] |= data_rate[pattrib->data_rate] << 4;
+		} else if (pattrib->data_rate >= 54 && pattrib->data_rate < 64) {
+			hdr_buf[rt_len + 1] |= 2;
+			hdr_buf[rt_len + 1] |= data_rate[pattrib->data_rate] << 4;
+		} else if (pattrib->data_rate >= 64 && pattrib->data_rate < 74) {
+			hdr_buf[rt_len + 2] |= 3;
+			hdr_buf[rt_len + 2] |= data_rate[pattrib->data_rate] << 4;
+		} else if (pattrib->data_rate >= 74 && pattrib->data_rate < 84) {
+			hdr_buf[rt_len + 3] |= 4;
+			hdr_buf[rt_len + 3] |= data_rate[pattrib->data_rate] << 4;
+		}
+		rt_len += 4;
+
+		/* coding */
+		hdr_buf[rt_len] = 0;
+		rt_len += 1;
+
+		/* group_id */
+		hdr_buf[rt_len] = 0;
+		rt_len += 1;
+
+		/* partial_aid */
+		tmp_16bit = 0;
+		memcpy(&hdr_buf[rt_len], &tmp_16bit, 2);
+		rt_len += 2;
+	}
+
+	/* push to skb */
+	pskb = (_pkt *)buf;
+	if (skb_headroom(pskb) < rt_len) {
+		RTW_INFO("%s:%d %s headroom is too small.\n", __FILE__, __LINE__, __func__);
+		ret = _FAIL;
+		return ret;
+	}
+
+	ptr = skb_push(pskb, rt_len);
+	if (ptr) {
+		rtap_hdr->it_len = cpu_to_le16(rt_len);
+		memcpy(ptr, rtap_hdr, rt_len);
+	} else
+		ret = _FAIL;
+
+	return ret;
+
+}
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24))
+static int recv_frame_monitor(_adapter *padapter, union recv_frame *rframe)
+{
+	int ret = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &rframe->u.hdr.attrib;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+	_pkt *pskb = NULL;
+
+	/* read skb information from recv frame */
+	pskb = rframe->u.hdr.pkt;
+	pskb->len = rframe->u.hdr.len;
+	pskb->data = rframe->u.hdr.rx_data;
+	skb_set_tail_pointer(pskb, rframe->u.hdr.len);
+
+	/* fill radiotap header */
+	if (fill_radiotap_hdr(padapter, rframe, (u8 *)pskb) == _FAIL) {
+		ret = _FAIL;
+		rtw_free_recvframe(rframe, pfree_recv_queue); /* free this recv_frame */
+		goto exit;
+	}
+
+	/* write skb information to recv frame */
+	skb_reset_mac_header(pskb);
+	rframe->u.hdr.len = pskb->len;
+	rframe->u.hdr.rx_data = pskb->data;
+	rframe->u.hdr.rx_head = pskb->head;
+	rframe->u.hdr.rx_tail = skb_tail_pointer(pskb);
+	rframe->u.hdr.rx_end = skb_end_pointer(pskb);
+
+	if (!RTW_CANNOT_RUN(padapter)) {
+		/* indicate this recv_frame */
+		ret = rtw_recv_monitor(padapter, rframe);
+		if (ret != _SUCCESS) {
+			ret = _FAIL;
+			rtw_free_recvframe(rframe, pfree_recv_queue); /* free this recv_frame */
+			goto exit;
+		}
+	} else {
+		ret = _FAIL;
+		rtw_free_recvframe(rframe, pfree_recv_queue); /* free this recv_frame */
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+#endif
+static int recv_func_prehandle(_adapter *padapter, union recv_frame *rframe)
+{
+	int ret = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &rframe->u.hdr.attrib;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+#ifdef DBG_RX_COUNTER_DUMP
+	if (padapter->dump_rx_cnt_mode & DUMP_DRV_RX_COUNTER) {
+		if (pattrib->crc_err == 1)
+			padapter->drv_rx_cnt_crcerror++;
+		else
+			padapter->drv_rx_cnt_ok++;
+	}
+#endif
+
+#ifdef CONFIG_MP_INCLUDED
+	if (padapter->registrypriv.mp_mode == 1 || padapter->mppriv.bRTWSmbCfg) {
+		mp_recv_frame(padapter, rframe);
+		ret = _FAIL;
+		goto exit;
+	} else
+#endif
+	{
+		/* check the frame crtl field and decache */
+		ret = validate_recv_frame(padapter, rframe);
+		if (ret != _SUCCESS) {
+			rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+			goto exit;
+		}
+	}
+exit:
+	return ret;
+}
+
+/*#define DBG_RX_BMC_FRAME*/
+static int recv_func_posthandle(_adapter *padapter, union recv_frame *prframe)
+{
+	int ret = _SUCCESS;
+	union recv_frame *orig_prframe = prframe;
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+#ifdef CONFIG_TDLS
+	u8 *psnap_type, *pcategory;
+#endif /* CONFIG_TDLS */
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post);
+
+	/* DATA FRAME */
+	rtw_led_control(padapter, LED_CTL_RX);
+
+	prframe = decryptor(padapter, prframe);
+	if (prframe == NULL) {
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME %s decryptor: drop pkt\n", __func__);
+#endif
+		ret = _FAIL;
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_err);
+		goto _recv_data_drop;
+	}
+
+#ifdef DBG_RX_BMC_FRAME
+	if (IS_MCAST(pattrib->ra)) {
+		u8 *pbuf = prframe->u.hdr.rx_data;
+		u8 *sa_addr = get_sa(pbuf);
+
+		RTW_INFO("%s =>"ADPT_FMT" Rx BC/MC from MAC: "MAC_FMT"\n", __func__, ADPT_ARG(padapter), MAC_ARG(sa_addr));
+	}
+#endif
+
+#ifdef CONFIG_TDLS
+	/* check TDLS frame */
+	psnap_type = get_recvframe_data(orig_prframe) + pattrib->hdrlen + pattrib->iv_len + SNAP_SIZE;
+	pcategory = psnap_type + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN;
+
+	if ((!memcmp(psnap_type, SNAP_ETH_TYPE_TDLS, ETH_TYPE_LEN)) &&
+	    ((*pcategory == RTW_WLAN_CATEGORY_TDLS) || (*pcategory == RTW_WLAN_CATEGORY_P2P))) {
+		ret = OnTDLS(padapter, prframe);
+		if (ret == _FAIL)
+			goto _exit_recv_func;
+	}
+#endif /* CONFIG_TDLS */
+
+	prframe = recvframe_chk_defrag(padapter, prframe);
+	if (prframe == NULL)	{
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME %s recvframe_chk_defrag: drop pkt\n", __func__);
+#endif
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_defrag_err);
+		goto _recv_data_drop;
+	}
+
+	prframe = portctrl(padapter, prframe);
+	if (prframe == NULL) {
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME %s portctrl: drop pkt\n", __func__);
+#endif
+		ret = _FAIL;
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_portctrl_err);
+		goto _recv_data_drop;
+	}
+
+	count_rx_stats(padapter, prframe, NULL);
+
+#ifdef CONFIG_WAPI_SUPPORT
+	rtw_wapi_update_info(padapter, prframe);
+#endif
+
+	ret = process_recv_indicatepkts(padapter, prframe);
+	if (ret != _SUCCESS) {
+#ifdef DBG_RX_DROP_FRAME
+		RTW_INFO("DBG_RX_DROP_FRAME %s process_recv_indicatepkts fail!\n", __func__);
+#endif
+		rtw_free_recvframe(orig_prframe, pfree_recv_queue);/* free this recv_frame */
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate_err);
+		goto _recv_data_drop;
+	}
+
+_exit_recv_func:
+	return ret;
+
+_recv_data_drop:
+	precvpriv->rx_drop++;
+	return ret;
+}
+
+
+int recv_func(_adapter *padapter, union recv_frame *rframe);
+int recv_func(_adapter *padapter, union recv_frame *rframe)
+{
+	int ret;
+	struct rx_pkt_attrib *prxattrib = &rframe->u.hdr.attrib;
+	struct recv_priv *recvpriv = &padapter->recvpriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+
+	if (check_fwstate(mlmepriv, WIFI_MONITOR_STATE)) {
+		/* monitor mode */
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24))
+		recv_frame_monitor(padapter, rframe);
+#endif
+		ret = _SUCCESS;
+		goto exit;
+	} else
+
+		/* check if need to handle uc_swdec_pending_queue*/
+		if (check_fwstate(mlmepriv, WIFI_STATION_STATE) && psecuritypriv->busetkipkey) {
+			union recv_frame *pending_frame;
+			int cnt = 0;
+
+			while ((pending_frame = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue))) {
+				cnt++;
+				DBG_COUNTER(padapter->rx_logs.core_rx_dequeue);
+				recv_func_posthandle(padapter, pending_frame);
+			}
+
+			if (cnt)
+				RTW_INFO(FUNC_ADPT_FMT" dequeue %d from uc_swdec_pending_queue\n",
+					 FUNC_ADPT_ARG(padapter), cnt);
+		}
+
+	DBG_COUNTER(padapter->rx_logs.core_rx);
+	ret = recv_func_prehandle(padapter, rframe);
+
+	if (ret == _SUCCESS) {
+
+		/* check if need to enqueue into uc_swdec_pending_queue*/
+		if (check_fwstate(mlmepriv, WIFI_STATION_STATE) &&
+		    !IS_MCAST(prxattrib->ra) && prxattrib->encrypt > 0 &&
+		    (prxattrib->bdecrypted == 0 || psecuritypriv->sw_decrypt) &&
+		    psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPAPSK &&
+		    !psecuritypriv->busetkipkey) {
+			DBG_COUNTER(padapter->rx_logs.core_rx_enqueue);
+			rtw_enqueue_recvframe(rframe, &padapter->recvpriv.uc_swdec_pending_queue);
+			/* RTW_INFO("%s: no key, enqueue uc_swdec_pending_queue\n", __func__); */
+
+			if (recvpriv->free_recvframe_cnt < NR_RECVFRAME / 4) {
+				/* to prevent from recvframe starvation, get recvframe from uc_swdec_pending_queue to free_recvframe_cnt */
+				rframe = rtw_alloc_recvframe(&padapter->recvpriv.uc_swdec_pending_queue);
+				if (rframe)
+					goto do_posthandle;
+			}
+			goto exit;
+		}
+
+do_posthandle:
+		ret = recv_func_posthandle(padapter, rframe);
+	}
+
+exit:
+	return ret;
+}
+
+
+s32 rtw_recv_entry(union recv_frame *precvframe)
+{
+	_adapter *padapter;
+	struct recv_priv *precvpriv;
+	s32 ret = _SUCCESS;
+
+
+
+	padapter = precvframe->u.hdr.adapter;
+
+	precvpriv = &padapter->recvpriv;
+
+
+	ret = recv_func(padapter, precvframe);
+	if (ret == _FAIL) {
+		goto _recv_entry_drop;
+	}
+
+
+	precvpriv->rx_pkts++;
+
+
+	return ret;
+
+_recv_entry_drop:
+
+#ifdef CONFIG_MP_INCLUDED
+	if (padapter->registrypriv.mp_mode == 1)
+		padapter->mppriv.rx_pktloss = precvpriv->rx_drop;
+#endif
+
+
+
+	return ret;
+}
+
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS)
+#else
+void rtw_signal_stat_timer_hdl(struct timer_list *t)
+#endif
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(4, 15, 0)
+	_adapter *adapter = (_adapter *)FunctionContext;
+#else
+	_adapter *adapter = from_timer(adapter, t, recvpriv.signal_stat_timer);
+#endif
+	struct recv_priv *recvpriv = &adapter->recvpriv;
+
+	u32 tmp_s, tmp_q;
+	u8 avg_signal_strength = 0;
+	u8 avg_signal_qual = 0;
+	u32 num_signal_strength = 0;
+	u32 num_signal_qual = 0;
+	u8 ratio_pre_stat = 0, ratio_curr_stat = 0, ratio_total = 0, ratio_profile = SIGNAL_STAT_CALC_PROFILE_0;
+
+	if (adapter->recvpriv.is_signal_dbg) {
+		/* update the user specific value, signal_strength_dbg, to signal_strength, rssi */
+		adapter->recvpriv.signal_strength = adapter->recvpriv.signal_strength_dbg;
+		adapter->recvpriv.rssi = (s8)translate_percentage_to_dbm((u8)adapter->recvpriv.signal_strength_dbg);
+	} else {
+
+		if (recvpriv->signal_strength_data.update_req == 0) { /* update_req is clear, means we got rx */
+			avg_signal_strength = recvpriv->signal_strength_data.avg_val;
+			num_signal_strength = recvpriv->signal_strength_data.total_num;
+			/* after avg_vals are accquired, we can re-stat the signal values */
+			recvpriv->signal_strength_data.update_req = 1;
+		}
+
+		if (recvpriv->signal_qual_data.update_req == 0) { /* update_req is clear, means we got rx */
+			avg_signal_qual = recvpriv->signal_qual_data.avg_val;
+			num_signal_qual = recvpriv->signal_qual_data.total_num;
+			/* after avg_vals are accquired, we can re-stat the signal values */
+			recvpriv->signal_qual_data.update_req = 1;
+		}
+
+		if (num_signal_strength == 0) {
+			if (rtw_get_on_cur_ch_time(adapter) == 0
+			    || rtw_get_passing_time_ms(rtw_get_on_cur_ch_time(adapter)) < 2 * adapter->mlmeextpriv.mlmext_info.bcn_interval
+			   )
+				goto set_timer;
+		}
+
+		if (check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY)
+		    || check_fwstate(&adapter->mlmepriv, _FW_LINKED) == false
+		   )
+			goto set_timer;
+
+#ifdef CONFIG_CONCURRENT_MODE
+		if (rtw_mi_buddy_check_fwstate(adapter, _FW_UNDER_SURVEY))
+			goto set_timer;
+#endif
+
+		if (RTW_SIGNAL_STATE_CALC_PROFILE < SIGNAL_STAT_CALC_PROFILE_MAX)
+			ratio_profile = RTW_SIGNAL_STATE_CALC_PROFILE;
+
+		ratio_pre_stat = signal_stat_calc_profile[ratio_profile][0];
+		ratio_curr_stat = signal_stat_calc_profile[ratio_profile][1];
+		ratio_total = ratio_pre_stat + ratio_curr_stat;
+
+		/* update value of signal_strength, rssi, signal_qual */
+		tmp_s = (ratio_curr_stat * avg_signal_strength + ratio_pre_stat * recvpriv->signal_strength);
+		if (tmp_s % ratio_total)
+			tmp_s = tmp_s / ratio_total + 1;
+		else
+			tmp_s = tmp_s / ratio_total;
+		if (tmp_s > 100)
+			tmp_s = 100;
+
+		tmp_q = (ratio_curr_stat * avg_signal_qual + ratio_pre_stat * recvpriv->signal_qual);
+		if (tmp_q % ratio_total)
+			tmp_q = tmp_q / ratio_total + 1;
+		else
+			tmp_q = tmp_q / ratio_total;
+		if (tmp_q > 100)
+			tmp_q = 100;
+
+		recvpriv->signal_strength = tmp_s;
+		recvpriv->rssi = (s8)translate_percentage_to_dbm(tmp_s);
+		recvpriv->signal_qual = tmp_q;
+
+#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1
+		RTW_INFO(FUNC_ADPT_FMT" signal_strength:%3u, rssi:%3d, signal_qual:%3u"
+			 ", num_signal_strength:%u, num_signal_qual:%u"
+			 ", on_cur_ch_ms:%d"
+			 "\n"
+			 , FUNC_ADPT_ARG(adapter)
+			 , recvpriv->signal_strength
+			 , recvpriv->rssi
+			 , recvpriv->signal_qual
+			 , num_signal_strength, num_signal_qual
+			, rtw_get_on_cur_ch_time(adapter) ? rtw_get_passing_time_ms(rtw_get_on_cur_ch_time(adapter)) : 0
+			);
+#endif
+	}
+
+set_timer:
+	rtw_set_signal_stat_timer(recvpriv);
+
+}
+#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */
+
+static void rx_process_rssi(_adapter *padapter, union recv_frame *prframe)
+{
+	u32	last_rssi, tmp_val;
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+	struct signal_stat *signal_stat = &padapter->recvpriv.signal_strength_data;
+#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */
+
+	/* RTW_INFO("process_rssi=> pattrib->rssil(%d) signal_strength(%d)\n ",pattrib->RecvSignalPower,pattrib->signal_strength); */
+	/* if(pRfd->Status.bPacketToSelf || pRfd->Status.bPacketBeacon) */
+	{
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+		if (signal_stat->update_req) {
+			signal_stat->total_num = 0;
+			signal_stat->total_val = 0;
+			signal_stat->update_req = 0;
+		}
+
+		signal_stat->total_num++;
+		signal_stat->total_val  += pattrib->phy_info.SignalStrength;
+		signal_stat->avg_val = signal_stat->total_val / signal_stat->total_num;
+#else /* CONFIG_NEW_SIGNAL_STAT_PROCESS */
+
+		/* Adapter->RxStats.RssiCalculateCnt++;	 */ /* For antenna Test */
+		if (padapter->recvpriv.signal_strength_data.total_num++ >= PHY_RSSI_SLID_WIN_MAX) {
+			padapter->recvpriv.signal_strength_data.total_num = PHY_RSSI_SLID_WIN_MAX;
+			last_rssi = padapter->recvpriv.signal_strength_data.elements[padapter->recvpriv.signal_strength_data.index];
+			padapter->recvpriv.signal_strength_data.total_val -= last_rssi;
+		}
+		padapter->recvpriv.signal_strength_data.total_val  += pattrib->phy_info.SignalStrength;
+
+		padapter->recvpriv.signal_strength_data.elements[padapter->recvpriv.signal_strength_data.index++] = pattrib->phy_info.SignalStrength;
+		if (padapter->recvpriv.signal_strength_data.index >= PHY_RSSI_SLID_WIN_MAX)
+			padapter->recvpriv.signal_strength_data.index = 0;
+
+
+		tmp_val = padapter->recvpriv.signal_strength_data.total_val / padapter->recvpriv.signal_strength_data.total_num;
+
+		if (padapter->recvpriv.is_signal_dbg) {
+			padapter->recvpriv.signal_strength = padapter->recvpriv.signal_strength_dbg;
+			padapter->recvpriv.rssi = (s8)translate_percentage_to_dbm(padapter->recvpriv.signal_strength_dbg);
+		} else {
+			padapter->recvpriv.signal_strength = tmp_val;
+			padapter->recvpriv.rssi = (s8)translate_percentage_to_dbm(tmp_val);
+		}
+
+#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */
+	}
+}
+
+static void rx_process_link_qual(_adapter *padapter, union recv_frame *prframe)
+{
+	u32	last_evm = 0, tmpVal;
+	struct rx_pkt_attrib *pattrib;
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+	struct signal_stat *signal_stat;
+#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */
+
+	if (prframe == NULL || padapter == NULL)
+		return;
+
+	pattrib = &prframe->u.hdr.attrib;
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+	signal_stat = &padapter->recvpriv.signal_qual_data;
+#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */
+
+	/* RTW_INFO("process_link_qual=> pattrib->signal_qual(%d)\n ",pattrib->signal_qual); */
+
+#ifdef CONFIG_NEW_SIGNAL_STAT_PROCESS
+	if (signal_stat->update_req) {
+		signal_stat->total_num = 0;
+		signal_stat->total_val = 0;
+		signal_stat->update_req = 0;
+	}
+
+	signal_stat->total_num++;
+	signal_stat->total_val  += pattrib->phy_info.SignalQuality;
+	signal_stat->avg_val = signal_stat->total_val / signal_stat->total_num;
+
+#else /* CONFIG_NEW_SIGNAL_STAT_PROCESS */
+	if (pattrib->phy_info.SignalQuality != 0) {
+		/*  */
+		/* 1. Record the general EVM to the sliding window. */
+		/*  */
+		if (padapter->recvpriv.signal_qual_data.total_num++ >= PHY_LINKQUALITY_SLID_WIN_MAX) {
+			padapter->recvpriv.signal_qual_data.total_num = PHY_LINKQUALITY_SLID_WIN_MAX;
+			last_evm = padapter->recvpriv.signal_qual_data.elements[padapter->recvpriv.signal_qual_data.index];
+			padapter->recvpriv.signal_qual_data.total_val -= last_evm;
+		}
+		padapter->recvpriv.signal_qual_data.total_val += pattrib->phy_info.SignalQuality;
+
+		padapter->recvpriv.signal_qual_data.elements[padapter->recvpriv.signal_qual_data.index++] = pattrib->phy_info.SignalQuality;
+		if (padapter->recvpriv.signal_qual_data.index >= PHY_LINKQUALITY_SLID_WIN_MAX)
+			padapter->recvpriv.signal_qual_data.index = 0;
+
+
+		/* <1> Showed on UI for user, in percentage. */
+		tmpVal = padapter->recvpriv.signal_qual_data.total_val / padapter->recvpriv.signal_qual_data.total_num;
+		padapter->recvpriv.signal_qual = (u8)tmpVal;
+
+	}
+#endif /* CONFIG_NEW_SIGNAL_STAT_PROCESS */
+}
+
+static void rx_process_phy_info(_adapter *padapter, union recv_frame *rframe)
+{
+	/* Check RSSI */
+	rx_process_rssi(padapter, rframe);
+
+	/* Check PWDB */
+	/* process_PWDB(padapter, rframe); */
+
+	/* UpdateRxSignalStatistics8192C(Adapter, pRfd); */
+
+	/* Check EVM */
+	rx_process_link_qual(padapter, rframe);
+	rtw_store_phy_info(padapter, rframe);
+}
+
+void rx_query_phy_status(
+	union recv_frame	*precvframe,
+	u8 *pphy_status)
+{
+	PADAPTER			padapter = precvframe->u.hdr.adapter;
+	struct rx_pkt_attrib	*pattrib = &precvframe->u.hdr.attrib;
+	HAL_DATA_TYPE		*pHalData = GET_HAL_DATA(padapter);
+	struct _odm_phy_status_info_	*pPHYInfo = (struct _odm_phy_status_info_ *)(&pattrib->phy_info);
+	u8					*wlanhdr;
+	struct _odm_per_pkt_info_	pkt_info;
+	u8 *sa;
+	struct sta_priv *pstapriv;
+	struct sta_info *psta = NULL;
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	/* unsigned long		irqL; */
+
+	pkt_info.is_packet_match_bssid = false;
+	pkt_info.is_packet_to_self = false;
+	pkt_info.is_packet_beacon = false;
+
+	wlanhdr = get_recvframe_data(precvframe);
+
+	pkt_info.is_packet_match_bssid = (!IsFrameTypeCtrl(wlanhdr))
+			     && (!pattrib->icv_err) && (!pattrib->crc_err)
+		&& !memcmp(get_hdr_bssid(wlanhdr), get_bssid(&padapter->mlmepriv), ETH_ALEN);
+
+	pkt_info.is_to_self = (!pattrib->icv_err) && (!pattrib->crc_err)
+		&& !memcmp(get_ra(wlanhdr), adapter_mac_addr(padapter), ETH_ALEN);
+
+	pkt_info.is_packet_to_self = pkt_info.is_packet_match_bssid
+		&& !memcmp(get_ra(wlanhdr), adapter_mac_addr(padapter), ETH_ALEN);
+
+	pkt_info.is_packet_beacon = pkt_info.is_packet_match_bssid
+				 && (get_frame_sub_type(wlanhdr) == WIFI_BEACON);
+
+	sa = get_ta(wlanhdr);
+
+	pkt_info.station_id = 0xFF;
+
+	if (!memcmp(adapter_mac_addr(padapter), sa, ETH_ALEN)) {
+		static u32 start_time = 0;
+
+		if ((start_time == 0) || (rtw_get_passing_time_ms(start_time) > 5000)) {
+			RTW_PRINT("Warning!!! %s: Confilc mac addr!!\n", __func__);
+			start_time = jiffies;
+		}
+		pdbgpriv->dbg_rx_conflic_mac_addr_cnt++;
+	} else {
+		pstapriv = &padapter->stapriv;
+		psta = rtw_get_stainfo(pstapriv, sa);
+		if (psta)
+			pkt_info.station_id = psta->mac_id;
+	}
+
+	pkt_info.data_rate = pattrib->data_rate;
+
+	/* _enter_critical_bh(&pHalData->odm_stainfo_lock, &irqL); */
+	odm_phy_status_query(&pHalData->odmpriv, pPHYInfo, pphy_status, &pkt_info);
+	if (psta)
+		psta->rssi = pattrib->phy_info.RecvSignalPower;
+	/* _exit_critical_bh(&pHalData->odm_stainfo_lock, &irqL); */
+
+	{
+		precvframe->u.hdr.psta = NULL;
+		if (pkt_info.is_packet_match_bssid
+		    && (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE))
+		   ) {
+			if (psta) {
+				precvframe->u.hdr.psta = psta;
+				rx_process_phy_info(padapter, precvframe);
+			}
+		} else if (pkt_info.is_packet_to_self || pkt_info.is_packet_beacon) {
+
+			if (psta)
+				precvframe->u.hdr.psta = psta;
+			rx_process_phy_info(padapter, precvframe);
+		}
+	}
+
+	rtw_odm_parse_rx_phy_status_chinfo(precvframe, pphy_status);
+}
+/*
+* Increase and check if the continual_no_rx_packet of this @param pmlmepriv is larger than MAX_CONTINUAL_NORXPACKET_COUNT
+* @return true:
+* @return false:
+*/
+int rtw_inc_and_chk_continual_no_rx_packet(struct sta_info *sta, int tid_index)
+{
+
+	int ret = false;
+	int value = ATOMIC_INC_RETURN(&sta->continual_no_rx_packet[tid_index]);
+
+	if (value >= MAX_CONTINUAL_NORXPACKET_COUNT)
+		ret = true;
+
+	return ret;
+}
+
+/*
+* Set the continual_no_rx_packet of this @param pmlmepriv to 0
+*/
+void rtw_reset_continual_no_rx_packet(struct sta_info *sta, int tid_index)
+{
+	ATOMIC_SET(&sta->continual_no_rx_packet[tid_index], 0);
+}
+
+
+s32 pre_recv_entry(union recv_frame *precvframe, u8 *pphy_status)
+{
+	s32 ret = _SUCCESS;
+#ifdef CONFIG_CONCURRENT_MODE
+	u8 *pda;
+	u8 *pbuf = precvframe->u.hdr.rx_data;
+	_adapter *iface = NULL;
+	_adapter *primary_padapter = precvframe->u.hdr.adapter;
+
+	pda = get_ra(pbuf);
+
+	if (IS_MCAST(pda) == false) { /*unicast packets*/
+		iface = rtw_get_iface_by_macddr(primary_padapter , pda);
+		if (NULL == iface) {
+			RTW_INFO("%s [WARN] Cannot find appropriate adapter - mac_addr : "MAC_FMT"\n", __func__, MAC_ARG(pda));
+			/*rtw_warn_on(1);*/
+		} else
+			precvframe->u.hdr.adapter = iface;
+	} else   /* Handle BC/MC Packets	*/
+		rtw_mi_buddy_clone_bcmc_packet(primary_padapter, precvframe, pphy_status);
+#endif
+
+	return ret;
+}
+
+#ifdef CONFIG_RECV_THREAD_MODE
+thread_return rtw_recv_thread(thread_context context)
+{
+	_adapter *adapter = (_adapter *)context;
+	struct recv_priv *recvpriv = &adapter->recvpriv;
+	s32 err = _SUCCESS;
+
+	thread_enter("RTW_RECV_THREAD");
+
+	RTW_INFO(FUNC_ADPT_FMT" enter\n", FUNC_ADPT_ARG(adapter));
+
+	do {
+		err = _rtw_down_sema(&recvpriv->recv_sema);
+		if (_FAIL == err) {
+			RTW_ERR(FUNC_ADPT_FMT" down recv_sema fail!\n", FUNC_ADPT_ARG(adapter));
+			goto exit;
+		}
+
+		if (RTW_CANNOT_RUN(adapter)) {
+			RTW_INFO(FUNC_ADPT_FMT" DS:%d, SR:%d\n", FUNC_ADPT_ARG(adapter)
+				, rtw_is_drv_stopped(adapter), rtw_is_surprise_removed(adapter));
+			goto exit;
+		}
+
+		err = rtw_hal_recv_hdl(adapter);
+
+		if (err == RTW_RFRAME_UNAVAIL
+			|| err == RTW_RFRAME_PKT_UNAVAIL
+		) {
+			rtw_msleep_os(1);
+			up(&recvpriv->recv_sema);
+		}
+
+		flush_signals_thread();
+
+	} while (err != _FAIL);
+
+exit:
+	up(&adapter->recvpriv.terminate_recvthread_sema);
+	RTW_INFO(FUNC_ADPT_FMT" exit\n", FUNC_ADPT_ARG(adapter));
+	thread_exit();
+}
+#endif /* CONFIG_RECV_THREAD_MODE */
+
diff --git a/drivers/staging/rtl8188eu/core/rtw_rf.c b/drivers/staging/rtl8188eu/core/rtw_rf.c
new file mode 100644
index 000000000000..1371d8a6f43d
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_rf.c
@@ -0,0 +1,1077 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_RF_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+u8 center_ch_2g[CENTER_CH_2G_NUM] = {
+/* G00 */1, 2,
+/* G01 */3, 4, 5,
+/* G02 */6, 7, 8,
+/* G03 */9, 10, 11,
+/* G04 */12, 13,
+/* G05 */14
+};
+
+u8 center_ch_2g_40m[CENTER_CH_2G_40M_NUM] = {
+	3,
+	4,
+	5,
+	6,
+	7,
+	8,
+	9,
+	10,
+	11,
+};
+
+static u8 op_chs_of_cch_2g_40m[CENTER_CH_2G_40M_NUM][2] = {
+	{1, 5}, /* 3 */
+	{2, 6}, /* 4 */
+	{3, 7}, /* 5 */
+	{4, 8}, /* 6 */
+	{5, 9}, /* 7 */
+	{6, 10}, /* 8 */
+	{7, 11}, /* 9 */
+	{8, 12}, /* 10 */
+	{9, 13}, /* 11 */
+};
+
+u8 center_ch_5g_all[CENTER_CH_5G_ALL_NUM] = {
+/* G00 */36, 38, 40,
+	42,
+/* G01 */44, 46, 48,
+	/* 50, */
+/* G02 */52, 54, 56,
+	58,
+/* G03 */60, 62, 64,
+/* G04 */100, 102, 104,
+	106,
+/* G05 */108, 110, 112,
+	/* 114, */
+/* G06 */116, 118, 120,
+	122,
+/* G07 */124, 126, 128,
+/* G08 */132, 134, 136,
+	138,
+/* G09 */140, 142, 144,
+/* G10 */149, 151, 153,
+	155,
+/* G11 */157, 159, 161,
+	/* 163, */
+/* G12 */165, 167, 169,
+	171,
+/* G13 */173, 175, 177
+};
+
+u8 center_ch_5g_20m[CENTER_CH_5G_20M_NUM] = {
+/* G00 */36, 40,
+/* G01 */44, 48,
+/* G02 */52, 56,
+/* G03 */60, 64,
+/* G04 */100, 104,
+/* G05 */108, 112,
+/* G06 */116, 120,
+/* G07 */124, 128,
+/* G08 */132, 136,
+/* G09 */140, 144,
+/* G10 */149, 153,
+/* G11 */157, 161,
+/* G12 */165, 169,
+/* G13 */173, 177
+};
+
+u8 center_ch_5g_40m[CENTER_CH_5G_40M_NUM] = {
+/* G00 */38,
+/* G01 */46,
+/* G02 */54,
+/* G03 */62,
+/* G04 */102,
+/* G05 */110,
+/* G06 */118,
+/* G07 */126,
+/* G08 */134,
+/* G09 */142,
+/* G10 */151,
+/* G11 */159,
+/* G12 */167,
+/* G13 */175
+};
+
+u8 center_ch_5g_20m_40m[CENTER_CH_5G_20M_NUM + CENTER_CH_5G_40M_NUM] = {
+/* G00 */36, 38, 40,
+/* G01 */44, 46, 48,
+/* G02 */52, 54, 56,
+/* G03 */60, 62, 64,
+/* G04 */100, 102, 104,
+/* G05 */108, 110, 112,
+/* G06 */116, 118, 120,
+/* G07 */124, 126, 128,
+/* G08 */132, 134, 136,
+/* G09 */140, 142, 144,
+/* G10 */149, 151, 153,
+/* G11 */157, 159, 161,
+/* G12 */165, 167, 169,
+/* G13 */173, 175, 177
+};
+
+static u8 op_chs_of_cch_5g_40m[CENTER_CH_5G_40M_NUM][2] = {
+	{36, 40}, /* 38 */
+	{44, 48}, /* 46 */
+	{52, 56}, /* 54 */
+	{60, 64}, /* 62 */
+	{100, 104}, /* 102 */
+	{108, 112}, /* 110 */
+	{116, 120}, /* 118 */
+	{124, 128}, /* 126 */
+	{132, 136}, /* 134 */
+	{140, 144}, /* 142 */
+	{149, 153}, /* 151 */
+	{157, 161}, /* 159 */
+	{165, 169}, /* 167 */
+	{173, 177}, /* 175 */
+};
+
+u8 center_ch_5g_80m[CENTER_CH_5G_80M_NUM] = {
+/* G00 ~ G01*/42,
+/* G02 ~ G03*/58,
+/* G04 ~ G05*/106,
+/* G06 ~ G07*/122,
+/* G08 ~ G09*/138,
+/* G10 ~ G11*/155,
+/* G12 ~ G13*/171
+};
+
+static u8 op_chs_of_cch_5g_80m[CENTER_CH_5G_80M_NUM][4] = {
+	{36, 40, 44, 48}, /* 42 */
+	{52, 56, 60, 64}, /* 58 */
+	{100, 104, 108, 112}, /* 106 */
+	{116, 120, 124, 128}, /* 122 */
+	{132, 136, 140, 144}, /* 138 */
+	{149, 153, 157, 161}, /* 155 */
+	{165, 169, 173, 177}, /* 171 */
+};
+
+static u8 center_ch_5g_160m[CENTER_CH_5G_160M_NUM] = {
+/* G00 ~ G03*/50,
+/* G04 ~ G07*/114,
+/* G10 ~ G13*/163
+};
+
+static u8 op_chs_of_cch_5g_160m[CENTER_CH_5G_160M_NUM][8] = {
+	{36, 40, 44, 48, 52, 56, 60, 64}, /* 50 */
+	{100, 104, 108, 112, 116, 120, 124, 128}, /* 114 */
+	{149, 153, 157, 161, 165, 169, 173, 177}, /* 163 */
+};
+
+struct center_chs_ent_t {
+	u8 ch_num;
+	u8 *chs;
+};
+
+static struct center_chs_ent_t center_chs_2g_by_bw[] = {
+	{CENTER_CH_2G_NUM, center_ch_2g},
+	{CENTER_CH_2G_40M_NUM, center_ch_2g_40m},
+};
+
+static struct center_chs_ent_t center_chs_5g_by_bw[] = {
+	{CENTER_CH_5G_20M_NUM, center_ch_5g_20m},
+	{CENTER_CH_5G_40M_NUM, center_ch_5g_40m},
+	{CENTER_CH_5G_80M_NUM, center_ch_5g_80m},
+	{CENTER_CH_5G_160M_NUM, center_ch_5g_160m},
+};
+
+/*
+ * Get center channel of smaller bandwidth by @param cch, @param bw, @param offset
+ * @cch: the given center channel
+ * @bw: the given bandwidth
+ * @offset: the given primary SC offset of the given bandwidth
+ *
+ * return center channel of smaller bandiwdth if valid, or 0
+ */
+u8 rtw_get_scch_by_cch_offset(u8 cch, u8 bw, u8 offset)
+{
+	int i;
+	u8 t_cch = 0;
+
+	if (bw == CHANNEL_WIDTH_20) {
+		t_cch = cch;
+		goto exit;
+	}
+
+	if (offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	/* 2.4G, 40MHz */
+	if (cch >= 3 && cch <= 11 && bw == CHANNEL_WIDTH_40) {
+		t_cch = (offset == HAL_PRIME_CHNL_OFFSET_UPPER) ? cch + 2 : cch - 2;
+		goto exit;
+	}
+
+	/* 5G, 160MHz */
+	if (cch >= 50 && cch <= 163 && bw == CHANNEL_WIDTH_160) {
+		t_cch = (offset == HAL_PRIME_CHNL_OFFSET_UPPER) ? cch + 8 : cch - 8;
+		goto exit;
+
+	/* 5G, 80MHz */
+	} else if (cch >= 42 && cch <= 171 && bw == CHANNEL_WIDTH_80) {
+		t_cch = (offset == HAL_PRIME_CHNL_OFFSET_UPPER) ? cch + 4 : cch - 4;
+		goto exit;
+
+	/* 5G, 40MHz */
+	} else if (cch >= 38 && cch <= 175 && bw == CHANNEL_WIDTH_40) {
+		t_cch = (offset == HAL_PRIME_CHNL_OFFSET_UPPER) ? cch + 2 : cch - 2;
+		goto exit;
+
+	} else {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+exit:
+	return t_cch;
+}
+
+struct op_chs_ent_t {
+	u8 ch_num;
+	u8 *chs;
+};
+
+static struct op_chs_ent_t op_chs_of_cch_2g_by_bw[] = {
+	{1, center_ch_2g},
+	{2, (u8 *)op_chs_of_cch_2g_40m},
+};
+
+static struct op_chs_ent_t op_chs_of_cch_5g_by_bw[] = {
+	{1, center_ch_5g_20m},
+	{2, (u8 *)op_chs_of_cch_5g_40m},
+	{4, (u8 *)op_chs_of_cch_5g_80m},
+	{8, (u8 *)op_chs_of_cch_5g_160m},
+};
+
+inline u8 center_chs_2g_num(u8 bw)
+{
+	if (bw > CHANNEL_WIDTH_40)
+		return 0;
+
+	return center_chs_2g_by_bw[bw].ch_num;
+}
+
+inline u8 center_chs_2g(u8 bw, u8 id)
+{
+	if (bw > CHANNEL_WIDTH_40)
+		return 0;
+
+	if (id >= center_chs_2g_num(bw))
+		return 0;
+
+	return center_chs_2g_by_bw[bw].chs[id];
+}
+
+inline u8 center_chs_5g_num(u8 bw)
+{
+	if (bw > CHANNEL_WIDTH_80)
+		return 0;
+
+	return center_chs_5g_by_bw[bw].ch_num;
+}
+
+inline u8 center_chs_5g(u8 bw, u8 id)
+{
+	if (bw > CHANNEL_WIDTH_80)
+		return 0;
+
+	if (id >= center_chs_5g_num(bw))
+		return 0;
+
+	return center_chs_5g_by_bw[bw].chs[id];
+}
+
+/*
+ * Get available op channels by @param cch, @param bw
+ * @cch: the given center channel
+ * @bw: the given bandwidth
+ * @op_chs: the pointer to return pointer of op channel array
+ * @op_ch_num: the pointer to return pointer of op channel number
+ *
+ * return valid (1) or not (0)
+ */
+u8 rtw_get_op_chs_by_cch_bw(u8 cch, u8 bw, u8 **op_chs, u8 *op_ch_num)
+{
+	int i;
+	struct center_chs_ent_t *c_chs_ent = NULL;
+	struct op_chs_ent_t *op_chs_ent = NULL;
+	u8 valid = 1;
+
+	if (cch <= 14
+		&& bw >= CHANNEL_WIDTH_20 && bw <= CHANNEL_WIDTH_40
+	) {
+		c_chs_ent = &center_chs_2g_by_bw[bw];
+		op_chs_ent = &op_chs_of_cch_2g_by_bw[bw];
+	} else if (cch >= 36 && cch <= 177
+		&& bw >= CHANNEL_WIDTH_20 && bw <= CHANNEL_WIDTH_160
+	) {
+		c_chs_ent = &center_chs_5g_by_bw[bw];
+		op_chs_ent = &op_chs_of_cch_5g_by_bw[bw];
+	} else {
+		valid = 0;
+		goto exit;
+	}
+
+	for (i = 0; i < c_chs_ent->ch_num; i++)
+		if (cch == *(c_chs_ent->chs + i))
+			break;
+
+	if (i == c_chs_ent->ch_num) {
+		valid = 0;
+		goto exit;
+	}
+
+	*op_chs = op_chs_ent->chs + op_chs_ent->ch_num * i;
+	*op_ch_num = op_chs_ent->ch_num;
+
+exit:
+	return valid;
+}
+
+u8 rtw_get_ch_group(u8 ch, u8 *group, u8 *cck_group)
+{
+	BAND_TYPE band = BAND_MAX;
+	s8 gp = -1, cck_gp = -1;
+
+	if (ch <= 14) {
+		band = BAND_ON_2_4G;
+
+		if (1 <= ch && ch <= 2)
+			gp = 0;
+		else if (3  <= ch && ch <= 5)
+			gp = 1;
+		else if (6  <= ch && ch <= 8)
+			gp = 2;
+		else if (9  <= ch && ch <= 11)
+			gp = 3;
+		else if (12 <= ch && ch <= 14)
+			gp = 4;
+		else
+			band = BAND_MAX;
+
+		if (ch == 14)
+			cck_gp = 5;
+		else
+			cck_gp = gp;
+	} else {
+		band = BAND_ON_5G;
+
+		if (36 <= ch && ch <= 42)
+			gp = 0;
+		else if (44   <= ch && ch <=  48)
+			gp = 1;
+		else if (50   <= ch && ch <=  58)
+			gp = 2;
+		else if (60   <= ch && ch <=  64)
+			gp = 3;
+		else if (100  <= ch && ch <= 106)
+			gp = 4;
+		else if (108  <= ch && ch <= 114)
+			gp = 5;
+		else if (116  <= ch && ch <= 122)
+			gp = 6;
+		else if (124  <= ch && ch <= 130)
+			gp = 7;
+		else if (132  <= ch && ch <= 138)
+			gp = 8;
+		else if (140  <= ch && ch <= 144)
+			gp = 9;
+		else if (149  <= ch && ch <= 155)
+			gp = 10;
+		else if (157  <= ch && ch <= 161)
+			gp = 11;
+		else if (165  <= ch && ch <= 171)
+			gp = 12;
+		else if (173  <= ch && ch <= 177)
+			gp = 13;
+		else
+			band = BAND_MAX;
+	}
+
+	if (band == BAND_MAX
+		|| (band == BAND_ON_2_4G && cck_gp == -1)
+		|| gp == -1
+	) {
+		RTW_WARN("%s invalid channel:%u", __func__, ch);
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	if (group)
+		*group = gp;
+	if (cck_group && band == BAND_ON_2_4G)
+		*cck_group = cck_gp;
+
+exit:
+	return band;
+}
+
+int rtw_ch2freq(int chan)
+{
+	/* see 802.11 17.3.8.3.2 and Annex J
+	* there are overlapping channel numbers in 5GHz and 2GHz bands */
+
+	/*
+	* RTK: don't consider the overlapping channel numbers: 5G channel <= 14,
+	* because we don't support it. simply judge from channel number
+	*/
+
+	if (chan >= 1 && chan <= 14) {
+		if (chan == 14)
+			return 2484;
+		else if (chan < 14)
+			return 2407 + chan * 5;
+	} else if (chan >= 36 && chan <= 177)
+		return 5000 + chan * 5;
+
+	return 0; /* not supported */
+}
+
+int rtw_freq2ch(int freq)
+{
+	/* see 802.11 17.3.8.3.2 and Annex J */
+	if (freq == 2484)
+		return 14;
+	else if (freq < 2484)
+		return (freq - 2407) / 5;
+	else if (freq >= 4910 && freq <= 4980)
+		return (freq - 4000) / 5;
+	else if (freq <= 45000) /* DMG band lower limit */
+		return (freq - 5000) / 5;
+	else if (freq >= 58320 && freq <= 64800)
+		return (freq - 56160) / 2160;
+	else
+		return 0;
+}
+
+bool rtw_chbw_to_freq_range(u8 ch, u8 bw, u8 offset, u32 *hi, u32 *lo)
+{
+	u8 c_ch;
+	u32 freq;
+	u32 hi_ret = 0, lo_ret = 0;
+	int i;
+	bool valid = false;
+
+	if (hi)
+		*hi = 0;
+	if (lo)
+		*lo = 0;
+
+	c_ch = rtw_get_center_ch(ch, bw, offset);
+	freq = rtw_ch2freq(c_ch);
+
+	if (!freq) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	if (bw == CHANNEL_WIDTH_80) {
+		hi_ret = freq + 40;
+		lo_ret = freq - 40;
+	} else if (bw == CHANNEL_WIDTH_40) {
+		hi_ret = freq + 20;
+		lo_ret = freq - 20;
+	} else if (bw == CHANNEL_WIDTH_20) {
+		hi_ret = freq + 10;
+		lo_ret = freq - 10;
+	} else
+		rtw_warn_on(1);
+
+	if (hi)
+		*hi = hi_ret;
+	if (lo)
+		*lo = lo_ret;
+
+	valid = true;
+
+exit:
+	return valid;
+}
+
+const char *const _ch_width_str[] = {
+	"20MHz",
+	"40MHz",
+	"80MHz",
+	"160MHz",
+	"80_80MHz",
+	"CHANNEL_WIDTH_MAX",
+};
+
+const u8 _ch_width_to_bw_cap[] = {
+	BW_CAP_20M,
+	BW_CAP_40M,
+	BW_CAP_80M,
+	BW_CAP_160M,
+	BW_CAP_80_80M,
+	0,
+};
+
+const char *const _band_str[] = {
+	"2.4G",
+	"5G",
+	"BOTH",
+	"BAND_MAX",
+};
+
+const u8 _band_to_band_cap[] = {
+	BAND_CAP_2G,
+	BAND_CAP_5G,
+	0,
+	0,
+};
+
+const u8 _rf_type_to_rf_tx_cnt[] = {
+	1,
+	2,
+	2,
+	1,
+	2,
+	2,
+	3,
+	3,
+	4,
+};
+
+const u8 _rf_type_to_rf_rx_cnt[] = {
+	2,
+	4,
+	2,
+	1,
+	2,
+	3,
+	3,
+	4,
+	4,
+};
+
+#define COUNTRY_CHPLAN_ASSIGN_EN_11AC(_val)
+
+#if RTW_DEF_MODULE_REGULATORY_CERT
+#define COUNTRY_CHPLAN_ASSIGN_DEF_MODULE_FLAGS(_val) , .def_module_flags = (_val)
+#else
+#define COUNTRY_CHPLAN_ASSIGN_DEF_MODULE_FLAGS(_val)
+#endif
+
+/* has def_module_flags specified, used by common map and HAL dfference map */
+#define COUNTRY_CHPLAN_ENT(_alpha2, _chplan, _en_11ac, _def_module_flags) \
+	{.alpha2 = (_alpha2), .chplan = (_chplan) \
+		COUNTRY_CHPLAN_ASSIGN_EN_11AC(_en_11ac) \
+		COUNTRY_CHPLAN_ASSIGN_DEF_MODULE_FLAGS(_def_module_flags) \
+	}
+
+#ifdef CONFIG_CUSTOMIZED_COUNTRY_CHPLAN_MAP
+
+#include "../platform/custom_country_chplan.h"
+
+#elif RTW_DEF_MODULE_REGULATORY_CERT
+
+#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8821AE_HMC_M2)
+static const struct country_chplan RTL8821AE_HMC_M2_country_chplan_map[] = {
+	COUNTRY_CHPLAN_ENT("CN", 0x51, 1, 0xFB), /* China */
+	COUNTRY_CHPLAN_ENT("RU", 0x59, 0, 0xFB), /* Russia(fac/gost), Kaliningrad */
+	COUNTRY_CHPLAN_ENT("UA", 0x26, 0, 0xFB), /* Ukraine */
+};
+static const u16 RTL8821AE_HMC_M2_country_chplan_map_sz = sizeof(RTL8821AE_HMC_M2_country_chplan_map) / sizeof(struct country_chplan);
+#endif
+
+#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8821AU)
+static const struct country_chplan RTL8821AU_country_chplan_map[] = {
+	COUNTRY_CHPLAN_ENT("RU", 0x59, 0, 0xFB), /* Russia(fac/gost), Kaliningrad */
+	COUNTRY_CHPLAN_ENT("UA", 0x26, 0, 0xFB), /* Ukraine */
+};
+static const u16 RTL8821AU_country_chplan_map_sz = sizeof(RTL8821AU_country_chplan_map) / sizeof(struct country_chplan);
+#endif
+
+#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8812AENF_NGFF)
+static const struct country_chplan RTL8812AENF_NGFF_country_chplan_map[] = {
+};
+static const u16 RTL8812AENF_NGFF_country_chplan_map_sz = sizeof(RTL8812AENF_NGFF_country_chplan_map) / sizeof(struct country_chplan);
+#endif
+
+#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8812AEBT_HMC)
+static const struct country_chplan RTL8812AEBT_HMC_country_chplan_map[] = {
+};
+static const u16 RTL8812AEBT_HMC_country_chplan_map_sz = sizeof(RTL8812AEBT_HMC_country_chplan_map) / sizeof(struct country_chplan);
+#endif
+
+#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8188EE_HMC_M2)
+static const struct country_chplan RTL8188EE_HMC_M2_country_chplan_map[] = {
+};
+static const u16 RTL8188EE_HMC_M2_country_chplan_map_sz = sizeof(RTL8188EE_HMC_M2_country_chplan_map) / sizeof(struct country_chplan);
+#endif
+
+#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8723BE_HMC_M2)
+static const struct country_chplan RTL8723BE_HMC_M2_country_chplan_map[] = {
+};
+static const u16 RTL8723BE_HMC_M2_country_chplan_map_sz = sizeof(RTL8723BE_HMC_M2_country_chplan_map) / sizeof(struct country_chplan);
+#endif
+
+#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8723BS_NGFF1216)
+static const struct country_chplan RTL8723BS_NGFF1216_country_chplan_map[] = {
+};
+static const u16 RTL8723BS_NGFF1216_country_chplan_map_sz = sizeof(RTL8723BS_NGFF1216_country_chplan_map) / sizeof(struct country_chplan);
+#endif
+
+#if (RTW_DEF_MODULE_REGULATORY_CERT & RTW_MODULE_RTL8192EEBT_HMC_M2)
+static const struct country_chplan RTL8192EEBT_HMC_M2_country_chplan_map[] = {
+};
+static const u16 RTL8192EEBT_HMC_M2_country_chplan_map_sz = sizeof(RTL8192EEBT_HMC_M2_country_chplan_map) / sizeof(struct country_chplan);
+#endif
+
+/**
+ * rtw_def_module_get_chplan_from_country -
+ * @country_code: string of country code
+ * @return:
+ * Return NULL for case referring to common map
+ */
+static const struct country_chplan *rtw_def_module_get_chplan_from_country(const char *country_code)
+{
+	const struct country_chplan *ent = NULL;
+	const struct country_chplan *hal_map = NULL;
+	u16 hal_map_sz = 0;
+	int i;
+
+	/* TODO: runtime selection for multi driver */
+#if (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8821AE_HMC_M2)
+	hal_map = RTL8821AE_HMC_M2_country_chplan_map;
+	hal_map_sz = RTL8821AE_HMC_M2_country_chplan_map_sz;
+#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8821AU)
+	hal_map = RTL8821AU_country_chplan_map;
+	hal_map_sz = RTL8821AU_country_chplan_map_sz;
+#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8812AENF_NGFF)
+	hal_map = RTL8812AENF_NGFF_country_chplan_map;
+	hal_map_sz = RTL8812AENF_NGFF_country_chplan_map_sz;
+#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8812AEBT_HMC)
+	hal_map = RTL8812AEBT_HMC_country_chplan_map;
+	hal_map_sz = RTL8812AEBT_HMC_country_chplan_map_sz;
+#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8188EE_HMC_M2)
+	hal_map = RTL8188EE_HMC_M2_country_chplan_map;
+	hal_map_sz = RTL8188EE_HMC_M2_country_chplan_map_sz;
+#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8723BE_HMC_M2)
+	hal_map = RTL8723BE_HMC_M2_country_chplan_map;
+	hal_map_sz = RTL8723BE_HMC_M2_country_chplan_map_sz;
+#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8723BS_NGFF1216)
+	hal_map = RTL8723BS_NGFF1216_country_chplan_map;
+	hal_map_sz = RTL8723BS_NGFF1216_country_chplan_map_sz;
+#elif (RTW_DEF_MODULE_REGULATORY_CERT == RTW_MODULE_RTL8192EEBT_HMC_M2)
+	hal_map = RTL8192EEBT_HMC_M2_country_chplan_map;
+	hal_map_sz = RTL8192EEBT_HMC_M2_country_chplan_map_sz;
+#endif
+
+	if (hal_map == NULL || hal_map_sz == 0)
+		goto exit;
+
+	for (i = 0; i < hal_map_sz; i++) {
+		if (strncmp(country_code, hal_map[i].alpha2, 2) == 0) {
+			ent = &hal_map[i];
+			break;
+		}
+	}
+
+exit:
+	return ent;
+}
+#endif /* CONFIG_CUSTOMIZED_COUNTRY_CHPLAN_MAP or RTW_DEF_MODULE_REGULATORY_CERT */
+
+static const struct country_chplan country_chplan_map[] = {
+	COUNTRY_CHPLAN_ENT("AD", 0x26, 1, 0x00), /* Andorra */
+	COUNTRY_CHPLAN_ENT("AE", 0x26, 1, 0xFB), /* United Arab Emirates */
+	COUNTRY_CHPLAN_ENT("AF", 0x42, 1, 0x00), /* Afghanistan */
+	COUNTRY_CHPLAN_ENT("AG", 0x30, 1, 0x00), /* Antigua & Barbuda */
+	COUNTRY_CHPLAN_ENT("AI", 0x26, 1, 0x00), /* Anguilla(UK) */
+	COUNTRY_CHPLAN_ENT("AL", 0x26, 1, 0xF1), /* Albania */
+	COUNTRY_CHPLAN_ENT("AM", 0x26, 1, 0xB0), /* Armenia */
+	COUNTRY_CHPLAN_ENT("AO", 0x26, 1, 0xE0), /* Angola */
+	COUNTRY_CHPLAN_ENT("AQ", 0x26, 1, 0x00), /* Antarctica */
+	COUNTRY_CHPLAN_ENT("AR", 0x57, 1, 0xF3), /* Argentina */
+	COUNTRY_CHPLAN_ENT("AS", 0x34, 1, 0x00), /* American Samoa */
+	COUNTRY_CHPLAN_ENT("AT", 0x26, 1, 0xFB), /* Austria */
+	COUNTRY_CHPLAN_ENT("AU", 0x45, 1, 0xFB), /* Australia */
+	COUNTRY_CHPLAN_ENT("AW", 0x34, 1, 0xB0), /* Aruba */
+	COUNTRY_CHPLAN_ENT("AZ", 0x26, 1, 0xF1), /* Azerbaijan */
+	COUNTRY_CHPLAN_ENT("BA", 0x26, 1, 0xF1), /* Bosnia & Herzegovina */
+	COUNTRY_CHPLAN_ENT("BB", 0x34, 1, 0x50), /* Barbados */
+	COUNTRY_CHPLAN_ENT("BD", 0x26, 1, 0xF1), /* Bangladesh */
+	COUNTRY_CHPLAN_ENT("BE", 0x26, 1, 0xFB), /* Belgium */
+	COUNTRY_CHPLAN_ENT("BF", 0x26, 1, 0xB0), /* Burkina Faso */
+	COUNTRY_CHPLAN_ENT("BG", 0x26, 1, 0xF1), /* Bulgaria */
+	COUNTRY_CHPLAN_ENT("BH", 0x47, 1, 0xF1), /* Bahrain */
+	COUNTRY_CHPLAN_ENT("BI", 0x26, 1, 0xB0), /* Burundi */
+	COUNTRY_CHPLAN_ENT("BJ", 0x26, 1, 0xB0), /* Benin */
+	COUNTRY_CHPLAN_ENT("BN", 0x47, 1, 0x10), /* Brunei */
+	COUNTRY_CHPLAN_ENT("BO", 0x30, 1, 0xF1), /* Bolivia */
+	COUNTRY_CHPLAN_ENT("BR", 0x34, 1, 0xF1), /* Brazil */
+	COUNTRY_CHPLAN_ENT("BS", 0x34, 1, 0x20), /* Bahamas */
+	COUNTRY_CHPLAN_ENT("BW", 0x26, 1, 0xF1), /* Botswana */
+	COUNTRY_CHPLAN_ENT("BY", 0x26, 1, 0xF1), /* Belarus */
+	COUNTRY_CHPLAN_ENT("BZ", 0x34, 1, 0x00), /* Belize */
+	COUNTRY_CHPLAN_ENT("CA", 0x34, 1, 0xFB), /* Canada */
+	COUNTRY_CHPLAN_ENT("CC", 0x26, 1, 0x00), /* Cocos (Keeling) Islands (Australia) */
+	COUNTRY_CHPLAN_ENT("CD", 0x26, 1, 0xB0), /* Congo, Republic of the */
+	COUNTRY_CHPLAN_ENT("CF", 0x26, 1, 0xB0), /* Central African Republic */
+	COUNTRY_CHPLAN_ENT("CG", 0x26, 1, 0xB0), /* Congo, Democratic Republic of the. Zaire */
+	COUNTRY_CHPLAN_ENT("CH", 0x26, 1, 0xFB), /* Switzerland */
+	COUNTRY_CHPLAN_ENT("CI", 0x26, 1, 0xF1), /* Cote d'Ivoire */
+	COUNTRY_CHPLAN_ENT("CK", 0x26, 1, 0x00), /* Cook Islands */
+	COUNTRY_CHPLAN_ENT("CL", 0x30, 1, 0xF1), /* Chile */
+	COUNTRY_CHPLAN_ENT("CM", 0x26, 1, 0xB0), /* Cameroon */
+	COUNTRY_CHPLAN_ENT("CN", 0x48, 1, 0xFB), /* China */
+	COUNTRY_CHPLAN_ENT("CO", 0x34, 1, 0xF1), /* Colombia */
+	COUNTRY_CHPLAN_ENT("CR", 0x34, 1, 0xF1), /* Costa Rica */
+	COUNTRY_CHPLAN_ENT("CV", 0x26, 1, 0xB0), /* Cape Verde */
+	COUNTRY_CHPLAN_ENT("CX", 0x45, 1, 0x00), /* Christmas Island (Australia) */
+	COUNTRY_CHPLAN_ENT("CY", 0x26, 1, 0xFB), /* Cyprus */
+	COUNTRY_CHPLAN_ENT("CZ", 0x26, 1, 0xFB), /* Czech Republic */
+	COUNTRY_CHPLAN_ENT("DE", 0x26, 1, 0xFB), /* Germany */
+	COUNTRY_CHPLAN_ENT("DJ", 0x26, 1, 0x80), /* Djibouti */
+	COUNTRY_CHPLAN_ENT("DK", 0x26, 1, 0xFB), /* Denmark */
+	COUNTRY_CHPLAN_ENT("DM", 0x34, 1, 0x00), /* Dominica */
+	COUNTRY_CHPLAN_ENT("DO", 0x34, 1, 0xF1), /* Dominican Republic */
+	COUNTRY_CHPLAN_ENT("DZ", 0x26, 1, 0xF1), /* Algeria */
+	COUNTRY_CHPLAN_ENT("EC", 0x34, 1, 0xF1), /* Ecuador */
+	COUNTRY_CHPLAN_ENT("EE", 0x26, 1, 0xFB), /* Estonia */
+	COUNTRY_CHPLAN_ENT("EG", 0x47, 0, 0xF1), /* Egypt */
+	COUNTRY_CHPLAN_ENT("EH", 0x47, 1, 0x80), /* Western Sahara */
+	COUNTRY_CHPLAN_ENT("ER", 0x26, 1, 0x00), /* Eritrea */
+	COUNTRY_CHPLAN_ENT("ES", 0x26, 1, 0xFB), /* Spain, Canary Islands, Ceuta, Melilla */
+	COUNTRY_CHPLAN_ENT("ET", 0x26, 1, 0xB0), /* Ethiopia */
+	COUNTRY_CHPLAN_ENT("FI", 0x26, 1, 0xFB), /* Finland */
+	COUNTRY_CHPLAN_ENT("FJ", 0x34, 1, 0x00), /* Fiji */
+	COUNTRY_CHPLAN_ENT("FK", 0x26, 1, 0x00), /* Falkland Islands (Islas Malvinas) (UK) */
+	COUNTRY_CHPLAN_ENT("FM", 0x34, 1, 0x00), /* Micronesia, Federated States of (USA) */
+	COUNTRY_CHPLAN_ENT("FO", 0x26, 1, 0x00), /* Faroe Islands (Denmark) */
+	COUNTRY_CHPLAN_ENT("FR", 0x26, 1, 0xFB), /* France */
+	COUNTRY_CHPLAN_ENT("GA", 0x26, 1, 0xB0), /* Gabon */
+	COUNTRY_CHPLAN_ENT("GB", 0x26, 1, 0xFB), /* Great Britain (United Kingdom; England) */
+	COUNTRY_CHPLAN_ENT("GD", 0x34, 1, 0xB0), /* Grenada */
+	COUNTRY_CHPLAN_ENT("GE", 0x26, 1, 0x00), /* Georgia */
+	COUNTRY_CHPLAN_ENT("GF", 0x26, 1, 0x80), /* French Guiana */
+	COUNTRY_CHPLAN_ENT("GG", 0x26, 1, 0x00), /* Guernsey (UK) */
+	COUNTRY_CHPLAN_ENT("GH", 0x26, 1, 0xF1), /* Ghana */
+	COUNTRY_CHPLAN_ENT("GI", 0x26, 1, 0x00), /* Gibraltar (UK) */
+	COUNTRY_CHPLAN_ENT("GL", 0x26, 1, 0x00), /* Greenland (Denmark) */
+	COUNTRY_CHPLAN_ENT("GM", 0x26, 1, 0xB0), /* Gambia */
+	COUNTRY_CHPLAN_ENT("GN", 0x26, 1, 0x10), /* Guinea */
+	COUNTRY_CHPLAN_ENT("GP", 0x26, 1, 0x00), /* Guadeloupe (France) */
+	COUNTRY_CHPLAN_ENT("GQ", 0x26, 1, 0xB0), /* Equatorial Guinea */
+	COUNTRY_CHPLAN_ENT("GR", 0x26, 1, 0xFB), /* Greece */
+	COUNTRY_CHPLAN_ENT("GS", 0x26, 1, 0x00), /* South Georgia and the Sandwich Islands (UK) */
+	COUNTRY_CHPLAN_ENT("GT", 0x34, 1, 0xF1), /* Guatemala */
+	COUNTRY_CHPLAN_ENT("GU", 0x34, 1, 0x00), /* Guam (USA) */
+	COUNTRY_CHPLAN_ENT("GW", 0x26, 1, 0xB0), /* Guinea-Bissau */
+	COUNTRY_CHPLAN_ENT("GY", 0x44, 1, 0x00), /* Guyana */
+	COUNTRY_CHPLAN_ENT("HK", 0x26, 1, 0xFB), /* Hong Kong */
+	COUNTRY_CHPLAN_ENT("HM", 0x45, 1, 0x00), /* Heard and McDonald Islands (Australia) */
+	COUNTRY_CHPLAN_ENT("HN", 0x32, 1, 0xF1), /* Honduras */
+	COUNTRY_CHPLAN_ENT("HR", 0x26, 1, 0xF9), /* Croatia */
+	COUNTRY_CHPLAN_ENT("HT", 0x34, 1, 0x50), /* Haiti */
+	COUNTRY_CHPLAN_ENT("HU", 0x26, 1, 0xFB), /* Hungary */
+	COUNTRY_CHPLAN_ENT("ID", 0x54, 0, 0xF3), /* Indonesia */
+	COUNTRY_CHPLAN_ENT("IE", 0x26, 1, 0xFB), /* Ireland */
+	COUNTRY_CHPLAN_ENT("IL", 0x47, 1, 0xF1), /* Israel */
+	COUNTRY_CHPLAN_ENT("IM", 0x26, 1, 0x00), /* Isle of Man (UK) */
+	COUNTRY_CHPLAN_ENT("IN", 0x47, 1, 0xF1), /* India */
+	COUNTRY_CHPLAN_ENT("IQ", 0x26, 1, 0x00), /* Iraq */
+	COUNTRY_CHPLAN_ENT("IR", 0x26, 0, 0x00), /* Iran */
+	COUNTRY_CHPLAN_ENT("IS", 0x26, 1, 0xFB), /* Iceland */
+	COUNTRY_CHPLAN_ENT("IT", 0x26, 1, 0xFB), /* Italy */
+	COUNTRY_CHPLAN_ENT("JE", 0x26, 1, 0x00), /* Jersey (UK) */
+	COUNTRY_CHPLAN_ENT("JM", 0x51, 1, 0xF1), /* Jamaica */
+	COUNTRY_CHPLAN_ENT("JO", 0x49, 1, 0xFB), /* Jordan */
+	COUNTRY_CHPLAN_ENT("JP", 0x27, 1, 0xFF), /* Japan- Telec */
+	COUNTRY_CHPLAN_ENT("KE", 0x47, 1, 0xF9), /* Kenya */
+	COUNTRY_CHPLAN_ENT("KG", 0x26, 1, 0xF1), /* Kyrgyzstan */
+	COUNTRY_CHPLAN_ENT("KH", 0x26, 1, 0xF1), /* Cambodia */
+	COUNTRY_CHPLAN_ENT("KI", 0x26, 1, 0x00), /* Kiribati */
+	COUNTRY_CHPLAN_ENT("KN", 0x34, 1, 0x00), /* Saint Kitts and Nevis */
+	COUNTRY_CHPLAN_ENT("KR", 0x28, 1, 0xFB), /* South Korea */
+	COUNTRY_CHPLAN_ENT("KW", 0x47, 1, 0xFB), /* Kuwait */
+	COUNTRY_CHPLAN_ENT("KY", 0x34, 1, 0x00), /* Cayman Islands (UK) */
+	COUNTRY_CHPLAN_ENT("KZ", 0x26, 1, 0x00), /* Kazakhstan */
+	COUNTRY_CHPLAN_ENT("LA", 0x26, 1, 0x00), /* Laos */
+	COUNTRY_CHPLAN_ENT("LB", 0x26, 1, 0xF1), /* Lebanon */
+	COUNTRY_CHPLAN_ENT("LC", 0x34, 1, 0x00), /* Saint Lucia */
+	COUNTRY_CHPLAN_ENT("LI", 0x26, 1, 0xFB), /* Liechtenstein */
+	COUNTRY_CHPLAN_ENT("LK", 0x26, 1, 0xF1), /* Sri Lanka */
+	COUNTRY_CHPLAN_ENT("LR", 0x26, 1, 0xB0), /* Liberia */
+	COUNTRY_CHPLAN_ENT("LS", 0x26, 1, 0xF1), /* Lesotho */
+	COUNTRY_CHPLAN_ENT("LT", 0x26, 1, 0xFB), /* Lithuania */
+	COUNTRY_CHPLAN_ENT("LU", 0x26, 1, 0xFB), /* Luxembourg */
+	COUNTRY_CHPLAN_ENT("LV", 0x26, 1, 0xFB), /* Latvia */
+	COUNTRY_CHPLAN_ENT("LY", 0x26, 1, 0x00), /* Libya */
+	COUNTRY_CHPLAN_ENT("MA", 0x47, 1, 0xF1), /* Morocco */
+	COUNTRY_CHPLAN_ENT("MC", 0x26, 1, 0xFB), /* Monaco */
+	COUNTRY_CHPLAN_ENT("MD", 0x26, 1, 0xF1), /* Moldova */
+	COUNTRY_CHPLAN_ENT("ME", 0x26, 1, 0xF1), /* Montenegro */
+	COUNTRY_CHPLAN_ENT("MF", 0x34, 1, 0x00), /* Saint Martin */
+	COUNTRY_CHPLAN_ENT("MG", 0x26, 1, 0x20), /* Madagascar */
+	COUNTRY_CHPLAN_ENT("MH", 0x34, 1, 0x00), /* Marshall Islands (USA) */
+	COUNTRY_CHPLAN_ENT("MK", 0x26, 1, 0xF1), /* Republic of Macedonia (FYROM) */
+	COUNTRY_CHPLAN_ENT("ML", 0x26, 1, 0xB0), /* Mali */
+	COUNTRY_CHPLAN_ENT("MM", 0x26, 1, 0x00), /* Burma (Myanmar) */
+	COUNTRY_CHPLAN_ENT("MN", 0x26, 1, 0x00), /* Mongolia */
+	COUNTRY_CHPLAN_ENT("MO", 0x26, 1, 0x00), /* Macau */
+	COUNTRY_CHPLAN_ENT("MP", 0x34, 1, 0x00), /* Northern Mariana Islands (USA) */
+	COUNTRY_CHPLAN_ENT("MQ", 0x26, 1, 0x40), /* Martinique (France) */
+	COUNTRY_CHPLAN_ENT("MR", 0x26, 1, 0xA0), /* Mauritania */
+	COUNTRY_CHPLAN_ENT("MS", 0x26, 1, 0x00), /* Montserrat (UK) */
+	COUNTRY_CHPLAN_ENT("MT", 0x26, 1, 0xFB), /* Malta */
+	COUNTRY_CHPLAN_ENT("MU", 0x26, 1, 0xB0), /* Mauritius */
+	COUNTRY_CHPLAN_ENT("MV", 0x26, 1, 0x00), /* Maldives */
+	COUNTRY_CHPLAN_ENT("MW", 0x26, 1, 0xB0), /* Malawi */
+	COUNTRY_CHPLAN_ENT("MX", 0x34, 1, 0xF1), /* Mexico */
+	COUNTRY_CHPLAN_ENT("MY", 0x47, 1, 0xF1), /* Malaysia */
+	COUNTRY_CHPLAN_ENT("MZ", 0x26, 1, 0xF1), /* Mozambique */
+	COUNTRY_CHPLAN_ENT("NA", 0x26, 0, 0x00), /* Namibia */
+	COUNTRY_CHPLAN_ENT("NC", 0x26, 1, 0x00), /* New Caledonia */
+	COUNTRY_CHPLAN_ENT("NE", 0x26, 1, 0xB0), /* Niger */
+	COUNTRY_CHPLAN_ENT("NF", 0x45, 1, 0x00), /* Norfolk Island (Australia) */
+	COUNTRY_CHPLAN_ENT("NG", 0x50, 1, 0xF9), /* Nigeria */
+	COUNTRY_CHPLAN_ENT("NI", 0x34, 1, 0xF1), /* Nicaragua */
+	COUNTRY_CHPLAN_ENT("NL", 0x26, 1, 0xFB), /* Netherlands */
+	COUNTRY_CHPLAN_ENT("NO", 0x26, 1, 0xFB), /* Norway */
+	COUNTRY_CHPLAN_ENT("NP", 0x47, 1, 0xF0), /* Nepal */
+	COUNTRY_CHPLAN_ENT("NR", 0x26, 1, 0x00), /* Nauru */
+	COUNTRY_CHPLAN_ENT("NU", 0x45, 1, 0x00), /* Niue */
+	COUNTRY_CHPLAN_ENT("NZ", 0x45, 1, 0xFB), /* New Zealand */
+	COUNTRY_CHPLAN_ENT("OM", 0x26, 1, 0xF9), /* Oman */
+	COUNTRY_CHPLAN_ENT("PA", 0x34, 1, 0xF1), /* Panama */
+	COUNTRY_CHPLAN_ENT("PE", 0x34, 1, 0xF1), /* Peru */
+	COUNTRY_CHPLAN_ENT("PF", 0x26, 1, 0x00), /* French Polynesia (France) */
+	COUNTRY_CHPLAN_ENT("PG", 0x26, 1, 0xF1), /* Papua New Guinea */
+	COUNTRY_CHPLAN_ENT("PH", 0x26, 1, 0xF1), /* Philippines */
+	COUNTRY_CHPLAN_ENT("PK", 0x51, 1, 0xF1), /* Pakistan */
+	COUNTRY_CHPLAN_ENT("PL", 0x26, 1, 0xFB), /* Poland */
+	COUNTRY_CHPLAN_ENT("PM", 0x26, 1, 0x00), /* Saint Pierre and Miquelon (France) */
+	COUNTRY_CHPLAN_ENT("PR", 0x34, 1, 0xF1), /* Puerto Rico */
+	COUNTRY_CHPLAN_ENT("PT", 0x26, 1, 0xFB), /* Portugal */
+	COUNTRY_CHPLAN_ENT("PW", 0x34, 1, 0x00), /* Palau */
+	COUNTRY_CHPLAN_ENT("PY", 0x34, 1, 0xF1), /* Paraguay */
+	COUNTRY_CHPLAN_ENT("QA", 0x51, 1, 0xF9), /* Qatar */
+	COUNTRY_CHPLAN_ENT("RE", 0x26, 1, 0x00), /* Reunion (France) */
+	COUNTRY_CHPLAN_ENT("RO", 0x26, 1, 0xF1), /* Romania */
+	COUNTRY_CHPLAN_ENT("RS", 0x26, 1, 0xF1), /* Serbia, Kosovo */
+	COUNTRY_CHPLAN_ENT("RU", 0x59, 1, 0xFB), /* Russia(fac/gost), Kaliningrad */
+	COUNTRY_CHPLAN_ENT("RW", 0x26, 1, 0xB0), /* Rwanda */
+	COUNTRY_CHPLAN_ENT("SA", 0x26, 1, 0xFB), /* Saudi Arabia */
+	COUNTRY_CHPLAN_ENT("SB", 0x26, 1, 0x00), /* Solomon Islands */
+	COUNTRY_CHPLAN_ENT("SC", 0x34, 1, 0x90), /* Seychelles */
+	COUNTRY_CHPLAN_ENT("SE", 0x26, 1, 0xFB), /* Sweden */
+	COUNTRY_CHPLAN_ENT("SG", 0x47, 1, 0xFB), /* Singapore */
+	COUNTRY_CHPLAN_ENT("SH", 0x26, 1, 0x00), /* Saint Helena (UK) */
+	COUNTRY_CHPLAN_ENT("SI", 0x26, 1, 0xFB), /* Slovenia */
+	COUNTRY_CHPLAN_ENT("SJ", 0x26, 1, 0x00), /* Svalbard (Norway) */
+	COUNTRY_CHPLAN_ENT("SK", 0x26, 1, 0xFB), /* Slovakia */
+	COUNTRY_CHPLAN_ENT("SL", 0x26, 1, 0xB0), /* Sierra Leone */
+	COUNTRY_CHPLAN_ENT("SM", 0x26, 1, 0x00), /* San Marino */
+	COUNTRY_CHPLAN_ENT("SN", 0x26, 1, 0xF1), /* Senegal */
+	COUNTRY_CHPLAN_ENT("SO", 0x26, 1, 0x00), /* Somalia */
+	COUNTRY_CHPLAN_ENT("SR", 0x34, 1, 0x00), /* Suriname */
+	COUNTRY_CHPLAN_ENT("ST", 0x34, 1, 0x80), /* Sao Tome and Principe */
+	COUNTRY_CHPLAN_ENT("SV", 0x30, 1, 0xF1), /* El Salvador */
+	COUNTRY_CHPLAN_ENT("SX", 0x34, 1, 0x00), /* Sint Marteen */
+	COUNTRY_CHPLAN_ENT("SZ", 0x26, 1, 0x20), /* Swaziland */
+	COUNTRY_CHPLAN_ENT("TC", 0x26, 1, 0x00), /* Turks and Caicos Islands (UK) */
+	COUNTRY_CHPLAN_ENT("TD", 0x26, 1, 0xB0), /* Chad */
+	COUNTRY_CHPLAN_ENT("TF", 0x26, 1, 0x80), /* French Southern and Antarctic Lands (FR Southern Territories) */
+	COUNTRY_CHPLAN_ENT("TG", 0x26, 1, 0xB0), /* Togo */
+	COUNTRY_CHPLAN_ENT("TH", 0x26, 1, 0xF1), /* Thailand */
+	COUNTRY_CHPLAN_ENT("TJ", 0x26, 1, 0x40), /* Tajikistan */
+	COUNTRY_CHPLAN_ENT("TK", 0x45, 1, 0x00), /* Tokelau */
+	COUNTRY_CHPLAN_ENT("TM", 0x26, 1, 0x00), /* Turkmenistan */
+	COUNTRY_CHPLAN_ENT("TN", 0x47, 1, 0xF1), /* Tunisia */
+	COUNTRY_CHPLAN_ENT("TO", 0x26, 1, 0x00), /* Tonga */
+	COUNTRY_CHPLAN_ENT("TR", 0x26, 1, 0xF1), /* Turkey, Northern Cyprus */
+	COUNTRY_CHPLAN_ENT("TT", 0x42, 1, 0xF1), /* Trinidad & Tobago */
+	COUNTRY_CHPLAN_ENT("TW", 0x39, 1, 0xFF), /* Taiwan */
+	COUNTRY_CHPLAN_ENT("TZ", 0x26, 1, 0xF0), /* Tanzania */
+	COUNTRY_CHPLAN_ENT("UA", 0x26, 1, 0xFB), /* Ukraine */
+	COUNTRY_CHPLAN_ENT("UG", 0x26, 1, 0xF1), /* Uganda */
+	COUNTRY_CHPLAN_ENT("US", 0x34, 1, 0xFF), /* United States of America (USA) */
+	COUNTRY_CHPLAN_ENT("UY", 0x34, 1, 0xF1), /* Uruguay */
+	COUNTRY_CHPLAN_ENT("UZ", 0x47, 1, 0xF0), /* Uzbekistan */
+	COUNTRY_CHPLAN_ENT("VA", 0x26, 1, 0x00), /* Holy See (Vatican City) */
+	COUNTRY_CHPLAN_ENT("VC", 0x34, 1, 0x10), /* Saint Vincent and the Grenadines */
+	COUNTRY_CHPLAN_ENT("VE", 0x30, 1, 0xF1), /* Venezuela */
+	COUNTRY_CHPLAN_ENT("VI", 0x34, 1, 0x00), /* United States Virgin Islands (USA) */
+	COUNTRY_CHPLAN_ENT("VN", 0x26, 1, 0xF1), /* Vietnam */
+	COUNTRY_CHPLAN_ENT("VU", 0x26, 1, 0x00), /* Vanuatu */
+	COUNTRY_CHPLAN_ENT("WF", 0x26, 1, 0x00), /* Wallis and Futuna (France) */
+	COUNTRY_CHPLAN_ENT("WS", 0x34, 1, 0x00), /* Samoa */
+	COUNTRY_CHPLAN_ENT("YE", 0x26, 1, 0x40), /* Yemen */
+	COUNTRY_CHPLAN_ENT("YT", 0x26, 1, 0x80), /* Mayotte (France) */
+	COUNTRY_CHPLAN_ENT("ZA", 0x26, 1, 0xF1), /* South Africa */
+	COUNTRY_CHPLAN_ENT("ZM", 0x26, 1, 0xB0), /* Zambia */
+	COUNTRY_CHPLAN_ENT("ZW", 0x26, 1, 0xF1), /* Zimbabwe */
+};
+
+static u16 const country_chplan_map_sz = sizeof(country_chplan_map) / sizeof(struct country_chplan);
+
+/*
+* rtw_get_chplan_from_country -
+* @country_code: string of country code
+*
+* Return pointer of struct country_chplan entry or NULL when unsupported country_code is given
+*/
+const struct country_chplan *rtw_get_chplan_from_country(const char *country_code)
+{
+	const struct country_chplan *ent = NULL;
+	const struct country_chplan *map = NULL;
+	u16 map_sz = 0;
+	char code[2];
+	int i;
+
+	code[0] = alpha_to_upper(country_code[0]);
+	code[1] = alpha_to_upper(country_code[1]);
+
+#if !defined(CONFIG_CUSTOMIZED_COUNTRY_CHPLAN_MAP) && RTW_DEF_MODULE_REGULATORY_CERT
+	ent = rtw_def_module_get_chplan_from_country(code);
+	if (ent != NULL)
+		goto exit;
+#endif
+
+#ifdef CONFIG_CUSTOMIZED_COUNTRY_CHPLAN_MAP
+	map = CUSTOMIZED_country_chplan_map;
+	map_sz = CUSTOMIZED_country_chplan_map_sz;
+#else
+	map = country_chplan_map;
+	map_sz = country_chplan_map_sz;
+#endif
+
+	for (i = 0; i < map_sz; i++) {
+		if (strncmp(code, map[i].alpha2, 2) == 0) {
+			ent = &map[i];
+			break;
+		}
+	}
+
+exit:
+	#if RTW_DEF_MODULE_REGULATORY_CERT
+	if (ent && !(COUNTRY_CHPLAN_DEF_MODULE_FALGS(ent) & RTW_DEF_MODULE_REGULATORY_CERT))
+		ent = NULL;
+	#endif
+
+	return ent;
+}
+
+int rtw_ch_to_bb_gain_sel(int ch)
+{
+	int sel = -1;
+
+	if (ch >= 1 && ch <= 14)
+		sel = BB_GAIN_2G;
+#ifdef CONFIG_IEEE80211_BAND_5GHZ
+	else if (ch >= 36 && ch < 48)
+		sel = BB_GAIN_5GLB1;
+	else if (ch >= 52 && ch <= 64)
+		sel = BB_GAIN_5GLB2;
+	else if (ch >= 100 && ch <= 120)
+		sel = BB_GAIN_5GMB1;
+	else if (ch >= 124 && ch <= 144)
+		sel = BB_GAIN_5GMB2;
+	else if (ch >= 149 && ch <= 177)
+		sel = BB_GAIN_5GHB;
+#endif
+
+	return sel;
+}
+
+static s8 rtw_rf_get_kfree_tx_gain_offset(_adapter *padapter, u8 path, u8 ch)
+{
+	s8 kfree_offset = 0;
+
+#ifdef CONFIG_RF_POWER_TRIM
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(padapter);
+	struct kfree_data_t *kfree_data = GET_KFREE_DATA(padapter);
+	s8 bb_gain_sel = rtw_ch_to_bb_gain_sel(ch);
+
+	if (bb_gain_sel < BB_GAIN_2G || bb_gain_sel >= BB_GAIN_NUM) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	if (kfree_data->flag & KFREE_FLAG_ON) {
+		kfree_offset = kfree_data->bb_gain[bb_gain_sel][path];
+		RTW_INFO("%s path:%u, ch:%u, bb_gain_sel:%d, kfree_offset:%d\n",
+			 __func__, path, ch, bb_gain_sel, kfree_offset);
+	}
+exit:
+#endif /* CONFIG_RF_POWER_TRIM */
+	return kfree_offset;
+}
+
+void rtw_rf_set_tx_gain_offset(_adapter *adapter, u8 path, s8 offset)
+{
+	u8 write_value;
+	u8 target_path = 0;
+	u32 val32 = 0;
+
+	target_path = path;
+	RTW_INFO("kfree gain_offset 0x55:0x%x ", rtw_hal_read_rfreg(adapter, target_path, 0x55, 0xffffffff));
+	
+	switch (rtw_get_chip_type(adapter)) {
+	default:
+		rtw_warn_on(1);
+		break;
+	}
+	
+	val32 = rtw_hal_read_rfreg(adapter, target_path, 0x55, 0xffffffff);
+	RTW_INFO(" after :0x%x\n", val32);
+}
+
+void rtw_rf_apply_tx_gain_offset(_adapter *adapter, u8 ch)
+{
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+	s8 kfree_offset = 0;
+	s8 tx_pwr_track_offset = 0; /* TODO: 8814A should consider tx pwr track when setting tx gain offset */
+	s8 total_offset;
+	int i, total = 0;
+
+	total = hal_data->NumTotalRFPath;
+
+	for (i = 0; i < total; i++) {
+		kfree_offset = rtw_rf_get_kfree_tx_gain_offset(adapter, i, ch);
+		total_offset = kfree_offset + tx_pwr_track_offset;
+		rtw_rf_set_tx_gain_offset(adapter, i, total_offset);
+	}
+}
+
+bool rtw_is_dfs_range(u32 hi, u32 lo)
+{
+	return rtw_is_range_overlap(hi, lo, 5720 + 10, 5260 - 10) ? true : false;
+}
+
+bool rtw_is_dfs_ch(u8 ch, u8 bw, u8 offset)
+{
+	u32 hi, lo;
+
+	if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == false)
+		return false;
+
+	return rtw_is_dfs_range(hi, lo) ? true : false;
+}
+
+bool rtw_is_long_cac_range(u32 hi, u32 lo, u8 dfs_region)
+{
+	return (dfs_region == PHYDM_DFS_DOMAIN_ETSI && rtw_is_range_overlap(hi, lo, 5660 + 10, 5600 - 10)) ? true : false;
+}
+
+bool rtw_is_long_cac_ch(u8 ch, u8 bw, u8 offset, u8 dfs_region)
+{
+	u32 hi, lo;
+
+	if (rtw_chbw_to_freq_range(ch, bw, offset, &hi, &lo) == false)
+		return false;
+
+	return rtw_is_long_cac_range(hi, lo, dfs_region) ? true : false;
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_security.c b/drivers/staging/rtl8188eu/core/rtw_security.c
new file mode 100644
index 000000000000..01e1728cdf0c
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_security.c
@@ -0,0 +1,3127 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define  _RTW_SECURITY_C_
+
+#include <drv_types.h>
+
+static const char *_security_type_str[] = {
+	"N/A",
+	"WEP40",
+	"TKIP",
+	"TKIP_WM",
+	"AES",
+	"WEP104",
+	"SMS4",
+	"WEP_WPA",
+	"BIP",
+};
+
+const char *security_type_str(u8 value)
+{
+#ifdef CONFIG_IEEE80211W
+	if (value <= _BIP_)
+#else
+	if (value <= _WEP_WPA_MIXED_)
+#endif
+		return _security_type_str[value];
+	return NULL;
+}
+
+#ifdef DBG_SW_SEC_CNT
+#define WEP_SW_ENC_CNT_INC(sec, ra) do {\
+	if (is_broadcast_mac_addr(ra)) \
+		sec->wep_sw_enc_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->wep_sw_enc_cnt_mc++; \
+	else \
+		sec->wep_sw_enc_cnt_uc++; \
+	} while (0)
+
+#define WEP_SW_DEC_CNT_INC(sec, ra) do {\
+	if (is_broadcast_mac_addr(ra)) \
+		sec->wep_sw_dec_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->wep_sw_dec_cnt_mc++; \
+	else \
+		sec->wep_sw_dec_cnt_uc++; \
+	} while (0)
+
+#define TKIP_SW_ENC_CNT_INC(sec, ra) do {\
+	if (is_broadcast_mac_addr(ra)) \
+		sec->tkip_sw_enc_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->tkip_sw_enc_cnt_mc++; \
+	else \
+		sec->tkip_sw_enc_cnt_uc++; \
+	} while (0)
+
+#define TKIP_SW_DEC_CNT_INC(sec, ra) do {\
+	if (is_broadcast_mac_addr(ra)) \
+		sec->tkip_sw_dec_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->tkip_sw_dec_cnt_mc++; \
+	else \
+		sec->tkip_sw_dec_cnt_uc++; \
+	} while (0)
+
+#define AES_SW_ENC_CNT_INC(sec, ra) do {\
+	if (is_broadcast_mac_addr(ra)) \
+		sec->aes_sw_enc_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->aes_sw_enc_cnt_mc++; \
+	else \
+		sec->aes_sw_enc_cnt_uc++; \
+	} while (0)
+
+#define AES_SW_DEC_CNT_INC(sec, ra) do {\
+	if (is_broadcast_mac_addr(ra)) \
+		sec->aes_sw_dec_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->aes_sw_dec_cnt_mc++; \
+	else \
+		sec->aes_sw_dec_cnt_uc++; \
+	} while (0)
+#else
+#define WEP_SW_ENC_CNT_INC(sec, ra)
+#define WEP_SW_DEC_CNT_INC(sec, ra)
+#define TKIP_SW_ENC_CNT_INC(sec, ra)
+#define TKIP_SW_DEC_CNT_INC(sec, ra)
+#define AES_SW_ENC_CNT_INC(sec, ra)
+#define AES_SW_DEC_CNT_INC(sec, ra)
+#endif /* DBG_SW_SEC_CNT */
+
+/* *****WEP related***** */
+
+#define CRC32_POLY 0x04c11db7
+
+struct arc4context {
+	u32 x;
+	u32 y;
+	u8 state[256];
+};
+
+
+static void arcfour_init(struct arc4context	*parc4ctx, u8 *key, u32	key_len)
+{
+	u32	t, u;
+	u32	keyindex;
+	u32	stateindex;
+	u8 *state;
+	u32	counter;
+	state = parc4ctx->state;
+	parc4ctx->x = 0;
+	parc4ctx->y = 0;
+	for (counter = 0; counter < 256; counter++)
+		state[counter] = (u8)counter;
+	keyindex = 0;
+	stateindex = 0;
+	for (counter = 0; counter < 256; counter++) {
+		t = state[counter];
+		stateindex = (stateindex + key[keyindex] + t) & 0xff;
+		u = state[stateindex];
+		state[stateindex] = (u8)t;
+		state[counter] = (u8)u;
+		if (++keyindex >= key_len)
+			keyindex = 0;
+	}
+}
+static u32 arcfour_byte(struct arc4context	*parc4ctx)
+{
+	u32 x;
+	u32 y;
+	u32 sx, sy;
+	u8 *state;
+	state = parc4ctx->state;
+	x = (parc4ctx->x + 1) & 0xff;
+	sx = state[x];
+	y = (sx + parc4ctx->y) & 0xff;
+	sy = state[y];
+	parc4ctx->x = x;
+	parc4ctx->y = y;
+	state[y] = (u8)sx;
+	state[x] = (u8)sy;
+	return state[(sx + sy) & 0xff];
+}
+
+
+static void arcfour_encrypt(struct arc4context	*parc4ctx,
+			    u8 *dest,
+			    u8 *src,
+			    u32 len)
+{
+	u32	i;
+	for (i = 0; i < len; i++)
+		dest[i] = src[i] ^ (unsigned char)arcfour_byte(parc4ctx);
+}
+
+static sint bcrc32initialized = 0;
+static u32 crc32_table[256];
+
+
+static u8 crc32_reverseBit(u8 data)
+{
+	return (u8)((data << 7) & 0x80) | ((data << 5) & 0x40) | ((data << 3) & 0x20) | ((data << 1) & 0x10) | ((data >> 1) & 0x08) | ((data >> 3) & 0x04) | ((data >> 5) & 0x02) | ((
+				data >> 7) & 0x01) ;
+}
+
+static void crc32_init(void)
+{
+	if (bcrc32initialized == 1)
+		goto exit;
+	else {
+		sint i, j;
+		u32 c;
+		u8 *p = (u8 *)&c, *p1;
+		u8 k;
+
+		c = 0x12340000;
+
+		for (i = 0; i < 256; ++i) {
+			k = crc32_reverseBit((u8)i);
+			for (c = ((u32)k) << 24, j = 8; j > 0; --j)
+				c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY : (c << 1);
+			p1 = (u8 *)&crc32_table[i];
+
+			p1[0] = crc32_reverseBit(p[3]);
+			p1[1] = crc32_reverseBit(p[2]);
+			p1[2] = crc32_reverseBit(p[1]);
+			p1[3] = crc32_reverseBit(p[0]);
+		}
+		bcrc32initialized = 1;
+	}
+exit:
+	return;
+}
+
+static __le32 getcrc32(u8 *buf, sint len)
+{
+	u8 *p;
+	u32  crc;
+	if (bcrc32initialized == 0)
+		crc32_init();
+
+	crc = 0xffffffff;       /* preload shift register, per CRC-32 spec */
+
+	for (p = buf; len > 0; ++p, --len)
+		crc = crc32_table[(crc ^ *p) & 0xff] ^ (crc >> 8);
+	return cpu_to_le32(~crc);    /* transmit complement, per CRC-32 spec */
+}
+
+
+/*
+	Need to consider the fragment  situation
+*/
+void rtw_wep_encrypt(_adapter *padapter, u8 *pxmitframe)
+{
+	/* exclude ICV */
+	unsigned char	crc[4];
+	struct arc4context	 mycontext;
+
+	sint	curfragnum, length;
+	u32	keylength;
+
+	u8	*pframe, *payload, *iv;   /* ,*wepkey */
+	u8	wepkey[16];
+	u8   hw_hdr_offset = 0;
+	struct	pkt_attrib	*pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct	xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+
+
+
+	if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+		return;
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+	hw_hdr_offset = TXDESC_SIZE +
+		(((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
+#else
+#ifdef CONFIG_TX_EARLY_MODE
+	hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE;
+#else
+	hw_hdr_offset = TXDESC_OFFSET;
+#endif
+#endif
+
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
+
+	/* start to encrypt each fragment */
+	if ((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) {
+		keylength = psecuritypriv->dot11DefKeylen[psecuritypriv->dot11PrivacyKeyIndex];
+
+		for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+			iv = pframe + pattrib->hdrlen;
+			memcpy(&wepkey[0], iv, 3);
+			memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength);
+			payload = pframe + pattrib->iv_len + pattrib->hdrlen;
+
+			if ((curfragnum + 1) == pattrib->nr_frags) {
+				/* the last fragment */
+
+				length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
+
+				*((__le32 *)crc) = getcrc32(payload, length);
+
+				arcfour_init(&mycontext, wepkey, 3 + keylength);
+				arcfour_encrypt(&mycontext, payload, payload, length);
+				arcfour_encrypt(&mycontext, payload + length, crc, 4);
+
+			} else {
+				length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len ;
+				*((__le32 *)crc) = getcrc32(payload, length);
+				arcfour_init(&mycontext, wepkey, 3 + keylength);
+				arcfour_encrypt(&mycontext, payload, payload, length);
+				arcfour_encrypt(&mycontext, payload + length, crc, 4);
+
+				pframe += pxmitpriv->frag_len;
+				pframe = (u8 *)RND4((SIZE_PTR)(pframe));
+
+			}
+
+		}
+
+		WEP_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra);
+	}
+
+
+}
+
+void rtw_wep_decrypt(_adapter  *padapter, u8 *precvframe)
+{
+	/* exclude ICV */
+	u8	crc[4];
+	struct arc4context	 mycontext;
+	sint	length;
+	u32	keylength;
+	u8	*pframe, *payload, *iv, wepkey[16];
+	u8	 keyindex;
+	struct	rx_pkt_attrib	*prxattrib = &(((union recv_frame *)precvframe)->u.hdr.attrib);
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+
+
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data;
+
+	/* start to decrypt recvframe */
+	if ((prxattrib->encrypt == _WEP40_) || (prxattrib->encrypt == _WEP104_)) {
+		iv = pframe + prxattrib->hdrlen;
+		/* keyindex=(iv[3]&0x3); */
+		keyindex = prxattrib->key_index;
+		keylength = psecuritypriv->dot11DefKeylen[keyindex];
+		memcpy(&wepkey[0], iv, 3);
+		/* memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0],keylength); */
+		memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0], keylength);
+		length = ((union recv_frame *)precvframe)->u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len;
+
+		payload = pframe + prxattrib->iv_len + prxattrib->hdrlen;
+
+		/* decrypt payload include icv */
+		arcfour_init(&mycontext, wepkey, 3 + keylength);
+		arcfour_encrypt(&mycontext, payload, payload,  length);
+
+		/* calculate icv and compare the icv */
+		*((__le32 *)crc) = getcrc32(payload, length - 4);
+
+
+		WEP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
+	}
+	return;
+}
+
+/* 3		=====TKIP related===== */
+
+static u32 secmicgetuint32(u8 *p)
+/* Convert from Byte[] to Us4Byte32 in a portable way */
+{
+	s32 i;
+	u32 res = 0;
+	for (i = 0; i < 4; i++)
+		res |= ((u32)(*p++)) << (8 * i);
+	return res;
+}
+
+static void secmicputuint32(u8 *p, u32 val)
+/* Convert from Us4Byte32 to Byte[] in a portable way */
+{
+	long i;
+	for (i = 0; i < 4; i++) {
+		*p++ = (u8)(val & 0xff);
+		val >>= 8;
+	}
+}
+
+static void secmicclear(struct mic_data *pmicdata)
+{
+	/* Reset the state to the empty message. */
+	pmicdata->L = pmicdata->K0;
+	pmicdata->R = pmicdata->K1;
+	pmicdata->nBytesInM = 0;
+	pmicdata->M = 0;
+}
+
+void rtw_secmicsetkey(struct mic_data *pmicdata, u8 *key)
+{
+	/* Set the key */
+	pmicdata->K0 = secmicgetuint32(key);
+	pmicdata->K1 = secmicgetuint32(key + 4);
+	/* and reset the message */
+	secmicclear(pmicdata);
+}
+
+void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b)
+{
+	/* Append the byte to our word-sized buffer */
+	pmicdata->M |= ((unsigned long)b) << (8 * pmicdata->nBytesInM);
+	pmicdata->nBytesInM++;
+	/* Process the word if it is full. */
+	if (pmicdata->nBytesInM >= 4) {
+		pmicdata->L ^= pmicdata->M;
+		pmicdata->R ^= ROL32(pmicdata->L, 17);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ((pmicdata->L & 0xff00ff00) >> 8) | ((pmicdata->L & 0x00ff00ff) << 8);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ROL32(pmicdata->L, 3);
+		pmicdata->L += pmicdata->R;
+		pmicdata->R ^= ROR32(pmicdata->L, 2);
+		pmicdata->L += pmicdata->R;
+		/* Clear the buffer */
+		pmicdata->M = 0;
+		pmicdata->nBytesInM = 0;
+	}
+}
+
+void rtw_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nbytes)
+{
+	/* This is simple */
+	while (nbytes > 0) {
+		rtw_secmicappendbyte(pmicdata, *src++);
+		nbytes--;
+	}
+}
+
+void rtw_secgetmic(struct mic_data *pmicdata, u8 *dst)
+{
+	/* Append the minimum padding */
+	rtw_secmicappendbyte(pmicdata, 0x5a);
+	rtw_secmicappendbyte(pmicdata, 0);
+	rtw_secmicappendbyte(pmicdata, 0);
+	rtw_secmicappendbyte(pmicdata, 0);
+	rtw_secmicappendbyte(pmicdata, 0);
+	/* and then zeroes until the length is a multiple of 4 */
+	while (pmicdata->nBytesInM != 0)
+		rtw_secmicappendbyte(pmicdata, 0);
+	/* The appendByte function has already computed the result. */
+	secmicputuint32(dst, pmicdata->L);
+	secmicputuint32(dst + 4, pmicdata->R);
+	/* Reset to the empty message. */
+	secmicclear(pmicdata);
+}
+
+
+void rtw_seccalctkipmic(u8 *key, u8 *header, u8 *data, u32 data_len, u8 *mic_code, u8 pri)
+{
+
+	struct mic_data	micdata;
+	u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
+	rtw_secmicsetkey(&micdata, key);
+	priority[0] = pri;
+
+	/* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */
+	if (header[1] & 1) { /* ToDS==1 */
+		rtw_secmicappend(&micdata, &header[16], 6);  /* DA */
+		if (header[1] & 2) /* From Ds==1 */
+			rtw_secmicappend(&micdata, &header[24], 6);
+		else
+			rtw_secmicappend(&micdata, &header[10], 6);
+	} else {	/* ToDS==0 */
+		rtw_secmicappend(&micdata, &header[4], 6);   /* DA */
+		if (header[1] & 2) /* From Ds==1 */
+			rtw_secmicappend(&micdata, &header[16], 6);
+		else
+			rtw_secmicappend(&micdata, &header[10], 6);
+
+	}
+	rtw_secmicappend(&micdata, &priority[0], 4);
+
+
+	rtw_secmicappend(&micdata, data, data_len);
+
+	rtw_secgetmic(&micdata, mic_code);
+}
+
+
+
+
+/* macros for extraction/creation of unsigned char/unsigned short values */
+#define RotR1(v16)   ((((v16) >> 1) & 0x7FFF) ^ (((v16) & 1) << 15))
+#define   Lo8(v16)   ((u8)((v16)       & 0x00FF))
+#define   Hi8(v16)   ((u8)(((v16) >> 8) & 0x00FF))
+#define  Lo16(v32)   ((u16)((v32)       & 0xFFFF))
+#define  Hi16(v32)   ((u16)(((v32) >> 16) & 0xFFFF))
+#define  Mk16(hi, lo) ((lo) ^ (((u16)(hi)) << 8))
+
+/* select the Nth 16-bit word of the temporal key unsigned char array TK[]  */
+#define  TK16(N)     Mk16(tk[2*(N)+1], tk[2*(N)])
+
+/* S-box lookup: 16 bits --> 16 bits */
+#define _S_(v16)     (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)])
+
+/* fixed algorithm "parameters" */
+#define PHASE1_LOOP_CNT   8    /* this needs to be "big enough"     */
+#define TA_SIZE           6    /*  48-bit transmitter address      */
+#define TK_SIZE          16    /* 128-bit temporal key             */
+#define P1K_SIZE         10    /*  80-bit Phase1 key               */
+#define RC4_KEY_SIZE     16    /* 128-bit RC4KEY (104 bits unknown) */
+
+
+/* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */
+static const unsigned short Sbox1[2][256] =      /* Sbox for hash (can be in ROM)    */
+{ {
+		0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
+		0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
+		0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
+		0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
+		0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
+		0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
+		0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
+		0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
+		0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
+		0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
+		0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
+		0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
+		0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
+		0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
+		0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
+		0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
+		0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
+		0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
+		0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
+		0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
+		0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
+		0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
+		0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
+		0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
+		0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
+		0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
+		0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
+		0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
+		0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
+		0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
+		0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
+		0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
+	},
+
+
+	{  /* second half of table is unsigned char-reversed version of first! */
+		0xA5C6, 0x84F8, 0x99EE, 0x8DF6, 0x0DFF, 0xBDD6, 0xB1DE, 0x5491,
+		0x5060, 0x0302, 0xA9CE, 0x7D56, 0x19E7, 0x62B5, 0xE64D, 0x9AEC,
+		0x458F, 0x9D1F, 0x4089, 0x87FA, 0x15EF, 0xEBB2, 0xC98E, 0x0BFB,
+		0xEC41, 0x67B3, 0xFD5F, 0xEA45, 0xBF23, 0xF753, 0x96E4, 0x5B9B,
+		0xC275, 0x1CE1, 0xAE3D, 0x6A4C, 0x5A6C, 0x417E, 0x02F5, 0x4F83,
+		0x5C68, 0xF451, 0x34D1, 0x08F9, 0x93E2, 0x73AB, 0x5362, 0x3F2A,
+		0x0C08, 0x5295, 0x6546, 0x5E9D, 0x2830, 0xA137, 0x0F0A, 0xB52F,
+		0x090E, 0x3624, 0x9B1B, 0x3DDF, 0x26CD, 0x694E, 0xCD7F, 0x9FEA,
+		0x1B12, 0x9E1D, 0x7458, 0x2E34, 0x2D36, 0xB2DC, 0xEEB4, 0xFB5B,
+		0xF6A4, 0x4D76, 0x61B7, 0xCE7D, 0x7B52, 0x3EDD, 0x715E, 0x9713,
+		0xF5A6, 0x68B9, 0x0000, 0x2CC1, 0x6040, 0x1FE3, 0xC879, 0xEDB6,
+		0xBED4, 0x468D, 0xD967, 0x4B72, 0xDE94, 0xD498, 0xE8B0, 0x4A85,
+		0x6BBB, 0x2AC5, 0xE54F, 0x16ED, 0xC586, 0xD79A, 0x5566, 0x9411,
+		0xCF8A, 0x10E9, 0x0604, 0x81FE, 0xF0A0, 0x4478, 0xBA25, 0xE34B,
+		0xF3A2, 0xFE5D, 0xC080, 0x8A05, 0xAD3F, 0xBC21, 0x4870, 0x04F1,
+		0xDF63, 0xC177, 0x75AF, 0x6342, 0x3020, 0x1AE5, 0x0EFD, 0x6DBF,
+		0x4C81, 0x1418, 0x3526, 0x2FC3, 0xE1BE, 0xA235, 0xCC88, 0x392E,
+		0x5793, 0xF255, 0x82FC, 0x477A, 0xACC8, 0xE7BA, 0x2B32, 0x95E6,
+		0xA0C0, 0x9819, 0xD19E, 0x7FA3, 0x6644, 0x7E54, 0xAB3B, 0x830B,
+		0xCA8C, 0x29C7, 0xD36B, 0x3C28, 0x79A7, 0xE2BC, 0x1D16, 0x76AD,
+		0x3BDB, 0x5664, 0x4E74, 0x1E14, 0xDB92, 0x0A0C, 0x6C48, 0xE4B8,
+		0x5D9F, 0x6EBD, 0xEF43, 0xA6C4, 0xA839, 0xA431, 0x37D3, 0x8BF2,
+		0x32D5, 0x438B, 0x596E, 0xB7DA, 0x8C01, 0x64B1, 0xD29C, 0xE049,
+		0xB4D8, 0xFAAC, 0x07F3, 0x25CF, 0xAFCA, 0x8EF4, 0xE947, 0x1810,
+		0xD56F, 0x88F0, 0x6F4A, 0x725C, 0x2438, 0xF157, 0xC773, 0x5197,
+		0x23CB, 0x7CA1, 0x9CE8, 0x213E, 0xDD96, 0xDC61, 0x860D, 0x850F,
+		0x90E0, 0x427C, 0xC471, 0xAACC, 0xD890, 0x0506, 0x01F7, 0x121C,
+		0xA3C2, 0x5F6A, 0xF9AE, 0xD069, 0x9117, 0x5899, 0x273A, 0xB927,
+		0x38D9, 0x13EB, 0xB32B, 0x3322, 0xBBD2, 0x70A9, 0x8907, 0xA733,
+		0xB62D, 0x223C, 0x9215, 0x20C9, 0x4987, 0xFFAA, 0x7850, 0x7AA5,
+		0x8F03, 0xF859, 0x8009, 0x171A, 0xDA65, 0x31D7, 0xC684, 0xB8D0,
+		0xC382, 0xB029, 0x775A, 0x111E, 0xCB7B, 0xFCA8, 0xD66D, 0x3A2C,
+	}
+};
+
+/*
+**********************************************************************
+* Routine: Phase 1 -- generate P1K, given TA, TK, IV32
+*
+* Inputs:
+*     tk[]      = temporal key                         [128 bits]
+*     ta[]      = transmitter's MAC address            [ 48 bits]
+*     iv32      = upper 32 bits of IV                  [ 32 bits]
+* Output:
+*     p1k[]     = Phase 1 key                          [ 80 bits]
+*
+* Note:
+*     This function only needs to be called every 2**16 packets,
+*     although in theory it could be called every packet.
+*
+**********************************************************************
+*/
+static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
+{
+	sint  i;
+	/* Initialize the 80 bits of P1K[] from IV32 and TA[0..5]    */
+	p1k[0]      = Lo16(iv32);
+	p1k[1]      = Hi16(iv32);
+	p1k[2]      = Mk16(ta[1], ta[0]); /* use TA[] as little-endian */
+	p1k[3]      = Mk16(ta[3], ta[2]);
+	p1k[4]      = Mk16(ta[5], ta[4]);
+
+	/* Now compute an unbalanced Feistel cipher with 80-bit block */
+	/* size on the 80-bit block P1K[], using the 128-bit key TK[] */
+	for (i = 0; i < PHASE1_LOOP_CNT ; i++) {
+		/* Each add operation here is mod 2**16 */
+		p1k[0] += _S_(p1k[4] ^ TK16((i & 1) + 0));
+		p1k[1] += _S_(p1k[0] ^ TK16((i & 1) + 2));
+		p1k[2] += _S_(p1k[1] ^ TK16((i & 1) + 4));
+		p1k[3] += _S_(p1k[2] ^ TK16((i & 1) + 6));
+		p1k[4] += _S_(p1k[3] ^ TK16((i & 1) + 0));
+		p1k[4] += (unsigned short)i;                     /* avoid "slide attacks" */
+	}
+}
+
+
+/*
+**********************************************************************
+* Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16
+*
+* Inputs:
+*     tk[]      = Temporal key                         [128 bits]
+*     p1k[]     = Phase 1 output key                   [ 80 bits]
+*     iv16      = low 16 bits of IV counter            [ 16 bits]
+* Output:
+*     rc4key[]  = the key used to encrypt the packet   [128 bits]
+*
+* Note:
+*     The value {TA,IV32,IV16} for Phase1/Phase2 must be unique
+*     across all packets using the same key TK value. Then, for a
+*     given value of TK[], this TKIP48 construction guarantees that
+*     the final RC4KEY value is unique across all packets.
+*
+* Suggested implementation optimization: if PPK[] is "overlaid"
+*     appropriately on RC4KEY[], there is no need for the final
+*     for loop below that copies the PPK[] result into RC4KEY[].
+*
+**********************************************************************
+*/
+static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
+{
+	sint  i;
+	u16 PPK[6];                          /* temporary key for mixing   */
+	/* Note: all adds in the PPK[] equations below are mod 2**16        */
+	for (i = 0; i < 5; i++)
+		PPK[i] = p1k[i];    /* first, copy P1K to PPK     */
+	PPK[5]  =  p1k[4] + iv16;            /* next,  add in IV16         */
+
+	/* Bijective non-linear mixing of the 96 bits of PPK[0..5]          */
+	PPK[0] +=    _S_(PPK[5] ^ TK16(0));   /* Mix key in each "round"     */
+	PPK[1] +=    _S_(PPK[0] ^ TK16(1));
+	PPK[2] +=    _S_(PPK[1] ^ TK16(2));
+	PPK[3] +=    _S_(PPK[2] ^ TK16(3));
+	PPK[4] +=    _S_(PPK[3] ^ TK16(4));
+	PPK[5] +=    _S_(PPK[4] ^ TK16(5));   /* Total # S-box lookups == 6 */
+
+	/* Final sweep: bijective, "linear". Rotates kill LSB correlations   */
+	PPK[0] +=  RotR1(PPK[5] ^ TK16(6));
+	PPK[1] +=  RotR1(PPK[0] ^ TK16(7));   /* Use all of TK[] in Phase2  */
+	PPK[2] +=  RotR1(PPK[1]);
+	PPK[3] +=  RotR1(PPK[2]);
+	PPK[4] +=  RotR1(PPK[3]);
+	PPK[5] +=  RotR1(PPK[4]);
+	/* Note: At this point, for a given key TK[0..15], the 96-bit output */
+	/*       value PPK[0..5] is guaranteed to be unique, as a function  */
+	/*       of the 96-bit "input" value   {TA,IV32,IV16}. That is, P1K  */
+	/*       is now a keyed permutation of {TA,IV32,IV16}.              */
+
+	/* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key   */
+	rc4key[0] = Hi8(iv16);                /* RC4KEY[0..2] is the WEP IV */
+	rc4key[1] = (Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys */
+	rc4key[2] = Lo8(iv16);
+	rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1);
+
+
+	/* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian)      */
+	for (i = 0; i < 6; i++) {
+		rc4key[4 + 2 * i] = Lo8(PPK[i]);
+		rc4key[5 + 2 * i] = Hi8(PPK[i]);
+	}
+}
+
+
+/* The hlen isn't include the IV */
+u32	rtw_tkip_encrypt(_adapter *padapter, u8 *pxmitframe)
+{
+	/* exclude ICV */
+	u16	pnl;
+	u32	pnh;
+	u8	rc4key[16];
+	u8   ttkey[16];
+	u8	crc[4];
+	u8   hw_hdr_offset = 0;
+	struct arc4context mycontext;
+	sint			curfragnum, length;
+	u32	prwskeylen;
+
+	u8	*pframe, *payload, *iv, *prwskey;
+	union pn48 dot11txpn;
+	/* struct	sta_info		*stainfo; */
+	struct	pkt_attrib	*pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct	xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	u32	res = _SUCCESS;
+
+	if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+		return _FAIL;
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+	hw_hdr_offset = TXDESC_SIZE +
+		(((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
+#else
+#ifdef CONFIG_TX_EARLY_MODE
+	hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE;
+#else
+	hw_hdr_offset = TXDESC_OFFSET;
+#endif
+#endif
+
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
+	/* 4 start to encrypt each fragment */
+	if (pattrib->encrypt == _TKIP_) {
+
+		/*
+				if(pattrib->psta)
+				{
+					stainfo = pattrib->psta;
+				}
+				else
+				{
+					RTW_INFO("%s, call rtw_get_stainfo()\n", __func__);
+					stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] );
+				}
+		*/
+		/* if (stainfo!=NULL) */
+		{
+			/*
+						if(!(stainfo->state &_FW_LINKED))
+						{
+							RTW_INFO("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
+							return _FAIL;
+						}
+			*/
+
+			if (IS_MCAST(pattrib->ra))
+				prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+			else {
+				/* prwskey=&stainfo->dot118021x_UncstKey.skey[0]; */
+				prwskey = pattrib->dot118021x_UncstKey.skey;
+			}
+
+			prwskeylen = 16;
+
+			for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+				iv = pframe + pattrib->hdrlen;
+				payload = pframe + pattrib->iv_len + pattrib->hdrlen;
+
+				GET_TKIP_PN(iv, dot11txpn);
+
+				pnl = (u16)(dot11txpn.val);
+				pnh = (u32)(dot11txpn.val >> 16);
+
+				phase1((u16 *)&ttkey[0], prwskey, &pattrib->ta[0], pnh);
+
+				phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0], pnl);
+
+				if ((curfragnum + 1) == pattrib->nr_frags) {	/* 4 the last fragment */
+					length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
+					*((__le32 *)crc) = getcrc32(payload, length); /* modified by Amy*/
+
+					arcfour_init(&mycontext, rc4key, 16);
+					arcfour_encrypt(&mycontext, payload, payload, length);
+					arcfour_encrypt(&mycontext, payload + length, crc, 4);
+
+				} else {
+					length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len ;
+					*((__le32 *)crc) = getcrc32(payload, length); /* modified by Amy*/
+					arcfour_init(&mycontext, rc4key, 16);
+					arcfour_encrypt(&mycontext, payload, payload, length);
+					arcfour_encrypt(&mycontext, payload + length, crc, 4);
+
+					pframe += pxmitpriv->frag_len;
+					pframe = (u8 *)RND4((SIZE_PTR)(pframe));
+
+				}
+			}
+
+			TKIP_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra);
+		}
+		/*
+				else{
+					RTW_INFO("%s, psta==NUL\n", __func__);
+					res=_FAIL;
+				}
+		*/
+
+	}
+	return res;
+
+}
+
+
+/* The hlen isn't include the IV */
+u32 rtw_tkip_decrypt(_adapter *padapter, u8 *precvframe)
+{
+	/* exclude ICV */
+	u16 pnl;
+	u32 pnh;
+	u8   rc4key[16];
+	u8   ttkey[16];
+	u8	crc[4];
+	struct arc4context mycontext;
+	sint			length;
+	u32	prwskeylen;
+
+	u8	*pframe, *payload, *iv, *prwskey;
+	union pn48 dot11txpn;
+	struct	sta_info		*stainfo;
+	struct	rx_pkt_attrib	*prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	/*	struct	recv_priv		*precvpriv=&padapter->recvpriv; */
+	u32		res = _SUCCESS;
+
+
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data;
+
+	/* 4 start to decrypt recvframe */
+	if (prxattrib->encrypt == _TKIP_) {
+
+		stainfo = rtw_get_stainfo(&padapter->stapriv , &prxattrib->ta[0]);
+		if (stainfo != NULL) {
+
+			if (IS_MCAST(prxattrib->ra)) {
+				static u32 start = 0;
+				static u32 no_gkey_bc_cnt = 0;
+				static u32 no_gkey_mc_cnt = 0;
+
+				if (psecuritypriv->binstallGrpkey == false) {
+					res = _FAIL;
+
+					if (start == 0)
+						start = jiffies;
+
+					if (is_broadcast_mac_addr(prxattrib->ra))
+						no_gkey_bc_cnt++;
+					else
+						no_gkey_mc_cnt++;
+
+					if (rtw_get_passing_time_ms(start) > 1000) {
+						if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
+							RTW_PRINT(FUNC_ADPT_FMT" no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
+								FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt);
+						}
+						start = jiffies;
+						no_gkey_bc_cnt = 0;
+						no_gkey_mc_cnt = 0;
+					}
+					goto exit;
+				}
+
+				if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
+					RTW_PRINT(FUNC_ADPT_FMT" gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
+						FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt);
+				}
+				start = 0;
+				no_gkey_bc_cnt = 0;
+				no_gkey_mc_cnt = 0;
+
+				/* RTW_INFO("rx bc/mc packets, to perform sw rtw_tkip_decrypt\n"); */
+				/* prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; */
+				prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+				prwskeylen = 16;
+			} else {
+				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+				prwskeylen = 16;
+			}
+
+			iv = pframe + prxattrib->hdrlen;
+			payload = pframe + prxattrib->iv_len + prxattrib->hdrlen;
+			length = ((union recv_frame *)precvframe)->u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len;
+
+			GET_TKIP_PN(iv, dot11txpn);
+
+			pnl = (u16)(dot11txpn.val);
+			pnh = (u32)(dot11txpn.val >> 16);
+
+			phase1((u16 *)&ttkey[0], prwskey, &prxattrib->ta[0], pnh);
+			phase2(&rc4key[0], prwskey, (unsigned short *)&ttkey[0], pnl);
+
+			/* 4 decrypt payload include icv */
+
+			arcfour_init(&mycontext, rc4key, 16);
+			arcfour_encrypt(&mycontext, payload, payload, length);
+
+			*((__le32 *)crc) = getcrc32(payload, length - 4);
+
+			if (crc[3] != payload[length - 1] || crc[2] != payload[length - 2] || crc[1] != payload[length - 3] || crc[0] != payload[length - 4]) {
+				res = _FAIL;
+			}
+
+			TKIP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
+		} else {
+			res = _FAIL;
+		}
+
+	}
+exit:
+	return res;
+
+}
+
+
+/* 3			=====AES related===== */
+
+
+
+#define MAX_MSG_SIZE	2048
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+static  u8 sbox_table[256] = {
+	0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
+	0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
+	0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
+	0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
+	0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
+	0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
+	0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
+	0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
+	0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
+	0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
+	0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
+	0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
+	0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
+	0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
+	0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
+	0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
+	0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
+	0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
+	0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
+	0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
+	0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
+	0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
+	0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
+	0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
+	0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
+	0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
+	0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
+	0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
+	0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
+	0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
+	0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
+	0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
+};
+
+/*****************************/
+/**** Function Prototypes ****/
+/*****************************/
+
+static void bitwise_xor(u8 *ina, u8 *inb, u8 *out);
+static void construct_mic_iv(
+	u8 *mic_header1,
+	sint qc_exists,
+	sint a4_exists,
+	u8 *mpdu,
+	uint payload_length,
+	u8 *pn_vector,
+	uint frtype);/* add for CONFIG_IEEE80211W, none 11w also can use */
+static void construct_mic_header1(
+	u8 *mic_header1,
+	sint header_length,
+	u8 *mpdu,
+	uint frtype);/* add for CONFIG_IEEE80211W, none 11w also can use */
+static void construct_mic_header2(
+	u8 *mic_header2,
+	u8 *mpdu,
+	sint a4_exists,
+	sint qc_exists);
+static void construct_ctr_preload(
+	u8 *ctr_preload,
+	sint a4_exists,
+	sint qc_exists,
+	u8 *mpdu,
+	u8 *pn_vector,
+	sint c,
+	uint frtype);/* add for CONFIG_IEEE80211W, none 11w also can use */
+static void xor_128(u8 *a, u8 *b, u8 *out);
+static void xor_32(u8 *a, u8 *b, u8 *out);
+static u8 sbox(u8 a);
+static void next_key(u8 *key, sint round);
+static void byte_sub(u8 *in, u8 *out);
+static void shift_row(u8 *in, u8 *out);
+static void mix_column(u8 *in, u8 *out);
+static void add_round_key(u8 *shiftrow_in,
+			  u8 *mcol_in,
+			  u8 *block_in,
+			  sint round,
+			  u8 *out);
+static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext);
+
+
+/****************************************/
+/* aes128k128d()                       */
+/* Performs a 128 bit AES encrypt with */
+/* 128 bit data.                       */
+/****************************************/
+static void xor_128(u8 *a, u8 *b, u8 *out)
+{
+	sint i;
+	for (i = 0; i < 16; i++)
+		out[i] = a[i] ^ b[i];
+}
+
+
+static void xor_32(u8 *a, u8 *b, u8 *out)
+{
+	sint i;
+	for (i = 0; i < 4; i++)
+		out[i] = a[i] ^ b[i];
+}
+
+
+static u8 sbox(u8 a)
+{
+	return sbox_table[(sint)a];
+}
+
+
+static void next_key(u8 *key, sint round)
+{
+	u8 rcon;
+	u8 sbox_key[4];
+	u8 rcon_table[12] = {
+		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+		0x1b, 0x36, 0x36, 0x36
+	};
+	sbox_key[0] = sbox(key[13]);
+	sbox_key[1] = sbox(key[14]);
+	sbox_key[2] = sbox(key[15]);
+	sbox_key[3] = sbox(key[12]);
+
+	rcon = rcon_table[round];
+
+	xor_32(&key[0], sbox_key, &key[0]);
+	key[0] = key[0] ^ rcon;
+
+	xor_32(&key[4], &key[0], &key[4]);
+	xor_32(&key[8], &key[4], &key[8]);
+	xor_32(&key[12], &key[8], &key[12]);
+}
+
+
+static void byte_sub(u8 *in, u8 *out)
+{
+	sint i;
+	for (i = 0; i < 16; i++)
+		out[i] = sbox(in[i]);
+}
+
+
+static void shift_row(u8 *in, u8 *out)
+{
+	out[0] =  in[0];
+	out[1] =  in[5];
+	out[2] =  in[10];
+	out[3] =  in[15];
+	out[4] =  in[4];
+	out[5] =  in[9];
+	out[6] =  in[14];
+	out[7] =  in[3];
+	out[8] =  in[8];
+	out[9] =  in[13];
+	out[10] = in[2];
+	out[11] = in[7];
+	out[12] = in[12];
+	out[13] = in[1];
+	out[14] = in[6];
+	out[15] = in[11];
+}
+
+
+static void mix_column(u8 *in, u8 *out)
+{
+	sint i;
+	u8 add1b[4];
+	u8 add1bf7[4];
+	u8 rotl[4];
+	u8 swap_halfs[4];
+	u8 andf7[4];
+	u8 rotr[4];
+	u8 temp[4];
+	u8 tempb[4];
+	for (i = 0 ; i < 4; i++) {
+		if ((in[i] & 0x80) == 0x80)
+			add1b[i] = 0x1b;
+		else
+			add1b[i] = 0x00;
+	}
+
+	swap_halfs[0] = in[2];    /* Swap halfs */
+	swap_halfs[1] = in[3];
+	swap_halfs[2] = in[0];
+	swap_halfs[3] = in[1];
+
+	rotl[0] = in[3];        /* Rotate left 8 bits */
+	rotl[1] = in[0];
+	rotl[2] = in[1];
+	rotl[3] = in[2];
+
+	andf7[0] = in[0] & 0x7f;
+	andf7[1] = in[1] & 0x7f;
+	andf7[2] = in[2] & 0x7f;
+	andf7[3] = in[3] & 0x7f;
+
+	for (i = 3; i > 0; i--) { /* logical shift left 1 bit */
+		andf7[i] = andf7[i] << 1;
+		if ((andf7[i - 1] & 0x80) == 0x80)
+			andf7[i] = (andf7[i] | 0x01);
+	}
+	andf7[0] = andf7[0] << 1;
+	andf7[0] = andf7[0] & 0xfe;
+
+	xor_32(add1b, andf7, add1bf7);
+
+	xor_32(in, add1bf7, rotr);
+
+	temp[0] = rotr[0];         /* Rotate right 8 bits */
+	rotr[0] = rotr[1];
+	rotr[1] = rotr[2];
+	rotr[2] = rotr[3];
+	rotr[3] = temp[0];
+
+	xor_32(add1bf7, rotr, temp);
+	xor_32(swap_halfs, rotl, tempb);
+	xor_32(temp, tempb, out);
+}
+
+
+static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext)
+{
+	sint round;
+	sint i;
+	u8 intermediatea[16];
+	u8 intermediateb[16];
+	u8 round_key[16];
+	for (i = 0; i < 16; i++)
+		round_key[i] = key[i];
+
+	for (round = 0; round < 11; round++) {
+		if (round == 0) {
+			xor_128(round_key, data, ciphertext);
+			next_key(round_key, round);
+		} else if (round == 10) {
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			xor_128(intermediateb, round_key, ciphertext);
+		} else { /* 1 - 9 */
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			mix_column(&intermediateb[0], &intermediatea[0]);
+			mix_column(&intermediateb[4], &intermediatea[4]);
+			mix_column(&intermediateb[8], &intermediatea[8]);
+			mix_column(&intermediateb[12], &intermediatea[12]);
+			xor_128(intermediatea, round_key, ciphertext);
+			next_key(round_key, round);
+		}
+	}
+}
+
+
+/************************************************/
+/* construct_mic_iv()                          */
+/* Builds the MIC IV from header fields and PN */
+/* Baron think the function is construct CCM   */
+/* nonce                                       */
+/************************************************/
+static void construct_mic_iv(
+	u8 *mic_iv,
+	sint qc_exists,
+	sint a4_exists,
+	u8 *mpdu,
+	uint payload_length,
+	u8 *pn_vector,
+	uint frtype/* add for CONFIG_IEEE80211W, none 11w also can use */
+)
+{
+	sint i;
+	mic_iv[0] = 0x59;
+	if (qc_exists && a4_exists)
+		mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC          */
+	if (qc_exists && !a4_exists)
+		mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4   */
+	if (!qc_exists)
+		mic_iv[1] = 0x00;
+#ifdef CONFIG_IEEE80211W
+	/* 802.11w management frame should set management bit(4) */
+	if (frtype == WIFI_MGT_TYPE)
+		mic_iv[1] |= BIT(4);
+#endif /* CONFIG_IEEE80211W */
+	for (i = 2; i < 8; i++)
+		mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+	for (i = 8; i < 14; i++)
+		mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
+#else
+	for (i = 8; i < 14; i++)
+		mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
+#endif
+	mic_iv[14] = (unsigned char)(payload_length / 256);
+	mic_iv[15] = (unsigned char)(payload_length % 256);
+}
+
+
+/************************************************/
+/* construct_mic_header1()                     */
+/* Builds the first MIC header block from      */
+/* header fields.                              */
+/* Build AAD SC,A1,A2                          */
+/************************************************/
+static void construct_mic_header1(
+	u8 *mic_header1,
+	sint header_length,
+	u8 *mpdu,
+	uint frtype/* add for CONFIG_IEEE80211W, none 11w also can use */
+)
+{
+	mic_header1[0] = (u8)((header_length - 2) / 256);
+	mic_header1[1] = (u8)((header_length - 2) % 256);
+#ifdef CONFIG_IEEE80211W
+	/* 802.11w management frame don't AND subtype bits 4,5,6 of frame control field */
+	if (frtype == WIFI_MGT_TYPE)
+		mic_header1[2] = mpdu[0];
+	else
+#endif /* CONFIG_IEEE80211W */
+		mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
+
+	mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
+	mic_header1[4] = mpdu[4];       /* A1 */
+	mic_header1[5] = mpdu[5];
+	mic_header1[6] = mpdu[6];
+	mic_header1[7] = mpdu[7];
+	mic_header1[8] = mpdu[8];
+	mic_header1[9] = mpdu[9];
+	mic_header1[10] = mpdu[10];     /* A2 */
+	mic_header1[11] = mpdu[11];
+	mic_header1[12] = mpdu[12];
+	mic_header1[13] = mpdu[13];
+	mic_header1[14] = mpdu[14];
+	mic_header1[15] = mpdu[15];
+}
+
+
+/************************************************/
+/* construct_mic_header2()                     */
+/* Builds the last MIC header block from       */
+/* header fields.                              */
+/************************************************/
+static void construct_mic_header2(
+	u8 *mic_header2,
+	u8 *mpdu,
+	sint a4_exists,
+	sint qc_exists
+)
+{
+	sint i;
+	for (i = 0; i < 16; i++)
+		mic_header2[i] = 0x00;
+
+	mic_header2[0] = mpdu[16];    /* A3 */
+	mic_header2[1] = mpdu[17];
+	mic_header2[2] = mpdu[18];
+	mic_header2[3] = mpdu[19];
+	mic_header2[4] = mpdu[20];
+	mic_header2[5] = mpdu[21];
+
+	/* mic_header2[6] = mpdu[22] & 0xf0;    SC */
+	mic_header2[6] = 0x00;
+	mic_header2[7] = 0x00; /* mpdu[23]; */
+
+
+	if (!qc_exists && a4_exists) {
+		for (i = 0; i < 6; i++)
+			mic_header2[8 + i] = mpdu[24 + i]; /* A4 */
+
+	}
+
+	if (qc_exists && !a4_exists) {
+		mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+		mic_header2[9] = mpdu[25] & 0x00;
+	}
+
+	if (qc_exists && a4_exists) {
+		for (i = 0; i < 6; i++)
+			mic_header2[8 + i] = mpdu[24 + i]; /* A4 */
+
+		mic_header2[14] = mpdu[30] & 0x0f;
+		mic_header2[15] = mpdu[31] & 0x00;
+	}
+
+}
+
+
+/************************************************/
+/* construct_mic_header2()                     */
+/* Builds the last MIC header block from       */
+/* header fields.                              */
+/* Baron think the function is construct CCM   */
+/* nonce                                       */
+/************************************************/
+static void construct_ctr_preload(
+	u8 *ctr_preload,
+	sint a4_exists,
+	sint qc_exists,
+	u8 *mpdu,
+	u8 *pn_vector,
+	sint c,
+	uint frtype /* add for CONFIG_IEEE80211W, none 11w also can use */
+)
+{
+	sint i = 0;
+	for (i = 0; i < 16; i++)
+		ctr_preload[i] = 0x00;
+	i = 0;
+
+	ctr_preload[0] = 0x01;                                  /* flag */
+	if (qc_exists && a4_exists)
+		ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control */
+	if (qc_exists && !a4_exists)
+		ctr_preload[1] = mpdu[24] & 0x0f;
+#ifdef CONFIG_IEEE80211W
+	/* 802.11w management frame should set management bit(4) */
+	if (frtype == WIFI_MGT_TYPE)
+		ctr_preload[1] |= BIT(4);
+#endif /* CONFIG_IEEE80211W */
+	for (i = 2; i < 8; i++)
+		ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+	for (i = 8; i < 14; i++)
+		ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
+#else
+	for (i = 8; i < 14; i++)
+		ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
+#endif
+	ctr_preload[14] = (unsigned char)(c / 256);   /* Ctr */
+	ctr_preload[15] = (unsigned char)(c % 256);
+}
+
+
+/************************************/
+/* bitwise_xor()                   */
+/* A 128 bit, bitwise exclusive or */
+/************************************/
+static void bitwise_xor(u8 *ina, u8 *inb, u8 *out)
+{
+	sint i;
+	for (i = 0; i < 16; i++)
+		out[i] = ina[i] ^ inb[i];
+}
+
+
+static sint aes_cipher(u8 *key, uint	hdrlen,
+		       u8 *pframe, uint plen)
+{
+	/*	static unsigned char	message[MAX_MSG_SIZE]; */
+	uint	qc_exists, a4_exists, i, j, payload_remainder,
+		num_blocks, payload_index;
+
+	u8 pn_vector[6];
+	u8 mic_iv[16];
+	u8 mic_header1[16];
+	u8 mic_header2[16];
+	u8 ctr_preload[16];
+
+	/* Intermediate Buffers */
+	u8 chain_buffer[16];
+	u8 aes_out[16];
+	u8 padded_buffer[16];
+	u8 mic[8];
+	/*	uint	offset = 0; */
+	u16	frtype  = GetFrameType(pframe);
+	uint	frsubtype  = get_frame_sub_type(pframe);
+
+	frsubtype = frsubtype >> 4;
+
+
+	memset((void *)mic_iv, 0, 16);
+	memset((void *)mic_header1, 0, 16);
+	memset((void *)mic_header2, 0, 16);
+	memset((void *)ctr_preload, 0, 16);
+	memset((void *)chain_buffer, 0, 16);
+	memset((void *)aes_out, 0, 16);
+	memset((void *)padded_buffer, 0, 16);
+
+	if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen ==  WLAN_HDR_A3_QOS_LEN))
+		a4_exists = 0;
+	else
+		a4_exists = 1;
+
+	if (
+		((frtype | frsubtype) == WIFI_DATA_CFACK) ||
+		((frtype | frsubtype) == WIFI_DATA_CFPOLL) ||
+		((frtype | frsubtype) == WIFI_DATA_CFACKPOLL)) {
+		qc_exists = 1;
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+
+			hdrlen += 2;
+	}
+	/* add for CONFIG_IEEE80211W, none 11w also can use */
+	else if ((frtype == WIFI_DATA) &&
+		 ((frsubtype == 0x08) ||
+		  (frsubtype == 0x09) ||
+		  (frsubtype == 0x0a) ||
+		  (frsubtype == 0x0b))) {
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+
+			hdrlen += 2;
+		qc_exists = 1;
+	} else
+		qc_exists = 0;
+
+	pn_vector[0] = pframe[hdrlen];
+	pn_vector[1] = pframe[hdrlen + 1];
+	pn_vector[2] = pframe[hdrlen + 4];
+	pn_vector[3] = pframe[hdrlen + 5];
+	pn_vector[4] = pframe[hdrlen + 6];
+	pn_vector[5] = pframe[hdrlen + 7];
+
+	construct_mic_iv(
+		mic_iv,
+		qc_exists,
+		a4_exists,
+		pframe,	 /* message, */
+		plen,
+		pn_vector,
+		frtype /* add for CONFIG_IEEE80211W, none 11w also can use */
+	);
+
+	construct_mic_header1(
+		mic_header1,
+		hdrlen,
+		pframe,	/* message */
+		frtype /* add for CONFIG_IEEE80211W, none 11w also can use */
+	);
+	construct_mic_header2(
+		mic_header2,
+		pframe,	/* message, */
+		a4_exists,
+		qc_exists
+	);
+
+
+	payload_remainder = plen % 16;
+	num_blocks = plen / 16;
+
+	/* Find start of payload */
+	payload_index = (hdrlen + 8);
+
+	/* Calculate MIC */
+	aes128k128d(key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+
+	for (i = 0; i < num_blocks; i++) {
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);/* bitwise_xor(aes_out, &message[payload_index], chain_buffer); */
+
+		payload_index += 16;
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+
+	/* Add on the final payload block if it needs padding */
+	if (payload_remainder > 0) {
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++) {
+			padded_buffer[j] = pframe[payload_index++];/* padded_buffer[j] = message[payload_index++]; */
+		}
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(key, chain_buffer, aes_out);
+
+	}
+
+	for (j = 0 ; j < 8; j++)
+		mic[j] = aes_out[j];
+
+	/* Insert MIC into payload */
+	for (j = 0; j < 8; j++)
+		pframe[payload_index + j] = mic[j];	/* message[payload_index+j] = mic[j]; */
+
+	payload_index = hdrlen + 8;
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(
+			ctr_preload,
+			a4_exists,
+			qc_exists,
+			pframe,	/* message, */
+			pn_vector,
+			i + 1,
+			frtype); /* add for CONFIG_IEEE80211W, none 11w also can use */
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);/* bitwise_xor(aes_out, &message[payload_index], chain_buffer); */
+		for (j = 0; j < 16; j++)
+			pframe[payload_index++] = chain_buffer[j];/* for (j=0; j<16;j++) message[payload_index++] = chain_buffer[j]; */
+	}
+
+	if (payload_remainder > 0) {        /* If there is a short final block, then pad it,*/
+		/* encrypt it and copy the unpadded part back  */
+		construct_ctr_preload(
+			ctr_preload,
+			a4_exists,
+			qc_exists,
+			pframe,	/* message, */
+			pn_vector,
+			num_blocks + 1,
+			frtype); /* add for CONFIG_IEEE80211W, none 11w also can use */
+
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++) {
+			padded_buffer[j] = pframe[payload_index + j]; /* padded_buffer[j] = message[payload_index+j]; */
+		}
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			pframe[payload_index++] = chain_buffer[j];/* for (j=0; j<payload_remainder;j++) message[payload_index++] = chain_buffer[j]; */
+	}
+
+	/* Encrypt the MIC */
+	construct_ctr_preload(
+		ctr_preload,
+		a4_exists,
+		qc_exists,
+		pframe,	/* message, */
+		pn_vector,
+		0,
+		frtype); /* add for CONFIG_IEEE80211W, none 11w also can use */
+
+	for (j = 0; j < 16; j++)
+		padded_buffer[j] = 0x00;
+	for (j = 0; j < 8; j++) {
+		padded_buffer[j] = pframe[j + hdrlen + 8 + plen]; /* padded_buffer[j] = message[j+hdrlen+8+plen]; */
+	}
+
+	aes128k128d(key, ctr_preload, aes_out);
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+	for (j = 0; j < 8; j++)
+		pframe[payload_index++] = chain_buffer[j];/* for (j=0; j<8;j++) message[payload_index++] = chain_buffer[j]; */
+	return _SUCCESS;
+}
+
+
+
+
+
+u32	rtw_aes_encrypt(_adapter *padapter, u8 *pxmitframe)
+{
+	/* exclude ICV */
+
+
+	/*static*/
+	/*	unsigned char	message[MAX_MSG_SIZE]; */
+
+	/* Intermediate Buffers */
+	sint	curfragnum, length;
+	u32	prwskeylen;
+	u8	*pframe, *prwskey;	/* , *payload,*iv */
+	u8   hw_hdr_offset = 0;
+	/* struct	sta_info		*stainfo=NULL; */
+	struct	pkt_attrib	*pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct	xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+
+	/*	uint	offset = 0; */
+	u32 res = _SUCCESS;
+
+	if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+		return _FAIL;
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+	hw_hdr_offset = TXDESC_SIZE +
+		(((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
+#else
+#ifdef CONFIG_TX_EARLY_MODE
+	hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE;
+#else
+	hw_hdr_offset = TXDESC_OFFSET;
+#endif
+#endif
+
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
+
+	/* 4 start to encrypt each fragment */
+	if ((pattrib->encrypt == _AES_)) {
+		/*
+				if(pattrib->psta)
+				{
+					stainfo = pattrib->psta;
+				}
+				else
+				{
+					RTW_INFO("%s, call rtw_get_stainfo()\n", __func__);
+					stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] );
+				}
+		*/
+		/* if (stainfo!=NULL) */
+		{
+			/*
+						if(!(stainfo->state &_FW_LINKED))
+						{
+							RTW_INFO("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
+							return _FAIL;
+						}
+			*/
+
+			if (IS_MCAST(pattrib->ra))
+				prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+			else {
+				/* prwskey=&stainfo->dot118021x_UncstKey.skey[0]; */
+				prwskey = pattrib->dot118021x_UncstKey.skey;
+			}
+
+#ifdef CONFIG_TDLS
+			{
+				/* Swencryption */
+				struct	sta_info		*ptdls_sta;
+				ptdls_sta = rtw_get_stainfo(&padapter->stapriv , &pattrib->dst[0]);
+				if ((ptdls_sta != NULL) && (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)) {
+					RTW_INFO("[%s] for tdls link\n", __func__);
+					prwskey = &ptdls_sta->tpk.tk[0];
+				}
+			}
+#endif /* CONFIG_TDLS */
+
+			prwskeylen = 16;
+
+			for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+
+				if ((curfragnum + 1) == pattrib->nr_frags) {	/* 4 the last fragment */
+					length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
+
+					aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
+				} else {
+					length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len ;
+
+					aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
+					pframe += pxmitpriv->frag_len;
+					pframe = (u8 *)RND4((SIZE_PTR)(pframe));
+
+				}
+			}
+
+			AES_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra);
+		}
+		/*
+				else{
+					RTW_INFO("%s, psta==NUL\n", __func__);
+					res=_FAIL;
+				}
+		*/
+	}
+
+
+
+	return res;
+}
+
+static sint aes_decipher(u8 *key, uint	hdrlen,
+			 u8 *pframe, uint plen)
+{
+	static u8	message[MAX_MSG_SIZE];
+	uint	qc_exists, a4_exists, i, j, payload_remainder,
+		num_blocks, payload_index;
+	sint res = _SUCCESS;
+	u8 pn_vector[6];
+	u8 mic_iv[16];
+	u8 mic_header1[16];
+	u8 mic_header2[16];
+	u8 ctr_preload[16];
+
+	/* Intermediate Buffers */
+	u8 chain_buffer[16];
+	u8 aes_out[16];
+	u8 padded_buffer[16];
+	u8 mic[8];
+
+
+	/*	uint	offset = 0; */
+	uint	frtype  = GetFrameType(pframe);
+	uint	frsubtype  = get_frame_sub_type(pframe);
+	frsubtype = frsubtype >> 4;
+
+
+	memset((void *)mic_iv, 0, 16);
+	memset((void *)mic_header1, 0, 16);
+	memset((void *)mic_header2, 0, 16);
+	memset((void *)ctr_preload, 0, 16);
+	memset((void *)chain_buffer, 0, 16);
+	memset((void *)aes_out, 0, 16);
+	memset((void *)padded_buffer, 0, 16);
+
+	/* start to decrypt the payload */
+
+	num_blocks = (plen - 8) / 16; /* (plen including LLC, payload_length and mic ) */
+
+	payload_remainder = (plen - 8) % 16;
+
+	pn_vector[0]  = pframe[hdrlen];
+	pn_vector[1]  = pframe[hdrlen + 1];
+	pn_vector[2]  = pframe[hdrlen + 4];
+	pn_vector[3]  = pframe[hdrlen + 5];
+	pn_vector[4]  = pframe[hdrlen + 6];
+	pn_vector[5]  = pframe[hdrlen + 7];
+
+	if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen ==  WLAN_HDR_A3_QOS_LEN))
+		a4_exists = 0;
+	else
+		a4_exists = 1;
+
+	if (
+		((frtype | frsubtype) == WIFI_DATA_CFACK) ||
+		((frtype | frsubtype) == WIFI_DATA_CFPOLL) ||
+		((frtype | frsubtype) == WIFI_DATA_CFACKPOLL)) {
+		qc_exists = 1;
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+
+			hdrlen += 2;
+	} /* only for data packet . add for CONFIG_IEEE80211W, none 11w also can use */
+	else if ((frtype == WIFI_DATA) &&
+		 ((frsubtype == 0x08) ||
+		  (frsubtype == 0x09) ||
+		  (frsubtype == 0x0a) ||
+		  (frsubtype == 0x0b))) {
+		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
+
+			hdrlen += 2;
+		qc_exists = 1;
+	} else
+		qc_exists = 0;
+
+
+	/* now, decrypt pframe with hdrlen offset and plen long */
+
+	payload_index = hdrlen + 8; /* 8 is for extiv */
+
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(
+			ctr_preload,
+			a4_exists,
+			qc_exists,
+			pframe,
+			pn_vector,
+			i + 1,
+			frtype /* add for CONFIG_IEEE80211W, none 11w also can use */
+		);
+
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+
+		for (j = 0; j < 16; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+
+	if (payload_remainder > 0) {        /* If there is a short final block, then pad it,*/
+		/* encrypt it and copy the unpadded part back  */
+		construct_ctr_preload(
+			ctr_preload,
+			a4_exists,
+			qc_exists,
+			pframe,
+			pn_vector,
+			num_blocks + 1,
+			frtype /* add for CONFIG_IEEE80211W, none 11w also can use */
+		);
+
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = pframe[payload_index + j];
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			pframe[payload_index++] = chain_buffer[j];
+	}
+
+	/* start to calculate the mic	 */
+	if ((hdrlen + plen + 8) <= MAX_MSG_SIZE)
+		memcpy((void *)message, pframe, (hdrlen + plen + 8)); /* 8 is for ext iv len */
+
+
+	pn_vector[0] = pframe[hdrlen];
+	pn_vector[1] = pframe[hdrlen + 1];
+	pn_vector[2] = pframe[hdrlen + 4];
+	pn_vector[3] = pframe[hdrlen + 5];
+	pn_vector[4] = pframe[hdrlen + 6];
+	pn_vector[5] = pframe[hdrlen + 7];
+
+
+
+	construct_mic_iv(
+		mic_iv,
+		qc_exists,
+		a4_exists,
+		message,
+		plen - 8,
+		pn_vector,
+		frtype /* add for CONFIG_IEEE80211W, none 11w also can use */
+	);
+
+	construct_mic_header1(
+		mic_header1,
+		hdrlen,
+		message,
+		frtype /* add for CONFIG_IEEE80211W, none 11w also can use */
+	);
+	construct_mic_header2(
+		mic_header2,
+		message,
+		a4_exists,
+		qc_exists
+	);
+
+
+	payload_remainder = (plen - 8) % 16;
+	num_blocks = (plen - 8) / 16;
+
+	/* Find start of payload */
+	payload_index = (hdrlen + 8);
+
+	/* Calculate MIC */
+	aes128k128d(key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(key, chain_buffer, aes_out);
+
+	for (i = 0; i < num_blocks; i++) {
+		bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+
+		payload_index += 16;
+		aes128k128d(key, chain_buffer, aes_out);
+	}
+
+	/* Add on the final payload block if it needs padding */
+	if (payload_remainder > 0) {
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = message[payload_index++];
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(key, chain_buffer, aes_out);
+
+	}
+
+	for (j = 0 ; j < 8; j++)
+		mic[j] = aes_out[j];
+
+	/* Insert MIC into payload */
+	for (j = 0; j < 8; j++)
+		message[payload_index + j] = mic[j];
+
+	payload_index = hdrlen + 8;
+	for (i = 0; i < num_blocks; i++) {
+		construct_ctr_preload(
+			ctr_preload,
+			a4_exists,
+			qc_exists,
+			message,
+			pn_vector,
+			i + 1,
+			frtype); /* add for CONFIG_IEEE80211W, none 11w also can use */
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+		for (j = 0; j < 16; j++)
+			message[payload_index++] = chain_buffer[j];
+	}
+
+	if (payload_remainder > 0) {        /* If there is a short final block, then pad it,*/
+		/* encrypt it and copy the unpadded part back  */
+		construct_ctr_preload(
+			ctr_preload,
+			a4_exists,
+			qc_exists,
+			message,
+			pn_vector,
+			num_blocks + 1,
+			frtype); /* add for CONFIG_IEEE80211W, none 11w also can use */
+
+		for (j = 0; j < 16; j++)
+			padded_buffer[j] = 0x00;
+		for (j = 0; j < payload_remainder; j++)
+			padded_buffer[j] = message[payload_index + j];
+		aes128k128d(key, ctr_preload, aes_out);
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		for (j = 0; j < payload_remainder; j++)
+			message[payload_index++] = chain_buffer[j];
+	}
+
+	/* Encrypt the MIC */
+	construct_ctr_preload(
+		ctr_preload,
+		a4_exists,
+		qc_exists,
+		message,
+		pn_vector,
+		0,
+		frtype); /* add for CONFIG_IEEE80211W, none 11w also can use */
+
+	for (j = 0; j < 16; j++)
+		padded_buffer[j] = 0x00;
+	for (j = 0; j < 8; j++)
+		padded_buffer[j] = message[j + hdrlen + 8 + plen - 8];
+
+	aes128k128d(key, ctr_preload, aes_out);
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+	for (j = 0; j < 8; j++)
+		message[payload_index++] = chain_buffer[j];
+
+	/* compare the mic */
+	for (i = 0; i < 8; i++) {
+		if (pframe[hdrlen + 8 + plen - 8 + i] != message[hdrlen + 8 + plen - 8 + i]) {
+			RTW_INFO("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x)\n",
+				i, pframe[hdrlen + 8 + plen - 8 + i], message[hdrlen + 8 + plen - 8 + i]);
+			res = _FAIL;
+		}
+	}
+	return res;
+}
+
+u32	rtw_aes_decrypt(_adapter *padapter, u8 *precvframe)
+{
+	/* exclude ICV */
+
+
+	/*static*/
+	/*	unsigned char	message[MAX_MSG_SIZE]; */
+
+
+	/* Intermediate Buffers */
+
+
+	sint		length;
+	u32	prwskeylen;
+	u8	*pframe, *prwskey;	/* , *payload,*iv */
+	struct	sta_info		*stainfo;
+	struct	rx_pkt_attrib	*prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	/*	struct	recv_priv		*precvpriv=&padapter->recvpriv; */
+	u32	res = _SUCCESS;
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data;
+	/* 4 start to encrypt each fragment */
+	if ((prxattrib->encrypt == _AES_)) {
+
+		stainfo = rtw_get_stainfo(&padapter->stapriv , &prxattrib->ta[0]);
+		if (stainfo != NULL) {
+
+			if (IS_MCAST(prxattrib->ra)) {
+				static u32 start = 0;
+				static u32 no_gkey_bc_cnt = 0;
+				static u32 no_gkey_mc_cnt = 0;
+
+				/* RTW_INFO("rx bc/mc packets, to perform sw rtw_aes_decrypt\n"); */
+				/* prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; */
+				if (psecuritypriv->binstallGrpkey == false) {
+					res = _FAIL;
+
+					if (start == 0)
+						start = jiffies;
+
+					if (is_broadcast_mac_addr(prxattrib->ra))
+						no_gkey_bc_cnt++;
+					else
+						no_gkey_mc_cnt++;
+
+					if (rtw_get_passing_time_ms(start) > 1000) {
+						if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
+							RTW_PRINT(FUNC_ADPT_FMT" no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
+								FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt);
+						}
+						start = jiffies;
+						no_gkey_bc_cnt = 0;
+						no_gkey_mc_cnt = 0;
+					}
+
+					goto exit;
+				}
+
+				if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
+					RTW_PRINT(FUNC_ADPT_FMT" gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
+						FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt);
+				}
+				start = 0;
+				no_gkey_bc_cnt = 0;
+				no_gkey_mc_cnt = 0;
+
+				prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+				if (psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index) {
+					RTW_DBG("not match packet_index=%d, install_index=%d\n"
+						, prxattrib->key_index, psecuritypriv->dot118021XGrpKeyid);
+					res = _FAIL;
+					goto exit;
+				}
+			} else
+				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+
+			length = ((union recv_frame *)precvframe)->u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len;
+
+			res = aes_decipher(prwskey, prxattrib->hdrlen, pframe, length);
+
+			AES_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
+		} else {
+			res = _FAIL;
+		}
+
+	}
+exit:
+	return res;
+}
+
+#ifdef CONFIG_IEEE80211W
+u32	rtw_BIP_verify(_adapter *padapter, u8 *precvframe)
+{
+	struct rx_pkt_attrib *pattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
+	u8 *pframe;
+	u8 *BIP_AAD, *p;
+	u32	res = _FAIL;
+	uint len, ori_len;
+	struct rtw_ieee80211_hdr *pwlanhdr;
+	u8 mic[16];
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	ori_len = pattrib->pkt_len - WLAN_HDR_A3_LEN + BIP_AAD_SIZE;
+	BIP_AAD = rtw_zmalloc(ori_len);
+
+	if (BIP_AAD == NULL) {
+		RTW_INFO("BIP AAD allocate fail\n");
+		return _FAIL;
+	}
+	/* PKT start */
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data;
+	/* mapping to wlan header */
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+	/* save the frame body + MME */
+	memcpy(BIP_AAD + BIP_AAD_SIZE, pframe + WLAN_HDR_A3_LEN, pattrib->pkt_len - WLAN_HDR_A3_LEN);
+	/* find MME IE pointer */
+	p = rtw_get_ie(BIP_AAD + BIP_AAD_SIZE, _MME_IE_, &len, pattrib->pkt_len - WLAN_HDR_A3_LEN);
+	/* Baron */
+	if (p) {
+		u16 keyid = 0;
+		u64 temp_ipn = 0;
+		/* save packet number */
+		memcpy(&temp_ipn, p + 4, 6);
+		temp_ipn = le64_to_cpu(temp_ipn);
+		/* BIP packet number should bigger than previous BIP packet */
+		if (temp_ipn < pmlmeext->mgnt_80211w_IPN_rx) {
+			RTW_INFO("replay BIP packet\n");
+			goto BIP_exit;
+		}
+		/* copy key index */
+		memcpy(&keyid, p + 2, 2);
+		keyid = le16_to_cpu(keyid);
+		if (keyid != padapter->securitypriv.dot11wBIPKeyid) {
+			RTW_INFO("BIP key index error!\n");
+			goto BIP_exit;
+		}
+		/* clear the MIC field of MME to zero */
+		memset(p + 2 + len - 8, 0, 8);
+
+		/* conscruct AAD, copy frame control field */
+		memcpy(BIP_AAD, &pwlanhdr->frame_ctl, 2);
+		ClearRetry(BIP_AAD);
+		ClearPwrMgt(BIP_AAD);
+		ClearMData(BIP_AAD);
+		/* conscruct AAD, copy address 1 to address 3 */
+		memcpy(BIP_AAD + 2, pwlanhdr->addr1, 18);
+
+		if (omac1_aes_128(padapter->securitypriv.dot11wBIPKey[padapter->securitypriv.dot11wBIPKeyid].skey
+				  , BIP_AAD, ori_len, mic))
+			goto BIP_exit;
+
+		/* MIC field should be last 8 bytes of packet (packet without FCS) */
+		if (!memcmp(mic, pframe + pattrib->pkt_len - 8, 8)) {
+			pmlmeext->mgnt_80211w_IPN_rx = temp_ipn;
+			res = _SUCCESS;
+		} else
+			RTW_INFO("BIP MIC error!\n");
+
+	} else
+		res = RTW_RX_HANDLED;
+BIP_exit:
+
+	rtw_mfree(BIP_AAD, ori_len);
+	return res;
+}
+#endif /* CONFIG_IEEE80211W */
+
+/* compress 512-bits */
+static int sha256_compress(struct sha256_state_rtk *md, unsigned char *buf)
+{
+	u32 S[8], W[64], t0, t1;
+	u32 t;
+	int i;
+
+	/* copy state into S */
+	for (i = 0; i < 8; i++)
+		S[i] = md->state[i];
+
+	/* copy the state into 512-bits into W[0..15] */
+	for (i = 0; i < 16; i++)
+		W[i] = WPA_GET_BE32(buf + (4 * i));
+
+	/* fill W[16..63] */
+	for (i = 16; i < 64; i++) {
+		W[i] = Gamma1(W[i - 2]) + W[i - 7] + Gamma0(W[i - 15]) +
+		       W[i - 16];
+	}
+
+	/* Compress */
+#define RND(a, b, c, d, e, f, g, h, i)                          do {\
+	t0 = h + Sigma1(e) + Ch(e, f, g) + K[i] + W[i];	\
+	t1 = Sigma0(a) + Maj(a, b, c);			\
+	d += t0;					\
+	h  = t0 + t1;	\
+	} while (0)
+
+	for (i = 0; i < 64; ++i) {
+		RND(S[0], S[1], S[2], S[3], S[4], S[5], S[6], S[7], i);
+		t = S[7];
+		S[7] = S[6];
+		S[6] = S[5];
+		S[5] = S[4];
+		S[4] = S[3];
+		S[3] = S[2];
+		S[2] = S[1];
+		S[1] = S[0];
+		S[0] = t;
+	}
+
+	/* feedback */
+	for (i = 0; i < 8; i++)
+		md->state[i] = md->state[i] + S[i];
+	return 0;
+}
+
+/* Initialize the hash state */
+static void sha256_init(struct sha256_state_rtk *md)
+{
+	md->curlen = 0;
+	md->length = 0;
+	md->state[0] = 0x6A09E667UL;
+	md->state[1] = 0xBB67AE85UL;
+	md->state[2] = 0x3C6EF372UL;
+	md->state[3] = 0xA54FF53AUL;
+	md->state[4] = 0x510E527FUL;
+	md->state[5] = 0x9B05688CUL;
+	md->state[6] = 0x1F83D9ABUL;
+	md->state[7] = 0x5BE0CD19UL;
+}
+
+/**
+   Process a block of memory though the hash
+   @param md     The hash state
+   @param in     The data to hash
+   @param inlen  The length of the data (octets)
+   @return CRYPT_OK if successful
+*/
+static int sha256_process(struct sha256_state_rtk *md, unsigned char *in,
+			  unsigned long inlen)
+{
+	unsigned long n;
+#define block_size 64
+
+	if (md->curlen > sizeof(md->buf))
+		return -1;
+
+	while (inlen > 0) {
+		if (md->curlen == 0 && inlen >= block_size) {
+			if (sha256_compress(md, (unsigned char *) in) < 0)
+				return -1;
+			md->length += block_size * 8;
+			in += block_size;
+			inlen -= block_size;
+		} else {
+			n = MIN(inlen, (block_size - md->curlen));
+			memcpy(md->buf + md->curlen, in, n);
+			md->curlen += n;
+			in += n;
+			inlen -= n;
+			if (md->curlen == block_size) {
+				if (sha256_compress(md, md->buf) < 0)
+					return -1;
+				md->length += 8 * block_size;
+				md->curlen = 0;
+			}
+		}
+	}
+
+	return 0;
+}
+
+
+/**
+   Terminate the hash to get the digest
+   @param md  The hash state
+   @param out [out] The destination of the hash (32 bytes)
+   @return CRYPT_OK if successful
+*/
+static int sha256_done(struct sha256_state_rtk *md, unsigned char *out)
+{
+	int i;
+
+	if (md->curlen >= sizeof(md->buf))
+		return -1;
+
+	/* increase the length of the message */
+	md->length += md->curlen * 8;
+
+	/* append the '1' bit */
+	md->buf[md->curlen++] = (unsigned char) 0x80;
+
+	/* if the length is currently above 56 bytes we append zeros
+	 * then compress.  Then we can fall back to padding zeros and length
+	 * encoding like normal.
+	 */
+	if (md->curlen > 56) {
+		while (md->curlen < 64)
+			md->buf[md->curlen++] = (unsigned char) 0;
+		sha256_compress(md, md->buf);
+		md->curlen = 0;
+	}
+
+	/* pad upto 56 bytes of zeroes */
+	while (md->curlen < 56)
+		md->buf[md->curlen++] = (unsigned char) 0;
+
+	/* store length */
+	WPA_PUT_BE64(md->buf + 56, md->length);
+	sha256_compress(md, md->buf);
+
+	/* copy output */
+	for (i = 0; i < 8; i++)
+		WPA_PUT_BE32(out + (4 * i), md->state[i]);
+
+	return 0;
+}
+
+/**
+ * sha256_vector - SHA256 hash for data vector
+ * @num_elem: Number of elements in the data vector
+ * @addr: Pointers to the data areas
+ * @len: Lengths of the data blocks
+ * @mac: Buffer for the hash
+ * Returns: 0 on success, -1 of failure
+ */
+static int sha256_vector(size_t num_elem, u8 *addr[], size_t *len,
+			 u8 *mac)
+{
+	struct sha256_state_rtk ctx;
+	size_t i;
+
+	sha256_init(&ctx);
+	for (i = 0; i < num_elem; i++)
+		if (sha256_process(&ctx, addr[i], len[i]))
+			return -1;
+	if (sha256_done(&ctx, mac))
+		return -1;
+	return 0;
+}
+
+static u8 os_strlen(const char *s)
+{
+	const char *p = s;
+	while (*p)
+		p++;
+	return p - s;
+}
+
+static int os_memcmp(void *s1, void *s2, u8 n)
+{
+	unsigned char *p1 = s1, *p2 = s2;
+
+	if (n == 0)
+		return 0;
+
+	while (*p1 == *p2) {
+		p1++;
+		p2++;
+		n--;
+		if (n == 0)
+			return 0;
+	}
+
+	return *p1 - *p2;
+}
+
+/**
+ * hmac_sha256_vector - HMAC-SHA256 over data vector (RFC 2104)
+ * @key: Key for HMAC operations
+ * @key_len: Length of the key in bytes
+ * @num_elem: Number of elements in the data vector
+ * @addr: Pointers to the data areas
+ * @len: Lengths of the data blocks
+ * @mac: Buffer for the hash (32 bytes)
+ */
+static void hmac_sha256_vector(u8 *key, size_t key_len, size_t num_elem,
+			       u8 *addr[], size_t *len, u8 *mac)
+{
+	unsigned char k_pad[64]; /* padding - key XORd with ipad/opad */
+	unsigned char tk[32];
+	u8 *_addr[6];
+	size_t _len[6], i;
+
+	if (num_elem > 5) {
+		/*
+		 * Fixed limit on the number of fragments to avoid having to
+		 * allocate memory (which could fail).
+		 */
+		return;
+	}
+
+	/* if key is longer than 64 bytes reset it to key = SHA256(key) */
+	if (key_len > 64) {
+		sha256_vector(1, &key, &key_len, tk);
+		key = tk;
+		key_len = 32;
+	}
+
+	/* the HMAC_SHA256 transform looks like:
+	 *
+	 * SHA256(K XOR opad, SHA256(K XOR ipad, text))
+	 *
+	 * where K is an n byte key
+	 * ipad is the byte 0x36 repeated 64 times
+	 * opad is the byte 0x5c repeated 64 times
+	 * and text is the data being protected */
+
+	/* start out by storing key in ipad */
+	memset(k_pad, 0, sizeof(k_pad));
+	memcpy(k_pad, key, key_len);
+	/* XOR key with ipad values */
+	for (i = 0; i < 64; i++)
+		k_pad[i] ^= 0x36;
+
+	/* perform inner SHA256 */
+	_addr[0] = k_pad;
+	_len[0] = 64;
+	for (i = 0; i < num_elem; i++) {
+		_addr[i + 1] = addr[i];
+		_len[i + 1] = len[i];
+	}
+	sha256_vector(1 + num_elem, _addr, _len, mac);
+
+	memset(k_pad, 0, sizeof(k_pad));
+	memcpy(k_pad, key, key_len);
+	/* XOR key with opad values */
+	for (i = 0; i < 64; i++)
+		k_pad[i] ^= 0x5c;
+
+	/* perform outer SHA256 */
+	_addr[0] = k_pad;
+	_len[0] = 64;
+	_addr[1] = mac;
+	_len[1] = 32;
+	sha256_vector(2, _addr, _len, mac);
+}
+/**
+ * sha256_prf - SHA256-based Pseudo-Random Function (IEEE 802.11r, 8.5.1.5.2)
+ * @key: Key for PRF
+ * @key_len: Length of the key in bytes
+ * @label: A unique label for each purpose of the PRF
+ * @data: Extra data to bind into the key
+ * @data_len: Length of the data
+ * @buf: Buffer for the generated pseudo-random key
+ * @buf_len: Number of bytes of key to generate
+ *
+ * This function is used to derive new, cryptographically separate keys from a
+ * given key.
+ */
+static void sha256_prf(u8 *key, size_t key_len, char *label,
+		       u8 *data, size_t data_len, u8 *buf, size_t buf_len)
+{
+	u16 counter = 1;
+	size_t pos, plen;
+	u8 hash[SHA256_MAC_LEN];
+	u8 *addr[4];
+	size_t len[4];
+	u8 counter_le[2], length_le[2];
+
+	addr[0] = counter_le;
+	len[0] = 2;
+	addr[1] = (u8 *) label;
+	len[1] = os_strlen(label);
+	addr[2] = data;
+	len[2] = data_len;
+	addr[3] = length_le;
+	len[3] = sizeof(length_le);
+
+	WPA_PUT_LE16(length_le, buf_len * 8);
+	pos = 0;
+	while (pos < buf_len) {
+		plen = buf_len - pos;
+		WPA_PUT_LE16(counter_le, counter);
+		if (plen >= SHA256_MAC_LEN) {
+			hmac_sha256_vector(key, key_len, 4, addr, len,
+					   &buf[pos]);
+			pos += SHA256_MAC_LEN;
+		} else {
+			hmac_sha256_vector(key, key_len, 4, addr, len, hash);
+			memcpy(&buf[pos], hash, plen);
+			break;
+		}
+		counter++;
+	}
+}
+
+/* AES tables*/
+const u32 Te0[256] = {
+	0xc66363a5U, 0xf87c7c84U, 0xee777799U, 0xf67b7b8dU,
+	0xfff2f20dU, 0xd66b6bbdU, 0xde6f6fb1U, 0x91c5c554U,
+	0x60303050U, 0x02010103U, 0xce6767a9U, 0x562b2b7dU,
+	0xe7fefe19U, 0xb5d7d762U, 0x4dababe6U, 0xec76769aU,
+	0x8fcaca45U, 0x1f82829dU, 0x89c9c940U, 0xfa7d7d87U,
+	0xeffafa15U, 0xb25959ebU, 0x8e4747c9U, 0xfbf0f00bU,
+	0x41adadecU, 0xb3d4d467U, 0x5fa2a2fdU, 0x45afafeaU,
+	0x239c9cbfU, 0x53a4a4f7U, 0xe4727296U, 0x9bc0c05bU,
+	0x75b7b7c2U, 0xe1fdfd1cU, 0x3d9393aeU, 0x4c26266aU,
+	0x6c36365aU, 0x7e3f3f41U, 0xf5f7f702U, 0x83cccc4fU,
+	0x6834345cU, 0x51a5a5f4U, 0xd1e5e534U, 0xf9f1f108U,
+	0xe2717193U, 0xabd8d873U, 0x62313153U, 0x2a15153fU,
+	0x0804040cU, 0x95c7c752U, 0x46232365U, 0x9dc3c35eU,
+	0x30181828U, 0x379696a1U, 0x0a05050fU, 0x2f9a9ab5U,
+	0x0e070709U, 0x24121236U, 0x1b80809bU, 0xdfe2e23dU,
+	0xcdebeb26U, 0x4e272769U, 0x7fb2b2cdU, 0xea75759fU,
+	0x1209091bU, 0x1d83839eU, 0x582c2c74U, 0x341a1a2eU,
+	0x361b1b2dU, 0xdc6e6eb2U, 0xb45a5aeeU, 0x5ba0a0fbU,
+	0xa45252f6U, 0x763b3b4dU, 0xb7d6d661U, 0x7db3b3ceU,
+	0x5229297bU, 0xdde3e33eU, 0x5e2f2f71U, 0x13848497U,
+	0xa65353f5U, 0xb9d1d168U, 0x00000000U, 0xc1eded2cU,
+	0x40202060U, 0xe3fcfc1fU, 0x79b1b1c8U, 0xb65b5bedU,
+	0xd46a6abeU, 0x8dcbcb46U, 0x67bebed9U, 0x7239394bU,
+	0x944a4adeU, 0x984c4cd4U, 0xb05858e8U, 0x85cfcf4aU,
+	0xbbd0d06bU, 0xc5efef2aU, 0x4faaaae5U, 0xedfbfb16U,
+	0x864343c5U, 0x9a4d4dd7U, 0x66333355U, 0x11858594U,
+	0x8a4545cfU, 0xe9f9f910U, 0x04020206U, 0xfe7f7f81U,
+	0xa05050f0U, 0x783c3c44U, 0x259f9fbaU, 0x4ba8a8e3U,
+	0xa25151f3U, 0x5da3a3feU, 0x804040c0U, 0x058f8f8aU,
+	0x3f9292adU, 0x219d9dbcU, 0x70383848U, 0xf1f5f504U,
+	0x63bcbcdfU, 0x77b6b6c1U, 0xafdada75U, 0x42212163U,
+	0x20101030U, 0xe5ffff1aU, 0xfdf3f30eU, 0xbfd2d26dU,
+	0x81cdcd4cU, 0x180c0c14U, 0x26131335U, 0xc3ecec2fU,
+	0xbe5f5fe1U, 0x359797a2U, 0x884444ccU, 0x2e171739U,
+	0x93c4c457U, 0x55a7a7f2U, 0xfc7e7e82U, 0x7a3d3d47U,
+	0xc86464acU, 0xba5d5de7U, 0x3219192bU, 0xe6737395U,
+	0xc06060a0U, 0x19818198U, 0x9e4f4fd1U, 0xa3dcdc7fU,
+	0x44222266U, 0x542a2a7eU, 0x3b9090abU, 0x0b888883U,
+	0x8c4646caU, 0xc7eeee29U, 0x6bb8b8d3U, 0x2814143cU,
+	0xa7dede79U, 0xbc5e5ee2U, 0x160b0b1dU, 0xaddbdb76U,
+	0xdbe0e03bU, 0x64323256U, 0x743a3a4eU, 0x140a0a1eU,
+	0x924949dbU, 0x0c06060aU, 0x4824246cU, 0xb85c5ce4U,
+	0x9fc2c25dU, 0xbdd3d36eU, 0x43acacefU, 0xc46262a6U,
+	0x399191a8U, 0x319595a4U, 0xd3e4e437U, 0xf279798bU,
+	0xd5e7e732U, 0x8bc8c843U, 0x6e373759U, 0xda6d6db7U,
+	0x018d8d8cU, 0xb1d5d564U, 0x9c4e4ed2U, 0x49a9a9e0U,
+	0xd86c6cb4U, 0xac5656faU, 0xf3f4f407U, 0xcfeaea25U,
+	0xca6565afU, 0xf47a7a8eU, 0x47aeaee9U, 0x10080818U,
+	0x6fbabad5U, 0xf0787888U, 0x4a25256fU, 0x5c2e2e72U,
+	0x381c1c24U, 0x57a6a6f1U, 0x73b4b4c7U, 0x97c6c651U,
+	0xcbe8e823U, 0xa1dddd7cU, 0xe874749cU, 0x3e1f1f21U,
+	0x964b4bddU, 0x61bdbddcU, 0x0d8b8b86U, 0x0f8a8a85U,
+	0xe0707090U, 0x7c3e3e42U, 0x71b5b5c4U, 0xcc6666aaU,
+	0x904848d8U, 0x06030305U, 0xf7f6f601U, 0x1c0e0e12U,
+	0xc26161a3U, 0x6a35355fU, 0xae5757f9U, 0x69b9b9d0U,
+	0x17868691U, 0x99c1c158U, 0x3a1d1d27U, 0x279e9eb9U,
+	0xd9e1e138U, 0xebf8f813U, 0x2b9898b3U, 0x22111133U,
+	0xd26969bbU, 0xa9d9d970U, 0x078e8e89U, 0x339494a7U,
+	0x2d9b9bb6U, 0x3c1e1e22U, 0x15878792U, 0xc9e9e920U,
+	0x87cece49U, 0xaa5555ffU, 0x50282878U, 0xa5dfdf7aU,
+	0x038c8c8fU, 0x59a1a1f8U, 0x09898980U, 0x1a0d0d17U,
+	0x65bfbfdaU, 0xd7e6e631U, 0x844242c6U, 0xd06868b8U,
+	0x824141c3U, 0x299999b0U, 0x5a2d2d77U, 0x1e0f0f11U,
+	0x7bb0b0cbU, 0xa85454fcU, 0x6dbbbbd6U, 0x2c16163aU,
+};
+const u32 Td0[256] = {
+	0x51f4a750U, 0x7e416553U, 0x1a17a4c3U, 0x3a275e96U,
+	0x3bab6bcbU, 0x1f9d45f1U, 0xacfa58abU, 0x4be30393U,
+	0x2030fa55U, 0xad766df6U, 0x88cc7691U, 0xf5024c25U,
+	0x4fe5d7fcU, 0xc52acbd7U, 0x26354480U, 0xb562a38fU,
+	0xdeb15a49U, 0x25ba1b67U, 0x45ea0e98U, 0x5dfec0e1U,
+	0xc32f7502U, 0x814cf012U, 0x8d4697a3U, 0x6bd3f9c6U,
+	0x038f5fe7U, 0x15929c95U, 0xbf6d7aebU, 0x955259daU,
+	0xd4be832dU, 0x587421d3U, 0x49e06929U, 0x8ec9c844U,
+	0x75c2896aU, 0xf48e7978U, 0x99583e6bU, 0x27b971ddU,
+	0xbee14fb6U, 0xf088ad17U, 0xc920ac66U, 0x7dce3ab4U,
+	0x63df4a18U, 0xe51a3182U, 0x97513360U, 0x62537f45U,
+	0xb16477e0U, 0xbb6bae84U, 0xfe81a01cU, 0xf9082b94U,
+	0x70486858U, 0x8f45fd19U, 0x94de6c87U, 0x527bf8b7U,
+	0xab73d323U, 0x724b02e2U, 0xe31f8f57U, 0x6655ab2aU,
+	0xb2eb2807U, 0x2fb5c203U, 0x86c57b9aU, 0xd33708a5U,
+	0x302887f2U, 0x23bfa5b2U, 0x02036abaU, 0xed16825cU,
+	0x8acf1c2bU, 0xa779b492U, 0xf307f2f0U, 0x4e69e2a1U,
+	0x65daf4cdU, 0x0605bed5U, 0xd134621fU, 0xc4a6fe8aU,
+	0x342e539dU, 0xa2f355a0U, 0x058ae132U, 0xa4f6eb75U,
+	0x0b83ec39U, 0x4060efaaU, 0x5e719f06U, 0xbd6e1051U,
+	0x3e218af9U, 0x96dd063dU, 0xdd3e05aeU, 0x4de6bd46U,
+	0x91548db5U, 0x71c45d05U, 0x0406d46fU, 0x605015ffU,
+	0x1998fb24U, 0xd6bde997U, 0x894043ccU, 0x67d99e77U,
+	0xb0e842bdU, 0x07898b88U, 0xe7195b38U, 0x79c8eedbU,
+	0xa17c0a47U, 0x7c420fe9U, 0xf8841ec9U, 0x00000000U,
+	0x09808683U, 0x322bed48U, 0x1e1170acU, 0x6c5a724eU,
+	0xfd0efffbU, 0x0f853856U, 0x3daed51eU, 0x362d3927U,
+	0x0a0fd964U, 0x685ca621U, 0x9b5b54d1U, 0x24362e3aU,
+	0x0c0a67b1U, 0x9357e70fU, 0xb4ee96d2U, 0x1b9b919eU,
+	0x80c0c54fU, 0x61dc20a2U, 0x5a774b69U, 0x1c121a16U,
+	0xe293ba0aU, 0xc0a02ae5U, 0x3c22e043U, 0x121b171dU,
+	0x0e090d0bU, 0xf28bc7adU, 0x2db6a8b9U, 0x141ea9c8U,
+	0x57f11985U, 0xaf75074cU, 0xee99ddbbU, 0xa37f60fdU,
+	0xf701269fU, 0x5c72f5bcU, 0x44663bc5U, 0x5bfb7e34U,
+	0x8b432976U, 0xcb23c6dcU, 0xb6edfc68U, 0xb8e4f163U,
+	0xd731dccaU, 0x42638510U, 0x13972240U, 0x84c61120U,
+	0x854a247dU, 0xd2bb3df8U, 0xaef93211U, 0xc729a16dU,
+	0x1d9e2f4bU, 0xdcb230f3U, 0x0d8652ecU, 0x77c1e3d0U,
+	0x2bb3166cU, 0xa970b999U, 0x119448faU, 0x47e96422U,
+	0xa8fc8cc4U, 0xa0f03f1aU, 0x567d2cd8U, 0x223390efU,
+	0x87494ec7U, 0xd938d1c1U, 0x8ccaa2feU, 0x98d40b36U,
+	0xa6f581cfU, 0xa57ade28U, 0xdab78e26U, 0x3fadbfa4U,
+	0x2c3a9de4U, 0x5078920dU, 0x6a5fcc9bU, 0x547e4662U,
+	0xf68d13c2U, 0x90d8b8e8U, 0x2e39f75eU, 0x82c3aff5U,
+	0x9f5d80beU, 0x69d0937cU, 0x6fd52da9U, 0xcf2512b3U,
+	0xc8ac993bU, 0x10187da7U, 0xe89c636eU, 0xdb3bbb7bU,
+	0xcd267809U, 0x6e5918f4U, 0xec9ab701U, 0x834f9aa8U,
+	0xe6956e65U, 0xaaffe67eU, 0x21bccf08U, 0xef15e8e6U,
+	0xbae79bd9U, 0x4a6f36ceU, 0xea9f09d4U, 0x29b07cd6U,
+	0x31a4b2afU, 0x2a3f2331U, 0xc6a59430U, 0x35a266c0U,
+	0x744ebc37U, 0xfc82caa6U, 0xe090d0b0U, 0x33a7d815U,
+	0xf104984aU, 0x41ecdaf7U, 0x7fcd500eU, 0x1791f62fU,
+	0x764dd68dU, 0x43efb04dU, 0xccaa4d54U, 0xe49604dfU,
+	0x9ed1b5e3U, 0x4c6a881bU, 0xc12c1fb8U, 0x4665517fU,
+	0x9d5eea04U, 0x018c355dU, 0xfa877473U, 0xfb0b412eU,
+	0xb3671d5aU, 0x92dbd252U, 0xe9105633U, 0x6dd64713U,
+	0x9ad7618cU, 0x37a10c7aU, 0x59f8148eU, 0xeb133c89U,
+	0xcea927eeU, 0xb761c935U, 0xe11ce5edU, 0x7a47b13cU,
+	0x9cd2df59U, 0x55f2733fU, 0x1814ce79U, 0x73c737bfU,
+	0x53f7cdeaU, 0x5ffdaa5bU, 0xdf3d6f14U, 0x7844db86U,
+	0xcaaff381U, 0xb968c43eU, 0x3824342cU, 0xc2a3405fU,
+	0x161dc372U, 0xbce2250cU, 0x283c498bU, 0xff0d9541U,
+	0x39a80171U, 0x080cb3deU, 0xd8b4e49cU, 0x6456c190U,
+	0x7bcb8461U, 0xd532b670U, 0x486c5c74U, 0xd0b85742U,
+};
+const u8 Td4s[256] = {
+	0x52U, 0x09U, 0x6aU, 0xd5U, 0x30U, 0x36U, 0xa5U, 0x38U,
+	0xbfU, 0x40U, 0xa3U, 0x9eU, 0x81U, 0xf3U, 0xd7U, 0xfbU,
+	0x7cU, 0xe3U, 0x39U, 0x82U, 0x9bU, 0x2fU, 0xffU, 0x87U,
+	0x34U, 0x8eU, 0x43U, 0x44U, 0xc4U, 0xdeU, 0xe9U, 0xcbU,
+	0x54U, 0x7bU, 0x94U, 0x32U, 0xa6U, 0xc2U, 0x23U, 0x3dU,
+	0xeeU, 0x4cU, 0x95U, 0x0bU, 0x42U, 0xfaU, 0xc3U, 0x4eU,
+	0x08U, 0x2eU, 0xa1U, 0x66U, 0x28U, 0xd9U, 0x24U, 0xb2U,
+	0x76U, 0x5bU, 0xa2U, 0x49U, 0x6dU, 0x8bU, 0xd1U, 0x25U,
+	0x72U, 0xf8U, 0xf6U, 0x64U, 0x86U, 0x68U, 0x98U, 0x16U,
+	0xd4U, 0xa4U, 0x5cU, 0xccU, 0x5dU, 0x65U, 0xb6U, 0x92U,
+	0x6cU, 0x70U, 0x48U, 0x50U, 0xfdU, 0xedU, 0xb9U, 0xdaU,
+	0x5eU, 0x15U, 0x46U, 0x57U, 0xa7U, 0x8dU, 0x9dU, 0x84U,
+	0x90U, 0xd8U, 0xabU, 0x00U, 0x8cU, 0xbcU, 0xd3U, 0x0aU,
+	0xf7U, 0xe4U, 0x58U, 0x05U, 0xb8U, 0xb3U, 0x45U, 0x06U,
+	0xd0U, 0x2cU, 0x1eU, 0x8fU, 0xcaU, 0x3fU, 0x0fU, 0x02U,
+	0xc1U, 0xafU, 0xbdU, 0x03U, 0x01U, 0x13U, 0x8aU, 0x6bU,
+	0x3aU, 0x91U, 0x11U, 0x41U, 0x4fU, 0x67U, 0xdcU, 0xeaU,
+	0x97U, 0xf2U, 0xcfU, 0xceU, 0xf0U, 0xb4U, 0xe6U, 0x73U,
+	0x96U, 0xacU, 0x74U, 0x22U, 0xe7U, 0xadU, 0x35U, 0x85U,
+	0xe2U, 0xf9U, 0x37U, 0xe8U, 0x1cU, 0x75U, 0xdfU, 0x6eU,
+	0x47U, 0xf1U, 0x1aU, 0x71U, 0x1dU, 0x29U, 0xc5U, 0x89U,
+	0x6fU, 0xb7U, 0x62U, 0x0eU, 0xaaU, 0x18U, 0xbeU, 0x1bU,
+	0xfcU, 0x56U, 0x3eU, 0x4bU, 0xc6U, 0xd2U, 0x79U, 0x20U,
+	0x9aU, 0xdbU, 0xc0U, 0xfeU, 0x78U, 0xcdU, 0x5aU, 0xf4U,
+	0x1fU, 0xddU, 0xa8U, 0x33U, 0x88U, 0x07U, 0xc7U, 0x31U,
+	0xb1U, 0x12U, 0x10U, 0x59U, 0x27U, 0x80U, 0xecU, 0x5fU,
+	0x60U, 0x51U, 0x7fU, 0xa9U, 0x19U, 0xb5U, 0x4aU, 0x0dU,
+	0x2dU, 0xe5U, 0x7aU, 0x9fU, 0x93U, 0xc9U, 0x9cU, 0xefU,
+	0xa0U, 0xe0U, 0x3bU, 0x4dU, 0xaeU, 0x2aU, 0xf5U, 0xb0U,
+	0xc8U, 0xebU, 0xbbU, 0x3cU, 0x83U, 0x53U, 0x99U, 0x61U,
+	0x17U, 0x2bU, 0x04U, 0x7eU, 0xbaU, 0x77U, 0xd6U, 0x26U,
+	0xe1U, 0x69U, 0x14U, 0x63U, 0x55U, 0x21U, 0x0cU, 0x7dU,
+};
+const u8 rcons[] = {
+	0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1B, 0x36
+	/* for 128-bit blocks, Rijndael never uses more than 10 rcon values */
+};
+
+/**
+ * Expand the cipher key into the encryption key schedule.
+ *
+ * @return	the number of rounds for the given cipher key size.
+ */
+static void rijndaelKeySetupEnc(u32 rk[/*44*/], const u8 cipherKey[])
+{
+	int i;
+	u32 temp;
+
+	rk[0] = GETU32(cipherKey);
+	rk[1] = GETU32(cipherKey +  4);
+	rk[2] = GETU32(cipherKey +  8);
+	rk[3] = GETU32(cipherKey + 12);
+	for (i = 0; i < 10; i++) {
+		temp  = rk[3];
+		rk[4] = rk[0] ^
+			TE421(temp) ^ TE432(temp) ^ TE443(temp) ^ TE414(temp) ^
+			RCON(i);
+		rk[5] = rk[1] ^ rk[4];
+		rk[6] = rk[2] ^ rk[5];
+		rk[7] = rk[3] ^ rk[6];
+		rk += 4;
+	}
+}
+
+static void rijndaelEncrypt(u32 rk[/*44*/], u8 pt[16], u8 ct[16])
+{
+	u32 s0, s1, s2, s3, t0, t1, t2, t3;
+	int Nr = 10;
+#ifndef FULL_UNROLL
+	int r;
+#endif /* ?FULL_UNROLL */
+
+	/*
+	 * map byte array block to cipher state
+	 * and add initial round key:
+	 */
+	s0 = GETU32(pt) ^ rk[0];
+	s1 = GETU32(pt +  4) ^ rk[1];
+	s2 = GETU32(pt +  8) ^ rk[2];
+	s3 = GETU32(pt + 12) ^ rk[3];
+
+#define ROUND(i, d, s) do {\
+	d##0 = TE0(s##0) ^ TE1(s##1) ^ TE2(s##2) ^ TE3(s##3) ^ rk[4 * i]; \
+	d##1 = TE0(s##1) ^ TE1(s##2) ^ TE2(s##3) ^ TE3(s##0) ^ rk[4 * i + 1]; \
+	d##2 = TE0(s##2) ^ TE1(s##3) ^ TE2(s##0) ^ TE3(s##1) ^ rk[4 * i + 2]; \
+	d##3 = TE0(s##3) ^ TE1(s##0) ^ TE2(s##1) ^ TE3(s##2) ^ rk[4 * i + 3]; \
+	} while (0)
+
+#ifdef FULL_UNROLL
+
+	ROUND(1, t, s);
+	ROUND(2, s, t);
+	ROUND(3, t, s);
+	ROUND(4, s, t);
+	ROUND(5, t, s);
+	ROUND(6, s, t);
+	ROUND(7, t, s);
+	ROUND(8, s, t);
+	ROUND(9, t, s);
+
+	rk += Nr << 2;
+
+#else  /* !FULL_UNROLL */
+
+	/* Nr - 1 full rounds: */
+	r = Nr >> 1;
+	for (;;) {
+		ROUND(1, t, s);
+		rk += 8;
+		if (--r == 0)
+			break;
+		ROUND(0, s, t);
+	}
+
+#endif /* ?FULL_UNROLL */
+
+#undef ROUND
+
+	/*
+	 * apply last round and
+	 * map cipher state to byte array block:
+	 */
+	s0 = TE41(t0) ^ TE42(t1) ^ TE43(t2) ^ TE44(t3) ^ rk[0];
+	PUTU32(ct     , s0);
+	s1 = TE41(t1) ^ TE42(t2) ^ TE43(t3) ^ TE44(t0) ^ rk[1];
+	PUTU32(ct +  4, s1);
+	s2 = TE41(t2) ^ TE42(t3) ^ TE43(t0) ^ TE44(t1) ^ rk[2];
+	PUTU32(ct +  8, s2);
+	s3 = TE41(t3) ^ TE42(t0) ^ TE43(t1) ^ TE44(t2) ^ rk[3];
+	PUTU32(ct + 12, s3);
+}
+
+static void *aes_encrypt_init(u8 *key, size_t len)
+{
+	u32 *rk;
+	if (len != 16)
+		return NULL;
+	rk = (u32 *)rtw_malloc(AES_PRIV_SIZE);
+	if (rk == NULL)
+		return NULL;
+	rijndaelKeySetupEnc(rk, key);
+	return rk;
+}
+
+static void aes_128_encrypt(void *ctx, u8 *plain, u8 *crypt)
+{
+	rijndaelEncrypt(ctx, plain, crypt);
+}
+
+
+static void gf_mulx(u8 *pad)
+{
+	int i, carry;
+
+	carry = pad[0] & 0x80;
+	for (i = 0; i < AES_BLOCK_SIZE - 1; i++)
+		pad[i] = (pad[i] << 1) | (pad[i + 1] >> 7);
+	pad[AES_BLOCK_SIZE - 1] <<= 1;
+	if (carry)
+		pad[AES_BLOCK_SIZE - 1] ^= 0x87;
+}
+
+static void aes_encrypt_deinit(void *ctx)
+{
+	memset(ctx, 0, AES_PRIV_SIZE);
+	rtw_mfree(ctx, AES_PRIV_SIZE);
+}
+
+
+/**
+ * omac1_aes_128_vector - One-Key CBC MAC (OMAC1) hash with AES-128
+ * @key: 128-bit key for the hash operation
+ * @num_elem: Number of elements in the data vector
+ * @addr: Pointers to the data areas
+ * @len: Lengths of the data blocks
+ * @mac: Buffer for MAC (128 bits, i.e., 16 bytes)
+ * Returns: 0 on success, -1 on failure
+ *
+ * This is a mode for using block cipher (AES in this case) for authentication.
+ * OMAC1 was standardized with the name CMAC by NIST in a Special Publication
+ * (SP) 800-38B.
+ */
+static int omac1_aes_128_vector(u8 *key, size_t num_elem,
+				u8 *addr[], size_t *len, u8 *mac)
+{
+	void *ctx;
+	u8 cbc[AES_BLOCK_SIZE], pad[AES_BLOCK_SIZE];
+	u8 *pos, *end;
+	size_t i, e, left, total_len;
+
+	ctx = aes_encrypt_init(key, 16);
+	if (ctx == NULL)
+		return -1;
+	memset(cbc, 0, AES_BLOCK_SIZE);
+
+	total_len = 0;
+	for (e = 0; e < num_elem; e++)
+		total_len += len[e];
+	left = total_len;
+
+	e = 0;
+	pos = addr[0];
+	end = pos + len[0];
+
+	while (left >= AES_BLOCK_SIZE) {
+		for (i = 0; i < AES_BLOCK_SIZE; i++) {
+			cbc[i] ^= *pos++;
+			if (pos >= end) {
+				e++;
+				pos = addr[e];
+				end = pos + len[e];
+			}
+		}
+		if (left > AES_BLOCK_SIZE)
+			aes_128_encrypt(ctx, cbc, cbc);
+		left -= AES_BLOCK_SIZE;
+	}
+
+	memset(pad, 0, AES_BLOCK_SIZE);
+	aes_128_encrypt(ctx, pad, pad);
+	gf_mulx(pad);
+
+	if (left || total_len == 0) {
+		for (i = 0; i < left; i++) {
+			cbc[i] ^= *pos++;
+			if (pos >= end) {
+				e++;
+				pos = addr[e];
+				end = pos + len[e];
+			}
+		}
+		cbc[left] ^= 0x80;
+		gf_mulx(pad);
+	}
+
+	for (i = 0; i < AES_BLOCK_SIZE; i++)
+		pad[i] ^= cbc[i];
+	aes_128_encrypt(ctx, pad, mac);
+	aes_encrypt_deinit(ctx);
+	return 0;
+}
+
+
+/**
+ * omac1_aes_128 - One-Key CBC MAC (OMAC1) hash with AES-128 (aka AES-CMAC)
+ * @key: 128-bit key for the hash operation
+ * @data: Data buffer for which a MAC is determined
+ * @data_len: Length of data buffer in bytes
+ * @mac: Buffer for MAC (128 bits, i.e., 16 bytes)
+ * Returns: 0 on success, -1 on failure
+ *
+ * This is a mode for using block cipher (AES in this case) for authentication.
+ * OMAC1 was standardized with the name CMAC by NIST in a Special Publication
+ * (SP) 800-38B.
+ */ /* modify for CONFIG_IEEE80211W */
+static int omac1_aes_128(u8 *key, u8 *data, size_t data_len, u8 *mac)
+{
+	return omac1_aes_128_vector(key, 1, &data, &data_len, mac);
+}
+
+#ifdef CONFIG_TDLS
+void wpa_tdls_generate_tpk(_adapter *padapter, void * sta)
+{
+	struct sta_info *psta = (struct sta_info *)sta;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	u8 *SNonce = psta->SNonce;
+	u8 *ANonce = psta->ANonce;
+
+	u8 key_input[SHA256_MAC_LEN];
+	u8 *nonce[2];
+	size_t len[2];
+	u8 data[3 * ETH_ALEN];
+
+	/* IEEE Std 802.11z-2010 8.5.9.1:
+	 * TPK-Key-Input = SHA-256(min(SNonce, ANonce) || max(SNonce, ANonce))
+	 */
+	len[0] = 32;
+	len[1] = 32;
+	if (os_memcmp(SNonce, ANonce, 32) < 0) {
+		nonce[0] = SNonce;
+		nonce[1] = ANonce;
+	} else {
+		nonce[0] = ANonce;
+		nonce[1] = SNonce;
+	}
+
+	sha256_vector(2, nonce, len, key_input);
+
+	/*
+	 * TPK-Key-Data = KDF-N_KEY(TPK-Key-Input, "TDLS PMK",
+	 *	min(MAC_I, MAC_R) || max(MAC_I, MAC_R) || BSSID || N_KEY)
+	 * TODO: is N_KEY really included in KDF Context and if so, in which
+	 * presentation format (little endian 16-bit?) is it used? It gets
+	 * added by the KDF anyway..
+	 */
+
+	if (os_memcmp(adapter_mac_addr(padapter), psta->hwaddr, ETH_ALEN) < 0) {
+		memcpy(data, adapter_mac_addr(padapter), ETH_ALEN);
+		memcpy(data + ETH_ALEN, psta->hwaddr, ETH_ALEN);
+	} else {
+		memcpy(data, psta->hwaddr, ETH_ALEN);
+		memcpy(data + ETH_ALEN, adapter_mac_addr(padapter), ETH_ALEN);
+	}
+	memcpy(data + 2 * ETH_ALEN, get_bssid(pmlmepriv), ETH_ALEN);
+
+	sha256_prf(key_input, SHA256_MAC_LEN, "TDLS PMK", data, sizeof(data), (u8 *) &psta->tpk, sizeof(psta->tpk));
+
+
+}
+
+/**
+ * wpa_tdls_ftie_mic - Calculate TDLS FTIE MIC
+ * @kck: TPK-KCK
+ * @lnkid: Pointer to the beginning of Link Identifier IE
+ * @rsnie: Pointer to the beginning of RSN IE used for handshake
+ * @timeoutie: Pointer to the beginning of Timeout IE used for handshake
+ * @ftie: Pointer to the beginning of FT IE
+ * @mic: Pointer for writing MIC
+ *
+ * Calculate MIC for TDLS frame.
+ */
+int wpa_tdls_ftie_mic(u8 *kck, u8 trans_seq,
+		      u8 *lnkid, u8 *rsnie, u8 *timeoutie, u8 *ftie,
+		      u8 *mic)
+{
+	u8 *buf, *pos;
+	struct wpa_tdls_ftie *_ftie;
+	struct wpa_tdls_lnkid *_lnkid;
+	int ret;
+	int len = 2 * ETH_ALEN + 1 + 2 + lnkid[1] + 2 + rsnie[1] +
+		  2 + timeoutie[1] + 2 + ftie[1];
+	buf = rtw_zmalloc(len);
+	if (!buf) {
+		RTW_INFO("TDLS: No memory for MIC calculation\n");
+		return -1;
+	}
+
+	pos = buf;
+	_lnkid = (struct wpa_tdls_lnkid *) lnkid;
+	/* 1) TDLS initiator STA MAC address */
+	memcpy(pos, _lnkid->init_sta, ETH_ALEN);
+	pos += ETH_ALEN;
+	/* 2) TDLS responder STA MAC address */
+	memcpy(pos, _lnkid->resp_sta, ETH_ALEN);
+	pos += ETH_ALEN;
+	/* 3) Transaction Sequence number */
+	*pos++ = trans_seq;
+	/* 4) Link Identifier IE */
+	memcpy(pos, lnkid, 2 + lnkid[1]);
+	pos += 2 + lnkid[1];
+	/* 5) RSN IE */
+	memcpy(pos, rsnie, 2 + rsnie[1]);
+	pos += 2 + rsnie[1];
+	/* 6) Timeout Interval IE */
+	memcpy(pos, timeoutie, 2 + timeoutie[1]);
+	pos += 2 + timeoutie[1];
+	/* 7) FTIE, with the MIC field of the FTIE set to 0 */
+	memcpy(pos, ftie, 2 + ftie[1]);
+	_ftie = (struct wpa_tdls_ftie *) pos;
+	memset(_ftie->mic, 0, TDLS_MIC_LEN);
+	pos += 2 + ftie[1];
+
+	ret = omac1_aes_128(kck, buf, pos - buf, mic);
+	rtw_mfree(buf, len);
+	return ret;
+
+}
+
+/**
+ * wpa_tdls_teardown_ftie_mic - Calculate TDLS TEARDOWN FTIE MIC
+ * @kck: TPK-KCK
+ * @lnkid: Pointer to the beginning of Link Identifier IE
+ * @reason: Reason code of TDLS Teardown
+ * @dialog_token: Dialog token that was used in the MIC calculation for TPK Handshake Message 3
+ * @trans_seq: Transaction Sequence number (1 octet) which shall be set to the value 4
+ * @ftie: Pointer to the beginning of FT IE
+ * @mic: Pointer for writing MIC
+ *
+ * Calculate MIC for TDLS TEARDOWN frame according to Section 10.22.5 in IEEE 802.11 - 2012.
+ */
+int wpa_tdls_teardown_ftie_mic(u8 *kck, u8 *lnkid, u16 reason,
+			       u8 dialog_token, u8 trans_seq, u8 *ftie, u8 *mic)
+{
+	u8 *buf, *pos;
+	struct wpa_tdls_ftie *_ftie;
+	int ret;
+	int len = 2 + lnkid[1] + 2 + 1 + 1 + 2 + ftie[1];
+
+	buf = rtw_zmalloc(len);
+	if (!buf) {
+		RTW_INFO("TDLS: No memory for MIC calculation\n");
+		return -1;
+	}
+
+	pos = buf;
+	/* 1) Link Identifier IE */
+	memcpy(pos, lnkid, 2 + lnkid[1]);
+	pos += 2 + lnkid[1];
+	/* 2) Reason Code */
+	memcpy(pos, (u8 *)&reason, 2);
+	pos += 2;
+	/* 3) Dialog Token */
+	*pos++ = dialog_token;
+	/* 4) Transaction Sequence number */
+	*pos++ = trans_seq;
+	/* 5) FTIE, with the MIC field of the FTIE set to 0 */
+	memcpy(pos, ftie, 2 + ftie[1]);
+	_ftie = (struct wpa_tdls_ftie *) pos;
+	memset(_ftie->mic, 0, TDLS_MIC_LEN);
+	pos += 2 + ftie[1];
+
+	ret = omac1_aes_128(kck, buf, pos - buf, mic);
+	rtw_mfree(buf, len);
+	return ret;
+
+}
+
+int tdls_verify_mic(u8 *kck, u8 trans_seq,
+		    u8 *lnkid, u8 *rsnie, u8 *timeoutie, u8 *ftie)
+{
+	u8 *buf, *pos;
+	int len;
+	u8 mic[16];
+	int ret;
+	u8 *rx_ftie, *tmp_ftie;
+
+	if (lnkid == NULL || rsnie == NULL ||
+	    timeoutie == NULL || ftie == NULL)
+		return _FAIL;
+
+	len = 2 * ETH_ALEN + 1 + 2 + 18 + 2 + *(rsnie + 1) + 2 + *(timeoutie + 1) + 2 + *(ftie + 1);
+
+	buf = rtw_zmalloc(len);
+	if (buf == NULL)
+		return _FAIL;
+
+	pos = buf;
+	/* 1) TDLS initiator STA MAC address */
+	memcpy(pos, lnkid + ETH_ALEN + 2, ETH_ALEN);
+	pos += ETH_ALEN;
+	/* 2) TDLS responder STA MAC address */
+	memcpy(pos, lnkid + 2 * ETH_ALEN + 2, ETH_ALEN);
+	pos += ETH_ALEN;
+	/* 3) Transaction Sequence number */
+	*pos++ = trans_seq;
+	/* 4) Link Identifier IE */
+	memcpy(pos, lnkid, 2 + 18);
+	pos += 2 + 18;
+	/* 5) RSN IE */
+	memcpy(pos, rsnie, 2 + *(rsnie + 1));
+	pos += 2 + *(rsnie + 1);
+	/* 6) Timeout Interval IE */
+	memcpy(pos, timeoutie, 2 + *(timeoutie + 1));
+	pos += 2 + *(timeoutie + 1);
+	/* 7) FTIE, with the MIC field of the FTIE set to 0 */
+	memcpy(pos, ftie, 2 + *(ftie + 1));
+	pos += 2;
+	tmp_ftie = (u8 *)(pos + 2);
+	memset(tmp_ftie, 0, 16);
+	pos += *(ftie + 1);
+
+	ret = omac1_aes_128(kck, buf, pos - buf, mic);
+	rtw_mfree(buf, len);
+	if (ret)
+		return _FAIL;
+	rx_ftie = ftie + 4;
+
+	if (os_memcmp(mic, rx_ftie, 16) == 0) {
+		/* Valid MIC */
+		return _SUCCESS;
+	}
+
+	/* Invalid MIC */
+	RTW_INFO("[%s] Invalid MIC\n", __func__);
+	return _FAIL;
+
+}
+#endif /* CONFIG_TDLS */
+
+void rtw_use_tkipkey_handler(RTW_TIMER_HDL_ARGS)
+{
+	_adapter *padapter = (_adapter *)FunctionContext;
+
+
+
+	/*
+		if (RTW_CANNOT_RUN(padapter)) {
+
+			return;
+		}
+		*/
+
+	padapter->securitypriv.busetkipkey = true;
+
+
+
+}
+
+/* Restore HW wep key setting according to key_mask */
+void rtw_sec_restore_wep_key(_adapter *adapter)
+{
+	struct security_priv *securitypriv = &(adapter->securitypriv);
+	sint keyid;
+
+	if ((_WEP40_ == securitypriv->dot11PrivacyAlgrthm) || (_WEP104_ == securitypriv->dot11PrivacyAlgrthm)) {
+		for (keyid = 0; keyid < 4; keyid++) {
+			if (securitypriv->key_mask & BIT(keyid)) {
+				if (keyid == securitypriv->dot11PrivacyKeyIndex)
+					rtw_set_key(adapter, securitypriv, keyid, 1, false);
+				else
+					rtw_set_key(adapter, securitypriv, keyid, 0, false);
+			}
+		}
+	}
+}
+
+u8 rtw_handle_tkip_countermeasure(_adapter *adapter, const char *caller)
+{
+	struct security_priv *securitypriv = &(adapter->securitypriv);
+	u8 status = _SUCCESS;
+
+	if (securitypriv->btkip_countermeasure == true) {
+		u32 passing_ms = rtw_get_passing_time_ms(securitypriv->btkip_countermeasure_time);
+		if (passing_ms > 60 * 1000) {
+			RTW_PRINT("%s("ADPT_FMT") countermeasure time:%ds > 60s\n",
+				  caller, ADPT_ARG(adapter), passing_ms / 1000);
+			securitypriv->btkip_countermeasure = false;
+			securitypriv->btkip_countermeasure_time = 0;
+		} else {
+			RTW_PRINT("%s("ADPT_FMT") countermeasure time:%ds < 60s\n",
+				  caller, ADPT_ARG(adapter), passing_ms / 1000);
+			status = _FAIL;
+		}
+	}
+
+	return status;
+}
+
+#ifdef CONFIG_WOWLAN
+u16 rtw_cal_crc16(u8 data, u16 crc)
+{
+	u8 shift_in, data_bit;
+	u8 crc_bit4, crc_bit11, crc_bit15;
+	u16 crc_result;
+	int index;
+
+	for (index = 0; index < 8; index++) {
+		crc_bit15 = ((crc & BIT15) ? 1 : 0);
+		data_bit = (data & (BIT0 << index) ? 1 : 0);
+		shift_in = crc_bit15 ^ data_bit;
+		/*printf("crc_bit15=%d, DataBit=%d, shift_in=%d\n",
+		 * crc_bit15, data_bit, shift_in);*/
+
+		crc_result = crc << 1;
+
+		if (shift_in == 0)
+			crc_result &= (~BIT0);
+		else
+			crc_result |= BIT0;
+		/*printf("CRC =%x\n",CRC_Result);*/
+
+		crc_bit11 = ((crc & BIT11) ? 1 : 0) ^ shift_in;
+
+		if (crc_bit11 == 0)
+			crc_result &= (~BIT12);
+		else
+			crc_result |= BIT12;
+
+		/*printf("bit12 CRC =%x\n",CRC_Result);*/
+
+		crc_bit4 = ((crc & BIT4) ? 1 : 0) ^ shift_in;
+
+		if (crc_bit4 == 0)
+			crc_result &= (~BIT5);
+		else
+			crc_result |= BIT5;
+
+		/* printf("bit5 CRC =%x\n",CRC_Result); */
+		/* repeat using the last result*/
+		crc = crc_result;
+	}
+	return crc;
+}
+
+/*
+ * function name :rtw_calc_crc
+ *
+ * input: char* pattern , pattern size
+ *
+ */
+u16 rtw_calc_crc(u8  *pdata, int length)
+{
+	u16 crc = 0xffff;
+	int i;
+
+	for (i = 0; i < length; i++)
+		crc = rtw_cal_crc16(pdata[i], crc);
+	/* get 1' complement */
+	crc = ~crc;
+
+	return crc;
+}
+#endif /*CONFIG_WOWLAN*/
diff --git a/drivers/staging/rtl8188eu/core/rtw_sreset.c b/drivers/staging/rtl8188eu/core/rtw_sreset.c
new file mode 100644
index 000000000000..d760d8bcc94a
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_sreset.c
@@ -0,0 +1,291 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+
+#include <drv_types.h>
+#include <hal_data.h>
+#include <rtw_sreset.h>
+
+void sreset_init_value(_adapter *padapter)
+{
+#if defined(DBG_CONFIG_ERROR_DETECT)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
+
+	_rtw_mutex_init(&psrtpriv->silentreset_mutex);
+	psrtpriv->silent_reset_inprogress = false;
+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;
+	psrtpriv->last_tx_time = 0;
+	psrtpriv->last_tx_complete_time = 0;
+#endif
+}
+void sreset_reset_value(_adapter *padapter)
+{
+#if defined(DBG_CONFIG_ERROR_DETECT)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
+
+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;
+	psrtpriv->last_tx_time = 0;
+	psrtpriv->last_tx_complete_time = 0;
+#endif
+}
+
+u8 sreset_get_wifi_status(_adapter *padapter)
+{
+#if defined(DBG_CONFIG_ERROR_DETECT)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
+
+	u8 status = WIFI_STATUS_SUCCESS;
+	u32 val32 = 0;
+	unsigned long irqL;
+	if (psrtpriv->silent_reset_inprogress == true)
+		return status;
+	val32 = rtw_read32(padapter, REG_TXDMA_STATUS);
+	if (val32 == 0xeaeaeaea)
+		psrtpriv->Wifi_Error_Status = WIFI_IF_NOT_EXIST;
+	else if (val32 != 0) {
+		RTW_INFO("txdmastatu(%x)\n", val32);
+		psrtpriv->Wifi_Error_Status = WIFI_MAC_TXDMA_ERROR;
+	}
+
+	if (WIFI_STATUS_SUCCESS != psrtpriv->Wifi_Error_Status) {
+		RTW_INFO("==>%s error_status(0x%x)\n", __func__, psrtpriv->Wifi_Error_Status);
+		status = (psrtpriv->Wifi_Error_Status & (~(USB_READ_PORT_FAIL | USB_WRITE_PORT_FAIL)));
+	}
+	RTW_INFO("==> %s wifi_status(0x%x)\n", __func__, status);
+
+	/* status restore */
+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;
+
+	return status;
+#else
+	return WIFI_STATUS_SUCCESS;
+#endif
+}
+
+void sreset_set_wifi_error_status(_adapter *padapter, u32 status)
+{
+#if defined(DBG_CONFIG_ERROR_DETECT)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	pHalData->srestpriv.Wifi_Error_Status = status;
+#endif
+}
+
+void sreset_set_trigger_point(_adapter *padapter, s32 tgp)
+{
+#if defined(DBG_CONFIG_ERROR_DETECT)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	pHalData->srestpriv.dbg_trigger_point = tgp;
+#endif
+}
+
+bool sreset_inprogress(_adapter *padapter)
+{
+#if defined(DBG_CONFIG_ERROR_RESET)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	return pHalData->srestpriv.silent_reset_inprogress;
+#else
+	return false;
+#endif
+}
+
+static void sreset_restore_security_station(_adapter *padapter)
+{
+	u8 EntryId = 0;
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta;
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	struct mlme_ext_info	*pmlmeinfo = &padapter->mlmeextpriv.mlmext_info;
+
+	{
+		u8 val8;
+
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) {
+			val8 = 0xcc;
+#ifdef CONFIG_WAPI_SUPPORT
+		} else if (padapter->wapiInfo.bWapiEnable && pmlmeinfo->auth_algo == dot11AuthAlgrthm_WAPI) {
+			/* Disable TxUseDefaultKey, RxUseDefaultKey, RxBroadcastUseDefaultKey. */
+			val8 = 0x4c;
+#endif
+		} else
+			val8 = 0xcf;
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+	}
+	if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
+	    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+		psta = rtw_get_stainfo(pstapriv, get_bssid(mlmepriv));
+		if (psta == NULL) {
+			/* DEBUG_ERR( ("Set wpa_set_encryption: Obtain Sta_info fail\n")); */
+		} else {
+			/* pairwise key */
+			rtw_setstakey_cmd(padapter, psta, UNICAST_KEY, false);
+			/* group key */
+			rtw_set_key(padapter, &padapter->securitypriv, padapter->securitypriv.dot118021XGrpKeyid, 0, false);
+		}
+	}
+}
+
+static void sreset_restore_network_station(_adapter *padapter)
+{
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 doiqk = false;
+
+	rtw_setopmode_cmd(padapter, Ndis802_11Infrastructure, false);
+
+	{
+		u8 threshold;
+		/* TH=1 => means that invalidate usb rx aggregation */
+		/* TH=0 => means that validate usb rx aggregation, use init value. */
+		if (mlmepriv->htpriv.ht_option) {
+			if (padapter->registrypriv.wifi_spec == 1)
+				threshold = 1;
+			else
+				threshold = 0;
+			rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+		} else {
+			threshold = 1;
+			rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+		}
+	}
+
+	doiqk = true;
+	rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK , &doiqk);
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	doiqk = false;
+	rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk);
+	/* disable dynamic functions, such as high power, DIG */
+	/*rtw_phydm_func_disable_all(padapter);*/
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
+
+	{
+		u8	join_type = 0;
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	}
+
+	Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+	mlmeext_joinbss_event_callback(padapter, 1);
+	/* restore Sequence No. */
+	rtw_hal_set_hwreg(padapter, HW_VAR_RESTORE_HW_SEQ, NULL);
+
+	sreset_restore_security_station(padapter);
+}
+
+
+static void sreset_restore_network_status(_adapter *padapter)
+{
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (check_fwstate(mlmepriv, WIFI_STATION_STATE)) {
+		RTW_INFO(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_STATION_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		sreset_restore_network_station(padapter);
+	} else if (check_fwstate(mlmepriv, WIFI_AP_STATE)) {
+		RTW_INFO(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_AP_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		rtw_ap_restore_network(padapter);
+	} else if (check_fwstate(mlmepriv, WIFI_ADHOC_STATE))
+		RTW_INFO(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_ADHOC_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+	else
+		RTW_INFO(FUNC_ADPT_FMT" fwstate:0x%08x - ???\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+}
+
+void sreset_stop_adapter(_adapter *padapter)
+{
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+
+	if (padapter == NULL)
+		return;
+
+	RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	rtw_netif_stop_queue(padapter->pnetdev);
+
+	rtw_cancel_all_timer(padapter);
+
+	/* TODO: OS and HCI independent */
+	tasklet_kill(&pxmitpriv->xmit_tasklet);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+		rtw_scan_abort(padapter);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+		rtw_set_to_roam(padapter, 0);
+		_rtw_join_timeout_handler(padapter);
+	}
+
+}
+
+void sreset_start_adapter(_adapter *padapter)
+{
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+
+	if (padapter == NULL)
+		return;
+
+	RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		sreset_restore_network_status(padapter);
+
+	/* TODO: OS and HCI independent */
+	tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);
+
+	if (is_primary_adapter(padapter))
+		_set_timer(&adapter_to_dvobj(padapter)->dynamic_chk_timer, 2000);
+
+	rtw_netif_wake_queue(padapter->pnetdev);
+}
+
+void sreset_reset(_adapter *padapter)
+{
+#ifdef DBG_CONFIG_ERROR_RESET
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	unsigned long irqL;
+	u32 start = jiffies;
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	RTW_INFO("%s\n", __func__);
+
+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;
+
+
+#ifdef CONFIG_LPS
+	rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, "SRESET");
+#endif/* #ifdef CONFIG_LPS */
+
+	_enter_pwrlock(&pwrpriv->lock);
+
+	psrtpriv->silent_reset_inprogress = true;
+	pwrpriv->change_rfpwrstate = rf_off;
+
+	rtw_mi_sreset_adapter_hdl(padapter, false);/*sreset_stop_adapter*/
+#ifdef CONFIG_IPS
+	_ips_enter(padapter);
+	_ips_leave(padapter);
+#endif
+	rtw_mi_sreset_adapter_hdl(padapter, true);/*sreset_start_adapter*/
+
+	psrtpriv->silent_reset_inprogress = false;
+
+	_exit_pwrlock(&pwrpriv->lock);
+
+	RTW_INFO("%s done in %d ms\n", __func__, rtw_get_passing_time_ms(start));
+	pdbgpriv->dbg_sreset_cnt++;
+#endif
+}
diff --git a/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c b/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
new file mode 100644
index 000000000000..c31687451d92
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
@@ -0,0 +1,1096 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_STA_MGT_C_
+
+#include <drv_types.h>
+
+static bool test_st_match_rule(_adapter *adapter, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port)
+{
+	if (ntohs(*((__be16 *)local_port)) == 5001 ||
+	    ntohs(*((__be16 *)remote_port)) == 5001)
+		return true;
+	return false;
+}
+
+static struct st_register test_st_reg = {
+	.s_proto = 0x06,
+	.rule = test_st_match_rule,
+};
+
+inline void rtw_st_ctl_init(struct st_ctl_t *st_ctl)
+{
+	memset(st_ctl->reg, 0 , sizeof(struct st_register) * SESSION_TRACKER_REG_ID_NUM);
+	_rtw_init_queue(&st_ctl->tracker_q);
+}
+
+inline void rtw_st_ctl_clear_tracker_q(struct st_ctl_t *st_ctl)
+{
+	unsigned long irqL;
+	_list *plist, *phead;
+	struct session_tracker *st;
+
+	_enter_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+	phead = &st_ctl->tracker_q.queue;
+	plist = get_next(phead);
+	while (rtw_end_of_queue_search(phead, plist) == false) {
+		st = LIST_CONTAINOR(plist, struct session_tracker, list);
+		plist = get_next(plist);
+		list_del_init(&st->list);
+		rtw_mfree((u8 *)st, sizeof(struct session_tracker));
+	}
+	_exit_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+}
+
+inline void rtw_st_ctl_deinit(struct st_ctl_t *st_ctl)
+{
+	rtw_st_ctl_clear_tracker_q(st_ctl);
+}
+
+inline void rtw_st_ctl_register(struct st_ctl_t *st_ctl, u8 st_reg_id, struct st_register *reg)
+{
+	if (st_reg_id >= SESSION_TRACKER_REG_ID_NUM) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	st_ctl->reg[st_reg_id].s_proto = reg->s_proto;
+	st_ctl->reg[st_reg_id].rule = reg->rule;
+}
+
+inline void rtw_st_ctl_unregister(struct st_ctl_t *st_ctl, u8 st_reg_id)
+{
+	int i;
+
+	if (st_reg_id >= SESSION_TRACKER_REG_ID_NUM) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	st_ctl->reg[st_reg_id].s_proto = 0;
+	st_ctl->reg[st_reg_id].rule = NULL;
+
+	/* clear tracker queue if no session trecker registered */
+	for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++)
+		if (st_ctl->reg[i].s_proto != 0)
+			break;
+	if (i >= SESSION_TRACKER_REG_ID_NUM)
+		rtw_st_ctl_clear_tracker_q(st_ctl);
+}
+
+inline bool rtw_st_ctl_chk_reg_s_proto(struct st_ctl_t *st_ctl, u8 s_proto)
+{
+	bool ret = false;
+	int i;
+
+	for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++) {
+		if (st_ctl->reg[i].s_proto == s_proto) {
+			ret = true;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+inline bool rtw_st_ctl_chk_reg_rule(struct st_ctl_t *st_ctl, _adapter *adapter, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port)
+{
+	bool ret = false;
+	int i;
+	st_match_rule rule;
+
+	for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++) {
+		rule = st_ctl->reg[i].rule;
+		if (rule && rule(adapter, local_naddr, local_port, remote_naddr, remote_port) == true) {
+			ret = true;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+#define SESSION_TRACKER_FMT IP_FMT":"PORT_FMT" "IP_FMT":"PORT_FMT" %u %d"
+#define SESSION_TRACKER_ARG(st) IP_ARG(&(st)->local_naddr), PORT_ARG(&(st)->local_port), IP_ARG(&(st)->remote_naddr), PORT_ARG(&(st)->remote_port), (st)->status, rtw_get_passing_time_ms((st)->set_time)
+
+void dump_st_ctl(void *sel, struct st_ctl_t *st_ctl)
+{
+	int i;
+	unsigned long irqL;
+	_list *plist, *phead;
+	struct session_tracker *st;
+
+	if (!DBG_SESSION_TRACKER)
+		return;
+
+	for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++)
+		RTW_PRINT_SEL(sel, "reg%d: %u %p\n", i, st_ctl->reg[i].s_proto, st_ctl->reg[i].rule);
+
+	_enter_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+	phead = &st_ctl->tracker_q.queue;
+	plist = get_next(phead);
+	while (rtw_end_of_queue_search(phead, plist) == false) {
+		st = LIST_CONTAINOR(plist, struct session_tracker, list);
+		plist = get_next(plist);
+
+		RTW_PRINT_SEL(sel, SESSION_TRACKER_FMT"\n", SESSION_TRACKER_ARG(st));
+	}
+	_exit_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+
+}
+
+void _rtw_init_stainfo(struct sta_info *psta);
+void _rtw_init_stainfo(struct sta_info *psta)
+{
+
+
+	memset((u8 *)psta, 0, sizeof(struct sta_info));
+
+	spin_lock_init(&psta->lock);
+	INIT_LIST_HEAD(&psta->list);
+	INIT_LIST_HEAD(&psta->hash_list);
+	/* INIT_LIST_HEAD(&psta->asoc_list); */
+	/* INIT_LIST_HEAD(&psta->sleep_list); */
+	/* INIT_LIST_HEAD(&psta->wakeup_list);	 */
+
+	_rtw_init_queue(&psta->sleep_q);
+	psta->sleepq_len = 0;
+
+	_rtw_init_sta_xmit_priv(&psta->sta_xmitpriv);
+	_rtw_init_sta_recv_priv(&psta->sta_recvpriv);
+
+#ifdef CONFIG_AP_MODE
+
+	INIT_LIST_HEAD(&psta->asoc_list);
+
+	INIT_LIST_HEAD(&psta->auth_list);
+
+	psta->expire_to = 0;
+
+	psta->flags = 0;
+
+	psta->capability = 0;
+
+	psta->bpairwise_key_installed = false;
+
+#ifdef CONFIG_RTW_80211R
+	psta->ft_pairwise_key_installed = false;
+#endif
+
+#ifdef CONFIG_NATIVEAP_MLME
+	psta->nonerp_set = 0;
+	psta->no_short_slot_time_set = 0;
+	psta->no_short_preamble_set = 0;
+	psta->no_ht_gf_set = 0;
+	psta->no_ht_set = 0;
+	psta->ht_20mhz_set = 0;
+	psta->ht_40mhz_intolerant = 0;
+#endif
+
+#ifdef CONFIG_TX_MCAST2UNI
+	psta->under_exist_checking = 0;
+#endif /* CONFIG_TX_MCAST2UNI */
+
+	psta->keep_alive_trycnt = 0;
+
+#endif /* CONFIG_AP_MODE	 */
+
+	rtw_st_ctl_init(&psta->st_ctl);
+
+
+}
+
+u32	_rtw_init_sta_priv(struct	sta_priv *pstapriv)
+{
+	struct sta_info *psta;
+	s32 i;
+
+
+	pstapriv->pallocated_stainfo_buf = rtw_zvmalloc(sizeof(struct sta_info) * NUM_STA + 4);
+
+	if (!pstapriv->pallocated_stainfo_buf)
+		return _FAIL;
+
+	pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 -
+			 ((SIZE_PTR)(pstapriv->pallocated_stainfo_buf) & 3);
+
+	_rtw_init_queue(&pstapriv->free_sta_queue);
+
+	spin_lock_init(&pstapriv->sta_hash_lock);
+
+	/* _rtw_init_queue(&pstapriv->asoc_q); */
+	pstapriv->asoc_sta_count = 0;
+	_rtw_init_queue(&pstapriv->sleep_q);
+	_rtw_init_queue(&pstapriv->wakeup_q);
+
+	psta = (struct sta_info *)(pstapriv->pstainfo_buf);
+
+
+	for (i = 0; i < NUM_STA; i++) {
+		_rtw_init_stainfo(psta);
+
+		INIT_LIST_HEAD(&(pstapriv->sta_hash[i]));
+
+		list_add_tail(&psta->list, get_list_head(&pstapriv->free_sta_queue));
+
+		psta++;
+	}
+
+	pstapriv->adhoc_expire_to = 4; /* 4 * 2 = 8 sec */
+
+#ifdef CONFIG_AP_MODE
+
+	pstapriv->sta_dz_bitmap = 0;
+	pstapriv->tim_bitmap = 0;
+
+	INIT_LIST_HEAD(&pstapriv->asoc_list);
+	INIT_LIST_HEAD(&pstapriv->auth_list);
+	spin_lock_init(&pstapriv->asoc_list_lock);
+	spin_lock_init(&pstapriv->auth_list_lock);
+	pstapriv->asoc_list_cnt = 0;
+	pstapriv->auth_list_cnt = 0;
+
+	pstapriv->auth_to = 3; /* 3*2 = 6 sec */
+	pstapriv->assoc_to = 3;
+	/* pstapriv->expire_to = 900; */ /* 900*2 = 1800 sec = 30 min, expire after no any traffic. */
+	/* pstapriv->expire_to = 30; */ /* 30*2 = 60 sec = 1 min, expire after no any traffic. */
+#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+	pstapriv->expire_to = 3; /* 3*2 = 6 sec */
+#else
+	pstapriv->expire_to = 60;/* 60*2 = 120 sec = 2 min, expire after no any traffic. */
+#endif
+#ifdef CONFIG_ATMEL_RC_PATCH
+	memset(pstapriv->atmel_rc_pattern, 0, ETH_ALEN);
+#endif
+	pstapriv->max_num_sta = NUM_STA;
+
+#endif
+
+#if CONFIG_RTW_MACADDR_ACL
+	_rtw_init_queue(&(pstapriv->acl_list.acl_node_q));
+#endif
+
+#if CONFIG_RTW_PRE_LINK_STA
+	rtw_pre_link_sta_ctl_init(pstapriv);
+#endif
+
+	return _SUCCESS;
+
+}
+
+inline int rtw_stainfo_offset(struct sta_priv *stapriv, struct sta_info *sta)
+{
+	int offset = (((u8 *)sta) - stapriv->pstainfo_buf) / sizeof(struct sta_info);
+
+	if (!stainfo_offset_valid(offset))
+		RTW_INFO("%s invalid offset(%d), out of range!!!", __func__, offset);
+
+	return offset;
+}
+
+inline struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int offset)
+{
+	if (!stainfo_offset_valid(offset))
+		RTW_INFO("%s invalid offset(%d), out of range!!!", __func__, offset);
+
+	return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info));
+}
+
+/* this function is used to free the memory of lock || sema for all stainfos */
+static void rtw_mfree_all_stainfo(struct sta_priv *pstapriv)
+{
+	unsigned long	 irqL;
+	_list	*plist, *phead;
+	struct sta_info *psta = NULL;
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	phead = get_list_head(&pstapriv->free_sta_queue);
+	plist = get_next(phead);
+
+	while ((rtw_end_of_queue_search(phead, plist)) == false) {
+		psta = LIST_CONTAINOR(plist, struct sta_info , list);
+		plist = get_next(plist);
+	}
+
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+}
+
+static void rtw_mfree_sta_priv_lock(struct	sta_priv *pstapriv)
+{
+	rtw_mfree_all_stainfo(pstapriv); /* be done before free sta_hash_lock */
+}
+
+u32	_rtw_free_sta_priv(struct	sta_priv *pstapriv)
+{
+	unsigned long	irqL;
+	_list	*phead, *plist;
+	struct sta_info *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int	index;
+
+	if (pstapriv) {
+
+		/*	delete all reordering_ctrl_timer		*/
+		_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+		for (index = 0; index < NUM_STA; index++) {
+			phead = &(pstapriv->sta_hash[index]);
+			plist = get_next(phead);
+
+			while ((rtw_end_of_queue_search(phead, plist)) == false) {
+				int i;
+				psta = LIST_CONTAINOR(plist, struct sta_info , hash_list);
+				plist = get_next(plist);
+
+				for (i = 0; i < 16 ; i++) {
+					preorder_ctrl = &psta->recvreorder_ctrl[i];
+					_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+				}
+			}
+		}
+		_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+		/*===============================*/
+
+		rtw_mfree_sta_priv_lock(pstapriv);
+
+#if CONFIG_RTW_PRE_LINK_STA
+		rtw_pre_link_sta_ctl_deinit(pstapriv);
+#endif
+
+		if (pstapriv->pallocated_stainfo_buf)
+			rtw_vmfree(pstapriv->pallocated_stainfo_buf, sizeof(struct sta_info) * NUM_STA + 4);
+	}
+
+	return _SUCCESS;
+}
+
+
+/* struct	sta_info *rtw_alloc_stainfo(_queue *pfree_sta_queue, unsigned char *hwaddr) */
+struct	sta_info *rtw_alloc_stainfo(struct	sta_priv *pstapriv, u8 *hwaddr)
+{
+	unsigned long irqL, irqL2;
+	uint tmp_aid;
+	s32	index;
+	_list	*phash_list;
+	struct sta_info	*psta;
+	_queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int i = 0;
+	u16  wRxSeqInitialValue = 0xffff;
+
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+
+	/* _enter_critical_bh(&(pfree_sta_queue->lock), &irqL); */
+	_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+	if (_rtw_queue_empty(pfree_sta_queue) == true) {
+		psta = NULL;
+		goto exit;
+	}
+	psta = LIST_CONTAINOR(get_next(&pfree_sta_queue->queue), struct sta_info, list);
+
+	list_del_init(&(psta->list));
+
+	/* _exit_critical_bh(&(pfree_sta_queue->lock), &irqL); */
+
+	tmp_aid = psta->aid;
+
+	_rtw_init_stainfo(psta);
+
+	psta->padapter = pstapriv->padapter;
+
+	memcpy(psta->hwaddr, hwaddr, ETH_ALEN);
+
+	index = wifi_mac_hash(hwaddr);
+
+
+	if (index >= NUM_STA) {
+		psta = NULL;
+		goto exit;
+	}
+	phash_list = &(pstapriv->sta_hash[index]);
+
+	/* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL2); */
+
+	list_add_tail(&psta->hash_list, phash_list);
+
+	pstapriv->asoc_sta_count++;
+
+	/* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2); */
+
+	/* Commented by Albert 2009/08/13
+	 * For the SMC router, the sequence number of first packet of WPS handshake will be 0.
+	 * In this case, this packet will be dropped by recv_decache function if we use the 0x00 as the default value for tid_rxseq variable.
+	 * So, we initialize the tid_rxseq variable as the 0xffff. */
+
+	for (i = 0; i < 16; i++) {
+		memcpy(&psta->sta_recvpriv.rxcache.tid_rxseq[i], &wRxSeqInitialValue, 2);
+		memset(&psta->sta_recvpriv.rxcache.iv[i], 0, sizeof(psta->sta_recvpriv.rxcache.iv[i]));
+	}
+
+	init_addba_retry_timer(pstapriv->padapter, psta);
+#ifdef CONFIG_IEEE80211W
+	init_dot11w_expire_timer(pstapriv->padapter, psta);
+#endif /* CONFIG_IEEE80211W */
+#ifdef CONFIG_TDLS
+	rtw_init_tdls_timer(pstapriv->padapter, psta);
+#endif /* CONFIG_TDLS */
+
+	/* for A-MPDU Rx reordering buffer control */
+	for (i = 0; i < 16 ; i++) {
+		preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+		preorder_ctrl->padapter = pstapriv->padapter;
+
+		preorder_ctrl->enable = false;
+
+		preorder_ctrl->indicate_seq = 0xffff;
+#ifdef DBG_RX_SEQ
+		RTW_INFO("DBG_RX_SEQ %s:%d IndicateSeq: %d\n", __func__, __LINE__,
+			 preorder_ctrl->indicate_seq);
+#endif
+		preorder_ctrl->wend_b = 0xffff;
+		/* preorder_ctrl->wsize_b = (NR_RECVBUFF-2); */
+		preorder_ctrl->wsize_b = 64;/* 64; */
+		preorder_ctrl->ampdu_size = RX_AMPDU_SIZE_INVALID;
+
+		_rtw_init_queue(&preorder_ctrl->pending_recvframe_queue);
+
+		rtw_init_recv_timer(preorder_ctrl);
+	}
+
+
+	/* init for DM */
+	psta->rssi_stat.undecorated_smoothed_pwdb = (-1);
+	psta->rssi_stat.undecorated_smoothed_cck = (-1);
+#ifdef CONFIG_ATMEL_RC_PATCH
+	psta->flag_atmel_rc = 0;
+#endif
+	/* init for the sequence number of received management frame */
+	psta->RxMgmtFrameSeqNum = 0xffff;
+	psta->ra_rpt_linked = false;
+
+	rtw_alloc_macid(pstapriv->padapter, psta);
+
+exit:
+
+	_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+
+	if (psta)
+		rtw_mi_update_iface_status(&(pstapriv->padapter->mlmepriv), 0);
+	return psta;
+}
+
+/* using pstapriv->sta_hash_lock to protect */
+u32	rtw_free_stainfo(_adapter *padapter , struct sta_info *psta)
+{
+	int i;
+	unsigned long irqL0;
+	_queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct	sta_xmit_priv	*pstaxmitpriv;
+	struct	xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct hw_xmit *phwxmit;
+	int pending_qcnt[4];
+	u8 is_pre_link_sta = false;
+
+	if (psta == NULL)
+		goto exit;
+
+	is_pre_link_sta = rtw_is_pre_link_sta(pstapriv, psta->hwaddr);
+
+	if (is_pre_link_sta == false) {
+		_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL0);
+		list_del_init(&psta->hash_list);
+		pstapriv->asoc_sta_count--;
+		_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL0);
+		rtw_mi_update_iface_status(&(padapter->mlmepriv), 0);
+	} else {
+		_enter_critical_bh(&psta->lock, &irqL0);
+		psta->state = WIFI_FW_PRE_LINK;
+		_exit_critical_bh(&psta->lock, &irqL0);
+	}
+
+	_enter_critical_bh(&psta->lock, &irqL0);
+	psta->state &= ~_FW_LINKED;
+	_exit_critical_bh(&psta->lock, &irqL0);
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+
+
+	pstaxmitpriv = &psta->sta_xmitpriv;
+
+	/* list_del_init(&psta->sleep_list); */
+
+	/* list_del_init(&psta->wakeup_list); */
+
+	_enter_critical_bh(&pxmitpriv->lock, &irqL0);
+
+	rtw_free_xmitframe_queue(pxmitpriv, &psta->sleep_q);
+	psta->sleepq_len = 0;
+
+	/* vo */
+	/* _enter_critical_bh(&(pxmitpriv->vo_pending.lock), &irqL0); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vo_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->vo_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits;
+	phwxmit->accnt -= pstaxmitpriv->vo_q.qcnt;
+	pending_qcnt[0] = pstaxmitpriv->vo_q.qcnt;
+	pstaxmitpriv->vo_q.qcnt = 0;
+	/* _exit_critical_bh(&(pxmitpriv->vo_pending.lock), &irqL0); */
+
+	/* vi */
+	/* _enter_critical_bh(&(pxmitpriv->vi_pending.lock), &irqL0); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vi_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->vi_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits + 1;
+	phwxmit->accnt -= pstaxmitpriv->vi_q.qcnt;
+	pending_qcnt[1] = pstaxmitpriv->vi_q.qcnt;
+	pstaxmitpriv->vi_q.qcnt = 0;
+	/* _exit_critical_bh(&(pxmitpriv->vi_pending.lock), &irqL0); */
+
+	/* be */
+	/* _enter_critical_bh(&(pxmitpriv->be_pending.lock), &irqL0); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->be_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->be_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits + 2;
+	phwxmit->accnt -= pstaxmitpriv->be_q.qcnt;
+	pending_qcnt[2] = pstaxmitpriv->be_q.qcnt;
+	pstaxmitpriv->be_q.qcnt = 0;
+	/* _exit_critical_bh(&(pxmitpriv->be_pending.lock), &irqL0); */
+
+	/* bk */
+	/* _enter_critical_bh(&(pxmitpriv->bk_pending.lock), &irqL0); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->bk_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->bk_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits + 3;
+	phwxmit->accnt -= pstaxmitpriv->bk_q.qcnt;
+	pending_qcnt[3] = pstaxmitpriv->bk_q.qcnt;
+	pstaxmitpriv->bk_q.qcnt = 0;
+	/* _exit_critical_bh(&(pxmitpriv->bk_pending.lock), &irqL0); */
+
+	rtw_os_wake_queue_at_free_stainfo(padapter, pending_qcnt);
+
+	_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+
+
+	/* re-init sta_info; 20061114 */ /* will be init in alloc_stainfo */
+	/* _rtw_init_sta_xmit_priv(&psta->sta_xmitpriv); */
+	/* _rtw_init_sta_recv_priv(&psta->sta_recvpriv); */
+#ifdef CONFIG_IEEE80211W
+	_cancel_timer_ex(&psta->dot11w_expire_timer);
+#endif /* CONFIG_IEEE80211W */
+	_cancel_timer_ex(&psta->addba_retry_timer);
+
+#ifdef CONFIG_TDLS
+	psta->tdls_sta_state = TDLS_STATE_NONE;
+	rtw_free_tdls_timer(psta);
+#endif /* CONFIG_TDLS */
+
+	/* for A-MPDU Rx reordering buffer control, cancel reordering_ctrl_timer */
+	for (i = 0; i < 16 ; i++) {
+		unsigned long irqL;
+		_list	*phead, *plist;
+		union recv_frame *prframe;
+		_queue *ppending_recvframe_queue;
+		_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+		preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+		_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+
+
+		ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+		_enter_critical_bh(&ppending_recvframe_queue->lock, &irqL);
+
+		phead =	get_list_head(ppending_recvframe_queue);
+		plist = get_next(phead);
+
+		while (!list_empty(phead)) {
+			prframe = LIST_CONTAINOR(plist, union recv_frame, u);
+
+			plist = get_next(plist);
+
+			list_del_init(&(prframe->u.hdr.list));
+
+			rtw_free_recvframe(prframe, pfree_recv_queue);
+		}
+
+		_exit_critical_bh(&ppending_recvframe_queue->lock, &irqL);
+
+	}
+
+	if (!((psta->state & WIFI_AP_STATE) || MacAddr_isBcst(psta->hwaddr)) && is_pre_link_sta == false)
+		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, false);
+
+
+	/* release mac id for non-bc/mc station, */
+	if (is_pre_link_sta == false)
+		rtw_release_macid(pstapriv->padapter, psta);
+
+#ifdef CONFIG_AP_MODE
+
+	/*
+		_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL0);
+		list_del_init(&psta->asoc_list);
+		_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL0);
+	*/
+	_enter_critical_bh(&pstapriv->auth_list_lock, &irqL0);
+	if (!list_empty(&psta->auth_list)) {
+		list_del_init(&psta->auth_list);
+		pstapriv->auth_list_cnt--;
+	}
+	_exit_critical_bh(&pstapriv->auth_list_lock, &irqL0);
+
+	psta->expire_to = 0;
+#ifdef CONFIG_ATMEL_RC_PATCH
+	psta->flag_atmel_rc = 0;
+#endif
+	psta->sleepq_ac_len = 0;
+	psta->qos_info = 0;
+
+	psta->max_sp_len = 0;
+	psta->uapsd_bk = 0;
+	psta->uapsd_be = 0;
+	psta->uapsd_vi = 0;
+	psta->uapsd_vo = 0;
+
+	psta->has_legacy_ac = 0;
+
+#ifdef CONFIG_NATIVEAP_MLME
+
+	pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+	pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+	/* rtw_indicate_sta_disassoc_event(padapter, psta); */
+
+	if ((psta->aid > 0) && (pstapriv->sta_aid[psta->aid - 1] == psta)) {
+		pstapriv->sta_aid[psta->aid - 1] = NULL;
+		psta->aid = 0;
+	}
+
+#endif /* CONFIG_NATIVEAP_MLME	 */
+
+#ifdef CONFIG_TX_MCAST2UNI
+	psta->under_exist_checking = 0;
+#endif /* CONFIG_TX_MCAST2UNI */
+
+#endif /* CONFIG_AP_MODE	 */
+
+	rtw_st_ctl_deinit(&psta->st_ctl);
+
+	if (is_pre_link_sta == false) {
+		/* _enter_critical_bh(&(pfree_sta_queue->lock), &irqL0); */
+		_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL0);
+		list_add_tail(&psta->list, get_list_head(pfree_sta_queue));
+		_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL0);
+		/* _exit_critical_bh(&(pfree_sta_queue->lock), &irqL0); */
+	}
+
+exit:
+	return _SUCCESS;
+}
+
+/* free all stainfo which in sta_hash[all] */
+void rtw_free_all_stainfo(_adapter *padapter)
+{
+	unsigned long	 irqL;
+	_list	*plist, *phead;
+	s32	index;
+	struct sta_info *psta = NULL;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *pbcmc_stainfo = rtw_get_bcmc_stainfo(padapter);
+	u8 free_sta_num = 0;
+	char free_sta_list[NUM_STA];
+	int stainfo_offset;
+
+
+	if (pstapriv->asoc_sta_count == 1)
+		goto exit;
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	for (index = 0; index < NUM_STA; index++) {
+		phead = &(pstapriv->sta_hash[index]);
+		plist = get_next(phead);
+
+		while ((rtw_end_of_queue_search(phead, plist)) == false) {
+			psta = LIST_CONTAINOR(plist, struct sta_info , hash_list);
+
+			plist = get_next(plist);
+
+			if (pbcmc_stainfo != psta) {
+				if (rtw_is_pre_link_sta(pstapriv, psta->hwaddr) == false)
+					list_del_init(&psta->hash_list);
+
+				stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
+				if (stainfo_offset_valid(stainfo_offset))
+					free_sta_list[free_sta_num++] = stainfo_offset;
+			}
+
+		}
+	}
+
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+
+	for (index = 0; index < free_sta_num; index++) {
+		psta = rtw_get_stainfo_by_offset(pstapriv, free_sta_list[index]);
+		rtw_free_stainfo(padapter , psta);
+	}
+
+exit:
+	return;
+}
+
+/* any station allocated can be searched by hash list */
+struct sta_info *rtw_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
+{
+
+	unsigned long	 irqL;
+
+	_list	*plist, *phead;
+
+	struct sta_info *psta = NULL;
+
+	u32	index;
+
+	u8 *addr;
+
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+
+	if (hwaddr == NULL)
+		return NULL;
+
+	if (IS_MCAST(hwaddr))
+		addr = bc_addr;
+	else
+		addr = hwaddr;
+
+	index = wifi_mac_hash(addr);
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	phead = &(pstapriv->sta_hash[index]);
+	plist = get_next(phead);
+
+
+	while ((rtw_end_of_queue_search(phead, plist)) == false) {
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+		if ((!memcmp(psta->hwaddr, addr, ETH_ALEN)) == true) {
+			/* if found the matched address */
+			break;
+		}
+		psta = NULL;
+		plist = get_next(plist);
+	}
+
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+	return psta;
+
+}
+
+u32 rtw_init_bcmc_stainfo(_adapter *padapter)
+{
+
+	struct sta_info	*psta;
+	struct tx_servq	*ptxservq;
+	u32 res = _SUCCESS;
+	NDIS_802_11_MAC_ADDRESS	bcast_addr = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	/* _queue	*pstapending = &padapter->xmitpriv.bm_pending; */
+
+
+	psta = rtw_alloc_stainfo(pstapriv, bcast_addr);
+
+	if (psta == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+#ifdef CONFIG_BEAMFORMING
+	psta->txbf_gid = 63;
+	psta->txbf_paid = 0;
+#endif
+	ptxservq = &(psta->sta_xmitpriv.be_q);
+
+	/*
+		_enter_critical(&pstapending->lock, &irqL0);
+
+		if (list_empty(&ptxservq->tx_pending))
+			list_add_tail(&ptxservq->tx_pending, get_list_head(pstapending));
+
+		_exit_critical(&pstapending->lock, &irqL0);
+	*/
+
+exit:
+	return _SUCCESS;
+
+}
+
+
+struct sta_info *rtw_get_bcmc_stainfo(_adapter *padapter)
+{
+	struct sta_info	*psta;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	psta = rtw_get_stainfo(pstapriv, bc_addr);
+	return psta;
+
+}
+
+#if CONFIG_RTW_MACADDR_ACL
+const char *const _acl_mode_str[] = {
+	"DISABLED",
+	"ACCEPT_UNLESS_LISTED",
+	"DENY_UNLESS_LISTED",
+};
+
+u8 rtw_access_ctrl(_adapter *adapter, u8 *mac_addr)
+{
+	u8 res = true;
+	unsigned long irqL;
+	_list *list, *head;
+	struct rtw_wlan_acl_node *acl_node;
+	u8 match = false;
+	struct sta_priv *stapriv = &adapter->stapriv;
+	struct wlan_acl_pool *acl = &stapriv->acl_list;
+	_queue	*acl_node_q = &acl->acl_node_q;
+
+	_enter_critical_bh(&(acl_node_q->lock), &irqL);
+	head = get_list_head(acl_node_q);
+	list = get_next(head);
+	while (rtw_end_of_queue_search(head, list) == false) {
+		acl_node = LIST_CONTAINOR(list, struct rtw_wlan_acl_node, list);
+		list = get_next(list);
+
+		if (!memcmp(acl_node->addr, mac_addr, ETH_ALEN)) {
+			if (acl_node->valid == true) {
+				match = true;
+				break;
+			}
+		}
+	}
+	_exit_critical_bh(&(acl_node_q->lock), &irqL);
+
+	if (acl->mode == RTW_ACL_MODE_ACCEPT_UNLESS_LISTED)
+		res = (match == true) ?  false : true;
+	else if (acl->mode == RTW_ACL_MODE_DENY_UNLESS_LISTED)
+		res = (match == true) ?  true : false;
+	else
+		res = true;
+
+	return res;
+}
+
+void dump_macaddr_acl(void *sel, _adapter *adapter)
+{
+	struct sta_priv *stapriv = &adapter->stapriv;
+	struct wlan_acl_pool *acl = &stapriv->acl_list;
+	int i;
+
+	RTW_PRINT_SEL(sel, "mode:%s(%d)\n", acl_mode_str(acl->mode), acl->mode);
+	RTW_PRINT_SEL(sel, "num:%d/%d\n", acl->num, NUM_ACL);
+	for (i = 0; i < NUM_ACL; i++) {
+		if (acl->aclnode[i].valid == false)
+			continue;
+		RTW_PRINT_SEL(sel, MAC_FMT"\n", MAC_ARG(acl->aclnode[i].addr));
+	}
+}
+#endif /* CONFIG_RTW_MACADDR_ACL */
+
+bool rtw_is_pre_link_sta(struct sta_priv *stapriv, u8 *addr)
+{
+#if CONFIG_RTW_PRE_LINK_STA
+	struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl;
+	struct sta_info *sta = NULL;
+	u8 exist = false;
+	int i;
+	unsigned long irqL;
+
+	_enter_critical_bh(&(pre_link_sta_ctl->lock), &irqL);
+	for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++) {
+		if (pre_link_sta_ctl->node[i].valid == true
+			&& !memcmp(pre_link_sta_ctl->node[i].addr, addr, ETH_ALEN) == true
+		) {
+			exist = true;
+			break;
+		}
+	}
+	_exit_critical_bh(&(pre_link_sta_ctl->lock), &irqL);
+
+	return exist;
+#else
+	return false;
+#endif
+}
+
+#if CONFIG_RTW_PRE_LINK_STA
+struct sta_info *rtw_pre_link_sta_add(struct sta_priv *stapriv, u8 *hwaddr)
+{
+	struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl;
+	struct pre_link_sta_node_t *node = NULL;
+	struct sta_info *sta = NULL;
+	u8 exist = false;
+	int i;
+	unsigned long irqL;
+
+	if (rtw_check_invalid_mac_address(hwaddr, false) == true)
+		goto exit;
+
+	_enter_critical_bh(&(pre_link_sta_ctl->lock), &irqL);
+	for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++) {
+		if (pre_link_sta_ctl->node[i].valid == true
+			&& !memcmp(pre_link_sta_ctl->node[i].addr, hwaddr, ETH_ALEN) == true
+		) {
+			node = &pre_link_sta_ctl->node[i];
+			exist = true;
+			break;
+		}
+
+		if (node == NULL && pre_link_sta_ctl->node[i].valid == false)
+			node = &pre_link_sta_ctl->node[i];
+	}
+
+	if (exist == false && node) {
+		memcpy(node->addr, hwaddr, ETH_ALEN);
+		node->valid = true;
+		pre_link_sta_ctl->num++;
+	}
+	_exit_critical_bh(&(pre_link_sta_ctl->lock), &irqL);
+
+	if (node == NULL)
+		goto exit;
+
+	sta = rtw_get_stainfo(stapriv, hwaddr);
+	if (sta)
+		goto odm_hook;
+
+	sta = rtw_alloc_stainfo(stapriv, hwaddr);
+	if (!sta)
+		goto exit;
+
+	sta->state = WIFI_FW_PRE_LINK;
+
+odm_hook:
+	rtw_hal_set_odm_var(stapriv->padapter, HAL_ODM_STA_INFO, sta, true);
+
+exit:
+	return sta;
+}
+
+void rtw_pre_link_sta_del(struct sta_priv *stapriv, u8 *hwaddr)
+{
+	struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl;
+	struct pre_link_sta_node_t *node = NULL;
+	struct sta_info *sta = NULL;
+	u8 exist = false;
+	int i;
+	unsigned long irqL;
+
+	if (rtw_check_invalid_mac_address(hwaddr, false) == true)
+		goto exit;
+
+	_enter_critical_bh(&(pre_link_sta_ctl->lock), &irqL);
+	for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++) {
+		if (pre_link_sta_ctl->node[i].valid == true
+			&& !memcmp(pre_link_sta_ctl->node[i].addr, hwaddr, ETH_ALEN) == true
+		) {
+			node = &pre_link_sta_ctl->node[i];
+			exist = true;
+			break;
+		}
+	}
+
+	if (exist == true && node) {
+		node->valid = false;
+		pre_link_sta_ctl->num--;
+	}
+	_exit_critical_bh(&(pre_link_sta_ctl->lock), &irqL);
+
+	if (exist == false)
+		goto exit;
+
+	sta = rtw_get_stainfo(stapriv, hwaddr);
+	if (!sta)
+		goto exit;
+
+	if (sta->state == WIFI_FW_PRE_LINK)
+		rtw_free_stainfo(stapriv->padapter, sta);
+
+exit:
+	return;
+}
+
+void rtw_pre_link_sta_ctl_reset(struct sta_priv *stapriv)
+{
+	struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl;
+	struct pre_link_sta_node_t *node = NULL;
+	struct sta_info *sta = NULL;
+	int i, j = 0;
+	unsigned long irqL;
+
+	u8 addrs[RTW_PRE_LINK_STA_NUM][ETH_ALEN];
+
+	memset(addrs, 0, RTW_PRE_LINK_STA_NUM * ETH_ALEN);
+
+	_enter_critical_bh(&(pre_link_sta_ctl->lock), &irqL);
+	for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++) {
+		if (pre_link_sta_ctl->node[i].valid == false)
+			continue;
+		memcpy(&(addrs[j][0]), pre_link_sta_ctl->node[i].addr, ETH_ALEN);
+		pre_link_sta_ctl->node[i].valid = false;
+		pre_link_sta_ctl->num--;
+		j++;
+	}
+	_exit_critical_bh(&(pre_link_sta_ctl->lock), &irqL);
+
+	for (i = 0; i < j; i++) {
+		sta = rtw_get_stainfo(stapriv, &(addrs[i][0]));
+		if (!sta)
+			continue;
+
+		if (sta->state == WIFI_FW_PRE_LINK)
+			rtw_free_stainfo(stapriv->padapter, sta);
+	}
+}
+
+void rtw_pre_link_sta_ctl_init(struct sta_priv *stapriv)
+{
+	struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl;
+	int i;
+
+	spin_lock_init(&pre_link_sta_ctl->lock);
+	pre_link_sta_ctl->num = 0;
+	for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++)
+		pre_link_sta_ctl->node[i].valid = false;
+}
+
+void rtw_pre_link_sta_ctl_deinit(struct sta_priv *stapriv)
+{
+	struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl;
+	int i;
+
+	rtw_pre_link_sta_ctl_reset(stapriv);
+}
+
+void dump_pre_link_sta_ctl(void *sel, struct sta_priv *stapriv)
+{
+	struct pre_link_sta_ctl_t *pre_link_sta_ctl = &stapriv->pre_link_sta_ctl;
+	int i;
+
+	RTW_PRINT_SEL(sel, "num:%d/%d\n", pre_link_sta_ctl->num, RTW_PRE_LINK_STA_NUM);
+
+	for (i = 0; i < RTW_PRE_LINK_STA_NUM; i++) {
+		if (pre_link_sta_ctl->node[i].valid == false)
+			continue;
+		RTW_PRINT_SEL(sel, MAC_FMT"\n", MAC_ARG(pre_link_sta_ctl->node[i].addr));
+	}
+}
+#endif /* CONFIG_RTW_PRE_LINK_STA */
+
diff --git a/drivers/staging/rtl8188eu/core/rtw_tdls.c b/drivers/staging/rtl8188eu/core/rtw_tdls.c
new file mode 100644
index 000000000000..1725d5fb9bef
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_tdls.c
@@ -0,0 +1,3092 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_TDLS_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#ifdef CONFIG_TDLS
+#define ONE_SEC 	1000 /* 1000 ms */
+
+extern unsigned char MCS_rate_2R[16];
+extern unsigned char MCS_rate_1R[16];
+extern void process_wmmps_data(_adapter *padapter, union recv_frame *precv_frame);
+
+void rtw_reset_tdls_info(_adapter *padapter)
+{
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+
+	ptdlsinfo->ap_prohibited = false;
+
+	/* For TDLS channel switch, currently we only allow it to work in wifi logo test mode */
+	if (padapter->registrypriv.wifi_spec == 1)
+		ptdlsinfo->ch_switch_prohibited = false;
+	else
+		ptdlsinfo->ch_switch_prohibited = true;
+
+	ptdlsinfo->link_established = false;
+	ptdlsinfo->sta_cnt = 0;
+	ptdlsinfo->sta_maximum = false;
+
+#ifdef CONFIG_TDLS_CH_SW
+	ptdlsinfo->chsw_info.ch_sw_state = TDLS_STATE_NONE;
+	ATOMIC_SET(&ptdlsinfo->chsw_info.chsw_on, false);
+	ptdlsinfo->chsw_info.off_ch_num = 0;
+	ptdlsinfo->chsw_info.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	ptdlsinfo->chsw_info.cur_time = 0;
+	ptdlsinfo->chsw_info.delay_switch_back = false;
+	ptdlsinfo->chsw_info.dump_stack = false;
+#endif
+
+	ptdlsinfo->ch_sensing = 0;
+	ptdlsinfo->watchdog_count = 0;
+	ptdlsinfo->dev_discovered = false;
+
+#ifdef CONFIG_WFD
+	ptdlsinfo->wfd_info = &padapter->wfd_info;
+#endif
+}
+
+int rtw_init_tdls_info(_adapter *padapter)
+{
+	int	res = _SUCCESS;
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+
+	rtw_reset_tdls_info(padapter);
+
+	ptdlsinfo->tdls_enable = true;
+#ifdef CONFIG_TDLS_DRIVER_SETUP
+	ptdlsinfo->driver_setup = true;
+#else
+	ptdlsinfo->driver_setup = false;
+#endif /* CONFIG_TDLS_DRIVER_SETUP */
+
+	spin_lock_init(&ptdlsinfo->cmd_lock);
+	spin_lock_init(&ptdlsinfo->hdl_lock);
+
+	return res;
+
+}
+
+void rtw_free_tdls_info(struct tdls_info *ptdlsinfo)
+{
+	memset(ptdlsinfo, 0, sizeof(struct tdls_info));
+}
+
+int check_ap_tdls_prohibited(u8 *pframe, u8 pkt_len)
+{
+	u8 tdls_prohibited_bit = 0x40; /* bit(38); TDLS_prohibited */
+
+	if (pkt_len < 5)
+		return false;
+
+	pframe += 4;
+	if ((*pframe) & tdls_prohibited_bit)
+		return true;
+
+	return false;
+}
+
+int check_ap_tdls_ch_switching_prohibited(u8 *pframe, u8 pkt_len)
+{
+	u8 tdls_ch_swithcing_prohibited_bit = 0x80; /* bit(39); TDLS_channel_switching prohibited */
+
+	if (pkt_len < 5)
+		return false;
+
+	pframe += 4;
+	if ((*pframe) & tdls_ch_swithcing_prohibited_bit)
+		return true;
+
+	return false;
+}
+
+u8 rtw_tdls_is_setup_allowed(_adapter *padapter)
+{
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+
+	if (ptdlsinfo->ap_prohibited == true)
+		return false;
+
+	return true;
+}
+
+#ifdef CONFIG_TDLS_CH_SW
+u8 rtw_tdls_is_chsw_allowed(_adapter *padapter)
+{
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+
+	if (ptdlsinfo->ch_switch_prohibited == true)
+		return false;
+
+	if (padapter->registrypriv.wifi_spec == 0)
+		return false;
+
+	return true;
+}
+#endif
+
+int _issue_nulldata_to_TDLS_peer_STA(_adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char					*pframe;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	unsigned short				*fctrl, *qc;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	pattrib->hdrlen += 2;
+	pattrib->qos_en = true;
+	pattrib->eosp = 1;
+	pattrib->ack_policy = 0;
+	pattrib->mdata = 0;
+	pattrib->retry_ctrl = false;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	if (power_mode)
+		SetPwrMgt(fctrl);
+
+	qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
+
+	SetPriority(qc, 7);	/* Set priority to VO */
+
+	SetEOSP(qc, pattrib->eosp);
+
+	SetAckpolicy(qc, pattrib->ack_policy);
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_QOS_DATA_NULL);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack)
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+
+}
+
+/*
+ *wait_ms == 0 means that there is no need to wait ack through C2H_CCX_TX_RPT
+ *wait_ms > 0 means you want to wait ack through C2H_CCX_TX_RPT, and the value of wait_ms means the interval between each TX
+ *try_cnt means the maximal TX count to try
+ */
+int issue_nulldata_to_TDLS_peer_STA(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	u32 start = jiffies;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	do {
+		ret = _issue_nulldata_to_TDLS_peer_STA(padapter, da, power_mode, wait_ms > 0 ? true : false);
+
+		i++;
+
+		if (RTW_CANNOT_RUN(padapter))
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			rtw_msleep_os(wait_ms);
+
+	} while ((i < try_cnt) && (ret == _FAIL || wait_ms == 0));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+#ifndef DBG_XMIT_ACK
+		goto exit;
+#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			RTW_INFO(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+		else
+			RTW_INFO(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+	}
+exit:
+	return ret;
+}
+
+void free_tdls_sta(_adapter *padapter, struct sta_info *ptdls_sta)
+{
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	unsigned long irqL;
+
+	/* free peer sta_info */
+	_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
+	if (ptdlsinfo->sta_cnt != 0)
+		ptdlsinfo->sta_cnt--;
+	_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL);
+	/* -2: AP + BC/MC sta, -4: default key */
+	if (ptdlsinfo->sta_cnt < MAX_ALLOWED_TDLS_STA_NUM) {
+		ptdlsinfo->sta_maximum = false;
+		memset(&ptdlsinfo->ss_record, 0x00, sizeof(struct tdls_ss_record));
+	}
+
+	/* clear cam */
+	rtw_clearstakey_cmd(padapter, ptdls_sta, true);
+
+	if (ptdlsinfo->sta_cnt == 0) {
+		rtw_tdls_cmd(padapter, NULL, TDLS_RS_RCR);
+		ptdlsinfo->link_established = false;
+	} else
+		RTW_INFO("Remain tdls sta:%02x\n", ptdlsinfo->sta_cnt);
+
+	rtw_free_stainfo(padapter,  ptdls_sta);
+
+}
+
+
+/* TDLS encryption(if needed) will always be CCMP */
+void rtw_tdls_set_key(_adapter *padapter, struct sta_info *ptdls_sta)
+{
+	ptdls_sta->dot118021XPrivacy = _AES_;
+	rtw_setstakey_cmd(padapter, ptdls_sta, TDLS_KEY, true);
+}
+
+void rtw_tdls_process_ht_cap(_adapter *padapter, struct sta_info *ptdls_sta, u8 *data, u8 Length)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+	u8	max_AMPDU_len, min_MPDU_spacing;
+	u8	cur_ldpc_cap = 0, cur_stbc_cap = 0, cur_beamform_cap = 0;
+
+	/* Save HT capabilities in the sta object */
+	memset(&ptdls_sta->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));
+	if (data && Length >= sizeof(struct rtw_ieee80211_ht_cap)) {
+		ptdls_sta->flags |= WLAN_STA_HT;
+		ptdls_sta->flags |= WLAN_STA_WME;
+
+		memcpy(&ptdls_sta->htpriv.ht_cap, data, sizeof(struct rtw_ieee80211_ht_cap));
+	} else
+		ptdls_sta->flags &= ~WLAN_STA_HT;
+
+	if (ptdls_sta->flags & WLAN_STA_HT) {
+		if (padapter->registrypriv.ht_enable == true) {
+			ptdls_sta->htpriv.ht_option = true;
+			ptdls_sta->qos_option = true;
+		} else {
+			ptdls_sta->htpriv.ht_option = false;
+			ptdls_sta->qos_option = false;
+		}
+	}
+
+	/* HT related cap */
+	if (ptdls_sta->htpriv.ht_option) {
+		/* Check if sta supports rx ampdu */
+		if (padapter->registrypriv.ampdu_enable == 1)
+			ptdls_sta->htpriv.ampdu_enable = true;
+
+		/* AMPDU Parameters field */
+		/* Get MIN of MAX AMPDU Length Exp */
+		if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (data[2] & 0x3))
+			max_AMPDU_len = (data[2] & 0x3);
+		else
+			max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3);
+		/* Get MAX of MIN MPDU Start Spacing */
+		if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (data[2] & 0x1c))
+			min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c);
+		else
+			min_MPDU_spacing = (data[2] & 0x1c);
+		ptdls_sta->htpriv.rx_ampdu_min_spacing = max_AMPDU_len | min_MPDU_spacing;
+
+		/* Check if sta support s Short GI 20M */
+		if (ptdls_sta->htpriv.ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SGI_20))
+			ptdls_sta->htpriv.sgi_20m = true;
+
+		/* Check if sta support s Short GI 40M */
+		if (ptdls_sta->htpriv.ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SGI_40))
+			ptdls_sta->htpriv.sgi_40m = true;
+
+		/* Bwmode would still followed AP's setting */
+		if (ptdls_sta->htpriv.ht_cap.cap_info & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH)) {
+			if (padapter->mlmeextpriv.cur_bwmode >= CHANNEL_WIDTH_40)
+				ptdls_sta->bw_mode = CHANNEL_WIDTH_40;
+			ptdls_sta->htpriv.ch_offset = padapter->mlmeextpriv.cur_ch_offset;
+		}
+
+		/* Config LDPC Coding Capability */
+		if (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX) && GET_HT_CAP_ELE_LDPC_CAP(data)) {
+			SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX));
+			RTW_INFO("Enable HT Tx LDPC!\n");
+		}
+		ptdls_sta->htpriv.ldpc_cap = cur_ldpc_cap;
+
+		/* Config STBC setting */
+		if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) && GET_HT_CAP_ELE_RX_STBC(data)) {
+			SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX));
+			RTW_INFO("Enable HT Tx STBC!\n");
+		}
+		ptdls_sta->htpriv.stbc_cap = cur_stbc_cap;
+
+#ifdef CONFIG_BEAMFORMING
+		/* Config Tx beamforming setting */
+		if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE) &&
+		    GET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(data))
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE);
+
+		if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE) &&
+		    GET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(data))
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE);
+		ptdls_sta->htpriv.beamform_cap = cur_beamform_cap;
+		if (cur_beamform_cap)
+			RTW_INFO("Client HT Beamforming Cap = 0x%02X\n", cur_beamform_cap);
+#endif /* CONFIG_BEAMFORMING */
+	}
+
+}
+
+u8 *rtw_tdls_set_ht_cap(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
+{
+	rtw_ht_use_default_setting(padapter);
+
+	rtw_restructure_ht_ie(padapter, NULL, pframe, 0, &(pattrib->pktlen), padapter->mlmeextpriv.cur_channel);
+
+	return pframe + pattrib->pktlen;
+}
+
+u8 *rtw_tdls_set_sup_ch(struct mlme_ext_priv *pmlmeext, u8 *pframe, struct pkt_attrib *pattrib)
+{
+	u8 sup_ch[30 * 2] = {0x00}, ch_set_idx = 0, sup_ch_idx = 2;
+
+	do {
+		if (pmlmeext->channel_set[ch_set_idx].ChannelNum <= 14) {
+			sup_ch[0] = 1;	/* First channel number */
+			sup_ch[1] = pmlmeext->channel_set[ch_set_idx].ChannelNum;	/* Number of channel */
+		} else {
+			sup_ch[sup_ch_idx++] = pmlmeext->channel_set[ch_set_idx].ChannelNum;
+			sup_ch[sup_ch_idx++] = 1;
+		}
+		ch_set_idx++;
+	} while (pmlmeext->channel_set[ch_set_idx].ChannelNum != 0 && ch_set_idx < MAX_CHANNEL_NUM);
+
+	return rtw_set_ie(pframe, _SUPPORTED_CH_IE_, sup_ch_idx, sup_ch, &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_rsnie(struct tdls_txmgmt *ptxmgmt, u8 *pframe, struct pkt_attrib *pattrib,  int init, struct sta_info *ptdls_sta)
+{
+	u8 *p = NULL;
+	int len = 0;
+
+	if (ptxmgmt->len > 0)
+		p = rtw_get_ie(ptxmgmt->buf, _RSN_IE_2_, &len, ptxmgmt->len);
+
+	if (p != NULL)
+		return rtw_set_ie(pframe, _RSN_IE_2_, len, p + 2, &(pattrib->pktlen));
+	else if (init == true)
+		return rtw_set_ie(pframe, _RSN_IE_2_, sizeof(TDLS_RSNIE), TDLS_RSNIE, &(pattrib->pktlen));
+	else
+		return rtw_set_ie(pframe, _RSN_IE_2_, sizeof(ptdls_sta->TDLS_RSNIE), ptdls_sta->TDLS_RSNIE, &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_ext_cap(u8 *pframe, struct pkt_attrib *pattrib)
+{
+	return rtw_set_ie(pframe, _EXT_CAP_IE_ , sizeof(TDLS_EXT_CAPIE), TDLS_EXT_CAPIE, &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_qos_cap(u8 *pframe, struct pkt_attrib *pattrib)
+{
+	return rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, sizeof(TDLS_WMMIE), TDLS_WMMIE,  &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_ftie(struct tdls_txmgmt *ptxmgmt, u8 *pframe, struct pkt_attrib *pattrib, u8 *ANonce, u8 *SNonce)
+{
+	struct wpa_tdls_ftie FTIE = {0};
+	u8 *p = NULL;
+	int len = 0;
+
+	if (ptxmgmt->len > 0)
+		p = rtw_get_ie(ptxmgmt->buf, _FTIE_, &len, ptxmgmt->len);
+
+	if (p != NULL)
+		return rtw_set_ie(pframe, _FTIE_, len, p + 2, &(pattrib->pktlen));
+	else {
+		if (ANonce != NULL)
+			memcpy(FTIE.Anonce, ANonce, WPA_NONCE_LEN);
+		if (SNonce != NULL)
+			memcpy(FTIE.Snonce, SNonce, WPA_NONCE_LEN);
+		return rtw_set_ie(pframe, _FTIE_ , 82, (u8 *)FTIE.mic_ctrl, &(pattrib->pktlen));
+	}
+}
+
+u8 *rtw_tdls_set_timeout_interval(struct tdls_txmgmt *ptxmgmt, u8 *pframe, struct pkt_attrib *pattrib, int init, struct sta_info *ptdls_sta)
+{
+	u8 timeout_itvl[5];	/* set timeout interval to maximum value */
+	u32 timeout_interval = TDLS_TPK_RESEND_COUNT;
+	u8 *p = NULL;
+	int len = 0;
+
+	if (ptxmgmt->len > 0)
+		p = rtw_get_ie(ptxmgmt->buf, _TIMEOUT_ITVL_IE_, &len, ptxmgmt->len);
+
+	if (p != NULL)
+		return rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, len, p + 2, &(pattrib->pktlen));
+	else {
+		/* Timeout interval */
+		timeout_itvl[0] = 0x02;
+		if (init == true)
+			memcpy(timeout_itvl + 1, &timeout_interval, 4);
+		else
+			memcpy(timeout_itvl + 1, (u8 *)(&ptdls_sta->TDLS_PeerKey_Lifetime), 4);
+
+		return rtw_set_ie(pframe, _TIMEOUT_ITVL_IE_, 5, timeout_itvl, &(pattrib->pktlen));
+	}
+}
+
+u8 *rtw_tdls_set_bss_coexist(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
+{
+	u8 iedata = 0;
+
+	if (padapter->mlmepriv.num_FortyMHzIntolerant > 0)
+		iedata |= BIT(2);	/* 20 MHz BSS Width Request */
+
+	/* Information Bit should be set by TDLS test plan 5.9 */
+	iedata |= BIT(0);
+	return rtw_set_ie(pframe, EID_BSSCoexistence, 1, &iedata, &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_payload_type(u8 *pframe, struct pkt_attrib *pattrib)
+{
+	u8 payload_type = 0x02;
+	return rtw_set_fixed_ie(pframe, 1, &(payload_type), &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_category(u8 *pframe, struct pkt_attrib *pattrib, u8 category)
+{
+	return rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_action(u8 *pframe, struct pkt_attrib *pattrib, struct tdls_txmgmt *ptxmgmt)
+{
+	return rtw_set_fixed_ie(pframe, 1, &(ptxmgmt->action_code), &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_status_code(u8 *pframe, struct pkt_attrib *pattrib, struct tdls_txmgmt *ptxmgmt)
+{
+	return rtw_set_fixed_ie(pframe, 2, (u8 *)&(ptxmgmt->status_code), &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_dialog(u8 *pframe, struct pkt_attrib *pattrib, struct tdls_txmgmt *ptxmgmt)
+{
+	u8 dialogtoken = 1;
+	if (ptxmgmt->dialog_token)
+		return rtw_set_fixed_ie(pframe, 1, &(ptxmgmt->dialog_token), &(pattrib->pktlen));
+	else
+		return rtw_set_fixed_ie(pframe, 1, &(dialogtoken), &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_reg_class(u8 *pframe, struct pkt_attrib *pattrib, struct sta_info *ptdls_sta)
+{
+	u8 reg_class = 22;
+	return rtw_set_fixed_ie(pframe, 1, &(reg_class), &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_second_channel_offset(u8 *pframe, struct pkt_attrib *pattrib, u8 ch_offset)
+{
+	return rtw_set_ie(pframe, EID_SecondaryChnlOffset , 1, &ch_offset, &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_capability(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
+	u8 cap_from_ie[2] = {0};
+
+	memcpy(cap_from_ie, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
+
+	return rtw_set_fixed_ie(pframe, 2, cap_from_ie, &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_supported_rate(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
+{
+	u8 bssrate[NDIS_802_11_LENGTH_RATES_EX];
+	int bssrate_len = 0;
+	u8 more_supportedrates = 0;
+
+	rtw_set_supported_rate(bssrate, (padapter->registrypriv.wireless_mode == WIRELESS_MODE_MAX) ? padapter->mlmeextpriv.cur_wireless_mode : padapter->registrypriv.wireless_mode);
+	bssrate_len = rtw_get_rateset_len(bssrate);
+
+	if (bssrate_len > 8) {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
+		more_supportedrates = 1;
+	} else
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
+
+	/* extended supported rates */
+	if (more_supportedrates == 1)
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
+
+	return pframe;
+}
+
+u8 *rtw_tdls_set_sup_reg_class(u8 *pframe, struct pkt_attrib *pattrib)
+{
+	return rtw_set_ie(pframe, _SRC_IE_ , sizeof(TDLS_SRC), TDLS_SRC, &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_linkid(u8 *pframe, struct pkt_attrib *pattrib, u8 init)
+{
+	u8 link_id_addr[18] = {0};
+	if (init == true) {
+		memcpy(link_id_addr, pattrib->ra, 6);
+		memcpy((link_id_addr + 6), pattrib->src, 6);
+		memcpy((link_id_addr + 12), pattrib->dst, 6);
+	} else {
+		memcpy(link_id_addr, pattrib->ra, 6);
+		memcpy((link_id_addr + 6), pattrib->dst, 6);
+		memcpy((link_id_addr + 12), pattrib->src, 6);
+	}
+	return rtw_set_ie(pframe, _LINK_ID_IE_, 18, link_id_addr, &(pattrib->pktlen));
+}
+
+#ifdef CONFIG_TDLS_CH_SW
+u8 *rtw_tdls_set_target_ch(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
+{
+	u8 target_ch = 1;
+	if (padapter->tdlsinfo.chsw_info.off_ch_num)
+		return rtw_set_fixed_ie(pframe, 1, &(padapter->tdlsinfo.chsw_info.off_ch_num), &(pattrib->pktlen));
+	else
+		return rtw_set_fixed_ie(pframe, 1, &(target_ch), &(pattrib->pktlen));
+}
+
+u8 *rtw_tdls_set_ch_sw(u8 *pframe, struct pkt_attrib *pattrib, struct sta_info *ptdls_sta)
+{
+	u8 ch_switch_timing[4] = {0};
+	u16 switch_time = (ptdls_sta->ch_switch_time >= TDLS_CH_SWITCH_TIME * 1000) ?
+			  ptdls_sta->ch_switch_time : TDLS_CH_SWITCH_TIME;
+	u16 switch_timeout = (ptdls_sta->ch_switch_timeout >= TDLS_CH_SWITCH_TIMEOUT * 1000) ?
+		     ptdls_sta->ch_switch_timeout : TDLS_CH_SWITCH_TIMEOUT;
+
+	memcpy(ch_switch_timing, &switch_time, 2);
+	memcpy(ch_switch_timing + 2, &switch_timeout, 2);
+
+	return rtw_set_ie(pframe, _CH_SWITCH_TIMING_,  4, ch_switch_timing, &(pattrib->pktlen));
+}
+
+void rtw_tdls_set_ch_sw_oper_control(_adapter *padapter, u8 enable)
+{
+	if (ATOMIC_READ(&padapter->tdlsinfo.chsw_info.chsw_on) != enable)
+		ATOMIC_SET(&padapter->tdlsinfo.chsw_info.chsw_on, enable);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_BCN_EARLY_C2H_RPT, &enable);
+	RTW_INFO("[TDLS] %s Bcn Early C2H Report\n", (enable == true) ? "Start" : "Stop");
+}
+
+void rtw_tdls_ch_sw_back_to_base_chnl(_adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv;
+	struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
+
+	pmlmepriv = &padapter->mlmepriv;
+
+	if ((ATOMIC_READ(&pchsw_info->chsw_on) == true) &&
+	    (padapter->mlmeextpriv.cur_channel != rtw_get_oper_ch(padapter)))
+		rtw_tdls_cmd(padapter, pchsw_info->addr, TDLS_CH_SW_TO_BASE_CHNL_UNSOLICITED);
+}
+
+static void rtw_tdls_chsw_oper_init(_adapter *padapter, u32 timeout_ms)
+{
+	struct submit_ctx	*chsw_sctx = &padapter->tdlsinfo.chsw_info.chsw_sctx;
+
+	rtw_sctx_init(chsw_sctx, timeout_ms);
+}
+
+static int rtw_tdls_chsw_oper_wait(_adapter *padapter)
+{
+	struct submit_ctx	*chsw_sctx = &padapter->tdlsinfo.chsw_info.chsw_sctx;
+
+	return rtw_sctx_wait(chsw_sctx, __func__);
+}
+
+void rtw_tdls_chsw_oper_done(_adapter *padapter)
+{
+	struct submit_ctx	*chsw_sctx = &padapter->tdlsinfo.chsw_info.chsw_sctx;
+
+	rtw_sctx_done(&chsw_sctx);
+}
+
+s32 rtw_tdls_do_ch_sw(_adapter *padapter, struct sta_info *ptdls_sta, u8 chnl_type, u8 channel, u8 channel_offset, u16 bwmode, u16 ch_switch_time)
+{
+	HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter);
+	u8 center_ch, chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	u32 ch_sw_time_start, ch_sw_time_spent, wait_time;
+	u8 take_care_iqk;
+	s32 ret = _FAIL;
+
+	ch_sw_time_start = rtw_systime_to_ms(jiffies);
+
+	rtw_tdls_chsw_oper_init(padapter, TDLS_CH_SWITCH_OPER_OFFLOAD_TIMEOUT);
+
+	/* set mac_id sleep before channel switch */
+	rtw_hal_macid_sleep(padapter, ptdls_sta->mac_id);
+
+	/* channel switch IOs offload to FW */
+	if (rtw_hal_ch_sw_oper_offload(padapter, channel, channel_offset, bwmode) == _SUCCESS) {
+		if (rtw_tdls_chsw_oper_wait(padapter) == _SUCCESS) {
+			/* set channel and bw related variables in driver */
+			_enter_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL);
+
+			rtw_set_oper_ch(padapter, channel);
+			rtw_set_oper_choffset(padapter, channel_offset);
+			rtw_set_oper_bw(padapter, bwmode);
+
+			center_ch = rtw_get_center_ch(channel, bwmode, channel_offset);
+			pHalData->current_channel = center_ch;
+			pHalData->CurrentCenterFrequencyIndex1 = center_ch;
+			pHalData->current_channel_bw = bwmode;
+			pHalData->nCur40MhzPrimeSC = channel_offset;
+
+			if (bwmode == CHANNEL_WIDTH_80) {
+				if (center_ch > channel)
+					chnl_offset80 = HAL_PRIME_CHNL_OFFSET_LOWER;
+				else if (center_ch < channel)
+					chnl_offset80 = HAL_PRIME_CHNL_OFFSET_UPPER;
+				else
+					chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			}
+			pHalData->nCur80MhzPrimeSC = chnl_offset80;
+
+			pHalData->CurrentCenterFrequencyIndex1 = center_ch;
+
+			_exit_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL);
+
+			rtw_hal_get_hwreg(padapter, HW_VAR_CH_SW_NEED_TO_TAKE_CARE_IQK_INFO, &take_care_iqk);
+			if (take_care_iqk == true)
+				rtw_hal_ch_sw_iqk_info_restore(padapter, CH_SW_USE_CASE_TDLS);
+
+			ch_sw_time_spent = rtw_systime_to_ms(jiffies) - ch_sw_time_start;
+
+			if (chnl_type == TDLS_CH_SW_OFF_CHNL) {
+				if ((u32)ch_switch_time / 1000 > ch_sw_time_spent)
+					wait_time = (u32)ch_switch_time / 1000 - ch_sw_time_spent;
+				else
+					wait_time = 0;
+
+				if (wait_time > 0)
+					rtw_msleep_os(wait_time);
+			}
+
+			ret = _SUCCESS;
+		} else
+			RTW_INFO("[TDLS] chsw oper wait fail !!\n");
+	}
+
+	/* set mac_id wakeup after channel switch */
+	rtw_hal_macid_wakeup(padapter, ptdls_sta->mac_id);
+
+	return ret;
+}
+#endif
+
+u8 *rtw_tdls_set_wmm_params(_adapter *padapter, u8 *pframe, struct pkt_attrib *pattrib)
+{
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 wmm_param_ele[24] = {0};
+
+	if (&pmlmeinfo->WMM_param) {
+		memcpy(wmm_param_ele, WMM_PARA_OUI, 6);
+		if (!memcmp(&pmlmeinfo->WMM_param, &wmm_param_ele[6], 18) == true)
+			/* Use default WMM Param */
+			memcpy(wmm_param_ele + 6, (u8 *)&TDLS_WMM_PARAM_IE, sizeof(TDLS_WMM_PARAM_IE));
+		else
+			memcpy(wmm_param_ele + 6, (u8 *)&pmlmeinfo->WMM_param, sizeof(pmlmeinfo->WMM_param));
+		return rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_,  24, wmm_param_ele, &(pattrib->pktlen));
+	} else
+		return pframe;
+}
+
+#ifdef CONFIG_WFD
+void rtw_tdls_process_wfd_ie(struct tdls_info *ptdlsinfo, u8 *ptr, u8 length)
+{
+	u8 *wfd_ie;
+	u32	wfd_ielen = 0;
+
+	if (!hal_chk_wl_func(tdls_info_to_adapter(ptdlsinfo), WL_FUNC_MIRACAST))
+		return;
+
+	/* Try to get the TCP port information when receiving the negotiation response. */
+
+	wfd_ie = rtw_get_wfd_ie(ptr, length, NULL, &wfd_ielen);
+	while (wfd_ie) {
+		u8 *attr_content;
+		u32	attr_contentlen = 0;
+		int	i;
+
+		RTW_INFO("[%s] WFD IE Found!!\n", __func__);
+		attr_content = rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, NULL, &attr_contentlen);
+		if (attr_content && attr_contentlen) {
+			ptdlsinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16(attr_content + 2);
+			RTW_INFO("[%s] Peer PORT NUM = %d\n", __func__, ptdlsinfo->wfd_info->peer_rtsp_ctrlport);
+		}
+
+		attr_content = rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_LOCAL_IP_ADDR, NULL, &attr_contentlen);
+		if (attr_content && attr_contentlen) {
+			memcpy(ptdlsinfo->wfd_info->peer_ip_address, (attr_content + 1), 4);
+			RTW_INFO("[%s] Peer IP = %02u.%02u.%02u.%02u\n", __func__,
+				ptdlsinfo->wfd_info->peer_ip_address[0], ptdlsinfo->wfd_info->peer_ip_address[1],
+				ptdlsinfo->wfd_info->peer_ip_address[2], ptdlsinfo->wfd_info->peer_ip_address[3]);
+		}
+
+		wfd_ie = rtw_get_wfd_ie(wfd_ie + wfd_ielen, (ptr + length) - (wfd_ie + wfd_ielen), NULL, &wfd_ielen);
+	}
+}
+
+int issue_tunneled_probe_req(_adapter *padapter)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	u8 baddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct tdls_txmgmt txmgmt;
+	int ret = _FAIL;
+
+	RTW_INFO("[%s]\n", __func__);
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	txmgmt.action_code = TUNNELED_PROBE_REQ;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+
+	pmgntframe->frame_tag = DATA_FRAMETAG;
+	pattrib->ether_type = 0x890d;
+
+	memcpy(pattrib->dst, baddr, ETH_ALEN);
+	memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+	update_tdls_attrib(padapter, pattrib);
+	pattrib->qsel = pattrib->priority;
+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit;
+	}
+	dump_mgntframe(padapter, pmgntframe);
+	ret = _SUCCESS;
+exit:
+
+	return ret;
+}
+
+int issue_tunneled_probe_rsp(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct tdls_txmgmt txmgmt;
+	int ret = _FAIL;
+
+	RTW_INFO("[%s]\n", __func__);
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	txmgmt.action_code = TUNNELED_PROBE_RSP;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+
+	pmgntframe->frame_tag = DATA_FRAMETAG;
+	pattrib->ether_type = 0x890d;
+
+	memcpy(pattrib->dst, precv_frame->u.hdr.attrib.src, ETH_ALEN);
+	memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+	update_tdls_attrib(padapter, pattrib);
+	pattrib->qsel = pattrib->priority;
+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit;
+	}
+	dump_mgntframe(padapter, pmgntframe);
+	ret = _SUCCESS;
+exit:
+
+	return ret;
+}
+#endif /* CONFIG_WFD */
+
+int issue_tdls_setup_req(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, int wait_ack)
+{
+	struct tdls_info	*ptdlsinfo = &padapter->tdlsinfo;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *ptdls_sta = NULL;
+	unsigned long irqL;
+	int ret = _FAIL;
+	/* Retry timer should be set at least 301 sec, using TPK_count counting 301 times. */
+	u32 timeout_interval = TDLS_TPK_RESEND_COUNT;
+
+	RTW_INFO("[TDLS] %s\n", __func__);
+
+	ptxmgmt->action_code = TDLS_SETUP_REQUEST;
+	if (rtw_tdls_is_setup_allowed(padapter) == false)
+		goto exit;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+	pmgntframe->frame_tag = DATA_FRAMETAG;
+	pattrib->ether_type = 0x890d;
+
+	memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
+	memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+	update_tdls_attrib(padapter, pattrib);
+
+	/* init peer sta_info */
+	ptdls_sta = rtw_get_stainfo(pstapriv, ptxmgmt->peer);
+	if (ptdls_sta == NULL) {
+		ptdls_sta = rtw_alloc_stainfo(pstapriv, ptxmgmt->peer);
+		if (ptdls_sta == NULL) {
+			RTW_INFO("[%s] rtw_alloc_stainfo fail\n", __func__);
+			rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+			rtw_free_xmitframe(pxmitpriv, pmgntframe);
+			goto exit;
+		}
+	}
+
+	if (!(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE))
+		ptdlsinfo->sta_cnt++;
+
+	if (ptdlsinfo->sta_cnt == MAX_ALLOWED_TDLS_STA_NUM)
+		ptdlsinfo->sta_maximum  = true;
+
+	ptdls_sta->tdls_sta_state |= TDLS_RESPONDER_STATE;
+
+	if (rtw_tdls_is_driver_setup(padapter) == true) {
+		ptdls_sta->TDLS_PeerKey_Lifetime = timeout_interval;
+		_set_timer(&ptdls_sta->handshake_timer, TDLS_HANDSHAKE_TIME);
+	}
+
+	pattrib->qsel = pattrib->priority;
+
+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit;
+	}
+
+	if (wait_ack)
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+
+	return ret;
+}
+
+int _issue_tdls_teardown(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, u8 wait_ack)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info	*ptdls_sta = NULL;
+	unsigned long irqL;
+	int ret = _FAIL;
+
+	RTW_INFO("[TDLS] %s\n", __func__);
+
+	ptxmgmt->action_code = TDLS_TEARDOWN;
+	ptdls_sta = rtw_get_stainfo(pstapriv, ptxmgmt->peer);
+	if (ptdls_sta == NULL) {
+		RTW_INFO("Np tdls_sta for tearing down\n");
+		goto exit;
+	}
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	rtw_mi_set_scan_deny(padapter, 550);
+	rtw_mi_scan_abort(padapter, true);
+
+	pattrib = &pmgntframe->attrib;
+
+	pmgntframe->frame_tag = DATA_FRAMETAG;
+	pattrib->ether_type = 0x890d;
+
+	memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
+	memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+	update_tdls_attrib(padapter, pattrib);
+	pattrib->qsel = pattrib->priority;
+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit;
+	}
+
+	if (rtw_tdls_is_driver_setup(padapter) == true)
+		if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)
+			if (pattrib->encrypt)
+				_cancel_timer_ex(&ptdls_sta->TPK_timer);
+
+	if (wait_ack)
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+	if (rtw_tdls_is_driver_setup(padapter))
+		rtw_tdls_cmd(padapter, ptxmgmt->peer, TDLS_TEARDOWN_STA_LOCALLY);
+
+exit:
+
+	return ret;
+}
+
+int issue_tdls_teardown(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, u8 wait_ack)
+{
+	int ret = _FAIL;
+
+	ret = _issue_tdls_teardown(padapter, ptxmgmt, wait_ack);
+	if ((ptxmgmt->status_code == _RSON_TDLS_TEAR_UN_RSN_) && (ret == _FAIL)) {
+		/* Change status code and send teardown again via AP */
+		ptxmgmt->status_code = _RSON_TDLS_TEAR_TOOFAR_;
+		ret = _issue_tdls_teardown(padapter, ptxmgmt, wait_ack);
+	}
+
+	return ret;
+}
+
+int issue_tdls_dis_req(_adapter *padapter, struct tdls_txmgmt *ptxmgmt)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	int ret = _FAIL;
+
+	RTW_INFO("[TDLS] %s\n", __func__);
+
+	ptxmgmt->action_code = TDLS_DISCOVERY_REQUEST;
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+	pmgntframe->frame_tag = DATA_FRAMETAG;
+	pattrib->ether_type = 0x890d;
+
+	memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
+	memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+	update_tdls_attrib(padapter, pattrib);
+	pattrib->qsel = pattrib->priority;
+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit;
+	}
+	dump_mgntframe(padapter, pmgntframe);
+	RTW_INFO("issue tdls dis req\n");
+
+	ret = _SUCCESS;
+exit:
+
+	return ret;
+}
+
+int issue_tdls_setup_rsp(_adapter *padapter, struct tdls_txmgmt *ptxmgmt)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	int ret = _FAIL;
+
+	RTW_INFO("[TDLS] %s\n", __func__);
+
+	ptxmgmt->action_code = TDLS_SETUP_RESPONSE;
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+	pmgntframe->frame_tag = DATA_FRAMETAG;
+	pattrib->ether_type = 0x890d;
+
+	memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
+	memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->ra, get_bssid(&(padapter->mlmepriv)), ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+	update_tdls_attrib(padapter, pattrib);
+	pattrib->qsel = pattrib->priority;
+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit;
+	}
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	ret = _SUCCESS;
+exit:
+
+	return ret;
+
+}
+
+int issue_tdls_setup_cfm(_adapter *padapter, struct tdls_txmgmt *ptxmgmt)
+{
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	int ret = _FAIL;
+
+	RTW_INFO("[TDLS] %s\n", __func__);
+
+	ptxmgmt->action_code = TDLS_SETUP_CONFIRM;
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+	pmgntframe->frame_tag = DATA_FRAMETAG;
+	pattrib->ether_type = 0x890d;
+
+	memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
+	memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->ra, get_bssid(&padapter->mlmepriv), ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+	update_tdls_attrib(padapter, pattrib);
+	pattrib->qsel = pattrib->priority;
+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit;
+	}
+
+	dump_mgntframe(padapter, pmgntframe);
+
+	ret = _SUCCESS;
+exit:
+
+	return ret;
+
+}
+
+/* TDLS Discovery Response frame is a management action frame */
+int issue_tdls_dis_rsp(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, u8 privacy)
+{
+	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);
+	int ret = _FAIL;
+
+	RTW_INFO("[TDLS] %s\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_ctl);
+	*(fctrl) = 0;
+
+	/* unicast probe request frame */
+	memcpy(pwlanhdr->addr1, ptxmgmt->peer, ETH_ALEN);
+	memcpy(pattrib->dst, pwlanhdr->addr1, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->src, pwlanhdr->addr2, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_bssid(&padapter->mlmepriv), ETH_ALEN);
+	memcpy(pattrib->ra, pwlanhdr->addr3, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	set_frame_sub_type(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	rtw_build_tdls_dis_rsp_ies(padapter, pmgntframe, pframe, ptxmgmt, privacy);
+
+	pattrib->nr_frags = 1;
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+	ret = _SUCCESS;
+
+exit:
+	return ret;
+}
+
+int issue_tdls_peer_traffic_rsp(_adapter *padapter, struct sta_info *ptdls_sta, struct tdls_txmgmt *ptxmgmt)
+{
+	struct xmit_frame	*pmgntframe;
+	struct pkt_attrib	*pattrib;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
+	int ret = _FAIL;
+
+	RTW_INFO("[TDLS] %s\n", __func__);
+
+	ptxmgmt->action_code = TDLS_PEER_TRAFFIC_RESPONSE;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+
+	pmgntframe->frame_tag = DATA_FRAMETAG;
+	pattrib->ether_type = 0x890d;
+
+	memcpy(pattrib->dst, ptdls_sta->hwaddr, ETH_ALEN);
+	memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+	update_tdls_attrib(padapter, pattrib);
+	pattrib->qsel = pattrib->priority;
+
+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit;
+	}
+
+	dump_mgntframe(padapter, pmgntframe);
+	ret = _SUCCESS;
+
+exit:
+
+	return ret;
+}
+
+int issue_tdls_peer_traffic_indication(_adapter *padapter, struct sta_info *ptdls_sta)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv			*pxmitpriv = &(padapter->xmitpriv);
+	struct tdls_txmgmt txmgmt;
+	int ret = _FAIL;
+
+	RTW_INFO("[TDLS] %s\n", __func__);
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	txmgmt.action_code = TDLS_PEER_TRAFFIC_INDICATION;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+
+	pmgntframe->frame_tag = DATA_FRAMETAG;
+	pattrib->ether_type = 0x890d;
+
+	memcpy(pattrib->dst, ptdls_sta->hwaddr, ETH_ALEN);
+	memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+	/* PTI frame's priority should be AC_VO */
+	pattrib->priority = 7;
+
+	update_tdls_attrib(padapter, pattrib);
+	pattrib->qsel = pattrib->priority;
+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit;
+	}
+
+	dump_mgntframe(padapter, pmgntframe);
+	ret = _SUCCESS;
+
+exit:
+
+	return ret;
+}
+
+#ifdef CONFIG_TDLS_CH_SW
+int issue_tdls_ch_switch_req(_adapter *padapter, struct sta_info *ptdls_sta)
+{
+	struct xmit_frame	*pmgntframe;
+	struct pkt_attrib	*pattrib;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
+	struct tdls_txmgmt txmgmt;
+	int ret = _FAIL;
+
+	RTW_INFO("[TDLS] %s\n", __func__);
+
+	if (rtw_tdls_is_chsw_allowed(padapter) == false) {
+		RTW_INFO("[TDLS] Ignore %s since channel switch is not allowed\n", __func__);
+		goto exit;
+	}
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	txmgmt.action_code = TDLS_CHANNEL_SWITCH_REQUEST;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+
+	pmgntframe->frame_tag = DATA_FRAMETAG;
+	pattrib->ether_type = 0x890d;
+
+	memcpy(pattrib->dst, ptdls_sta->hwaddr, ETH_ALEN);
+	memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+	update_tdls_attrib(padapter, pattrib);
+	pattrib->qsel = pattrib->priority;
+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, &txmgmt) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit;
+	}
+
+	dump_mgntframe(padapter, pmgntframe);
+	ret = _SUCCESS;
+exit:
+
+	return ret;
+}
+
+int issue_tdls_ch_switch_rsp(_adapter *padapter, struct tdls_txmgmt *ptxmgmt, int wait_ack)
+{
+	struct xmit_frame	*pmgntframe;
+	struct pkt_attrib	*pattrib;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
+	int ret = _FAIL;
+
+	RTW_INFO("[TDLS] %s\n", __func__);
+
+	if (rtw_tdls_is_chsw_allowed(padapter) == false) {
+		RTW_INFO("[TDLS] Ignore %s since channel switch is not allowed\n", __func__);
+		goto exit;
+	}
+
+	ptxmgmt->action_code = TDLS_CHANNEL_SWITCH_RESPONSE;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	pattrib = &pmgntframe->attrib;
+
+	pmgntframe->frame_tag = DATA_FRAMETAG;
+	pattrib->ether_type = 0x890d;
+
+	memcpy(pattrib->dst, ptxmgmt->peer, ETH_ALEN);
+	memcpy(pattrib->src, adapter_mac_addr(padapter), ETH_ALEN);
+	memcpy(pattrib->ra, ptxmgmt->peer, ETH_ALEN);
+	memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+	update_tdls_attrib(padapter, pattrib);
+	pattrib->qsel = pattrib->priority;
+	/*
+		_enter_critical_bh(&pxmitpriv->lock, &irqL);
+		if(xmitframe_enqueue_for_tdls_sleeping_sta(padapter, pmgntframe)==true){
+			_exit_critical_bh(&pxmitpriv->lock, &irqL);
+			return false;
+		}
+	*/
+	if (rtw_xmit_tdls_coalesce(padapter, pmgntframe, ptxmgmt) != _SUCCESS) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit;
+	}
+
+	if (wait_ack)
+		ret = dump_mgntframe_and_wait_ack_timeout(padapter, pmgntframe, 10);
+	else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+exit:
+
+	return ret;
+}
+#endif
+
+int On_TDLS_Dis_Rsp(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct sta_info *ptdls_sta = NULL, *psta = rtw_get_stainfo(&(padapter->stapriv), get_bssid(&(padapter->mlmepriv)));
+	struct recv_priv *precvpriv = &(padapter->recvpriv);
+	u8 *ptr = precv_frame->u.hdr.rx_data, *psa;
+	struct rx_pkt_attrib *pattrib = &(precv_frame->u.hdr.attrib);
+	struct tdls_info *ptdlsinfo = &(padapter->tdlsinfo);
+	u8 empty_addr[ETH_ALEN] = { 0x00 };
+	int undecorated_smoothed_pwdb;
+	struct tdls_txmgmt txmgmt;
+	int ret = _SUCCESS;
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	/* WFDTDLS: for sigma test, not to setup direct link automatically */
+	ptdlsinfo->dev_discovered = true;
+
+	psa = get_sa(ptr);
+	ptdls_sta = rtw_get_stainfo(&(padapter->stapriv), psa);
+	if (ptdls_sta != NULL)
+		ptdls_sta->sta_stats.rx_tdls_disc_rsp_pkts++;
+
+#ifdef CONFIG_TDLS_AUTOSETUP
+	if (ptdls_sta != NULL) {
+		/* Record the tdls sta with lowest signal strength */
+		if (ptdlsinfo->sta_maximum == true && ptdls_sta->alive_count >= 1) {
+			if (!memcmp(ptdlsinfo->ss_record.macaddr, empty_addr, ETH_ALEN)) {
+				memcpy(ptdlsinfo->ss_record.macaddr, psa, ETH_ALEN);
+				ptdlsinfo->ss_record.RxPWDBAll = pattrib->phy_info.RxPWDBAll;
+			} else {
+				if (ptdlsinfo->ss_record.RxPWDBAll < pattrib->phy_info.RxPWDBAll) {
+					memcpy(ptdlsinfo->ss_record.macaddr, psa, ETH_ALEN);
+					ptdlsinfo->ss_record.RxPWDBAll = pattrib->phy_info.RxPWDBAll;
+				}
+			}
+		}
+	} else {
+		if (ptdlsinfo->sta_maximum == true) {
+			if (!memcmp(ptdlsinfo->ss_record.macaddr, empty_addr, ETH_ALEN)) {
+				/* All traffics are busy, do not set up another direct link. */
+				ret = _FAIL;
+				goto exit;
+			} else {
+				if (pattrib->phy_info.RxPWDBAll > ptdlsinfo->ss_record.RxPWDBAll) {
+					memcpy(txmgmt.peer, ptdlsinfo->ss_record.macaddr, ETH_ALEN);
+					/* issue_tdls_teardown(padapter, ptdlsinfo->ss_record.macaddr, false); */
+				} else {
+					ret = _FAIL;
+					goto exit;
+				}
+			}
+		}
+
+		rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &undecorated_smoothed_pwdb);
+
+		if (pattrib->phy_info.RxPWDBAll + TDLS_SIGNAL_THRESH >= undecorated_smoothed_pwdb) {
+			RTW_INFO("pattrib->RxPWDBAll=%d, pdmpriv->undecorated_smoothed_pwdb=%d\n", pattrib->phy_info.RxPWDBAll, undecorated_smoothed_pwdb);
+			memcpy(txmgmt.peer, psa, ETH_ALEN);
+			issue_tdls_setup_req(padapter, &txmgmt, false);
+		}
+	}
+#endif /* CONFIG_TDLS_AUTOSETUP */
+
+exit:
+	return ret;
+
+}
+
+sint On_TDLS_Setup_Req(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	u8 *psa, *pmyid;
+	struct sta_info *ptdls_sta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	unsigned long irqL;
+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;
+	u8 *prsnie, *ppairwise_cipher;
+	u8 i, k;
+	u8 ccmp_included = 0, rsnie_included = 0;
+	u16 j, pairwise_count;
+	u8 SNonce[32];
+	u32 timeout_interval = TDLS_TPK_RESEND_COUNT;
+	sint parsing_length;	/* Frame body length, without icv_len */
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8 FIXED_IE = 5;
+	unsigned char		supportRate[16];
+	int				supportRateNum = 0;
+	struct tdls_txmgmt txmgmt;
+
+	if (rtw_tdls_is_setup_allowed(padapter) == false)
+		goto exit;
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	psa = get_sa(ptr);
+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);
+
+	pmyid = adapter_mac_addr(padapter);
+	ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN;
+	parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len
+			 - prx_pkt_attrib->hdrlen
+			 - prx_pkt_attrib->iv_len
+			 - prx_pkt_attrib->icv_len
+			 - LLC_HEADER_SIZE
+			 - ETH_TYPE_LEN
+			 - PAYLOAD_TYPE_LEN
+			 - FIXED_IE;
+
+	if (ptdls_sta == NULL)
+		ptdls_sta = rtw_alloc_stainfo(pstapriv, psa);
+	else {
+		if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) {
+			/* If the direct link is already set up */
+			/* Process as re-setup after tear down */
+			RTW_INFO("re-setup a direct link\n");
+		}
+		/* Already receiving TDLS setup request */
+		else if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE) {
+			RTW_INFO("receive duplicated TDLS setup request frame in handshaking\n");
+			goto exit;
+		}
+		/* When receiving and sending setup_req to the same link at the same time */
+		/* STA with higher MAC_addr would be initiator */
+		else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE) {
+			RTW_INFO("receive setup_req after sending setup_req\n");
+			for (i = 0; i < 6; i++) {
+				if (*(pmyid + i) == *(psa + i)) {
+				} else if (*(pmyid + i) > *(psa + i)) {
+					ptdls_sta->tdls_sta_state = TDLS_INITIATOR_STATE;
+					break;
+				} else if (*(pmyid + i) < *(psa + i))
+					goto exit;
+			}
+		}
+	}
+
+	if (ptdls_sta) {
+		txmgmt.dialog_token = *(ptr + 2);	/* Copy dialog token */
+		txmgmt.status_code = _STATS_SUCCESSFUL_;
+
+		/* Parsing information element */
+		for (j = FIXED_IE; j < parsing_length;) {
+
+			pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j);
+
+			switch (pIE->ElementID) {
+			case _SUPPORTEDRATES_IE_:
+				memcpy(supportRate, pIE->data, pIE->Length);
+				supportRateNum = pIE->Length;
+				break;
+			case _COUNTRY_IE_:
+				break;
+			case _EXT_SUPPORTEDRATES_IE_:
+				if (supportRateNum <= sizeof(supportRate)) {
+					memcpy(supportRate + supportRateNum, pIE->data, pIE->Length);
+					supportRateNum += pIE->Length;
+				}
+				break;
+			case _SUPPORTED_CH_IE_:
+				break;
+			case _RSN_IE_2_:
+				rsnie_included = 1;
+				if (prx_pkt_attrib->encrypt) {
+					prsnie = (u8 *)pIE;
+					/* Check CCMP pairwise_cipher presence. */
+					ppairwise_cipher = prsnie + 10;
+					memcpy(ptdls_sta->TDLS_RSNIE, pIE->data, pIE->Length);
+					pairwise_count = *(u16 *)(ppairwise_cipher - 2);
+					for (k = 0; k < pairwise_count; k++) {
+						if (!memcmp(ppairwise_cipher + 4 * k, RSN_CIPHER_SUITE_CCMP, 4) == true)
+							ccmp_included = 1;
+					}
+
+					if (ccmp_included == 0)
+						txmgmt.status_code = _STATS_INVALID_RSNIE_;
+				}
+				break;
+			case _EXT_CAP_IE_:
+				break;
+			case _VENDOR_SPECIFIC_IE_:
+				break;
+			case _FTIE_:
+				if (prx_pkt_attrib->encrypt)
+					memcpy(SNonce, (ptr + j + 52), 32);
+				break;
+			case _TIMEOUT_ITVL_IE_:
+				if (prx_pkt_attrib->encrypt)
+					timeout_interval = cpu_to_le32(*(u32 *)(ptr + j + 3));
+				break;
+			case _RIC_Descriptor_IE_:
+				break;
+			case _HT_CAPABILITY_IE_:
+				rtw_tdls_process_ht_cap(padapter, ptdls_sta, pIE->data, pIE->Length);
+				break;
+			case EID_BSSCoexistence:
+				break;
+			case _LINK_ID_IE_:
+				if (!memcmp(get_bssid(pmlmepriv), pIE->data, 6) == false)
+					txmgmt.status_code = _STATS_NOT_IN_SAME_BSS_;
+				break;
+			default:
+				break;
+			}
+
+			j += (pIE->Length + 2);
+
+		}
+
+		/* Check status code */
+		/* If responder STA has/hasn't security on AP, but request hasn't/has RSNIE, it should reject */
+		if (txmgmt.status_code == _STATS_SUCCESSFUL_) {
+			if (rsnie_included && prx_pkt_attrib->encrypt == 0)
+				txmgmt.status_code = _STATS_SEC_DISABLED_;
+			else if (rsnie_included == 0 && prx_pkt_attrib->encrypt)
+				txmgmt.status_code = _STATS_INVALID_PARAMETERS_;
+
+#ifdef CONFIG_WFD
+			/* WFD test plan version 0.18.2 test item 5.1.5 */
+			/* SoUT does not use TDLS if AP uses weak security */
+			if (padapter->wdinfo.wfd_tdls_enable && (rsnie_included && prx_pkt_attrib->encrypt != _AES_))
+				txmgmt.status_code = _STATS_SEC_DISABLED_;
+#endif /* CONFIG_WFD */
+		}
+
+		ptdls_sta->tdls_sta_state |= TDLS_INITIATOR_STATE;
+		if (prx_pkt_attrib->encrypt) {
+			memcpy(ptdls_sta->SNonce, SNonce, 32);
+
+			if (timeout_interval <= 300)
+				ptdls_sta->TDLS_PeerKey_Lifetime = TDLS_TPK_RESEND_COUNT;
+			else
+				ptdls_sta->TDLS_PeerKey_Lifetime = timeout_interval;
+		}
+
+		/* Update station supportRate */
+		ptdls_sta->bssratelen = supportRateNum;
+		memcpy(ptdls_sta->bssrateset, supportRate, supportRateNum);
+
+		if (!(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE))
+			ptdlsinfo->sta_cnt++;
+		/* -2: AP + BC/MC sta, -4: default key */
+		if (ptdlsinfo->sta_cnt == MAX_ALLOWED_TDLS_STA_NUM)
+			ptdlsinfo->sta_maximum = true;
+
+#ifdef CONFIG_WFD
+		rtw_tdls_process_wfd_ie(ptdlsinfo, ptr + FIXED_IE, parsing_length);
+#endif
+
+	} else
+		goto exit;
+
+	memcpy(txmgmt.peer, prx_pkt_attrib->src, ETH_ALEN);
+
+	if (rtw_tdls_is_driver_setup(padapter)) {
+		issue_tdls_setup_rsp(padapter, &txmgmt);
+
+		if (txmgmt.status_code == _STATS_SUCCESSFUL_)
+			_set_timer(&ptdls_sta->handshake_timer, TDLS_HANDSHAKE_TIME);
+		else
+			free_tdls_sta(padapter, ptdls_sta);
+	}
+
+exit:
+
+	return _SUCCESS;
+}
+
+int On_TDLS_Setup_Rsp(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	struct sta_info *ptdls_sta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	unsigned long irqL;
+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;
+	u8 *psa;
+	u16 status_code = 0;
+	sint parsing_length;	/* Frame body length, without icv_len */
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8 FIXED_IE = 7;
+	u8 ANonce[32];
+	u8  *pftie = NULL, *ptimeout_ie = NULL, *plinkid_ie = NULL, *prsnie = NULL, *pftie_mic = NULL, *ppairwise_cipher = NULL;
+	u16 pairwise_count, j, k;
+	u8 verify_ccmp = 0;
+	unsigned char		supportRate[16];
+	int				supportRateNum = 0;
+	struct tdls_txmgmt txmgmt;
+	int ret = _SUCCESS;
+	u32 timeout_interval = TDLS_TPK_RESEND_COUNT;
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	psa = get_sa(ptr);
+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);
+
+	if (ptdls_sta == NULL) {
+		RTW_INFO("[%s] Direct Link Peer = "MAC_FMT" not found\n", __func__, MAC_ARG(psa));
+		ret = _FAIL;
+		goto exit;
+	}
+
+	ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN;
+	parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len
+			 - prx_pkt_attrib->hdrlen
+			 - prx_pkt_attrib->iv_len
+			 - prx_pkt_attrib->icv_len
+			 - LLC_HEADER_SIZE
+			 - ETH_TYPE_LEN
+			 - PAYLOAD_TYPE_LEN
+			 - FIXED_IE;
+
+	memcpy(&status_code, ptr + 2, 2);
+
+	if (status_code != 0) {
+		RTW_INFO("[TDLS] %s status_code = %d, free_tdls_sta\n", __func__, status_code);
+		free_tdls_sta(padapter, ptdls_sta);
+		ret = _FAIL;
+		goto exit;
+	}
+
+	status_code = 0;
+
+	/* parsing information element */
+	for (j = FIXED_IE; j < parsing_length;) {
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j);
+
+		switch (pIE->ElementID) {
+		case _SUPPORTEDRATES_IE_:
+			memcpy(supportRate, pIE->data, pIE->Length);
+			supportRateNum = pIE->Length;
+			break;
+		case _COUNTRY_IE_:
+			break;
+		case _EXT_SUPPORTEDRATES_IE_:
+			if (supportRateNum <= sizeof(supportRate)) {
+				memcpy(supportRate + supportRateNum, pIE->data, pIE->Length);
+				supportRateNum += pIE->Length;
+			}
+			break;
+		case _SUPPORTED_CH_IE_:
+			break;
+		case _RSN_IE_2_:
+			prsnie = (u8 *)pIE;
+			/* Check CCMP pairwise_cipher presence. */
+			ppairwise_cipher = prsnie + 10;
+			memcpy(&pairwise_count, (u16 *)(ppairwise_cipher - 2), 2);
+			for (k = 0; k < pairwise_count; k++) {
+				if (!memcmp(ppairwise_cipher + 4 * k, RSN_CIPHER_SUITE_CCMP, 4) == true)
+					verify_ccmp = 1;
+			}
+		case _EXT_CAP_IE_:
+			break;
+		case _VENDOR_SPECIFIC_IE_:
+			if (!memcmp((u8 *)pIE + 2, WMM_INFO_OUI, 6) == true) {
+				/* WMM Info ID and OUI */
+				if ((pregistrypriv->wmm_enable == true) || (padapter->mlmepriv.htpriv.ht_option == true))
+					ptdls_sta->qos_option = true;
+			}
+			break;
+		case _FTIE_:
+			pftie = (u8 *)pIE;
+			memcpy(ANonce, (ptr + j + 20), 32);
+			break;
+		case _TIMEOUT_ITVL_IE_:
+			ptimeout_ie = (u8 *)pIE;
+			timeout_interval = cpu_to_le32(*(u32 *)(ptimeout_ie + 3));
+			break;
+		case _RIC_Descriptor_IE_:
+			break;
+		case _HT_CAPABILITY_IE_:
+			rtw_tdls_process_ht_cap(padapter, ptdls_sta, pIE->data, pIE->Length);
+			break;
+		case EID_BSSCoexistence:
+			break;
+		case _LINK_ID_IE_:
+			plinkid_ie = (u8 *)pIE;
+			break;
+		default:
+			break;
+		}
+
+		j += (pIE->Length + 2);
+
+	}
+
+	ptdls_sta->bssratelen = supportRateNum;
+	memcpy(ptdls_sta->bssrateset, supportRate, supportRateNum);
+	memcpy(ptdls_sta->ANonce, ANonce, 32);
+
+#ifdef CONFIG_WFD
+	rtw_tdls_process_wfd_ie(ptdlsinfo, ptr + FIXED_IE, parsing_length);
+#endif
+
+	if (status_code != _STATS_SUCCESSFUL_)
+		txmgmt.status_code = status_code;
+	else {
+		if (prx_pkt_attrib->encrypt) {
+			if (verify_ccmp == 1) {
+				txmgmt.status_code = _STATS_SUCCESSFUL_;
+				if (rtw_tdls_is_driver_setup(padapter) == true) {
+					wpa_tdls_generate_tpk(padapter, ptdls_sta);
+					if (tdls_verify_mic(ptdls_sta->tpk.kck, 2, plinkid_ie, prsnie, ptimeout_ie, pftie) == _FAIL) {
+						RTW_INFO("[TDLS] %s tdls_verify_mic fail, free_tdls_sta\n", __func__);
+						free_tdls_sta(padapter, ptdls_sta);
+						ret = _FAIL;
+						goto exit;
+					}
+					ptdls_sta->TDLS_PeerKey_Lifetime = timeout_interval;
+				}
+			} else
+				txmgmt.status_code = _STATS_INVALID_RSNIE_;
+
+		} else
+			txmgmt.status_code = _STATS_SUCCESSFUL_;
+	}
+
+	if (rtw_tdls_is_driver_setup(padapter) == true) {
+		memcpy(txmgmt.peer, prx_pkt_attrib->src, ETH_ALEN);
+		issue_tdls_setup_cfm(padapter, &txmgmt);
+
+		if (txmgmt.status_code == _STATS_SUCCESSFUL_) {
+			ptdlsinfo->link_established = true;
+
+			if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE) {
+				ptdls_sta->tdls_sta_state |= TDLS_LINKED_STATE;
+				ptdls_sta->state |= _FW_LINKED;
+				_cancel_timer_ex(&ptdls_sta->handshake_timer);
+			}
+
+			if (prx_pkt_attrib->encrypt)
+				rtw_tdls_set_key(padapter, ptdls_sta);
+
+			rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_ESTABLISHED);
+
+		}
+	}
+
+exit:
+	if (rtw_tdls_is_driver_setup(padapter) == true)
+		return ret;
+	else
+		return _SUCCESS;
+
+}
+
+int On_TDLS_Setup_Cfm(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	struct sta_info *ptdls_sta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	unsigned long irqL;
+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;
+	u8 *psa;
+	u16 status_code = 0;
+	sint parsing_length;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8 FIXED_IE = 5;
+	u8  *pftie = NULL, *ptimeout_ie = NULL, *plinkid_ie = NULL, *prsnie = NULL, *pftie_mic = NULL, *ppairwise_cipher = NULL;
+	u16 j, pairwise_count;
+	int ret = _SUCCESS;
+
+	psa = get_sa(ptr);
+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);
+
+	if (ptdls_sta == NULL) {
+		RTW_INFO("[%s] Direct Link Peer = "MAC_FMT" not found\n", __func__, MAC_ARG(psa));
+		ret = _FAIL;
+		goto exit;
+	}
+
+	ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN;
+	parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len
+			 - prx_pkt_attrib->hdrlen
+			 - prx_pkt_attrib->iv_len
+			 - prx_pkt_attrib->icv_len
+			 - LLC_HEADER_SIZE
+			 - ETH_TYPE_LEN
+			 - PAYLOAD_TYPE_LEN
+			 - FIXED_IE;
+
+	memcpy(&status_code, ptr + 2, 2);
+
+	if (status_code != 0) {
+		RTW_INFO("[%s] status_code = %d\n, free_tdls_sta", __func__, status_code);
+		free_tdls_sta(padapter, ptdls_sta);
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* Parsing information element */
+	for (j = FIXED_IE; j < parsing_length;) {
+
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j);
+
+		switch (pIE->ElementID) {
+		case _RSN_IE_2_:
+			prsnie = (u8 *)pIE;
+			break;
+		case _VENDOR_SPECIFIC_IE_:
+			if (!memcmp((u8 *)pIE + 2, WMM_PARA_OUI, 6) == true) {
+				/* WMM Parameter ID and OUI */
+				ptdls_sta->qos_option = true;
+			}
+			break;
+		case _FTIE_:
+			pftie = (u8 *)pIE;
+			break;
+		case _TIMEOUT_ITVL_IE_:
+			ptimeout_ie = (u8 *)pIE;
+			break;
+		case _HT_EXTRA_INFO_IE_:
+			break;
+		case _LINK_ID_IE_:
+			plinkid_ie = (u8 *)pIE;
+			break;
+		default:
+			break;
+		}
+
+		j += (pIE->Length + 2);
+
+	}
+
+	if (prx_pkt_attrib->encrypt) {
+		/* Verify mic in FTIE MIC field */
+		if (rtw_tdls_is_driver_setup(padapter) &&
+		    (tdls_verify_mic(ptdls_sta->tpk.kck, 3, plinkid_ie, prsnie, ptimeout_ie, pftie) == _FAIL)) {
+			free_tdls_sta(padapter, ptdls_sta);
+			ret = _FAIL;
+			goto exit;
+		}
+	}
+
+	if (rtw_tdls_is_driver_setup(padapter)) {
+		ptdlsinfo->link_established = true;
+
+		if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE) {
+			ptdls_sta->tdls_sta_state |= TDLS_LINKED_STATE;
+			ptdls_sta->state |= _FW_LINKED;
+			_cancel_timer_ex(&ptdls_sta->handshake_timer);
+		}
+
+		if (prx_pkt_attrib->encrypt) {
+			rtw_tdls_set_key(padapter, ptdls_sta);
+
+			/* Start  TPK timer */
+			ptdls_sta->TPK_count = 0;
+			_set_timer(&ptdls_sta->TPK_timer, ONE_SEC);
+		}
+
+		rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_ESTABLISHED);
+	}
+
+exit:
+	return ret;
+
+}
+
+int On_TDLS_Dis_Req(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta_ap;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	sint parsing_length;	/* Frame body length, without icv_len */
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8 FIXED_IE = 3, *dst;
+	u16 j;
+	struct tdls_txmgmt txmgmt;
+	int ret = _SUCCESS;
+
+	if (rtw_tdls_is_driver_setup(padapter) == false)
+		goto exit;
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN;
+	txmgmt.dialog_token = *(ptr + 2);
+	memcpy(&txmgmt.peer, precv_frame->u.hdr.attrib.src, ETH_ALEN);
+	txmgmt.action_code = TDLS_DISCOVERY_RESPONSE;
+	parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len
+			 - prx_pkt_attrib->hdrlen
+			 - prx_pkt_attrib->iv_len
+			 - prx_pkt_attrib->icv_len
+			 - LLC_HEADER_SIZE
+			 - ETH_TYPE_LEN
+			 - PAYLOAD_TYPE_LEN
+			 - FIXED_IE;
+
+	/* Parsing information element */
+	for (j = FIXED_IE; j < parsing_length;) {
+
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j);
+
+		switch (pIE->ElementID) {
+		case _LINK_ID_IE_:
+			psta_ap = rtw_get_stainfo(pstapriv, pIE->data);
+			if (psta_ap == NULL)
+				goto exit;
+			dst = pIE->data + 12;
+			if (MacAddr_isBcst(dst) == false && (!memcmp(adapter_mac_addr(padapter), dst, 6) == false))
+				goto exit;
+			break;
+		default:
+			break;
+		}
+
+		j += (pIE->Length + 2);
+
+	}
+
+	issue_tdls_dis_rsp(padapter, &txmgmt, prx_pkt_attrib->privacy);
+
+exit:
+	return ret;
+
+}
+
+int On_TDLS_Teardown(_adapter *padapter, union recv_frame *precv_frame)
+{
+	u8 *psa;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	struct sta_info *ptdls_sta = NULL;
+	unsigned long irqL;
+	u8 reason;
+
+	reason = *(ptr + prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN + 2);
+	RTW_INFO("[TDLS] %s Reason code(%d)\n", __func__, reason);
+
+	psa = get_sa(ptr);
+
+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);
+	if (ptdls_sta != NULL) {
+		if (rtw_tdls_is_driver_setup(padapter))
+			rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_TEARDOWN_STA_LOCALLY);
+	}
+
+	return _SUCCESS;
+
+}
+
+int On_TDLS_Peer_Traffic_Indication(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct rx_pkt_attrib	*pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_info *ptdls_sta = rtw_get_stainfo(&padapter->stapriv, pattrib->src);
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct tdls_txmgmt txmgmt;
+
+	ptr += pattrib->hdrlen + pattrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN;
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+
+	if (ptdls_sta != NULL) {
+		txmgmt.dialog_token = *(ptr + 2);
+		issue_tdls_peer_traffic_rsp(padapter, ptdls_sta, &txmgmt);
+		/* issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta->hwaddr, 0, 0, 0); */
+	} else {
+		RTW_INFO("from unknown sta:"MAC_FMT"\n", MAC_ARG(pattrib->src));
+		return _FAIL;
+	}
+
+	return _SUCCESS;
+}
+
+/* We process buffered data for 1. U-APSD, 2. ch. switch, 3. U-APSD + ch. switch here */
+int On_TDLS_Peer_Traffic_Rsp(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct rx_pkt_attrib	*pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->src);
+	u8 wmmps_ac = 0;
+	/* u8 state=TDLS_check_ch_state(ptdls_sta->tdls_sta_state); */
+	int i;
+
+	ptdls_sta->sta_stats.rx_data_pkts++;
+
+	ptdls_sta->tdls_sta_state &= ~(TDLS_WAIT_PTR_STATE);
+
+	/* Check 4-AC queue bit */
+	if (ptdls_sta->uapsd_vo || ptdls_sta->uapsd_vi || ptdls_sta->uapsd_be || ptdls_sta->uapsd_bk)
+		wmmps_ac = 1;
+
+	/* If it's a direct link and have buffered frame */
+	if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) {
+		if (wmmps_ac) {
+			unsigned long irqL;
+			_list	*xmitframe_plist, *xmitframe_phead;
+			struct xmit_frame *pxmitframe = NULL;
+
+			_enter_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);
+
+			xmitframe_phead = get_list_head(&ptdls_sta->sleep_q);
+			xmitframe_plist = get_next(xmitframe_phead);
+
+			/* transmit buffered frames */
+			while (rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist) == false) {
+				pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+				xmitframe_plist = get_next(xmitframe_plist);
+				list_del_init(&pxmitframe->list);
+
+				ptdls_sta->sleepq_len--;
+				ptdls_sta->sleepq_ac_len--;
+				if (ptdls_sta->sleepq_len > 0) {
+					pxmitframe->attrib.mdata = 1;
+					pxmitframe->attrib.eosp = 0;
+				} else {
+					pxmitframe->attrib.mdata = 0;
+					pxmitframe->attrib.eosp = 1;
+				}
+				pxmitframe->attrib.triggered = 1;
+
+				rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+			}
+
+			if (ptdls_sta->sleepq_len == 0)
+				RTW_INFO("no buffered packets for tdls to xmit\n");
+			else {
+				RTW_INFO("error!psta->sleepq_len=%d\n", ptdls_sta->sleepq_len);
+				ptdls_sta->sleepq_len = 0;
+			}
+
+			_exit_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);
+
+		}
+
+	}
+
+	return _SUCCESS;
+}
+
+#ifdef CONFIG_TDLS_CH_SW
+sint On_TDLS_Ch_Switch_Req(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
+	struct sta_info *ptdls_sta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;
+	u8 *psa;
+	sint parsing_length;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8 FIXED_IE = 4;
+	u16 j;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct tdls_txmgmt txmgmt;
+	u8 zaddr[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	u16 switch_time = TDLS_CH_SWITCH_TIME * 1000, switch_timeout = TDLS_CH_SWITCH_TIMEOUT * 1000;
+	u8 take_care_iqk;
+
+	if (rtw_tdls_is_chsw_allowed(padapter) == false) {
+		RTW_INFO("[TDLS] Ignore %s since channel switch is not allowed\n", __func__);
+		return _FAIL;
+	}
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	psa = get_sa(ptr);
+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);
+
+	if (ptdls_sta == NULL) {
+		RTW_INFO("[%s] Direct Link Peer = "MAC_FMT" not found\n", __func__, MAC_ARG(psa));
+		return _FAIL;
+	}
+
+	ptdls_sta->ch_switch_time = switch_time;
+	ptdls_sta->ch_switch_timeout = switch_timeout;
+
+	ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN;
+	parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len
+			 - prx_pkt_attrib->hdrlen
+			 - prx_pkt_attrib->iv_len
+			 - prx_pkt_attrib->icv_len
+			 - LLC_HEADER_SIZE
+			 - ETH_TYPE_LEN
+			 - PAYLOAD_TYPE_LEN
+			 - FIXED_IE;
+
+	pchsw_info->off_ch_num = *(ptr + 2);
+
+	if ((*(ptr + 2) == 2) && (hal_is_band_support(padapter, BAND_ON_5G)))
+		pchsw_info->off_ch_num = 44;
+
+	if (pchsw_info->off_ch_num != pmlmeext->cur_channel)
+		pchsw_info->delay_switch_back = false;
+
+	/* Parsing information element */
+	for (j = FIXED_IE; j < parsing_length;) {
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j);
+
+		switch (pIE->ElementID) {
+		case EID_SecondaryChnlOffset:
+			switch (*(pIE->data)) {
+			case EXTCHNL_OFFSET_UPPER:
+				pchsw_info->ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+				break;
+
+			case EXTCHNL_OFFSET_LOWER:
+				pchsw_info->ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+				break;
+
+			default:
+				pchsw_info->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+				break;
+			}
+			break;
+		case _LINK_ID_IE_:
+			break;
+		case _CH_SWITCH_TIMING_:
+			ptdls_sta->ch_switch_time = (RTW_GET_LE16(pIE->data) >= TDLS_CH_SWITCH_TIME * 1000) ?
+				RTW_GET_LE16(pIE->data) : TDLS_CH_SWITCH_TIME * 1000;
+			ptdls_sta->ch_switch_timeout = (RTW_GET_LE16(pIE->data + 2) >= TDLS_CH_SWITCH_TIMEOUT * 1000) ?
+				RTW_GET_LE16(pIE->data + 2) : TDLS_CH_SWITCH_TIMEOUT * 1000;
+			RTW_INFO("[TDLS] %s ch_switch_time:%d, ch_switch_timeout:%d\n"
+				, __func__, RTW_GET_LE16(pIE->data), RTW_GET_LE16(pIE->data + 2));
+		default:
+			break;
+		}
+
+		j += (pIE->Length + 2);
+	}
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_CH_SW_NEED_TO_TAKE_CARE_IQK_INFO, &take_care_iqk);
+	if (take_care_iqk == true) {
+		u8 central_chnl;
+		u8 bw_mode;
+
+		bw_mode = (pchsw_info->ch_offset) ? CHANNEL_WIDTH_40 : CHANNEL_WIDTH_20;
+		central_chnl = rtw_get_center_ch(pchsw_info->off_ch_num, bw_mode, pchsw_info->ch_offset);
+		if (rtw_hal_ch_sw_iqk_info_search(padapter, central_chnl, bw_mode) < 0) {
+			if (!(pchsw_info->ch_sw_state & TDLS_CH_SWITCH_PREPARE_STATE))
+				rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_PREPARE);
+
+			return _FAIL;
+		}
+	}
+
+	/* cancel ch sw monitor timer for responder */
+	if (!(pchsw_info->ch_sw_state & TDLS_CH_SW_INITIATOR_STATE))
+		_cancel_timer_ex(&ptdls_sta->ch_sw_monitor_timer);
+
+	/* Todo: check status */
+	txmgmt.status_code = 0;
+	memcpy(txmgmt.peer, psa, ETH_ALEN);
+
+	if (!memcmp(pchsw_info->addr, zaddr, ETH_ALEN) == true)
+		memcpy(pchsw_info->addr, ptdls_sta->hwaddr, ETH_ALEN);
+
+	if (ATOMIC_READ(&pchsw_info->chsw_on) == false)
+		rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_START);
+
+	rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_RESP);
+
+	return _SUCCESS;
+}
+
+sint On_TDLS_Ch_Switch_Rsp(_adapter *padapter, union recv_frame *precv_frame)
+{
+	struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
+	struct sta_info *ptdls_sta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib	*prx_pkt_attrib = &precv_frame->u.hdr.attrib;
+	u8 *psa;
+	sint parsing_length;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8 FIXED_IE = 4;
+	u16 status_code, j, switch_time, switch_timeout;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	int ret = _SUCCESS;
+
+	if (rtw_tdls_is_chsw_allowed(padapter) == false) {
+		RTW_INFO("[TDLS] Ignore %s since channel switch is not allowed\n", __func__);
+		return _SUCCESS;
+	}
+
+	psa = get_sa(ptr);
+	ptdls_sta = rtw_get_stainfo(pstapriv, psa);
+
+	if (ptdls_sta == NULL) {
+		RTW_INFO("[%s] Direct Link Peer = "MAC_FMT" not found\n", __func__, MAC_ARG(psa));
+		return _FAIL;
+	}
+
+	/* If we receive Unsolicited TDLS Channel Switch Response when channel switch is running, */
+	/* we will go back to base channel and terminate this channel switch procedure */
+	if (ATOMIC_READ(&pchsw_info->chsw_on) == true) {
+		if (pmlmeext->cur_channel != rtw_get_oper_ch(padapter)) {
+			RTW_INFO("[TDLS] Rx unsolicited channel switch response\n");
+			rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_TO_BASE_CHNL);
+			goto exit;
+		}
+	}
+
+	ptr += prx_pkt_attrib->hdrlen + prx_pkt_attrib->iv_len + LLC_HEADER_SIZE + ETH_TYPE_LEN + PAYLOAD_TYPE_LEN;
+	parsing_length = ((union recv_frame *)precv_frame)->u.hdr.len
+			 - prx_pkt_attrib->hdrlen
+			 - prx_pkt_attrib->iv_len
+			 - prx_pkt_attrib->icv_len
+			 - LLC_HEADER_SIZE
+			 - ETH_TYPE_LEN
+			 - PAYLOAD_TYPE_LEN
+			 - FIXED_IE;
+
+	memcpy(&status_code, ptr + 2, 2);
+
+	if (status_code != 0) {
+		RTW_INFO("[TDLS] %s status_code:%d\n", __func__, status_code);
+		pchsw_info->ch_sw_state &= ~(TDLS_CH_SW_INITIATOR_STATE);
+		rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_END);
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* Parsing information element */
+	for (j = FIXED_IE; j < parsing_length;) {
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(ptr + j);
+
+		switch (pIE->ElementID) {
+		case _LINK_ID_IE_:
+			break;
+		case _CH_SWITCH_TIMING_:
+			memcpy(&switch_time, pIE->data, 2);
+			if (switch_time > ptdls_sta->ch_switch_time)
+				memcpy(&ptdls_sta->ch_switch_time, &switch_time, 2);
+
+			memcpy(&switch_timeout, pIE->data + 2, 2);
+			if (switch_timeout > ptdls_sta->ch_switch_timeout)
+				memcpy(&ptdls_sta->ch_switch_timeout, &switch_timeout, 2);
+			break;
+		default:
+			break;
+		}
+
+		j += (pIE->Length + 2);
+	}
+
+	if ((pmlmeext->cur_channel == rtw_get_oper_ch(padapter)) &&
+	    (pchsw_info->ch_sw_state & TDLS_WAIT_CH_RSP_STATE)) {
+		if (ATOMIC_READ(&pchsw_info->chsw_on) == true)
+			rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_TO_OFF_CHNL);
+	}
+
+exit:
+	return ret;
+}
+#endif /* CONFIG_TDLS_CH_SW */
+
+#ifdef CONFIG_WFD
+void wfd_ie_tdls(_adapter *padapter, u8 *pframe, u32 *pktlen)
+{
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct wifi_display_info	*pwfd_info = padapter->tdlsinfo.wfd_info;
+	u8 wfdie[MAX_WFD_IE_LEN] = { 0x00 };
+	u32 wfdielen = 0;
+
+	if (!hal_chk_wl_func(padapter, WL_FUNC_MIRACAST))
+		return;
+
+	/* WFD OUI */
+	wfdielen = 0;
+	wfdie[wfdielen++] = 0x50;
+	wfdie[wfdielen++] = 0x6F;
+	wfdie[wfdielen++] = 0x9A;
+	wfdie[wfdielen++] = 0x0A;	/* WFA WFD v1.0 */
+
+	/*
+	 *	Commented by Albert 20110825
+	 *	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes
+	 *	1. WFD Device Information
+	 *	2. Associated BSSID ( Optional )
+	 *	3. Local IP Adress ( Optional )
+	 */
+
+	/* WFD Device Information ATTR */
+	/* Type: */
+	wfdie[wfdielen++] = WFD_ATTR_DEVICE_INFO;
+
+	/* Length: */
+	/* Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/* Value1: */
+	/* WFD device information */
+	/* available for WFD session + Preferred TDLS + WSD ( WFD Service Discovery ) */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL
+		     | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_WSD);
+	wfdielen += 2;
+
+	/* Value2: */
+	/* Session Management Control Port */
+	/* Default TCP port for RTSP messages is 554 */
+	RTW_PUT_BE16(wfdie + wfdielen, pwfd_info->tdls_rtsp_ctrlport);
+	wfdielen += 2;
+
+	/* Value3: */
+	/* WFD Device Maximum Throughput */
+	/* 300Mbps is the maximum throughput */
+	RTW_PUT_BE16(wfdie + wfdielen, 300);
+	wfdielen += 2;
+
+	/* Associated BSSID ATTR */
+	/* Type: */
+	wfdie[wfdielen++] = WFD_ATTR_ASSOC_BSSID;
+
+	/* Length: */
+	/* Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0006);
+	wfdielen += 2;
+
+	/* Value: */
+	/* Associated BSSID */
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[0], ETH_ALEN);
+	else
+		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
+
+	/* Local IP Address ATTR */
+	wfdie[wfdielen++] = WFD_ATTR_LOCAL_IP_ADDR;
+
+	/* Length: */
+	/* Note: In the WFD specification, the size of length field is 2. */
+	RTW_PUT_BE16(wfdie + wfdielen, 0x0005);
+	wfdielen += 2;
+
+	/* Version: */
+	/* 0x01: Version1;IPv4 */
+	wfdie[wfdielen++] = 0x01;
+
+	/* IPv4 Address */
+	memcpy(wfdie + wfdielen, pwfd_info->ip_address, 4);
+	wfdielen += 4;
+
+	pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, pktlen);
+
+}
+#endif /* CONFIG_WFD */
+
+void rtw_build_tdls_setup_req_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
+{
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_info *ptdls_sta = rtw_get_stainfo((&padapter->stapriv) , pattrib->dst);
+
+	int i = 0 ;
+	u32 time;
+	u8 *pframe_head;
+
+	/* SNonce */
+	if (pattrib->encrypt) {
+		for (i = 0; i < 8; i++) {
+			time = jiffies;
+			memcpy(&ptdls_sta->SNonce[4 * i], (u8 *)&time, 4);
+		}
+	}
+
+	pframe_head = pframe;	/* For rtw_tdls_set_ht_cap() */
+
+	pframe = rtw_tdls_set_payload_type(pframe, pattrib);
+	pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
+	pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
+
+	pframe = rtw_tdls_set_capability(padapter, pframe, pattrib);
+	pframe = rtw_tdls_set_supported_rate(padapter, pframe, pattrib);
+	pframe = rtw_tdls_set_sup_ch(&(padapter->mlmeextpriv), pframe, pattrib);
+	pframe = rtw_tdls_set_sup_reg_class(pframe, pattrib);
+
+	if (pattrib->encrypt)
+		pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib,  true, ptdls_sta);
+
+	pframe = rtw_tdls_set_ext_cap(pframe, pattrib);
+
+	if (pattrib->encrypt) {
+		pframe = rtw_tdls_set_ftie(ptxmgmt
+					   , pframe
+					   , pattrib
+					   , NULL
+					   , ptdls_sta->SNonce);
+
+		pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib, true, ptdls_sta);
+	}
+
+	/* Sup_reg_classes(optional) */
+	if (pregistrypriv->ht_enable == true)
+		pframe = rtw_tdls_set_ht_cap(padapter, pframe_head, pattrib);
+
+	pframe = rtw_tdls_set_bss_coexist(padapter, pframe, pattrib);
+
+	pframe = rtw_tdls_set_linkid(pframe, pattrib, true);
+
+	if ((pregistrypriv->wmm_enable == true) || (padapter->mlmepriv.htpriv.ht_option == true))
+		pframe = rtw_tdls_set_qos_cap(pframe, pattrib);
+
+#ifdef CONFIG_WFD
+	if (padapter->wdinfo.wfd_tdls_enable == 1)
+		wfd_ie_tdls(padapter, pframe, &(pattrib->pktlen));
+#endif
+
+}
+
+void rtw_build_tdls_setup_rsp_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
+{
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_info *ptdls_sta;
+	u8 k; /* for random ANonce */
+	u8  *pftie = NULL, *ptimeout_ie = NULL, *plinkid_ie = NULL, *prsnie = NULL, *pftie_mic = NULL;
+	u32 time;
+	u8 *pframe_head;
+
+	ptdls_sta = rtw_get_stainfo(&(padapter->stapriv) , pattrib->dst);
+
+	if (ptdls_sta == NULL)
+		RTW_INFO("[%s] %d ptdls_sta is NULL\n", __func__, __LINE__);
+
+	if (pattrib->encrypt && ptdls_sta != NULL) {
+		for (k = 0; k < 8; k++) {
+			time = jiffies;
+			memcpy(&ptdls_sta->ANonce[4 * k], (u8 *)&time, 4);
+		}
+	}
+
+	pframe_head = pframe;
+
+	pframe = rtw_tdls_set_payload_type(pframe, pattrib);
+	pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
+	pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt);
+
+	if (ptxmgmt->status_code != 0) {
+		RTW_INFO("[%s] status_code:%04x\n", __func__, ptxmgmt->status_code);
+		return;
+	}
+
+	pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_capability(padapter, pframe, pattrib);
+	pframe = rtw_tdls_set_supported_rate(padapter, pframe, pattrib);
+	pframe = rtw_tdls_set_sup_ch(&(padapter->mlmeextpriv), pframe, pattrib);
+	pframe = rtw_tdls_set_sup_reg_class(pframe, pattrib);
+
+	if (pattrib->encrypt) {
+		prsnie = pframe;
+		pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib,  false, ptdls_sta);
+	}
+
+	pframe = rtw_tdls_set_ext_cap(pframe, pattrib);
+
+	if (pattrib->encrypt) {
+		if (rtw_tdls_is_driver_setup(padapter) == true)
+			wpa_tdls_generate_tpk(padapter, ptdls_sta);
+
+		pftie = pframe;
+		pftie_mic = pframe + 4;
+		pframe = rtw_tdls_set_ftie(ptxmgmt
+					   , pframe
+					   , pattrib
+					   , ptdls_sta->ANonce
+					   , ptdls_sta->SNonce);
+
+		ptimeout_ie = pframe;
+		pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib, false, ptdls_sta);
+	}
+
+	/* Sup_reg_classes(optional) */
+	if (pregistrypriv->ht_enable == true)
+		pframe = rtw_tdls_set_ht_cap(padapter, pframe_head, pattrib);
+
+	pframe = rtw_tdls_set_bss_coexist(padapter, pframe, pattrib);
+
+	plinkid_ie = pframe;
+	pframe = rtw_tdls_set_linkid(pframe, pattrib, false);
+
+	/* Fill FTIE mic */
+	if (pattrib->encrypt && rtw_tdls_is_driver_setup(padapter) == true)
+		wpa_tdls_ftie_mic(ptdls_sta->tpk.kck, 2, plinkid_ie, prsnie, ptimeout_ie, pftie, pftie_mic);
+
+	if ((pregistrypriv->wmm_enable == true) || (padapter->mlmepriv.htpriv.ht_option == true))
+		pframe = rtw_tdls_set_qos_cap(pframe, pattrib);
+
+#ifdef CONFIG_WFD
+	if (padapter->wdinfo.wfd_tdls_enable)
+		wfd_ie_tdls(padapter, pframe, &(pattrib->pktlen));
+#endif
+
+}
+
+void rtw_build_tdls_setup_cfm_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
+{
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_info *ptdls_sta = rtw_get_stainfo((&padapter->stapriv) , pattrib->dst);
+
+	unsigned int ie_len;
+	unsigned char *p;
+	u8 wmm_param_ele[24] = {0};
+	u8  *pftie = NULL, *ptimeout_ie = NULL, *plinkid_ie = NULL, *prsnie = NULL, *pftie_mic = NULL;
+
+	pframe = rtw_tdls_set_payload_type(pframe, pattrib);
+	pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
+	pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
+
+	if (ptxmgmt->status_code != 0)
+		return;
+
+	if (pattrib->encrypt) {
+		prsnie = pframe;
+		pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib, true, ptdls_sta);
+	}
+
+	if (pattrib->encrypt) {
+		pftie = pframe;
+		pftie_mic = pframe + 4;
+		pframe = rtw_tdls_set_ftie(ptxmgmt
+					   , pframe
+					   , pattrib
+					   , ptdls_sta->ANonce
+					   , ptdls_sta->SNonce);
+
+		ptimeout_ie = pframe;
+		pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib, true, ptdls_sta);
+
+		if (rtw_tdls_is_driver_setup(padapter) == true) {
+			/* Start TPK timer */
+			ptdls_sta->TPK_count = 0;
+			_set_timer(&ptdls_sta->TPK_timer, ONE_SEC);
+		}
+	}
+
+	/* HT operation; todo */
+
+	plinkid_ie = pframe;
+	pframe = rtw_tdls_set_linkid(pframe, pattrib, true);
+
+	if (pattrib->encrypt && (rtw_tdls_is_driver_setup(padapter) == true))
+		wpa_tdls_ftie_mic(ptdls_sta->tpk.kck, 3, plinkid_ie, prsnie, ptimeout_ie, pftie, pftie_mic);
+
+	if (ptdls_sta->qos_option == true)
+		pframe = rtw_tdls_set_wmm_params(padapter, pframe, pattrib);
+}
+
+void rtw_build_tdls_teardown_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
+{
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_info *ptdls_sta = rtw_get_stainfo(&(padapter->stapriv) , pattrib->dst);
+	u8  *pftie = NULL, *pftie_mic = NULL, *plinkid_ie = NULL;
+
+	pframe = rtw_tdls_set_payload_type(pframe, pattrib);
+	pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
+	pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt);
+
+	if (pattrib->encrypt) {
+		pftie = pframe;
+		pftie_mic = pframe + 4;
+		pframe = rtw_tdls_set_ftie(ptxmgmt
+					   , pframe
+					   , pattrib
+					   , ptdls_sta->ANonce
+					   , ptdls_sta->SNonce);
+	}
+
+	plinkid_ie = pframe;
+	if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE)
+		pframe = rtw_tdls_set_linkid(pframe, pattrib, false);
+	else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE)
+		pframe = rtw_tdls_set_linkid(pframe, pattrib, true);
+
+	if (pattrib->encrypt && (rtw_tdls_is_driver_setup(padapter) == true))
+		wpa_tdls_teardown_ftie_mic(ptdls_sta->tpk.kck, plinkid_ie, ptxmgmt->status_code, 1, 4, pftie, pftie_mic);
+}
+
+void rtw_build_tdls_dis_req_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
+{
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+	pframe = rtw_tdls_set_payload_type(pframe, pattrib);
+	pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
+	pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_linkid(pframe, pattrib, true);
+
+}
+
+void rtw_build_tdls_dis_rsp_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt, u8 privacy)
+{
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	u8 *pframe_head, pktlen_index;
+
+	pktlen_index = pattrib->pktlen;
+	pframe_head = pframe;
+
+	pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_PUBLIC);
+	pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_capability(padapter, pframe, pattrib);
+
+	pframe = rtw_tdls_set_supported_rate(padapter, pframe, pattrib);
+
+	pframe = rtw_tdls_set_sup_ch(pmlmeext, pframe, pattrib);
+
+	if (privacy)
+		pframe = rtw_tdls_set_rsnie(ptxmgmt, pframe, pattrib, true, NULL);
+
+	pframe = rtw_tdls_set_ext_cap(pframe, pattrib);
+
+	if (privacy) {
+		pframe = rtw_tdls_set_ftie(ptxmgmt, pframe, pattrib, NULL, NULL);
+		pframe = rtw_tdls_set_timeout_interval(ptxmgmt, pframe, pattrib,  true, NULL);
+	}
+
+	if (pregistrypriv->ht_enable == true)
+		pframe = rtw_tdls_set_ht_cap(padapter, pframe_head - pktlen_index, pattrib);
+
+	pframe = rtw_tdls_set_bss_coexist(padapter, pframe, pattrib);
+	pframe = rtw_tdls_set_linkid(pframe, pattrib, false);
+}
+
+void rtw_build_tdls_peer_traffic_indication_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
+{
+
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	u8 AC_queue = 0;
+	struct sta_info *ptdls_sta = rtw_get_stainfo(&padapter->stapriv, pattrib->dst);
+
+	pframe = rtw_tdls_set_payload_type(pframe, pattrib);
+	pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
+	pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
+
+	if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE)
+		pframe = rtw_tdls_set_linkid(pframe, pattrib, false);
+	else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE)
+		pframe = rtw_tdls_set_linkid(pframe, pattrib, true);
+
+	/* PTI control */
+	/* PU buffer status */
+	if (ptdls_sta->uapsd_bk & BIT(1))
+		AC_queue = BIT(0);
+	if (ptdls_sta->uapsd_be & BIT(1))
+		AC_queue = BIT(1);
+	if (ptdls_sta->uapsd_vi & BIT(1))
+		AC_queue = BIT(2);
+	if (ptdls_sta->uapsd_vo & BIT(1))
+		AC_queue = BIT(3);
+	pframe = rtw_set_ie(pframe, _PTI_BUFFER_STATUS_, 1, &AC_queue, &(pattrib->pktlen));
+
+}
+
+void rtw_build_tdls_peer_traffic_rsp_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
+{
+
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_info *ptdls_sta = rtw_get_stainfo(&padapter->stapriv, pattrib->dst);
+
+	pframe = rtw_tdls_set_payload_type(pframe, pattrib);
+	pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
+	pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_dialog(pframe, pattrib, ptxmgmt);
+
+	if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE)
+		pframe = rtw_tdls_set_linkid(pframe, pattrib, false);
+	else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE)
+		pframe = rtw_tdls_set_linkid(pframe, pattrib, true);
+}
+
+#ifdef CONFIG_TDLS_CH_SW
+void rtw_build_tdls_ch_switch_req_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
+{
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	struct sta_info *ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst);
+	u16 switch_time = TDLS_CH_SWITCH_TIME * 1000, switch_timeout = TDLS_CH_SWITCH_TIMEOUT * 1000;
+
+	ptdls_sta->ch_switch_time = switch_time;
+	ptdls_sta->ch_switch_timeout = switch_timeout;
+
+	pframe = rtw_tdls_set_payload_type(pframe, pattrib);
+	pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
+	pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_target_ch(padapter, pframe, pattrib);
+	pframe = rtw_tdls_set_reg_class(pframe, pattrib, ptdls_sta);
+
+	if (ptdlsinfo->chsw_info.ch_offset != HAL_PRIME_CHNL_OFFSET_DONT_CARE) {
+		switch (ptdlsinfo->chsw_info.ch_offset) {
+		case HAL_PRIME_CHNL_OFFSET_LOWER:
+			pframe = rtw_tdls_set_second_channel_offset(pframe, pattrib, SCA);
+			break;
+		case HAL_PRIME_CHNL_OFFSET_UPPER:
+			pframe = rtw_tdls_set_second_channel_offset(pframe, pattrib, SCB);
+			break;
+		}
+	}
+
+	if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE)
+		pframe = rtw_tdls_set_linkid(pframe, pattrib, false);
+	else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE)
+		pframe = rtw_tdls_set_linkid(pframe, pattrib, true);
+
+	pframe = rtw_tdls_set_ch_sw(pframe, pattrib, ptdls_sta);
+
+}
+
+void rtw_build_tdls_ch_switch_rsp_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
+{
+
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	struct sta_info *ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst);
+
+	pframe = rtw_tdls_set_payload_type(pframe, pattrib);
+	pframe = rtw_tdls_set_category(pframe, pattrib, RTW_WLAN_CATEGORY_TDLS);
+	pframe = rtw_tdls_set_action(pframe, pattrib, ptxmgmt);
+	pframe = rtw_tdls_set_status_code(pframe, pattrib, ptxmgmt);
+
+	if (ptdls_sta->tdls_sta_state & TDLS_INITIATOR_STATE)
+		pframe = rtw_tdls_set_linkid(pframe, pattrib, false);
+	else if (ptdls_sta->tdls_sta_state & TDLS_RESPONDER_STATE)
+		pframe = rtw_tdls_set_linkid(pframe, pattrib, true);
+
+	pframe = rtw_tdls_set_ch_sw(pframe, pattrib, ptdls_sta);
+}
+#endif
+
+#ifdef CONFIG_WFD
+void rtw_build_tunneled_probe_req_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe)
+{
+	u8 i;
+	_adapter *iface = NULL;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct wifidirect_info *pwdinfo;
+
+	u8 category = RTW_WLAN_CATEGORY_P2P;
+	u8 WFA_OUI[3] = { 0x50, 0x6f, 0x9a};
+	u8 probe_req = 4;
+	u8 wfdielen = 0;
+
+	pframe = rtw_tdls_set_payload_type(pframe, pattrib);
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 3, WFA_OUI, &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(probe_req), &(pattrib->pktlen));
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		if ((iface) && rtw_is_adapter_up(iface)) {
+			pwdinfo = &iface->wdinfo;
+			if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
+				wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe);
+				pframe += wfdielen;
+				pattrib->pktlen += wfdielen;
+			}
+		}
+	}
+}
+
+void rtw_build_tunneled_probe_rsp_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe)
+{
+	u8 i;
+	_adapter *iface = NULL;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct wifidirect_info *pwdinfo;
+	u8 category = RTW_WLAN_CATEGORY_P2P;
+	u8 WFA_OUI[3] = { 0x50, 0x6f, 0x9a};
+	u8 probe_rsp = 5;
+	u8 wfdielen = 0;
+
+	pframe = rtw_tdls_set_payload_type(pframe, pattrib);
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 3, WFA_OUI, &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(probe_rsp), &(pattrib->pktlen));
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		if ((iface) && rtw_is_adapter_up(iface)) {
+			pwdinfo = &iface->wdinfo;
+			if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
+				wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 1);
+				pframe += wfdielen;
+				pattrib->pktlen += wfdielen;
+			}
+		}
+	}
+}
+#endif /* CONFIG_WFD */
+
+void _tdls_tpk_timer_hdl(void *FunctionContext)
+{
+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
+	struct tdls_txmgmt txmgmt;
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	ptdls_sta->TPK_count++;
+	/* TPK_timer expired in a second */
+	/* Retry timer should set at least 301 sec. */
+	if (ptdls_sta->TPK_count >= (ptdls_sta->TDLS_PeerKey_Lifetime - 3)) {
+		RTW_INFO("[TDLS] %s, Re-Setup TDLS link with "MAC_FMT" since TPK lifetime expires!\n", __func__, MAC_ARG(ptdls_sta->hwaddr));
+		ptdls_sta->TPK_count = 0;
+		memcpy(txmgmt.peer, ptdls_sta->hwaddr, ETH_ALEN);
+		issue_tdls_setup_req(ptdls_sta->padapter, &txmgmt, false);
+	}
+
+	_set_timer(&ptdls_sta->TPK_timer, ONE_SEC);
+}
+
+#ifdef CONFIG_TDLS_CH_SW
+void _tdls_ch_switch_timer_hdl(void *FunctionContext)
+{
+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
+	_adapter *padapter = ptdls_sta->padapter;
+	struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
+
+	rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_END_TO_BASE_CHNL);
+	RTW_INFO("[TDLS] %s, can't get traffic from op_ch:%d\n", __func__, rtw_get_oper_ch(padapter));
+}
+
+void _tdls_delay_timer_hdl(void *FunctionContext)
+{
+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
+	_adapter *padapter = ptdls_sta->padapter;
+	struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
+
+	RTW_INFO("[TDLS] %s, op_ch:%d, tdls_state:0x%08x\n", __func__, rtw_get_oper_ch(padapter), ptdls_sta->tdls_sta_state);
+	pchsw_info->delay_switch_back = true;
+}
+
+void _tdls_stay_on_base_chnl_timer_hdl(void *FunctionContext)
+{
+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
+	_adapter *padapter = ptdls_sta->padapter;
+	struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
+
+	if (ptdls_sta != NULL) {
+		issue_tdls_ch_switch_req(padapter, ptdls_sta);
+		pchsw_info->ch_sw_state |= TDLS_WAIT_CH_RSP_STATE;
+	}
+}
+
+void _tdls_ch_switch_monitor_timer_hdl(void *FunctionContext)
+{
+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
+	_adapter *padapter = ptdls_sta->padapter;
+	struct tdls_ch_switch *pchsw_info = &padapter->tdlsinfo.chsw_info;
+
+	rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_CH_SW_END);
+	RTW_INFO("[TDLS] %s, does not receive ch sw req\n", __func__);
+}
+
+#endif
+
+void _tdls_handshake_timer_hdl(void *FunctionContext)
+{
+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
+	_adapter *padapter = ptdls_sta->padapter;
+	struct tdls_txmgmt txmgmt;
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	memcpy(txmgmt.peer, ptdls_sta->hwaddr, ETH_ALEN);
+	txmgmt.status_code = _RSON_TDLS_TEAR_UN_RSN_;
+
+	if (ptdls_sta != NULL) {
+		RTW_INFO("[TDLS] Handshake time out\n");
+		if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)
+			rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_TEARDOWN_STA);
+		else
+			rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_TEARDOWN_STA_LOCALLY);
+	}
+}
+
+void _tdls_pti_timer_hdl(void *FunctionContext)
+{
+	struct sta_info *ptdls_sta = (struct sta_info *)FunctionContext;
+	_adapter *padapter = ptdls_sta->padapter;
+	struct tdls_txmgmt txmgmt;
+
+	memset(&txmgmt, 0x00, sizeof(struct tdls_txmgmt));
+	memcpy(txmgmt.peer, ptdls_sta->hwaddr, ETH_ALEN);
+	txmgmt.status_code = _RSON_TDLS_TEAR_TOOFAR_;
+
+	if (ptdls_sta != NULL) {
+		if (ptdls_sta->tdls_sta_state & TDLS_WAIT_PTR_STATE) {
+			RTW_INFO("[TDLS] Doesn't receive PTR from peer dev:"MAC_FMT"; "
+				"Send TDLS Tear Down\n", MAC_ARG(ptdls_sta->hwaddr));
+			issue_tdls_teardown(padapter, &txmgmt, false);
+		}
+	}
+}
+
+void rtw_init_tdls_timer(_adapter *padapter, struct sta_info *psta)
+{
+	psta->padapter = padapter;
+	_init_timer(&psta->TPK_timer, padapter->pnetdev, _tdls_tpk_timer_hdl, psta);
+#ifdef CONFIG_TDLS_CH_SW
+	_init_timer(&psta->ch_sw_timer, padapter->pnetdev, _tdls_ch_switch_timer_hdl, psta);
+	_init_timer(&psta->delay_timer, padapter->pnetdev, _tdls_delay_timer_hdl, psta);
+	_init_timer(&psta->stay_on_base_chnl_timer, padapter->pnetdev, _tdls_stay_on_base_chnl_timer_hdl, psta);
+	_init_timer(&psta->ch_sw_monitor_timer, padapter->pnetdev, _tdls_ch_switch_monitor_timer_hdl, psta);
+#endif
+	_init_timer(&psta->handshake_timer, padapter->pnetdev, _tdls_handshake_timer_hdl, psta);
+	_init_timer(&psta->pti_timer, padapter->pnetdev, _tdls_pti_timer_hdl, psta);
+}
+
+void rtw_free_tdls_timer(struct sta_info *psta)
+{
+	_cancel_timer_ex(&psta->TPK_timer);
+#ifdef CONFIG_TDLS_CH_SW
+	_cancel_timer_ex(&psta->ch_sw_timer);
+	_cancel_timer_ex(&psta->delay_timer);
+	_cancel_timer_ex(&psta->stay_on_base_chnl_timer);
+	_cancel_timer_ex(&psta->ch_sw_monitor_timer);
+#endif
+	_cancel_timer_ex(&psta->handshake_timer);
+	_cancel_timer_ex(&psta->pti_timer);
+}
+
+u32 update_mask_tdls(_adapter *padapter, struct sta_info *psta)
+{
+	unsigned char sta_band = 0;
+	unsigned int tx_ra_bitmap = 0;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	WLAN_BSSID_EX *pcur_network = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;
+
+	rtw_hal_update_sta_rate_mask(padapter, psta);
+	tx_ra_bitmap = psta->ra_mask;
+
+	if (pcur_network->Configuration.DSConfig > 14) {
+		if (tx_ra_bitmap & 0xffff000)
+			sta_band |= WIRELESS_11_5N | WIRELESS_11A;
+		else
+			sta_band |= WIRELESS_11A;
+	} else {
+		if (tx_ra_bitmap & 0xffff000)
+			sta_band |= WIRELESS_11_24N | WIRELESS_11G | WIRELESS_11B;
+		else if (tx_ra_bitmap & 0xff0)
+			sta_band |= WIRELESS_11G | WIRELESS_11B;
+		else
+			sta_band |= WIRELESS_11B;
+	}
+
+	psta->wireless_mode = sta_band;
+
+	psta->raid = rtw_hal_networktype_to_raid(padapter, psta);
+	tx_ra_bitmap |= ((psta->raid << 28) & 0xf0000000);
+	return tx_ra_bitmap;
+}
+
+int rtw_tdls_is_driver_setup(_adapter *padapter)
+{
+	return padapter->tdlsinfo.driver_setup;
+}
+
+const char *rtw_tdls_action_txt(enum TDLS_ACTION_FIELD action)
+{
+	switch (action) {
+	case TDLS_SETUP_REQUEST:
+		return "TDLS_SETUP_REQUEST";
+	case TDLS_SETUP_RESPONSE:
+		return "TDLS_SETUP_RESPONSE";
+	case TDLS_SETUP_CONFIRM:
+		return "TDLS_SETUP_CONFIRM";
+	case TDLS_TEARDOWN:
+		return "TDLS_TEARDOWN";
+	case TDLS_PEER_TRAFFIC_INDICATION:
+		return "TDLS_PEER_TRAFFIC_INDICATION";
+	case TDLS_CHANNEL_SWITCH_REQUEST:
+		return "TDLS_CHANNEL_SWITCH_REQUEST";
+	case TDLS_CHANNEL_SWITCH_RESPONSE:
+		return "TDLS_CHANNEL_SWITCH_RESPONSE";
+	case TDLS_PEER_PSM_REQUEST:
+		return "TDLS_PEER_PSM_REQUEST";
+	case TDLS_PEER_PSM_RESPONSE:
+		return "TDLS_PEER_PSM_RESPONSE";
+	case TDLS_PEER_TRAFFIC_RESPONSE:
+		return "TDLS_PEER_TRAFFIC_RESPONSE";
+	case TDLS_DISCOVERY_REQUEST:
+		return "TDLS_DISCOVERY_REQUEST";
+	case TDLS_DISCOVERY_RESPONSE:
+		return "TDLS_DISCOVERY_RESPONSE";
+	default:
+		return "UNKNOWN";
+	}
+}
+
+#endif /* CONFIG_TDLS */
diff --git a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
new file mode 100644
index 000000000000..b0c71cba0c1c
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
@@ -0,0 +1,4140 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_WLAN_UTIL_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+	#include <linux/inetdevice.h>
+	#define ETH_TYPE_OFFSET	12
+	#define PROTOCOL_OFFSET	23
+	#define IP_OFFSET	30
+#endif
+
+static unsigned char ATHEROS_OUI1[] = {0x00, 0x03, 0x7f};
+static unsigned char ATHEROS_OUI2[] = {0x00, 0x13, 0x74};
+
+static unsigned char BROADCOM_OUI1[] = {0x00, 0x10, 0x18};
+static unsigned char BROADCOM_OUI2[] = {0x00, 0x0a, 0xf7};
+static unsigned char BROADCOM_OUI3[] = {0x00, 0x05, 0xb5};
+u8 REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
+
+
+static unsigned char CISCO_OUI[] = {0x00, 0x40, 0x96};
+static unsigned char MARVELL_OUI[] = {0x00, 0x50, 0x43};
+static unsigned char RALINK_OUI[] = {0x00, 0x0c, 0x43};
+static unsigned char REALTEK_OUI[] = {0x00, 0xe0, 0x4c};
+static unsigned char AIRGOCAP_OUI[] = {0x00, 0x0a, 0xf5};
+
+extern unsigned char RTW_WPA_OUI[];
+
+#define R2T_PHY_DELAY	(0)
+
+/* #define WAIT_FOR_BCN_TO_MIN	(3000) */
+#define WAIT_FOR_BCN_TO_MIN	(6000)
+#define WAIT_FOR_BCN_TO_MAX	(20000)
+
+#define DISCONNECT_BY_CHK_BCN_FAIL_OBSERV_PERIOD_IN_MS 1000
+#define DISCONNECT_BY_CHK_BCN_FAIL_THRESHOLD 3
+
+static u8 rtw_basic_rate_cck[4] = {
+	IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK
+};
+
+static u8 rtw_basic_rate_ofdm[3] = {
+	IEEE80211_OFDM_RATE_6MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_24MB | IEEE80211_BASIC_RATE_MASK
+};
+
+static u8 rtw_basic_rate_mix[7] = {
+	IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_6MB | IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_24MB | IEEE80211_BASIC_RATE_MASK
+};
+
+int new_bcn_max = 3;
+
+int cckrates_included(unsigned char *rate, int ratelen)
+{
+	int	i;
+
+	for (i = 0; i < ratelen; i++) {
+		if ((((rate[i]) & 0x7f) == 2)	|| (((rate[i]) & 0x7f) == 4) ||
+		    (((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22))
+			return true;
+	}
+
+	return false;
+
+}
+
+int cckratesonly_included(unsigned char *rate, int ratelen)
+{
+	int	i;
+
+	for (i = 0; i < ratelen; i++) {
+		if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
+		    (((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22))
+			return false;
+	}
+
+	return true;
+}
+
+#ifdef CONFIG_GET_RAID_BY_DRV
+static s8 rtw_get_tx_nss(_adapter *adapter, struct sta_info *psta)
+{
+	struct hal_spec_t *hal_spec = GET_HAL_SPEC(adapter);
+	u8 rf_type = RF_1T1R, custom_rf_type;
+	s8 nss = 1;
+
+	if (!psta)
+		return nss;
+
+	custom_rf_type = adapter->registrypriv.rf_config;
+	rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	if (RF_TYPE_VALID(custom_rf_type))
+		rf_type = custom_rf_type;
+
+	if (psta->htpriv.ht_option) {
+		nss = rtw_min(rf_type_to_rf_tx_cnt(rf_type), hal_spec->tx_nss_num);
+		nss = rtw_min(nss, rtw_ht_mcsset_to_nss(psta->htpriv.ht_cap.supp_mcs_set));
+	}
+
+	RTW_INFO("%s: %d SS\n", __func__, nss);
+	return nss;
+}
+
+u8 networktype_to_raid(_adapter *adapter, struct sta_info *psta)
+{
+	unsigned char raid;
+	switch (psta->wireless_mode) {
+	case WIRELESS_11B:
+		raid = RATR_INX_WIRELESS_B;
+		break;
+	case WIRELESS_11A:
+	case WIRELESS_11G:
+		raid = RATR_INX_WIRELESS_G;
+		break;
+	case WIRELESS_11BG:
+		raid = RATR_INX_WIRELESS_GB;
+		break;
+	case WIRELESS_11_24N:
+	case WIRELESS_11_5N:
+		raid = RATR_INX_WIRELESS_N;
+		break;
+	case WIRELESS_11A_5N:
+	case WIRELESS_11G_24N:
+		raid = RATR_INX_WIRELESS_NG;
+		break;
+	case WIRELESS_11BG_24N:
+		raid = RATR_INX_WIRELESS_NGB;
+		break;
+	default:
+		raid = RATR_INX_WIRELESS_GB;
+		break;
+
+	}
+	return raid;
+
+}
+
+u8 networktype_to_raid_ex(_adapter *adapter, struct sta_info *psta)
+{
+	struct mlme_ext_priv	*pmlmeext = &adapter->mlmeextpriv;
+	u8 raid = RATEID_IDX_BGN_40M_1SS, cur_rf_type, rf_type, custom_rf_type;
+	s8 tx_nss;
+
+	tx_nss = rtw_get_tx_nss(adapter, psta);
+
+	switch (psta->wireless_mode) {
+	case WIRELESS_11B:
+		raid = RATEID_IDX_B;
+		break;
+	case WIRELESS_11A:
+	case WIRELESS_11G:
+		raid = RATEID_IDX_G;
+		break;
+	case WIRELESS_11BG:
+		raid = RATEID_IDX_BG;
+		break;
+	case WIRELESS_11_24N:
+	case WIRELESS_11_5N:
+	case WIRELESS_11A_5N:
+	case WIRELESS_11G_24N:
+		if (tx_nss == 1)
+			raid = RATEID_IDX_GN_N1SS;
+		else if (tx_nss == 2)
+			raid = RATEID_IDX_GN_N2SS;
+		else if (tx_nss == 3)
+			raid = RATEID_IDX_BGN_3SS;
+		else
+			RTW_INFO("tx_nss error!(tx_nss=%d)\n", tx_nss);
+		break;
+	case WIRELESS_11B_24N:
+	case WIRELESS_11BG_24N:
+		if (psta->bw_mode == CHANNEL_WIDTH_20) {
+			if (tx_nss == 1)
+				raid = RATEID_IDX_BGN_20M_1SS_BN;
+			else if (tx_nss == 2)
+				raid = RATEID_IDX_BGN_20M_2SS_BN;
+			else if (tx_nss == 3)
+				raid = RATEID_IDX_BGN_3SS;
+			else
+				RTW_INFO("tx_nss error!(tx_nss=%d)\n", tx_nss);
+		} else {
+			if (tx_nss == 1)
+				raid = RATEID_IDX_BGN_40M_1SS;
+			else if (tx_nss == 2)
+				raid = RATEID_IDX_BGN_40M_2SS;
+			else if (tx_nss == 3)
+				raid = RATEID_IDX_BGN_3SS;
+			else
+				RTW_INFO("tx_nss error!(tx_nss=%d)\n", tx_nss);
+		}
+		break;
+	default:
+		RTW_INFO("unexpected wireless mode!(psta->wireless_mode=%x)\n", psta->wireless_mode);
+		break;
+
+	}
+
+	return raid;
+}
+
+#endif
+u8 judge_network_type(_adapter *padapter, unsigned char *rate, int ratelen)
+{
+	u8 network_type = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+
+	if (pmlmeext->cur_channel > 14) {
+		if (pmlmeinfo->VHT_enable)
+			network_type = WIRELESS_11AC;
+		else if (pmlmeinfo->HT_enable)
+			network_type = WIRELESS_11_5N;
+
+		network_type |= WIRELESS_11A;
+	} else {
+		if (pmlmeinfo->HT_enable)
+			network_type = WIRELESS_11_24N;
+
+		if ((cckratesonly_included(rate, ratelen)))
+			network_type |= WIRELESS_11B;
+		else if ((cckrates_included(rate, ratelen)))
+			network_type |= WIRELESS_11BG;
+		else
+			network_type |= WIRELESS_11G;
+	}
+
+	return	network_type;
+}
+
+unsigned char ratetbl_val_2wifirate(unsigned char rate);
+unsigned char ratetbl_val_2wifirate(unsigned char rate)
+{
+	unsigned char val = 0;
+
+	switch (rate & 0x7f) {
+	case 0:
+		val = IEEE80211_CCK_RATE_1MB;
+		break;
+
+	case 1:
+		val = IEEE80211_CCK_RATE_2MB;
+		break;
+
+	case 2:
+		val = IEEE80211_CCK_RATE_5MB;
+		break;
+
+	case 3:
+		val = IEEE80211_CCK_RATE_11MB;
+		break;
+
+	case 4:
+		val = IEEE80211_OFDM_RATE_6MB;
+		break;
+
+	case 5:
+		val = IEEE80211_OFDM_RATE_9MB;
+		break;
+
+	case 6:
+		val = IEEE80211_OFDM_RATE_12MB;
+		break;
+
+	case 7:
+		val = IEEE80211_OFDM_RATE_18MB;
+		break;
+
+	case 8:
+		val = IEEE80211_OFDM_RATE_24MB;
+		break;
+
+	case 9:
+		val = IEEE80211_OFDM_RATE_36MB;
+		break;
+
+	case 10:
+		val = IEEE80211_OFDM_RATE_48MB;
+		break;
+
+	case 11:
+		val = IEEE80211_OFDM_RATE_54MB;
+		break;
+
+	}
+
+	return val;
+
+}
+
+int is_basicrate(_adapter *padapter, unsigned char rate);
+int is_basicrate(_adapter *padapter, unsigned char rate)
+{
+	int i;
+	unsigned char val;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	for (i = 0; i < NumRates; i++) {
+		val = pmlmeext->basicrate[i];
+
+		if ((val != 0xff) && (val != 0xfe)) {
+			if (rate == ratetbl_val_2wifirate(val))
+				return true;
+		}
+	}
+
+	return false;
+}
+
+unsigned int ratetbl2rateset(_adapter *padapter, unsigned char *rateset);
+unsigned int ratetbl2rateset(_adapter *padapter, unsigned char *rateset)
+{
+	int i;
+	unsigned char rate;
+	unsigned int	len = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	for (i = 0; i < NumRates; i++) {
+		rate = pmlmeext->datarate[i];
+
+		if (rtw_get_oper_ch(padapter) > 14 && rate < _6M_RATE_) /*5G no support CCK rate*/
+			continue;
+
+		switch (rate) {
+		case 0xff:
+			return len;
+
+		case 0xfe:
+			continue;
+
+		default:
+			rate = ratetbl_val_2wifirate(rate);
+
+			if (is_basicrate(padapter, rate))
+				rate |= IEEE80211_BASIC_RATE_MASK;
+
+			rateset[len] = rate;
+			len++;
+			break;
+		}
+	}
+	return len;
+}
+
+void get_rate_set(_adapter *padapter, unsigned char *pbssrate, int *bssrate_len)
+{
+	unsigned char supportedrates[NumRates];
+
+	memset(supportedrates, 0, NumRates);
+	*bssrate_len = ratetbl2rateset(padapter, supportedrates);
+	memcpy(pbssrate, supportedrates, *bssrate_len);
+}
+
+void set_mcs_rate_by_mask(u8 *mcs_set, u32 mask)
+{
+	u8 mcs_rate_1r = (u8)(mask & 0xff);
+	u8 mcs_rate_2r = (u8)((mask >> 8) & 0xff);
+	u8 mcs_rate_3r = (u8)((mask >> 16) & 0xff);
+	u8 mcs_rate_4r = (u8)((mask >> 24) & 0xff);
+
+	mcs_set[0] &= mcs_rate_1r;
+	mcs_set[1] &= mcs_rate_2r;
+	mcs_set[2] &= mcs_rate_3r;
+	mcs_set[3] &= mcs_rate_4r;
+}
+
+void UpdateBrateTbl(
+	PADAPTER		Adapter,
+	u8			*mBratesOS
+)
+{
+	u8	i;
+	u8	rate;
+
+	/* 1M, 2M, 5.5M, 11M, 6M, 12M, 24M are mandatory. */
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		rate = mBratesOS[i] & 0x7f;
+		switch (rate) {
+		case IEEE80211_CCK_RATE_1MB:
+		case IEEE80211_CCK_RATE_2MB:
+		case IEEE80211_CCK_RATE_5MB:
+		case IEEE80211_CCK_RATE_11MB:
+		case IEEE80211_OFDM_RATE_6MB:
+		case IEEE80211_OFDM_RATE_12MB:
+		case IEEE80211_OFDM_RATE_24MB:
+			mBratesOS[i] |= IEEE80211_BASIC_RATE_MASK;
+			break;
+		}
+	}
+
+}
+
+void UpdateBrateTblForSoftAP(u8 *bssrateset, u32 bssratelen)
+{
+	u8	i;
+	u8	rate;
+
+	for (i = 0; i < bssratelen; i++) {
+		rate = bssrateset[i] & 0x7f;
+		switch (rate) {
+		case IEEE80211_CCK_RATE_1MB:
+		case IEEE80211_CCK_RATE_2MB:
+		case IEEE80211_CCK_RATE_5MB:
+		case IEEE80211_CCK_RATE_11MB:
+			bssrateset[i] |= IEEE80211_BASIC_RATE_MASK;
+			break;
+		}
+	}
+
+}
+void Set_MSR(_adapter *padapter, u8 type)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_MEDIA_STATUS, (u8 *)(&type));
+}
+
+inline u8 rtw_get_oper_ch(_adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_channel;
+}
+
+inline void rtw_set_oper_ch(_adapter *adapter, u8 ch)
+{
+#ifdef DBG_CH_SWITCH
+	const int len = 128;
+	char msg[128] = {0};
+	int cnt = 0;
+	int i = 0;
+#endif  /* DBG_CH_SWITCH */
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+
+	if (dvobj->oper_channel != ch) {
+		dvobj->on_oper_ch_time = jiffies;
+
+#ifdef DBG_CH_SWITCH
+		cnt += snprintf(msg + cnt, len - cnt, "switch to ch %3u", ch);
+
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			_adapter *iface = dvobj->padapters[i];
+			cnt += snprintf(msg + cnt, len - cnt, " ["ADPT_FMT":", ADPT_ARG(iface));
+			if (iface->mlmeextpriv.cur_channel == ch)
+				cnt += snprintf(msg + cnt, len - cnt, "C");
+			else
+				cnt += snprintf(msg + cnt, len - cnt, "_");
+			if (iface->wdinfo.listen_channel == ch && !rtw_p2p_chk_state(&iface->wdinfo, P2P_STATE_NONE))
+				cnt += snprintf(msg + cnt, len - cnt, "L");
+			else
+				cnt += snprintf(msg + cnt, len - cnt, "_");
+			cnt += snprintf(msg + cnt, len - cnt, "]");
+		}
+
+		RTW_INFO(FUNC_ADPT_FMT" %s\n", FUNC_ADPT_ARG(adapter), msg);
+#endif /* DBG_CH_SWITCH */
+	}
+
+	dvobj->oper_channel = ch;
+}
+
+inline u8 rtw_get_oper_bw(_adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_bwmode;
+}
+
+inline void rtw_set_oper_bw(_adapter *adapter, u8 bw)
+{
+	adapter_to_dvobj(adapter)->oper_bwmode = bw;
+}
+
+inline u8 rtw_get_oper_choffset(_adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_ch_offset;
+}
+
+inline void rtw_set_oper_choffset(_adapter *adapter, u8 offset)
+{
+	adapter_to_dvobj(adapter)->oper_ch_offset = offset;
+}
+
+u8 rtw_get_offset_by_chbw(u8 ch, u8 bw, u8 *r_offset)
+{
+	u8 valid = 1;
+	u8 offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	if (bw == CHANNEL_WIDTH_20)
+		goto exit;
+
+	if (bw >= CHANNEL_WIDTH_80 && ch <= 14) {
+		valid = 0;
+		goto exit;
+	}
+
+	if (ch >= 1 && ch <= 4)
+		offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+	else if (ch >= 5 && ch <= 9) {
+		if (*r_offset == HAL_PRIME_CHNL_OFFSET_LOWER || *r_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
+			offset = *r_offset; /* both lower and upper is valid, obey input value */
+		else
+			offset = HAL_PRIME_CHNL_OFFSET_UPPER; /* default use upper */
+	} else if (ch >= 10 && ch <= 13)
+		offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+	else if (ch == 14) {
+		valid = 0; /* ch14 doesn't support 40MHz bandwidth */
+		goto exit;
+	} else if (ch >= 36 && ch <= 177) {
+		switch (ch) {
+		case 36:
+		case 44:
+		case 52:
+		case 60:
+		case 100:
+		case 108:
+		case 116:
+		case 124:
+		case 132:
+		case 140:
+		case 149:
+		case 157:
+		case 165:
+		case 173:
+			offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+			break;
+		case 40:
+		case 48:
+		case 56:
+		case 64:
+		case 104:
+		case 112:
+		case 120:
+		case 128:
+		case 136:
+		case 144:
+		case 153:
+		case 161:
+		case 169:
+		case 177:
+			offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+			break;
+		default:
+			valid = 0;
+			break;
+		}
+	} else
+		valid = 0;
+
+exit:
+	if (valid && r_offset)
+		*r_offset = offset;
+	return valid;
+}
+
+u8 rtw_get_offset_by_ch(u8 channel)
+{
+	u8 offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	if (channel >= 1 && channel <= 4)
+		offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+	else if (channel >= 5 && channel <= 14)
+		offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+	else {
+		switch (channel) {
+		case 36:
+		case 44:
+		case 52:
+		case 60:
+		case 100:
+		case 108:
+		case 116:
+		case 124:
+		case 132:
+		case 149:
+		case 157:
+			offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+			break;
+		case 40:
+		case 48:
+		case 56:
+		case 64:
+		case 104:
+		case 112:
+		case 120:
+		case 128:
+		case 136:
+		case 153:
+		case 161:
+			offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+			break;
+		default:
+			offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			break;
+		}
+
+	}
+
+	return offset;
+
+}
+
+u8 rtw_get_center_ch(u8 channel, u8 chnl_bw, u8 chnl_offset)
+{
+	u8 center_ch = channel;
+
+	if (chnl_bw == CHANNEL_WIDTH_80) {
+		if (channel == 36 || channel == 40 || channel == 44 || channel == 48)
+			center_ch = 42;
+		else if (channel == 52 || channel == 56 || channel == 60 || channel == 64)
+			center_ch = 58;
+		else if (channel == 100 || channel == 104 || channel == 108 || channel == 112)
+			center_ch = 106;
+		else if (channel == 116 || channel == 120 || channel == 124 || channel == 128)
+			center_ch = 122;
+		else if (channel == 132 || channel == 136 || channel == 140 || channel == 144)
+			center_ch = 138;
+		else if (channel == 149 || channel == 153 || channel == 157 || channel == 161)
+			center_ch = 155;
+		else if (channel == 165 || channel == 169 || channel == 173 || channel == 177)
+			center_ch = 171;
+		else if (channel <= 14)
+			center_ch = 7;
+	} else if (chnl_bw == CHANNEL_WIDTH_40) {
+		if (chnl_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
+			center_ch = channel + 2;
+		else
+			center_ch = channel - 2;
+	} else if (chnl_bw == CHANNEL_WIDTH_20)
+		center_ch = channel;
+	else
+		rtw_warn_on(1);
+
+	return center_ch;
+}
+
+inline u32 rtw_get_on_oper_ch_time(_adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->on_oper_ch_time;
+}
+
+inline u32 rtw_get_on_cur_ch_time(_adapter *adapter)
+{
+	if (adapter->mlmeextpriv.cur_channel == adapter_to_dvobj(adapter)->oper_channel)
+		return adapter_to_dvobj(adapter)->on_oper_ch_time;
+	else
+		return 0;
+}
+
+void set_channel_bwmode(_adapter *padapter, unsigned char channel, unsigned char channel_offset, unsigned short bwmode)
+{
+	u8 center_ch, chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+#if (defined(CONFIG_TDLS) && defined(CONFIG_TDLS_CH_SW)) || defined(CONFIG_MCC_MODE)
+	u8 iqk_info_backup = false;
+#endif
+
+	if (padapter->bNotifyChannelChange)
+		RTW_INFO("[%s] ch = %d, offset = %d, bwmode = %d\n", __func__, channel, channel_offset, bwmode);
+
+	center_ch = rtw_get_center_ch(channel, bwmode, channel_offset);
+
+	if (bwmode == CHANNEL_WIDTH_80) {
+		if (center_ch > channel)
+			chnl_offset80 = HAL_PRIME_CHNL_OFFSET_LOWER;
+		else if (center_ch < channel)
+			chnl_offset80 = HAL_PRIME_CHNL_OFFSET_UPPER;
+		else
+			chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	}
+	_enter_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL);
+
+#ifdef CONFIG_MCC_MODE
+	if (MCC_EN(padapter)) {
+		/* driver doesn't set channel setting reg under MCC */
+		if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_DOING_MCC)) {
+			RTW_INFO("Warning: Do not set channel setting reg MCC mode\n");
+			rtw_warn_on(1);
+		}
+	}
+#endif
+
+#ifdef CONFIG_DFS_MASTER
+	{
+		struct rf_ctl_t *rfctl = adapter_to_rfctl(padapter);
+		bool ori_overlap_radar_detect_ch = rtw_rfctl_overlap_radar_detect_ch(rfctl);
+		bool new_overlap_radar_detect_ch = _rtw_rfctl_overlap_radar_detect_ch(rfctl, channel, bwmode, channel_offset);
+
+		if (new_overlap_radar_detect_ch)
+			rtw_odm_radar_detect_enable(padapter);
+
+		if (new_overlap_radar_detect_ch && IS_CH_WAITING(rfctl)) {
+			u8 pause = 0xFF;
+
+			rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &pause);
+		}
+#endif /* CONFIG_DFS_MASTER */
+
+		/* set Channel */
+		/* saved channel/bw info */
+		rtw_set_oper_ch(padapter, channel);
+		rtw_set_oper_bw(padapter, bwmode);
+		rtw_set_oper_choffset(padapter, channel_offset);
+
+#if (defined(CONFIG_TDLS) && defined(CONFIG_TDLS_CH_SW)) || defined(CONFIG_MCC_MODE)
+		/* To check if we need to backup iqk info after switch chnl & bw */
+		{
+			u8 take_care_iqk, do_iqk;
+
+			rtw_hal_get_hwreg(padapter, HW_VAR_CH_SW_NEED_TO_TAKE_CARE_IQK_INFO, &take_care_iqk);
+			rtw_hal_get_hwreg(padapter, HW_VAR_DO_IQK, &do_iqk);
+			if ((take_care_iqk) && (do_iqk == true))
+				iqk_info_backup = true;
+		}
+#endif
+
+		rtw_hal_set_chnl_bw(padapter, center_ch, bwmode, channel_offset, chnl_offset80); /* set center channel */
+
+#if (defined(CONFIG_TDLS) && defined(CONFIG_TDLS_CH_SW)) || defined(CONFIG_MCC_MODE)
+		if (iqk_info_backup)
+			rtw_hal_ch_sw_iqk_info_backup(padapter);
+#endif
+
+#ifdef CONFIG_DFS_MASTER
+		if (ori_overlap_radar_detect_ch && !new_overlap_radar_detect_ch) {
+			u8 pause = 0x00;
+
+			rtw_odm_radar_detect_disable(padapter);
+			rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &pause);
+		}
+	}
+#endif /* CONFIG_DFS_MASTER */
+
+	_exit_critical_mutex(&(adapter_to_dvobj(padapter)->setch_mutex), NULL);
+}
+
+int get_bsstype(unsigned short capability)
+{
+	if (capability & BIT(0))
+		return WIFI_FW_AP_STATE;
+	else if (capability & BIT(1))
+		return WIFI_FW_ADHOC_STATE;
+	else
+		return 0;
+}
+
+__inline u8 *get_my_bssid(WLAN_BSSID_EX *pnetwork)
+{
+	return pnetwork->MacAddress;
+}
+
+u16 get_beacon_interval(WLAN_BSSID_EX *bss)
+{
+	__le16  val;
+	memcpy((unsigned char *)&val, rtw_get_beacon_interval_from_ie(bss->IEs), 2);
+
+	return le16_to_cpu(val);
+}
+
+int is_client_associated_to_ap(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext;
+	struct mlme_ext_info	*pmlmeinfo;
+
+	if (!padapter)
+		return _FAIL;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE))
+		return true;
+	else
+		return _FAIL;
+}
+
+int is_client_associated_to_ibss(_adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE))
+		return true;
+	else
+		return _FAIL;
+}
+
+int is_IBSS_empty(_adapter *padapter)
+{
+	int i;
+	struct macid_ctl_t *macid_ctl = &padapter->dvobj->macid_ctl;
+
+	for (i = 0; i < macid_ctl->num; i++) {
+		if (!rtw_macid_is_used(macid_ctl, i))
+			continue;
+		if (rtw_macid_get_if_g(macid_ctl, i) != padapter->iface_id)
+			continue;
+		if (!GET_H2CCMD_MSRRPT_PARM_OPMODE(&macid_ctl->h2c_msr[i]))
+			continue;
+		if (GET_H2CCMD_MSRRPT_PARM_ROLE(&macid_ctl->h2c_msr[i]) == H2C_MSR_ROLE_ADHOC)
+			return _FAIL;
+	}
+
+	return true;
+}
+
+unsigned int decide_wait_for_beacon_timeout(unsigned int bcn_interval)
+{
+	if ((bcn_interval << 2) < WAIT_FOR_BCN_TO_MIN)
+		return WAIT_FOR_BCN_TO_MIN;
+	else if ((bcn_interval << 2) > WAIT_FOR_BCN_TO_MAX)
+		return WAIT_FOR_BCN_TO_MAX;
+	else
+		return bcn_interval << 2;
+}
+
+void CAM_empty_entry(
+	PADAPTER	Adapter,
+	u8			ucIndex
+)
+{
+	rtw_hal_set_hwreg(Adapter, HW_VAR_CAM_EMPTY_ENTRY, (u8 *)(&ucIndex));
+}
+
+void invalidate_cam_all(_adapter *padapter)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+	u8 val8 = 0;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_CAM_INVALID_ALL, &val8);
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+	rtw_sec_cam_map_clr_all(&cam_ctl->used);
+	memset(dvobj->cam_cache, 0, sizeof(struct sec_cam_ent) * SEC_CAM_ENT_NUM_SW_LIMIT);
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+}
+
+void _clear_cam_entry(_adapter *padapter, u8 entry)
+{
+	unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	unsigned char null_key[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+	rtw_sec_write_cam_ent(padapter, entry, 0, null_sta, null_key);
+}
+
+inline void write_cam(_adapter *adapter, u8 id, u16 ctrl, u8 *mac, u8 *key)
+{
+#ifdef CONFIG_WRITE_CACHE_ONLY
+	write_cam_cache(adapter, id , ctrl, mac, key);
+#else
+	rtw_sec_write_cam_ent(adapter, id, ctrl, mac, key);
+	write_cam_cache(adapter, id , ctrl, mac, key);
+#endif
+}
+
+inline void clear_cam_entry(_adapter *adapter, u8 id)
+{
+	_clear_cam_entry(adapter, id);
+	clear_cam_cache(adapter, id);
+}
+
+inline void write_cam_from_cache(_adapter *adapter, u8 id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+	struct sec_cam_ent cache;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+	memcpy(&cache, &dvobj->cam_cache[id], sizeof(struct sec_cam_ent));
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+
+	rtw_sec_write_cam_ent(adapter, id, cache.ctrl, cache.mac, cache.key);
+}
+void write_cam_cache(_adapter *adapter, u8 id, u16 ctrl, u8 *mac, u8 *key)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+
+	dvobj->cam_cache[id].ctrl = ctrl;
+	memcpy(dvobj->cam_cache[id].mac, mac, ETH_ALEN);
+	memcpy(dvobj->cam_cache[id].key, key, 16);
+
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+}
+
+void clear_cam_cache(_adapter *adapter, u8 id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+
+	memset(&(dvobj->cam_cache[id]), 0, sizeof(struct sec_cam_ent));
+
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+}
+
+inline bool _rtw_camctl_chk_cap(_adapter *adapter, u8 cap)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+
+	if (cam_ctl->sec_cap & cap)
+		return true;
+	return false;
+}
+
+inline void _rtw_camctl_set_flags(_adapter *adapter, u32 flags)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+
+	cam_ctl->flags |= flags;
+}
+
+inline void rtw_camctl_set_flags(_adapter *adapter, u32 flags)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+	_rtw_camctl_set_flags(adapter, flags);
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+}
+
+inline void _rtw_camctl_clr_flags(_adapter *adapter, u32 flags)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+
+	cam_ctl->flags &= ~flags;
+}
+
+inline void rtw_camctl_clr_flags(_adapter *adapter, u32 flags)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+	_rtw_camctl_clr_flags(adapter, flags);
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+}
+
+inline bool _rtw_camctl_chk_flags(_adapter *adapter, u32 flags)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+
+	if (cam_ctl->flags & flags)
+		return true;
+	return false;
+}
+
+void dump_sec_cam_map(void *sel, struct sec_cam_bmp *map, u8 max_num)
+{
+	RTW_PRINT_SEL(sel, "0x%08x\n", map->m0);
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32)
+	if (max_num && max_num > 32)
+		RTW_PRINT_SEL(sel, "0x%08x\n", map->m1);
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64)
+	if (max_num && max_num > 64)
+		RTW_PRINT_SEL(sel, "0x%08x\n", map->m2);
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96)
+	if (max_num && max_num > 96)
+		RTW_PRINT_SEL(sel, "0x%08x\n", map->m3);
+#endif
+}
+
+inline bool rtw_sec_camid_is_set(struct sec_cam_bmp *map, u8 id)
+{
+	if (id < 32)
+		return map->m0 & BIT(id);
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32)
+	else if (id < 64)
+		return map->m1 & BIT(id - 32);
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64)
+	else if (id < 96)
+		return map->m2 & BIT(id - 64);
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96)
+	else if (id < 128)
+		return map->m3 & BIT(id - 96);
+#endif
+	else
+		rtw_warn_on(1);
+
+	return 0;
+}
+
+inline void rtw_sec_cam_map_set(struct sec_cam_bmp *map, u8 id)
+{
+	if (id < 32)
+		map->m0 |= BIT(id);
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32)
+	else if (id < 64)
+		map->m1 |= BIT(id - 32);
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64)
+	else if (id < 96)
+		map->m2 |= BIT(id - 64);
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96)
+	else if (id < 128)
+		map->m3 |= BIT(id - 96);
+#endif
+	else
+		rtw_warn_on(1);
+}
+
+inline void rtw_sec_cam_map_clr(struct sec_cam_bmp *map, u8 id)
+{
+	if (id < 32)
+		map->m0 &= ~BIT(id);
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32)
+	else if (id < 64)
+		map->m1 &= ~BIT(id - 32);
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64)
+	else if (id < 96)
+		map->m2 &= ~BIT(id - 64);
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96)
+	else if (id < 128)
+		map->m3 &= ~BIT(id - 96);
+#endif
+	else
+		rtw_warn_on(1);
+}
+
+inline void rtw_sec_cam_map_clr_all(struct sec_cam_bmp *map)
+{
+	map->m0 = 0;
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32)
+	map->m1 = 0;
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64)
+	map->m2 = 0;
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96)
+	map->m3 = 0;
+#endif
+}
+
+inline bool rtw_sec_camid_is_drv_forbid(struct cam_ctl_t *cam_ctl, u8 id)
+{
+	struct sec_cam_bmp forbid_map;
+
+	forbid_map.m0 = 0x00000ff0;
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32)
+	forbid_map.m1 = 0x00000000;
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64)
+	forbid_map.m2 = 0x00000000;
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96)
+	forbid_map.m3 = 0x00000000;
+#endif
+
+	if (id < 32)
+		return forbid_map.m0 & BIT(id);
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 32)
+	else if (id < 64)
+		return forbid_map.m1 & BIT(id - 32);
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 64)
+	else if (id < 96)
+		return forbid_map.m2 & BIT(id - 64);
+#endif
+#if (SEC_CAM_ENT_NUM_SW_LIMIT > 96)
+	else if (id < 128)
+		return forbid_map.m3 & BIT(id - 96);
+#endif
+	else
+		rtw_warn_on(1);
+
+	return 1;
+}
+
+static bool _rtw_sec_camid_is_used(struct cam_ctl_t *cam_ctl, u8 id)
+{
+	bool ret = false;
+
+	if (id >= cam_ctl->num) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	ret = rtw_sec_camid_is_set(&cam_ctl->used, id);
+
+exit:
+	return ret;
+}
+
+inline bool rtw_sec_camid_is_used(struct cam_ctl_t *cam_ctl, u8 id)
+{
+	unsigned long irqL;
+	bool ret;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+	ret = _rtw_sec_camid_is_used(cam_ctl, id);
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+
+	return ret;
+}
+u8 rtw_get_sec_camid(_adapter *adapter, u8 max_bk_key_num, u8 *sec_key_id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	int i;
+	unsigned long irqL;
+	u8 sec_cam_num = 0;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+	for (i = 0; i < cam_ctl->num; i++) {
+		if (_rtw_sec_camid_is_used(cam_ctl, i)) {
+			sec_key_id[sec_cam_num++] = i;
+			if (sec_cam_num == max_bk_key_num)
+				break;
+		}
+	}
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+
+	return sec_cam_num;
+}
+
+inline bool _rtw_camid_is_gk(_adapter *adapter, u8 cam_id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	bool ret = false;
+
+	if (cam_id >= cam_ctl->num) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	if (_rtw_sec_camid_is_used(cam_ctl, cam_id) == false)
+		goto exit;
+
+	ret = (dvobj->cam_cache[cam_id].ctrl & BIT6) ? true : false;
+
+exit:
+	return ret;
+}
+
+inline bool rtw_camid_is_gk(_adapter *adapter, u8 cam_id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+	bool ret;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+	ret = _rtw_camid_is_gk(adapter, cam_id);
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+
+	return ret;
+}
+
+static bool cam_cache_chk(_adapter *adapter, u8 id, u8 *addr, s16 kid, s8 gk)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	bool ret = false;
+
+	if (addr && !memcmp(dvobj->cam_cache[id].mac, addr, ETH_ALEN) == false)
+		goto exit;
+	if (kid >= 0 && kid != (dvobj->cam_cache[id].ctrl & 0x03))
+		goto exit;
+	if (gk != -1 && (gk ? true : false) != _rtw_camid_is_gk(adapter, id))
+		goto exit;
+
+	ret = true;
+
+exit:
+	return ret;
+}
+
+static s16 _rtw_camid_search(_adapter *adapter, u8 *addr, s16 kid, s8 gk)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	int i;
+	s16 cam_id = -1;
+
+	for (i = 0; i < cam_ctl->num; i++) {
+		if (cam_cache_chk(adapter, i, addr, kid, gk)) {
+			cam_id = i;
+			break;
+		}
+	}
+
+	if (0) {
+		if (addr)
+			RTW_INFO(FUNC_ADPT_FMT" addr:"MAC_FMT" kid:%d, gk:%d, return cam_id:%d\n"
+				, FUNC_ADPT_ARG(adapter), MAC_ARG(addr), kid, gk, cam_id);
+		else
+			RTW_INFO(FUNC_ADPT_FMT" addr:%p kid:%d, gk:%d, return cam_id:%d\n"
+				, FUNC_ADPT_ARG(adapter), addr, kid, gk, cam_id);
+	}
+
+	return cam_id;
+}
+
+s16 rtw_camid_search(_adapter *adapter, u8 *addr, s16 kid, s8 gk)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+	s16 cam_id = -1;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+	cam_id = _rtw_camid_search(adapter, addr, kid, gk);
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+
+	return cam_id;
+}
+
+static s16 rtw_get_camid(_adapter *adapter, struct sta_info *sta, u8 *addr, s16 kid)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	int i;
+	u8 start_id = 0;
+	s16 cam_id = -1;
+
+	if (addr == NULL) {
+		RTW_PRINT(FUNC_ADPT_FMT" mac_address is NULL\n"
+			  , FUNC_ADPT_ARG(adapter));
+		rtw_warn_on(1);
+		goto _exit;
+	}
+
+	/* find cam entry which has the same addr, kid (, gk bit) */
+	if (_rtw_camctl_chk_cap(adapter, SEC_CAP_CHK_BMC))
+		i = _rtw_camid_search(adapter, addr, kid, sta ? false : true);
+	else
+		i = _rtw_camid_search(adapter, addr, kid, -1);
+
+	if (i >= 0) {
+		cam_id = i;
+		goto _exit;
+	}
+
+	for (i = 0; i < cam_ctl->num; i++) {
+		/* bypass default key which is allocated statically */
+#ifndef CONFIG_CONCURRENT_MODE
+		if (((i + start_id) % cam_ctl->num) < 4)
+			continue;
+#endif
+		if (_rtw_sec_camid_is_used(cam_ctl, ((i + start_id) % cam_ctl->num)) == false)
+			break;
+	}
+
+	if (i == cam_ctl->num) {
+		if (sta)
+			RTW_PRINT(FUNC_ADPT_FMT" pairwise key with "MAC_FMT" id:%u no room\n"
+				  , FUNC_ADPT_ARG(adapter), MAC_ARG(addr), kid);
+		else
+			RTW_PRINT(FUNC_ADPT_FMT" group key with "MAC_FMT" id:%u no room\n"
+				  , FUNC_ADPT_ARG(adapter), MAC_ARG(addr), kid);
+		rtw_warn_on(1);
+		goto _exit;
+	}
+
+	cam_id = ((i + start_id) % cam_ctl->num);
+	start_id = ((i + start_id + 1) % cam_ctl->num);
+
+_exit:
+	return cam_id;
+}
+
+s16 rtw_camid_alloc(_adapter *adapter, struct sta_info *sta, u8 kid, bool *used)
+{
+	struct mlme_ext_info *mlmeinfo = &adapter->mlmeextpriv.mlmext_info;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+	s16 cam_id = -1;
+
+	*used = false;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+
+	if ((((mlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) || ((mlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE))
+	    && !sta) {
+#ifndef CONFIG_CONCURRENT_MODE
+		/* AP/Ad-hoc mode group key static alloction to default key by key ID on Non-concurrent*/
+		if (kid > 3) {
+			RTW_PRINT(FUNC_ADPT_FMT" group key with invalid key id:%u\n"
+				  , FUNC_ADPT_ARG(adapter), kid);
+			rtw_warn_on(1);
+			goto bitmap_handle;
+		}
+		cam_id = kid;
+#else
+		u8 *addr = adapter_mac_addr(adapter);
+
+		cam_id = rtw_get_camid(adapter, sta, addr, kid);
+		if (1)
+			RTW_PRINT(FUNC_ADPT_FMT" group key with "MAC_FMT" assigned cam_id:%u\n"
+				, FUNC_ADPT_ARG(adapter), MAC_ARG(addr), cam_id);
+#endif
+	} else {
+		u8 *addr = sta ? sta->hwaddr : NULL;
+
+		if (!sta) {
+			if (!(mlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+				/* bypass STA mode group key setting before connected(ex:WEP) because bssid is not ready */
+				goto bitmap_handle;
+			}
+			addr = get_bssid(&adapter->mlmepriv);/*A2*/
+		}
+		cam_id = rtw_get_camid(adapter, sta, addr, kid);
+	}
+
+
+bitmap_handle:
+	if (cam_id >= 0) {
+		*used = _rtw_sec_camid_is_used(cam_ctl, cam_id);
+		rtw_sec_cam_map_set(&cam_ctl->used, cam_id);
+	}
+
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+
+	return cam_id;
+}
+
+static void rtw_camid_set(_adapter *adapter, u8 cam_id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+
+	if (cam_id < cam_ctl->num)
+		rtw_sec_cam_map_set(&cam_ctl->used, cam_id);
+
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+}
+
+void rtw_camid_free(_adapter *adapter, u8 cam_id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+
+	if (cam_id < cam_ctl->num)
+		rtw_sec_cam_map_clr(&cam_ctl->used, cam_id);
+
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+}
+
+/*Must pause TX/RX before use this API*/
+inline void rtw_sec_cam_swap(_adapter *adapter, u8 cam_id_a, u8 cam_id_b)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	struct sec_cam_ent cache_a, cache_b;
+	unsigned long irqL;
+	bool cam_a_used, cam_b_used;
+
+	if (1)
+		RTW_INFO(ADPT_FMT" - sec_cam %d,%d swap\n", ADPT_ARG(adapter), cam_id_a, cam_id_b);
+
+	if (cam_id_a == cam_id_b)
+		return;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	rtw_mi_update_ap_bmc_camid(adapter, cam_id_a, cam_id_b);
+#endif
+
+	/*setp-1. backup org cam_info*/
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+
+	cam_a_used = _rtw_sec_camid_is_used(cam_ctl, cam_id_a);
+	cam_b_used = _rtw_sec_camid_is_used(cam_ctl, cam_id_b);
+
+	if (cam_a_used)
+		memcpy(&cache_a, &dvobj->cam_cache[cam_id_a], sizeof(struct sec_cam_ent));
+
+	if (cam_b_used)
+		memcpy(&cache_b, &dvobj->cam_cache[cam_id_b], sizeof(struct sec_cam_ent));
+
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+
+	/*setp-2. clean cam_info*/
+	if (cam_a_used) {
+		rtw_camid_free(adapter, cam_id_a);
+		clear_cam_entry(adapter, cam_id_a);
+	}
+	if (cam_b_used) {
+		rtw_camid_free(adapter, cam_id_b);
+		clear_cam_entry(adapter, cam_id_b);
+	}
+
+	/*setp-3. set cam_info*/
+	if (cam_a_used) {
+		write_cam(adapter, cam_id_b, cache_a.ctrl, cache_a.mac, cache_a.key);
+		rtw_camid_set(adapter, cam_id_b);
+	}
+
+	if (cam_b_used) {
+		write_cam(adapter, cam_id_a, cache_b.ctrl, cache_b.mac, cache_b.key);
+		rtw_camid_set(adapter, cam_id_a);
+	}
+}
+
+static s16 rtw_get_empty_cam_entry(_adapter *adapter, u8 start_camid)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	unsigned long irqL;
+	int i;
+	s16 cam_id = -1;
+
+	_enter_critical_bh(&cam_ctl->lock, &irqL);
+	for (i = start_camid; i < cam_ctl->num; i++) {
+		if (false == _rtw_sec_camid_is_used(cam_ctl, i)) {
+			cam_id = i;
+			break;
+		}
+	}
+	_exit_critical_bh(&cam_ctl->lock, &irqL);
+
+	return cam_id;
+}
+void rtw_clean_dk_section(_adapter *adapter)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = dvobj_to_sec_camctl(dvobj);
+	s16 ept_cam_id;
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		if (rtw_sec_camid_is_used(cam_ctl, i)) {
+			ept_cam_id = rtw_get_empty_cam_entry(adapter, 4);
+			if (ept_cam_id > 0)
+				rtw_sec_cam_swap(adapter, i, ept_cam_id);
+		}
+	}
+}
+void rtw_clean_hw_dk_cam(_adapter *adapter)
+{
+	int i;
+
+	for (i = 0; i < 4; i++)
+		rtw_sec_clr_cam_ent(adapter, i);
+		/*_clear_cam_entry(adapter, i);*/
+}
+
+void flush_all_cam_entry(_adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct security_priv *psecpriv = &padapter->securitypriv;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		struct sta_priv	*pstapriv = &padapter->stapriv;
+		struct sta_info		*psta;
+
+		psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
+		if (psta) {
+			if (psta->state & WIFI_AP_STATE) {
+				/*clear cam when ap free per sta_info*/
+			} else
+				rtw_clearstakey_cmd(padapter, psta, false);
+		}
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		int cam_id = -1;
+		u8 *addr = adapter_mac_addr(padapter);
+
+		while ((cam_id = rtw_camid_search(padapter, addr, -1, -1)) >= 0) {
+			RTW_PRINT("clear wep or group key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(addr), cam_id);
+			clear_cam_entry(padapter, cam_id);
+			rtw_camid_free(padapter, cam_id);
+		}
+	}
+#else /*NON CONFIG_CONCURRENT_MODE*/
+
+	invalidate_cam_all(padapter);
+	/* clear default key related key search setting */
+	rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8 *)false);
+#endif
+}
+
+#if defined(CONFIG_P2P) && defined(CONFIG_WFD)
+void rtw_process_wfd_ie(_adapter *adapter, u8 *wfd_ie, u8 wfd_ielen, const char *tag)
+{
+	struct wifidirect_info *wdinfo = &adapter->wdinfo;
+
+	u8 *attr_content;
+	u32 attr_contentlen = 0;
+
+	if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
+		return;
+
+	RTW_INFO("[%s] Found WFD IE\n", tag);
+	attr_content = rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, NULL, &attr_contentlen);
+	if (attr_content && attr_contentlen) {
+		wdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16(attr_content + 2);
+		RTW_INFO("[%s] Peer PORT NUM = %d\n", tag, wdinfo->wfd_info->peer_rtsp_ctrlport);
+	}
+}
+
+void rtw_process_wfd_ies(_adapter *adapter, u8 *ies, u8 ies_len, const char *tag)
+{
+	u8 *wfd_ie;
+	u32	wfd_ielen;
+
+	if (!hal_chk_wl_func(adapter, WL_FUNC_MIRACAST))
+		return;
+
+	wfd_ie = rtw_get_wfd_ie(ies, ies_len, NULL, &wfd_ielen);
+	while (wfd_ie) {
+		rtw_process_wfd_ie(adapter, wfd_ie, wfd_ielen, tag);
+		wfd_ie = rtw_get_wfd_ie(wfd_ie + wfd_ielen, (ies + ies_len) - (wfd_ie + wfd_ielen), NULL, &wfd_ielen);
+	}
+}
+#endif /* defined(CONFIG_P2P) && defined(CONFIG_WFD) */
+
+int WMM_param_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs	pIE)
+{
+	/* struct registry_priv	*pregpriv = &padapter->registrypriv; */
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pmlmepriv->qospriv.qos_option == 0) {
+		pmlmeinfo->WMM_enable = 0;
+		return false;
+	}
+
+	if (!memcmp(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element)))
+		return false;
+	else
+		memcpy(&(pmlmeinfo->WMM_param), (pIE->data + 6), sizeof(struct WMM_para_element));
+	pmlmeinfo->WMM_enable = 1;
+	return true;
+}
+
+void WMMOnAssocRsp(_adapter *padapter)
+{
+	u8	ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime;
+	u8	acm_mask;
+	u16	TXOP;
+	u32	acParm, i;
+	u32	edca[4], inx[4];
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+
+	acm_mask = 0;
+
+	if (is_supported_5g(pmlmeext->cur_wireless_mode) ||
+	    (pmlmeext->cur_wireless_mode & WIRELESS_11_24N))
+		aSifsTime = 16;
+	else
+		aSifsTime = 10;
+
+	if (pmlmeinfo->WMM_enable == 0) {
+		padapter->mlmepriv.acm_mask = 0;
+
+		AIFS = aSifsTime + (2 * pmlmeinfo->slotTime);
+
+		if (pmlmeext->cur_wireless_mode & (WIRELESS_11G | WIRELESS_11A)) {
+			ECWMin = 4;
+			ECWMax = 10;
+		} else if (pmlmeext->cur_wireless_mode & WIRELESS_11B) {
+			ECWMin = 5;
+			ECWMax = 10;
+		} else {
+			ECWMin = 4;
+			ECWMax = 10;
+		}
+
+		TXOP = 0;
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm));
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm));
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm));
+
+		ECWMin = 2;
+		ECWMax = 3;
+		TXOP = 0x2f;
+		acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+		rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm));
+	} else {
+		edca[0] = edca[1] = edca[2] = edca[3] = 0;
+
+		for (i = 0; i < 4; i++) {
+			ACI = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 5) & 0x03;
+			ACM = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 4) & 0x01;
+
+			/* AIFS = AIFSN * slot time + SIFS - r2t phy delay */
+			AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) * pmlmeinfo->slotTime + aSifsTime;
+
+			ECWMin = (pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f);
+			ECWMax = (pmlmeinfo->WMM_param.ac_param[i].CW & 0xf0) >> 4;
+			TXOP = le16_to_cpu(pmlmeinfo->WMM_param.ac_param[i].TXOP_limit);
+
+			acParm = AIFS | (ECWMin << 8) | (ECWMax << 12) | (TXOP << 16);
+
+			switch (ACI) {
+			case 0x0:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm));
+				acm_mask |= (ACM ? BIT(1) : 0);
+				edca[XMIT_BE_QUEUE] = acParm;
+				break;
+
+			case 0x1:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm));
+				/* acm_mask |= (ACM? BIT(0):0); */
+				edca[XMIT_BK_QUEUE] = acParm;
+				break;
+
+			case 0x2:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm));
+				acm_mask |= (ACM ? BIT(2) : 0);
+				edca[XMIT_VI_QUEUE] = acParm;
+				break;
+
+			case 0x3:
+				rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm));
+				acm_mask |= (ACM ? BIT(3) : 0);
+				edca[XMIT_VO_QUEUE] = acParm;
+				break;
+			}
+
+			RTW_INFO("WMM(%x): %x, %x\n", ACI, ACM, acParm);
+		}
+
+		if (padapter->registrypriv.acm_method == 1)
+			rtw_hal_set_hwreg(padapter, HW_VAR_ACM_CTRL, (u8 *)(&acm_mask));
+		else
+			padapter->mlmepriv.acm_mask = acm_mask;
+
+		inx[0] = 0;
+		inx[1] = 1;
+		inx[2] = 2;
+		inx[3] = 3;
+
+		if (pregpriv->wifi_spec == 1) {
+			u32	j, tmp, change_inx = false;
+
+			/* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */
+			for (i = 0; i < 4; i++) {
+				for (j = i + 1; j < 4; j++) {
+					/* compare CW and AIFS */
+					if ((edca[j] & 0xFFFF) < (edca[i] & 0xFFFF))
+						change_inx = true;
+					else if ((edca[j] & 0xFFFF) == (edca[i] & 0xFFFF)) {
+						/* compare TXOP */
+						if ((edca[j] >> 16) > (edca[i] >> 16))
+							change_inx = true;
+					}
+
+					if (change_inx) {
+						tmp = edca[i];
+						edca[i] = edca[j];
+						edca[j] = tmp;
+
+						tmp = inx[i];
+						inx[i] = inx[j];
+						inx[j] = tmp;
+
+						change_inx = false;
+					}
+				}
+			}
+		}
+
+		for (i = 0; i < 4; i++) {
+			pxmitpriv->wmm_para_seq[i] = inx[i];
+			RTW_INFO("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]);
+		}
+#ifdef CONFIG_WMMPS
+		if (pmlmeinfo->WMM_param.QoS_info & BIT(7))
+			rtw_hal_set_hwreg(padapter, HW_VAR_UAPSD_TID, NULL);
+#endif
+	}
+}
+
+static void bwmode_update_check(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE)
+{
+	unsigned char	 new_bwmode;
+	unsigned char  new_ch_offset;
+	struct HT_info_element	*pHT_info;
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+	u8	cbw40_enable = 0;
+
+	if (!pIE)
+		return;
+
+	if (phtpriv->ht_option == false)
+		return;
+
+	if (pmlmeext->cur_bwmode >= CHANNEL_WIDTH_80)
+		return;
+
+	if (pIE->Length > sizeof(struct HT_info_element))
+		return;
+
+	pHT_info = (struct HT_info_element *)pIE->data;
+
+	if (hal_chk_bw_cap(padapter, BW_CAP_40M)) {
+		if (pmlmeext->cur_channel > 14) {
+			if (REGSTY_IS_BW_5G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40))
+				cbw40_enable = 1;
+		} else {
+			if (REGSTY_IS_BW_2G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_40))
+				cbw40_enable = 1;
+		}
+	}
+
+	if ((pHT_info->infos[0] & BIT(2)) && cbw40_enable) {
+		new_bwmode = CHANNEL_WIDTH_40;
+
+		switch (pHT_info->infos[0] & 0x3) {
+		case 1:
+			new_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+			break;
+
+		case 3:
+			new_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+			break;
+
+		default:
+			new_bwmode = CHANNEL_WIDTH_20;
+			new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			break;
+		}
+	} else {
+		new_bwmode = CHANNEL_WIDTH_20;
+		new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	}
+
+
+	if ((new_bwmode != pmlmeext->cur_bwmode || new_ch_offset != pmlmeext->cur_ch_offset)
+	    && new_bwmode < pmlmeext->cur_bwmode
+	   ) {
+		pmlmeinfo->bwmode_updated = true;
+
+		pmlmeext->cur_bwmode = new_bwmode;
+		pmlmeext->cur_ch_offset = new_ch_offset;
+
+		/* update HT info also */
+		HT_info_handler(padapter, pIE);
+	} else
+		pmlmeinfo->bwmode_updated = false;
+
+
+	if (pmlmeinfo->bwmode_updated) {
+		struct sta_info *psta;
+		WLAN_BSSID_EX	*cur_network = &(pmlmeinfo->network);
+		struct sta_priv	*pstapriv = &padapter->stapriv;
+
+		/* set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
+
+
+		/* update ap's stainfo */
+		psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
+		if (psta) {
+			struct ht_priv	*phtpriv_sta = &psta->htpriv;
+
+			if (phtpriv_sta->ht_option) {
+				/* bwmode				 */
+				psta->bw_mode = pmlmeext->cur_bwmode;
+				phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;
+			} else {
+				psta->bw_mode = CHANNEL_WIDTH_20;
+				phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			}
+
+			rtw_dm_ra_mask_wk_cmd(padapter, (u8 *)psta);
+		}
+
+		/* pmlmeinfo->bwmode_updated = false; */ /* bwmode_updated done, reset it! */
+	}
+}
+
+void HT_caps_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE)
+{
+	unsigned int	i;
+	u8	rf_type = RF_1T1R;
+	u8	max_AMPDU_len, min_MPDU_spacing;
+	u8	cur_ldpc_cap = 0, cur_stbc_cap = 0, cur_beamform_cap = 0, tx_nss = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	struct hal_spec_t *hal_spec = GET_HAL_SPEC(padapter);
+
+	if (pIE == NULL)
+		return;
+
+	if (phtpriv->ht_option == false)
+		return;
+
+	pmlmeinfo->HT_caps_enable = 1;
+
+	for (i = 0; i < (pIE->Length); i++) {
+		if (i != 2) {
+			/*	Commented by Albert 2010/07/12 */
+			/*	Got the endian issue here. */
+			pmlmeinfo->HT_caps.u.HT_cap[i] &= (pIE->data[i]);
+		} else {
+			/* AMPDU Parameters field */
+
+			/* Get MIN of MAX AMPDU Length Exp */
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (pIE->data[i] & 0x3))
+				max_AMPDU_len = (pIE->data[i] & 0x3);
+			else
+				max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3);
+
+			/* Get MAX of MIN MPDU Start Spacing */
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (pIE->data[i] & 0x1c))
+				min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c);
+			else
+				min_MPDU_spacing = (pIE->data[i] & 0x1c);
+
+			pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para = max_AMPDU_len | min_MPDU_spacing;
+		}
+	}
+
+	/*	Commented by Albert 2010/07/12 */
+	/*	Have to handle the endian issue after copying. */
+	/*	HT_ext_caps didn't be used yet.	 */
+	pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info;
+	pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps = pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps;
+
+	/* update the MCS set */
+	for (i = 0; i < 16; i++)
+		pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= pmlmeext->default_supported_mcs_set[i];
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	tx_nss = rtw_min(rf_type_to_rf_tx_cnt(rf_type), hal_spec->tx_nss_num);
+
+	switch (tx_nss) {
+	case 1:
+		set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R);
+		break;
+	case 2:
+		#ifdef CONFIG_DISABLE_MCS13TO15
+		if (pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 && pregistrypriv->wifi_spec != 1)
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R_13TO15_OFF);
+		else
+		#endif
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R);
+		break;
+	case 3:
+		set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_3R);
+		break;
+	case 4:
+		set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_4R);
+		break;
+	default:
+		RTW_WARN("rf_type:%d or tx_nss:%u is not expected\n", rf_type, hal_spec->tx_nss_num);
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		/* Config STBC setting */
+		if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) && GET_HT_CAP_ELE_RX_STBC(pIE->data)) {
+			SET_FLAG(cur_stbc_cap, STBC_HT_ENABLE_TX);
+			RTW_INFO("Enable HT Tx STBC !\n");
+		}
+		phtpriv->stbc_cap = cur_stbc_cap;
+
+#ifdef CONFIG_BEAMFORMING
+		/* Config Tx beamforming setting */
+		if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE) &&
+		    GET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(pIE->data)) {
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE);
+			/* Shift to BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP*/
+			SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS(pIE->data) << 6);
+		}
+
+		if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE) &&
+		    GET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(pIE->data)) {
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE);
+			/* Shift to BEAMFORMING_HT_BEAMFORMER_STEER_NUM*/
+			SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(pIE->data) << 4);
+		}
+		phtpriv->beamform_cap = cur_beamform_cap;
+		if (cur_beamform_cap)
+			RTW_INFO("AP HT Beamforming Cap = 0x%02X\n", cur_beamform_cap);
+#endif /*CONFIG_BEAMFORMING*/
+	} else {
+		/*WIFI_STATION_STATEorI_ADHOC_STATE or WIFI_ADHOC_MASTER_STATE*/
+		/* Config LDPC Coding Capability */
+		if (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX) && GET_HT_CAP_ELE_LDPC_CAP(pIE->data)) {
+			SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX));
+			RTW_INFO("Enable HT Tx LDPC!\n");
+		}
+		phtpriv->ldpc_cap = cur_ldpc_cap;
+
+		/* Config STBC setting */
+		if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) && GET_HT_CAP_ELE_RX_STBC(pIE->data)) {
+			SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX));
+			RTW_INFO("Enable HT Tx STBC!\n");
+		}
+		phtpriv->stbc_cap = cur_stbc_cap;
+
+#ifdef CONFIG_BEAMFORMING
+#ifdef RTW_BEAMFORMING_VERSION_2
+		/* Config beamforming setting */
+		if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE) &&
+		    GET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(pIE->data)) {
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE);
+			/* Shift to BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP*/
+			SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS(pIE->data) << 6);
+		}
+
+		if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE) &&
+		    GET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(pIE->data)) {
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE);
+			/* Shift to BEAMFORMING_HT_BEAMFORMER_STEER_NUM*/
+			SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(pIE->data) << 4);
+		}
+#else /* !RTW_BEAMFORMING_VERSION_2 */
+		/* Config Tx beamforming setting */
+		if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE) &&
+		    GET_HT_CAP_TXBF_EXPLICIT_COMP_STEERING_CAP(pIE->data)) {
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE);
+			/* Shift to BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP*/
+			SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_CHNL_ESTIMATION_NUM_ANTENNAS(pIE->data) << 6);
+		}
+
+		if (TEST_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE) &&
+		    GET_HT_CAP_TXBF_EXPLICIT_COMP_FEEDBACK_CAP(pIE->data)) {
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE);
+			/* Shift to BEAMFORMING_HT_BEAMFORMER_STEER_NUM*/
+			SET_FLAG(cur_beamform_cap, GET_HT_CAP_TXBF_COMP_STEERING_NUM_ANTENNAS(pIE->data) << 4);
+		}
+#endif /* !RTW_BEAMFORMING_VERSION_2 */
+		phtpriv->beamform_cap = cur_beamform_cap;
+		if (cur_beamform_cap)
+			RTW_INFO("Client HT Beamforming Cap = 0x%02X\n", cur_beamform_cap);
+#endif /*CONFIG_BEAMFORMING*/
+	}
+}
+
+void HT_info_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+
+	if (pIE == NULL)
+		return;
+
+	if (phtpriv->ht_option == false)
+		return;
+
+
+	if (pIE->Length > sizeof(struct HT_info_element))
+		return;
+
+	pmlmeinfo->HT_info_enable = 1;
+	memcpy(&(pmlmeinfo->HT_info), pIE->data, pIE->Length);
+	return;
+}
+
+void HTOnAssocRsp(_adapter *padapter)
+{
+	unsigned char		max_AMPDU_len;
+	unsigned char		min_MPDU_spacing;
+	/* struct registry_priv	 *pregpriv = &padapter->registrypriv; */
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	RTW_INFO("%s\n", __func__);
+
+	if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable))
+		pmlmeinfo->HT_enable = 1;
+	else {
+		pmlmeinfo->HT_enable = 0;
+		/* set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
+		return;
+	}
+
+	/* handle A-MPDU parameter field */
+	/*
+		AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+		AMPDU_para [4:2]:Min MPDU Start Spacing
+	*/
+	max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
+
+	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));
+}
+
+void ERP_IE_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pIE->Length > 1)
+		return;
+
+	pmlmeinfo->ERP_enable = 1;
+	memcpy(&(pmlmeinfo->ERP_IE), pIE->data, pIE->Length);
+}
+
+void VCS_update(_adapter *padapter, struct sta_info *psta)
+{
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	switch (pregpriv->vrtl_carrier_sense) { /* 0:off 1:on 2:auto */
+	case 0: /* off */
+		psta->rtsen = 0;
+		psta->cts2self = 0;
+		break;
+
+	case 1: /* on */
+		if (pregpriv->vcs_type == 1) { /* 1:RTS/CTS 2:CTS to self */
+			psta->rtsen = 1;
+			psta->cts2self = 0;
+		} else {
+			psta->rtsen = 0;
+			psta->cts2self = 1;
+		}
+		break;
+
+	case 2: /* auto */
+	default:
+		if (((pmlmeinfo->ERP_enable) && (pmlmeinfo->ERP_IE & BIT(1)))
+			/*||(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT)*/
+		) {
+			if (pregpriv->vcs_type == 1) {
+				psta->rtsen = 1;
+				psta->cts2self = 0;
+			} else {
+				psta->rtsen = 0;
+				psta->cts2self = 1;
+			}
+		} else {
+			psta->rtsen = 0;
+			psta->cts2self = 0;
+		}
+		break;
+	}
+}
+
+void	update_ldpc_stbc_cap(struct sta_info *psta)
+{
+	if (psta->htpriv.ht_option) {
+		if (TEST_FLAG(psta->htpriv.ldpc_cap, LDPC_HT_ENABLE_TX))
+			psta->ldpc = 1;
+
+		if (TEST_FLAG(psta->htpriv.stbc_cap, STBC_HT_ENABLE_TX))
+			psta->stbc = 1;
+	} else {
+		psta->ldpc = 0;
+		psta->stbc = 0;
+	}
+}
+
+static int check_ielen(u8 *start, uint len)
+{
+	int left = len;
+	u8 *pos = start;
+	int unknown = 0;
+	u8 id, elen;
+
+	while (left >= 2) {
+		id = *pos++;
+		elen = *pos++;
+		left -= 2;
+
+		if (elen > left) {
+			RTW_INFO("IEEE 802.11 element parse failed (id=%d elen=%d left=%lu)\n",
+					id, elen, (unsigned long) left);
+			return false;
+		}
+		if ((id == WLAN_EID_VENDOR_SPECIFIC) && (elen < 4))
+				return false;
+
+		left -= elen;
+		pos += elen;
+	}
+	if (left)
+		return false;
+
+	return true;
+}
+
+int validate_beacon_len(u8 *pframe, u32 len)
+{
+	u8 ie_offset = _BEACON_IE_OFFSET_ + sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if (len < ie_offset) {
+		RTW_INFO("%s: incorrect beacon length(%d)\n", __func__, len);
+		return false;
+	}
+
+	if (check_ielen(pframe + ie_offset, len - ie_offset) == false)
+		return false;
+
+	return true;
+}
+
+/*
+ * rtw_get_bcn_keys: get beacon keys from recv frame
+ *
+ * TODO:
+ *	WLAN_EID_COUNTRY
+ *	WLAN_EID_ERP_INFO
+ *	WLAN_EID_CHANNEL_SWITCH
+ *	WLAN_EID_PWR_CONSTRAINT
+ */
+int rtw_get_bcn_keys(ADAPTER *Adapter, u8 *pframe, u32 packet_len,
+		     struct beacon_keys *recv_beacon)
+{
+	int left;
+	u16 capability;
+	unsigned char *pos;
+	struct rtw_ieee802_11_elems elems;
+	struct rtw_ieee80211_ht_cap *pht_cap = NULL;
+	struct HT_info_element *pht_info = NULL;
+
+	memset(recv_beacon, 0, sizeof(*recv_beacon));
+
+	/* checking capabilities */
+	capability = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 10));
+
+	/* checking IEs */
+	left = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_;
+	pos = pframe + sizeof(struct rtw_ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_;
+	if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed)
+		return false;
+
+	/* check bw and channel offset */
+	if (elems.ht_capabilities) {
+		if (elems.ht_capabilities_len != sizeof(*pht_cap))
+			return false;
+
+		pht_cap = (struct rtw_ieee80211_ht_cap *) elems.ht_capabilities;
+		recv_beacon->ht_cap_info = le16_to_cpu(pht_cap->cap_info);
+	}
+
+	if (elems.ht_operation) {
+		if (elems.ht_operation_len != sizeof(*pht_info))
+			return false;
+
+		pht_info = (struct HT_info_element *) elems.ht_operation;
+		recv_beacon->ht_info_infos_0_sco = pht_info->infos[0] & 0x03;
+	}
+
+	/* Checking for channel */
+	if (elems.ds_params && elems.ds_params_len == sizeof(recv_beacon->bcn_channel))
+		memcpy(&recv_beacon->bcn_channel, elems.ds_params,
+			    sizeof(recv_beacon->bcn_channel));
+	else if (pht_info)
+		/* In 5G, some ap do not have DSSET IE checking HT info for channel */
+		recv_beacon->bcn_channel = pht_info->primary_channel;
+	else {
+		/* we don't find channel IE, so don't check it */
+		/* RTW_INFO("Oops: %s we don't find channel IE, so don't check it\n", __func__); */
+		recv_beacon->bcn_channel = Adapter->mlmeextpriv.cur_channel;
+	}
+
+	/* checking SSID */
+	if (elems.ssid) {
+		if (elems.ssid_len > sizeof(recv_beacon->ssid))
+			return false;
+
+		memcpy(recv_beacon->ssid, elems.ssid, elems.ssid_len);
+		recv_beacon->ssid_len = elems.ssid_len;
+	} else
+		; /* means hidden ssid */
+
+	/* checking RSN first */
+	if (elems.rsn_ie && elems.rsn_ie_len) {
+		recv_beacon->encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+		rtw_parse_wpa2_ie(elems.rsn_ie - 2, elems.rsn_ie_len + 2,
+			&recv_beacon->group_cipher, &recv_beacon->pairwise_cipher,
+				  &recv_beacon->is_8021x);
+	}
+	/* checking WPA secon */
+	else if (elems.wpa_ie && elems.wpa_ie_len) {
+		recv_beacon->encryp_protocol = ENCRYP_PROTOCOL_WPA;
+		rtw_parse_wpa_ie(elems.wpa_ie - 2, elems.wpa_ie_len + 2,
+			&recv_beacon->group_cipher, &recv_beacon->pairwise_cipher,
+				 &recv_beacon->is_8021x);
+	} else if (capability & BIT(4))
+		recv_beacon->encryp_protocol = ENCRYP_PROTOCOL_WEP;
+
+	return true;
+}
+
+void rtw_dump_bcn_keys(struct beacon_keys *recv_beacon)
+{
+	int i;
+	char *p;
+	u8 ssid[IW_ESSID_MAX_SIZE + 1];
+
+	memcpy(ssid, recv_beacon->ssid, recv_beacon->ssid_len);
+	ssid[recv_beacon->ssid_len] = '\0';
+
+	RTW_INFO("%s: ssid = %s\n", __func__, ssid);
+	RTW_INFO("%s: channel = %x\n", __func__, recv_beacon->bcn_channel);
+	RTW_INFO("%s: ht_cap = %x\n", __func__,	recv_beacon->ht_cap_info);
+	RTW_INFO("%s: ht_info_infos_0_sco = %x\n", __func__, recv_beacon->ht_info_infos_0_sco);
+	RTW_INFO("%s: sec=%d, group = %x, pair = %x, 8021X = %x\n", __func__,
+		 recv_beacon->encryp_protocol, recv_beacon->group_cipher,
+		 recv_beacon->pairwise_cipher, recv_beacon->is_8021x);
+}
+
+int rtw_check_bcn_info(ADAPTER *Adapter, u8 *pframe, u32 packet_len)
+{
+	unsigned int len;
+	u8 *pbssid = GetAddr3Ptr(pframe);
+	struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
+	struct wlan_network *cur_network = &(Adapter->mlmepriv.cur_network);
+	struct beacon_keys recv_beacon;
+
+	if (is_client_associated_to_ap(Adapter) == false)
+		return true;
+
+	len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ) {
+		RTW_WARN("%s IE too long for survey event\n", __func__);
+		return _FAIL;
+	}
+
+	if (!memcmp(cur_network->network.MacAddress, pbssid, 6) == false) {
+		RTW_WARN("Oops: rtw_check_network_encrypt linked but recv other bssid bcn\n" MAC_FMT MAC_FMT,
+			MAC_ARG(pbssid), MAC_ARG(cur_network->network.MacAddress));
+		return true;
+	}
+
+	if (rtw_get_bcn_keys(Adapter, pframe, packet_len, &recv_beacon) == false)
+		return true; /* parsing failed => broken IE */
+
+	/* don't care hidden ssid, use current beacon ssid directly */
+	if (recv_beacon.ssid_len == 0) {
+		memcpy(recv_beacon.ssid, pmlmepriv->cur_beacon_keys.ssid,
+			    pmlmepriv->cur_beacon_keys.ssid_len);
+		recv_beacon.ssid_len = pmlmepriv->cur_beacon_keys.ssid_len;
+	}
+
+	if (!memcmp(&recv_beacon, &pmlmepriv->cur_beacon_keys, sizeof(recv_beacon)))
+		pmlmepriv->new_beacon_cnts = 0;
+	else if ((pmlmepriv->new_beacon_cnts == 0) ||
+		!memcmp(&recv_beacon, &pmlmepriv->new_beacon_keys, sizeof(recv_beacon)) == false) {
+		RTW_DBG("%s: start new beacon (seq=%d)\n", __func__, GetSequence(pframe));
+
+		if (pmlmepriv->new_beacon_cnts == 0) {
+			RTW_ERR("%s: cur beacon key\n", __func__);
+			RTW_DBG_EXPR(rtw_dump_bcn_keys(&pmlmepriv->cur_beacon_keys));
+		}
+
+		RTW_DBG("%s: new beacon key\n", __func__);
+		RTW_DBG_EXPR(rtw_dump_bcn_keys(&recv_beacon));
+
+		memcpy(&pmlmepriv->new_beacon_keys, &recv_beacon, sizeof(recv_beacon));
+		pmlmepriv->new_beacon_cnts = 1;
+	} else {
+		RTW_DBG("%s: new beacon again (seq=%d)\n", __func__, GetSequence(pframe));
+		pmlmepriv->new_beacon_cnts++;
+	}
+
+	/* if counter >= max, it means beacon is changed really */
+	if (pmlmepriv->new_beacon_cnts >= new_bcn_max) {
+		/* check bw mode change only? */
+		pmlmepriv->cur_beacon_keys.ht_cap_info = recv_beacon.ht_cap_info;
+		pmlmepriv->cur_beacon_keys.ht_info_infos_0_sco = recv_beacon.ht_info_infos_0_sco;
+		if (!memcmp(&recv_beacon, &pmlmepriv->cur_beacon_keys,
+				sizeof(recv_beacon)) == false) {
+			/* beacon is changed, have to do disconnect/connect */
+			RTW_WARN("%s: new beacon occur!!\n", __func__);
+			return _FAIL;
+		}
+
+		RTW_INFO("%s bw mode change\n", __func__);
+		RTW_INFO("%s bcn now: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
+			 cur_network->BcnInfo.ht_cap_info,
+			 cur_network->BcnInfo.ht_info_infos_0);
+
+		cur_network->BcnInfo.ht_cap_info = recv_beacon.ht_cap_info;
+		cur_network->BcnInfo.ht_info_infos_0 =
+			(cur_network->BcnInfo.ht_info_infos_0 & (~0x03)) |
+			recv_beacon.ht_info_infos_0_sco;
+
+		RTW_INFO("%s bcn link: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
+			 cur_network->BcnInfo.ht_cap_info,
+			 cur_network->BcnInfo.ht_info_infos_0);
+
+		memcpy(&pmlmepriv->cur_beacon_keys, &recv_beacon, sizeof(recv_beacon));
+		pmlmepriv->new_beacon_cnts = 0;
+	}
+
+	return _SUCCESS;
+}
+
+void update_beacon_info(_adapter *padapter, u8 *pframe, uint pkt_len, struct sta_info *psta)
+{
+	unsigned int i;
+	unsigned int len;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+
+#ifdef CONFIG_TDLS
+	struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
+	u8 tdls_prohibited[] = { 0x00, 0x00, 0x00, 0x00, 0x10 }; /* bit(38): TDLS_prohibited */
+#endif /* CONFIG_TDLS */
+
+	len = pkt_len - (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN);
+
+	for (i = 0; i < len;) {
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN) + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			/* to update WMM paramter set while receiving beacon */
+			if (!memcmp(pIE->data, WMM_PARA_OUI, 6) && pIE->Length == WLAN_WMM_LEN)	/* WMM */
+				if (WMM_param_handler(padapter, pIE))
+					report_wmm_edca_update(padapter);
+			break;
+		case _HT_EXTRA_INFO_IE_:	/* HT info */
+			/* HT_info_handler(padapter, pIE); */
+			bwmode_update_check(padapter, pIE);
+			break;
+		case _ERPINFO_IE_:
+			ERP_IE_handler(padapter, pIE);
+			VCS_update(padapter, psta);
+			break;
+
+#ifdef CONFIG_TDLS
+		case _EXT_CAP_IE_:
+			if (check_ap_tdls_prohibited(pIE->data, pIE->Length))
+				ptdlsinfo->ap_prohibited = true;
+			if (check_ap_tdls_ch_switching_prohibited(pIE->data, pIE->Length))
+				ptdlsinfo->ch_switch_prohibited = true;
+			break;
+#endif /* CONFIG_TDLS */
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+}
+
+#ifdef CONFIG_DFS
+void process_csa_ie(_adapter *padapter, u8 *pframe, uint pkt_len)
+{
+	unsigned int i;
+	unsigned int len;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	u8 new_ch_no = 0;
+
+	if (padapter->mlmepriv.handle_dfs)
+		return;
+
+	len = pkt_len - (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN);
+
+	for (i = 0; i < len;) {
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN) + i);
+
+		switch (pIE->ElementID) {
+		case _CH_SWTICH_ANNOUNCE_:
+			padapter->mlmepriv.handle_dfs = true;
+			memcpy(&new_ch_no, pIE->data + 1, 1);
+			rtw_set_csa_cmd(padapter, new_ch_no);
+			break;
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+}
+#endif /* CONFIG_DFS */
+
+unsigned int is_ap_in_tkip(_adapter *padapter)
+{
+	u32 i;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*cur_network = &(pmlmeinfo->network);
+
+	if (rtw_get_capability((WLAN_BSSID_EX *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;) {
+			pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i);
+
+			switch (pIE->ElementID) {
+			case _VENDOR_SPECIFIC_IE_:
+				if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) && (!memcmp((pIE->data + 12), WPA_TKIP_CIPHER, 4)))
+					return true;
+				break;
+
+			case _RSN_IE_2_:
+				if (!memcmp((pIE->data + 8), RSN_TKIP_CIPHER, 4))
+					return true;
+
+			default:
+				break;
+			}
+
+			i += (pIE->Length + 2);
+		}
+
+		return false;
+	} else
+		return false;
+
+}
+
+unsigned int should_forbid_n_rate(_adapter *padapter)
+{
+	u32 i;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	WLAN_BSSID_EX  *cur_network = &pmlmepriv->cur_network.network;
+
+	if (rtw_get_capability((WLAN_BSSID_EX *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = sizeof(NDIS_802_11_FIXED_IEs); i < cur_network->IELength;) {
+			pIE = (PNDIS_802_11_VARIABLE_IEs)(cur_network->IEs + i);
+
+			switch (pIE->ElementID) {
+			case _VENDOR_SPECIFIC_IE_:
+				if (!memcmp(pIE->data, RTW_WPA_OUI, 4) &&
+				    ((!memcmp((pIE->data + 12), WPA_CIPHER_SUITE_CCMP, 4)) ||
+				     (!memcmp((pIE->data + 16), WPA_CIPHER_SUITE_CCMP, 4))))
+					return false;
+				break;
+
+			case _RSN_IE_2_:
+				if ((!memcmp((pIE->data + 8), RSN_CIPHER_SUITE_CCMP, 4))  ||
+				    (!memcmp((pIE->data + 12), RSN_CIPHER_SUITE_CCMP, 4)))
+					return false;
+
+			default:
+				break;
+			}
+
+			i += (pIE->Length + 2);
+		}
+
+		return true;
+	} else
+		return false;
+
+}
+
+
+unsigned int is_ap_in_wep(_adapter *padapter)
+{
+	u32 i;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*cur_network = &(pmlmeinfo->network);
+
+	if (rtw_get_capability((WLAN_BSSID_EX *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;) {
+			pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i);
+
+			switch (pIE->ElementID) {
+			case _VENDOR_SPECIFIC_IE_:
+				if (!memcmp(pIE->data, RTW_WPA_OUI, 4))
+					return false;
+				break;
+
+			case _RSN_IE_2_:
+				return false;
+
+			default:
+				break;
+			}
+
+			i += (pIE->Length + 2);
+		}
+
+		return true;
+	} else
+		return false;
+
+}
+
+int wifirate2_ratetbl_inx(unsigned char rate)
+{
+	int	inx = 0;
+	rate = rate & 0x7f;
+
+	switch (rate) {
+	case 54*2:
+		inx = 11;
+		break;
+
+	case 48*2:
+		inx = 10;
+		break;
+
+	case 36*2:
+		inx = 9;
+		break;
+
+	case 24*2:
+		inx = 8;
+		break;
+
+	case 18*2:
+		inx = 7;
+		break;
+
+	case 12*2:
+		inx = 6;
+		break;
+
+	case 9*2:
+		inx = 5;
+		break;
+
+	case 6*2:
+		inx = 4;
+		break;
+
+	case 11*2:
+		inx = 3;
+		break;
+	case 11:
+		inx = 2;
+		break;
+
+	case 2*2:
+		inx = 1;
+		break;
+
+	case 1*2:
+		inx = 0;
+		break;
+
+	}
+	return inx;
+}
+
+unsigned int update_basic_rate(unsigned char *ptn, unsigned int ptn_sz)
+{
+	unsigned int i, num_of_rate;
+	unsigned int mask = 0;
+
+	num_of_rate = (ptn_sz > NumRates) ? NumRates : ptn_sz;
+
+	for (i = 0; i < num_of_rate; i++) {
+		if ((*(ptn + i)) & 0x80)
+			mask |= 0x1 << wifirate2_ratetbl_inx(*(ptn + i));
+	}
+	return mask;
+}
+
+unsigned int update_supported_rate(unsigned char *ptn, unsigned int ptn_sz)
+{
+	unsigned int i, num_of_rate;
+	unsigned int mask = 0;
+
+	num_of_rate = (ptn_sz > NumRates) ? NumRates : ptn_sz;
+
+	for (i = 0; i < num_of_rate; i++)
+		mask |= 0x1 << wifirate2_ratetbl_inx(*(ptn + i));
+
+	return mask;
+}
+
+int support_short_GI(_adapter *padapter, struct HT_caps_element *pHT_caps, u8 bwmode)
+{
+	unsigned char					bit_offset;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (!(pmlmeinfo->HT_enable))
+		return _FAIL;
+
+	bit_offset = (bwmode & CHANNEL_WIDTH_40) ? 6 : 5;
+
+	if (le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info) & (0x1 << bit_offset))
+		return _SUCCESS;
+	else
+		return _FAIL;
+}
+
+unsigned char get_highest_rate_idx(u32 mask)
+{
+	int i;
+	unsigned char rate_idx = 0;
+
+	for (i = 31; i >= 0; i--) {
+		if (mask & BIT(i)) {
+			rate_idx = i;
+			break;
+		}
+	}
+
+	return rate_idx;
+}
+
+void Update_RA_Entry(_adapter *padapter, struct sta_info *psta)
+{
+	rtw_hal_update_ra_mask(psta, psta->rssi_level, true);
+}
+
+void set_sta_rate(_adapter *padapter, struct sta_info *psta)
+{
+	/* rate adaptive	 */
+	rtw_hal_update_ra_mask(psta, psta->rssi_level, true);
+}
+
+/* Update RRSR and Rate for USERATE */
+void update_tx_basic_rate(_adapter *padapter, u8 wirelessmode)
+{
+	NDIS_802_11_RATES_EX	supported_rates;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
+
+	/*	Added by Albert 2011/03/22 */
+	/*	In the P2P mode, the driver should not support the b mode. */
+	/*	So, the Tx packet shouldn't use the CCK rate */
+	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
+		return;
+#endif /* CONFIG_P2P */
+#ifdef CONFIG_INTEL_WIDI
+	if (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_NONE)
+		return;
+#endif /* CONFIG_INTEL_WIDI */
+
+	memset(supported_rates, 0, NDIS_802_11_LENGTH_RATES_EX);
+
+	/* clear B mod if current channel is in 5G band, avoid tx cck rate in 5G band. */
+	if (pmlmeext->cur_channel > 14)
+		wirelessmode &= ~(WIRELESS_11B);
+
+	if ((wirelessmode & WIRELESS_11B) && (wirelessmode == WIRELESS_11B))
+		memcpy(supported_rates, rtw_basic_rate_cck, 4);
+	else if (wirelessmode & WIRELESS_11B)
+		memcpy(supported_rates, rtw_basic_rate_mix, 7);
+	else
+		memcpy(supported_rates, rtw_basic_rate_ofdm, 3);
+
+	if (wirelessmode & WIRELESS_11B)
+		update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB);
+	else
+		update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, supported_rates);
+}
+
+unsigned char check_assoc_AP(u8 *pframe, uint len)
+{
+	unsigned int	i;
+	PNDIS_802_11_VARIABLE_IEs	pIE;
+
+	for (i = sizeof(NDIS_802_11_FIXED_IEs); i < len;) {
+		pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			if ((!memcmp(pIE->data, ATHEROS_OUI1, 3)) || (!memcmp(pIE->data, ATHEROS_OUI2, 3))) {
+				RTW_INFO("link to Artheros AP\n");
+				return HT_IOT_PEER_ATHEROS;
+			} else if ((!memcmp(pIE->data, BROADCOM_OUI1, 3))
+				   || (!memcmp(pIE->data, BROADCOM_OUI2, 3))
+				|| (!memcmp(pIE->data, BROADCOM_OUI3, 3))) {
+				RTW_INFO("link to Broadcom AP\n");
+				return HT_IOT_PEER_BROADCOM;
+			} else if (!memcmp(pIE->data, MARVELL_OUI, 3)) {
+				RTW_INFO("link to Marvell AP\n");
+				return HT_IOT_PEER_MARVELL;
+			} else if (!memcmp(pIE->data, RALINK_OUI, 3)) {
+				RTW_INFO("link to Ralink AP\n");
+				return HT_IOT_PEER_RALINK;
+			} else if (!memcmp(pIE->data, CISCO_OUI, 3)) {
+				RTW_INFO("link to Cisco AP\n");
+				return HT_IOT_PEER_CISCO;
+			} else if (!memcmp(pIE->data, REALTEK_OUI, 3)) {
+				u32	Vender = HT_IOT_PEER_REALTEK;
+
+				if (pIE->Length >= 5) {
+					if (pIE->data[4] == 1) {
+						/* if(pIE->data[5] & RT_HT_CAP_USE_LONG_PREAMBLE) */
+						/*	bssDesc->BssHT.RT2RT_HT_Mode |= RT_HT_CAP_USE_LONG_PREAMBLE; */
+
+						if (pIE->data[5] & RT_HT_CAP_USE_92SE) {
+							/* bssDesc->BssHT.RT2RT_HT_Mode |= RT_HT_CAP_USE_92SE; */
+							Vender = HT_IOT_PEER_REALTEK_92SE;
+						}
+					}
+
+					if (pIE->data[5] & RT_HT_CAP_USE_SOFTAP)
+						Vender = HT_IOT_PEER_REALTEK_SOFTAP;
+
+					if (pIE->data[4] == 2) {
+						if (pIE->data[6] & RT_HT_CAP_USE_JAGUAR_BCUT) {
+							Vender = HT_IOT_PEER_REALTEK_JAGUAR_BCUTAP;
+							RTW_INFO("link to Realtek JAGUAR_BCUTAP\n");
+						}
+						if (pIE->data[6] & RT_HT_CAP_USE_JAGUAR_CCUT) {
+							Vender = HT_IOT_PEER_REALTEK_JAGUAR_CCUTAP;
+							RTW_INFO("link to Realtek JAGUAR_CCUTAP\n");
+						}
+					}
+				}
+
+				RTW_INFO("link to Realtek AP\n");
+				return Vender;
+			} else if (!memcmp(pIE->data, AIRGOCAP_OUI, 3)) {
+				RTW_INFO("link to Airgo Cap\n");
+				return HT_IOT_PEER_AIRGO;
+			} else
+				break;
+
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+	RTW_INFO("link to new AP\n");
+	return HT_IOT_PEER_UNKNOWN;
+}
+
+void update_capinfo(PADAPTER Adapter, u16 updateCap)
+{
+	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	bool		ShortPreamble;
+
+	/* Check preamble mode, 2005.01.06, by rcnjko. */
+	/* Mark to update preamble value forever, 2008.03.18 by lanhsin */
+	/* if( pMgntInfo->RegPreambleMode == PREAMBLE_AUTO ) */
+	{
+
+		if (updateCap & cShortPreamble) {
+			/* Short Preamble */
+			if (pmlmeinfo->preamble_mode != PREAMBLE_SHORT) { /* PREAMBLE_LONG or PREAMBLE_AUTO */
+				ShortPreamble = true;
+				pmlmeinfo->preamble_mode = PREAMBLE_SHORT;
+				rtw_hal_set_hwreg(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble);
+			}
+		} else {
+			/* Long Preamble */
+			if (pmlmeinfo->preamble_mode != PREAMBLE_LONG) { /* PREAMBLE_SHORT or PREAMBLE_AUTO */
+				ShortPreamble = false;
+				pmlmeinfo->preamble_mode = PREAMBLE_LONG;
+				rtw_hal_set_hwreg(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble);
+			}
+		}
+	}
+
+	if (updateCap & cIBSS) {
+		/* Filen: See 802.11-2007 p.91 */
+		pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+	} else {
+		/* Filen: See 802.11-2007 p.90 */
+		if (pmlmeext->cur_wireless_mode & (WIRELESS_11_24N | WIRELESS_11A | WIRELESS_11_5N | WIRELESS_11AC))
+			pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+		else if (pmlmeext->cur_wireless_mode & (WIRELESS_11G)) {
+			if ((updateCap & cShortSlotTime) /* && (!(pMgntInfo->pHTInfo->RT2RT_HT_Mode & RT_HT_CAP_USE_LONG_PREAMBLE)) */) {
+				/* Short Slot Time */
+				pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+			} else {
+				/* Long Slot Time */
+				pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+			}
+		} else {
+			/* B Mode */
+			pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+		}
+	}
+
+	rtw_hal_set_hwreg(Adapter, HW_VAR_SLOT_TIME, &pmlmeinfo->slotTime);
+
+}
+
+/*
+* set adapter.mlmeextpriv.mlmext_info.HT_enable
+* set adapter.mlmeextpriv.cur_wireless_mode
+* set SIFS register
+* set mgmt tx rate
+*/
+void update_wireless_mode(_adapter *padapter)
+{
+	int ratelen, network_type = 0;
+	u32 SIFS_Timer;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	WLAN_BSSID_EX		*cur_network = &(pmlmeinfo->network);
+	unsigned char			*rate = cur_network->SupportedRates;
+#ifdef CONFIG_P2P
+	struct wifidirect_info	*pwdinfo = &(padapter->wdinfo);
+#endif /* CONFIG_P2P */
+
+	ratelen = rtw_get_rateset_len(cur_network->SupportedRates);
+
+	if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable))
+		pmlmeinfo->HT_enable = 1;
+
+	if (pmlmeext->cur_channel > 14) {
+		if (pmlmeinfo->VHT_enable)
+			network_type = WIRELESS_11AC;
+		else if (pmlmeinfo->HT_enable)
+			network_type = WIRELESS_11_5N;
+
+		network_type |= WIRELESS_11A;
+	} else {
+		if (pmlmeinfo->VHT_enable)
+			network_type = WIRELESS_11AC;
+		else if (pmlmeinfo->HT_enable)
+			network_type = WIRELESS_11_24N;
+
+		if ((cckratesonly_included(rate, ratelen)))
+			network_type |= WIRELESS_11B;
+		else if ((cckrates_included(rate, ratelen)))
+			network_type |= WIRELESS_11BG;
+		else
+			network_type |= WIRELESS_11G;
+	}
+
+	pmlmeext->cur_wireless_mode = network_type & padapter->registrypriv.wireless_mode;
+
+	SIFS_Timer = 0x0a0a0808; /* 0x0808->for CCK, 0x0a0a->for OFDM
+                              * change this value if having IOT issues. */
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_RESP_SIFS, (u8 *)&SIFS_Timer);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_WIRELESS_MODE, (u8 *)&(pmlmeext->cur_wireless_mode));
+
+	if ((pmlmeext->cur_wireless_mode & WIRELESS_11B)
+		#ifdef CONFIG_P2P
+		&& (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)
+			#ifdef CONFIG_IOCTL_CFG80211
+			|| !rtw_cfg80211_iface_has_p2p_group_cap(padapter)
+			#endif
+			)
+		#endif
+	)
+		update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB);
+	else
+		update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB);
+}
+
+void update_sta_basic_rate(struct sta_info *psta, u8 wireless_mode)
+{
+	if (IsSupportedTxCCK(wireless_mode)) {
+		/* Only B, B/G, and B/G/N AP could use CCK rate */
+		memcpy(psta->bssrateset, rtw_basic_rate_cck, 4);
+		psta->bssratelen = 4;
+	} else {
+		memcpy(psta->bssrateset, rtw_basic_rate_ofdm, 3);
+		psta->bssratelen = 3;
+	}
+}
+
+int rtw_ies_get_supported_rate(u8 *ies, uint ies_len, u8 *rate_set, u8 *rate_num)
+{
+	u8 *ie;
+	unsigned int ie_len;
+
+	if (!rate_set || !rate_num)
+		return false;
+
+	*rate_num = 0;
+
+	ie = rtw_get_ie(ies, _SUPPORTEDRATES_IE_, &ie_len, ies_len);
+	if (ie == NULL)
+		goto ext_rate;
+
+	memcpy(rate_set, ie + 2, ie_len);
+	*rate_num = ie_len;
+
+ext_rate:
+	ie = rtw_get_ie(ies, _EXT_SUPPORTEDRATES_IE_, &ie_len, ies_len);
+	if (ie) {
+		memcpy(rate_set + *rate_num, ie + 2, ie_len);
+		*rate_num += ie_len;
+	}
+
+	if (*rate_num == 0)
+		return _FAIL;
+
+	if (0) {
+		int i;
+
+		for (i = 0; i < *rate_num; i++)
+			RTW_INFO("rate:0x%02x\n", *(rate_set + i));
+	}
+
+	return _SUCCESS;
+}
+
+void process_addba_req(_adapter *padapter, u8 *paddba_req, u8 *addr)
+{
+	struct sta_info *psta;
+	u16 tid, start_seq, param;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct ADDBA_request	*preq = (struct ADDBA_request *)paddba_req;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 size, accept = false;
+
+	psta = rtw_get_stainfo(pstapriv, addr);
+	if (!psta)
+		goto exit;
+
+	start_seq = le16_to_cpu(preq->BA_starting_seqctrl) >> 4;
+
+	param = le16_to_cpu(preq->BA_para_set);
+	tid = (param >> 2) & 0x0f;
+
+
+	accept = rtw_rx_ampdu_is_accept(padapter);
+	size = rtw_rx_ampdu_size(padapter);
+
+	if (accept)
+		rtw_addbarsp_cmd(padapter, addr, tid, 0, size, start_seq);
+	else
+		rtw_addbarsp_cmd(padapter, addr, tid, 37, size, start_seq); /* reject ADDBA Req */
+
+exit:
+	return;
+}
+
+void update_TSF(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len)
+{
+	u8 *pIE;
+	__le32 *pbuf;
+
+	pIE = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	pbuf = (__le32 *)pIE;
+
+	pmlmeext->TSFValue = le32_to_cpu(*(pbuf + 1));
+
+	pmlmeext->TSFValue = pmlmeext->TSFValue << 32;
+
+	pmlmeext->TSFValue |= le32_to_cpu(*pbuf);
+}
+
+void correct_TSF(_adapter *padapter, struct mlme_ext_priv *pmlmeext)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_CORRECT_TSF, NULL);
+}
+
+void adaptive_early_32k(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len)
+{
+	int i;
+	u8 *pIE;
+	__le32 *pbuf;
+	u64 tsf = 0;
+	u32 delay_ms;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pmlmeext->bcn_cnt++;
+
+	pIE = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	pbuf = (__le32 *)pIE;
+
+	tsf = le32_to_cpu(*(pbuf + 1));
+	tsf = tsf << 32;
+	tsf |= le32_to_cpu(*pbuf);
+
+	/* RTW_INFO("%s(): tsf_upper= 0x%08x, tsf_lower=0x%08x\n", __func__, (u32)(tsf>>32), (u32)tsf); */
+
+	/* delay = (timestamp mod 1024*100)/1000 (unit: ms) */
+	/* delay_ms = do_div(tsf, (pmlmeinfo->bcn_interval*1024))/1000; */
+	delay_ms = rtw_modular64(tsf, (pmlmeinfo->bcn_interval * 1024));
+	delay_ms = delay_ms / 1000;
+
+	if (delay_ms >= 8) {
+		pmlmeext->bcn_delay_cnt[8]++;
+		/* pmlmeext->bcn_delay_ratio[8] = (pmlmeext->bcn_delay_cnt[8] * 100) /pmlmeext->bcn_cnt; */
+	} else {
+		pmlmeext->bcn_delay_cnt[delay_ms]++;
+		/* pmlmeext->bcn_delay_ratio[delay_ms] = (pmlmeext->bcn_delay_cnt[delay_ms] * 100) /pmlmeext->bcn_cnt; */
+	}
+
+	/*
+		RTW_INFO("%s(): (a)bcn_cnt = %d\n", __func__, pmlmeext->bcn_cnt);
+
+
+		for(i=0; i<9; i++)
+		{
+			RTW_INFO("%s():bcn_delay_cnt[%d]=%d,  bcn_delay_ratio[%d]=%d\n", __func__, i,
+				pmlmeext->bcn_delay_cnt[i] , i, pmlmeext->bcn_delay_ratio[i]);
+		}
+	*/
+
+	/* dump for  adaptive_early_32k */
+	if (pmlmeext->bcn_cnt > 100 && (pmlmeext->adaptive_tsf_done)) {
+		u8 ratio_20_delay, ratio_80_delay;
+		u8 DrvBcnEarly, DrvBcnTimeOut;
+
+		ratio_20_delay = 0;
+		ratio_80_delay = 0;
+		DrvBcnEarly = 0xff;
+		DrvBcnTimeOut = 0xff;
+
+		RTW_INFO("%s(): bcn_cnt = %d\n", __func__, pmlmeext->bcn_cnt);
+
+		for (i = 0; i < 9; i++) {
+			pmlmeext->bcn_delay_ratio[i] = (pmlmeext->bcn_delay_cnt[i] * 100) / pmlmeext->bcn_cnt;
+
+
+			/* RTW_INFO("%s():bcn_delay_cnt[%d]=%d,  bcn_delay_ratio[%d]=%d\n", __func__, i,  */
+			/*	pmlmeext->bcn_delay_cnt[i] , i, pmlmeext->bcn_delay_ratio[i]); */
+
+			ratio_20_delay += pmlmeext->bcn_delay_ratio[i];
+			ratio_80_delay += pmlmeext->bcn_delay_ratio[i];
+
+			if (ratio_20_delay > 20 && DrvBcnEarly == 0xff) {
+				DrvBcnEarly = i;
+				/* RTW_INFO("%s(): DrvBcnEarly = %d\n", __func__, DrvBcnEarly); */
+			}
+
+			if (ratio_80_delay > 80 && DrvBcnTimeOut == 0xff) {
+				DrvBcnTimeOut = i;
+				/* RTW_INFO("%s(): DrvBcnTimeOut = %d\n", __func__, DrvBcnTimeOut); */
+			}
+
+			/* reset adaptive_early_32k cnt */
+			pmlmeext->bcn_delay_cnt[i] = 0;
+			pmlmeext->bcn_delay_ratio[i] = 0;
+		}
+
+		pmlmeext->DrvBcnEarly = DrvBcnEarly;
+		pmlmeext->DrvBcnTimeOut = DrvBcnTimeOut;
+
+		pmlmeext->bcn_cnt = 0;
+	}
+
+}
+
+
+void beacon_timing_control(_adapter *padapter)
+{
+	rtw_hal_bcn_related_reg_setting(padapter);
+}
+
+#define CONFIG_SHARED_BMC_MACID
+
+void dump_macid_map(void *sel, struct macid_bmp *map, u8 max_num)
+{
+	RTW_PRINT_SEL(sel, "0x%08x\n", map->m0);
+#if (MACID_NUM_SW_LIMIT > 32)
+	if (max_num && max_num > 32)
+		RTW_PRINT_SEL(sel, "0x%08x\n", map->m1);
+#endif
+#if (MACID_NUM_SW_LIMIT > 64)
+	if (max_num && max_num > 64)
+		RTW_PRINT_SEL(sel, "0x%08x\n", map->m2);
+#endif
+#if (MACID_NUM_SW_LIMIT > 96)
+	if (max_num && max_num > 96)
+		RTW_PRINT_SEL(sel, "0x%08x\n", map->m3);
+#endif
+}
+
+inline bool rtw_macid_is_set(struct macid_bmp *map, u8 id)
+{
+	if (id < 32)
+		return map->m0 & BIT(id);
+#if (MACID_NUM_SW_LIMIT > 32)
+	else if (id < 64)
+		return map->m1 & BIT(id - 32);
+#endif
+#if (MACID_NUM_SW_LIMIT > 64)
+	else if (id < 96)
+		return map->m2 & BIT(id - 64);
+#endif
+#if (MACID_NUM_SW_LIMIT > 96)
+	else if (id < 128)
+		return map->m3 & BIT(id - 96);
+#endif
+	else
+		rtw_warn_on(1);
+
+	return 0;
+}
+
+inline void rtw_macid_map_set(struct macid_bmp *map, u8 id)
+{
+	if (id < 32)
+		map->m0 |= BIT(id);
+#if (MACID_NUM_SW_LIMIT > 32)
+	else if (id < 64)
+		map->m1 |= BIT(id - 32);
+#endif
+#if (MACID_NUM_SW_LIMIT > 64)
+	else if (id < 96)
+		map->m2 |= BIT(id - 64);
+#endif
+#if (MACID_NUM_SW_LIMIT > 96)
+	else if (id < 128)
+		map->m3 |= BIT(id - 96);
+#endif
+	else
+		rtw_warn_on(1);
+}
+
+/*Record bc's mac-id and sec-cam-id*/
+inline void rtw_iface_bcmc_id_set(_adapter *padapter, u8 mac_id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
+
+	macid_ctl->iface_bmc[padapter->iface_id] = mac_id;
+}
+inline u8 rtw_iface_bcmc_id_get(_adapter *padapter)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
+
+	return macid_ctl->iface_bmc[padapter->iface_id];
+}
+
+inline void rtw_macid_map_clr(struct macid_bmp *map, u8 id)
+{
+	if (id < 32)
+		map->m0 &= ~BIT(id);
+#if (MACID_NUM_SW_LIMIT > 32)
+	else if (id < 64)
+		map->m1 &= ~BIT(id - 32);
+#endif
+#if (MACID_NUM_SW_LIMIT > 64)
+	else if (id < 96)
+		map->m2 &= ~BIT(id - 64);
+#endif
+#if (MACID_NUM_SW_LIMIT > 96)
+	else if (id < 128)
+		map->m3 &= ~BIT(id - 96);
+#endif
+	else
+		rtw_warn_on(1);
+}
+
+inline bool rtw_macid_is_used(struct macid_ctl_t *macid_ctl, u8 id)
+{
+	return rtw_macid_is_set(&macid_ctl->used, id);
+}
+
+inline bool rtw_macid_is_bmc(struct macid_ctl_t *macid_ctl, u8 id)
+{
+	return rtw_macid_is_set(&macid_ctl->bmc, id);
+}
+
+inline s8 rtw_macid_get_if_g(struct macid_ctl_t *macid_ctl, u8 id)
+{
+	int i;
+
+#ifdef CONFIG_SHARED_BMC_MACID
+	if (rtw_macid_is_bmc(macid_ctl, id)) {
+		for (i = 0; i < CONFIG_IFACE_NUMBER; i++)
+			if (macid_ctl->iface_bmc[i] == id)
+				return i;
+		return -1;
+	}
+#endif
+
+	for (i = 0; i < CONFIG_IFACE_NUMBER; i++) {
+		if (rtw_macid_is_set(&macid_ctl->if_g[i], id))
+			return i;
+	}
+	return -1;
+}
+
+inline s8 rtw_macid_get_ch_g(struct macid_ctl_t *macid_ctl, u8 id)
+{
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		if (rtw_macid_is_set(&macid_ctl->ch_g[i], id))
+			return i;
+	}
+	return -1;
+}
+
+void rtw_alloc_macid(_adapter *padapter, struct sta_info *psta)
+{
+	int i;
+	unsigned long irqL;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
+	struct macid_bmp *used_map = &macid_ctl->used;
+	/* static u8 last_id = 0;  for testing */
+	u8 last_id = 0;
+	u8 is_bc_sta = false;
+
+	if (!memcmp(psta->hwaddr, adapter_mac_addr(padapter), ETH_ALEN)) {
+		psta->mac_id = macid_ctl->num;
+		return;
+	}
+
+	if (!memcmp(psta->hwaddr, bc_addr, ETH_ALEN)) {
+		is_bc_sta = true;
+		rtw_iface_bcmc_id_set(padapter, INVALID_SEC_MAC_CAM_ID);	/*init default value*/
+	}
+
+#ifdef CONFIG_SHARED_BMC_MACID
+	if (is_bc_sta
+#ifdef CONFIG_CONCURRENT_MODE
+	    && (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE) || check_fwstate(&padapter->mlmepriv, WIFI_NULL_STATE))
+#endif
+	   ) {
+		/* use shared broadcast & multicast macid 1 for all ifaces which configure to station mode*/
+		_enter_critical_bh(&macid_ctl->lock, &irqL);
+		rtw_macid_map_set(used_map, 1);
+		rtw_macid_map_set(&macid_ctl->bmc, 1);
+		rtw_macid_map_set(&macid_ctl->if_g[padapter->iface_id], 1);
+		macid_ctl->sta[1] = psta;
+		/* TODO ch_g? */
+		_exit_critical_bh(&macid_ctl->lock, &irqL);
+		i = 1;
+		goto assigned;
+	}
+#endif
+
+#ifdef CONFIG_MCC_MODE
+	if (MCC_EN(padapter)) {
+		if (MLME_IS_AP(padapter) || MLME_IS_GO(padapter))
+			/* GO/AP assign client macid from 8 */
+			last_id = 8;
+	}
+#endif /* CONFIG_MCC_MODE */
+
+	_enter_critical_bh(&macid_ctl->lock, &irqL);
+
+	for (i = last_id; i < macid_ctl->num; i++) {
+#ifdef CONFIG_SHARED_BMC_MACID
+		if (i == 1)
+			continue;
+#endif
+
+#ifdef CONFIG_MCC_MODE
+		/* macid 0/1 reserve for mcc for mgnt queue macid */
+		if (MCC_EN(padapter)) {
+			if (i == MCC_ROLE_STA_GC_MGMT_QUEUE_MACID)
+				continue;
+			if (i == MCC_ROLE_SOFTAP_GO_MGMT_QUEUE_MACID)
+				continue;
+		}
+#endif /* CONFIG_MCC_MODE */
+
+		if (is_bc_sta) {/*for SoftAP's Broadcast sta-info*/
+			/*TODO:non-security AP may allociated macid = 1*/
+			struct cam_ctl_t *cam_ctl = dvobj_to_sec_camctl(dvobj);
+
+			if ((!rtw_macid_is_used(macid_ctl, i)) && (!rtw_sec_camid_is_used(cam_ctl, i)))
+				break;
+		} else {
+			if (!rtw_macid_is_used(macid_ctl, i))
+				break;
+		}
+	}
+
+	if (i < macid_ctl->num) {
+
+		rtw_macid_map_set(used_map, i);
+
+		if (is_bc_sta) {
+			struct cam_ctl_t *cam_ctl = dvobj_to_sec_camctl(dvobj);
+
+			rtw_macid_map_set(&macid_ctl->bmc, i);
+			rtw_iface_bcmc_id_set(padapter, i);
+			rtw_sec_cam_map_set(&cam_ctl->used, i);
+		}
+
+		rtw_macid_map_set(&macid_ctl->if_g[padapter->iface_id], i);
+		macid_ctl->sta[i] = psta;
+
+		/* TODO ch_g? */
+
+		last_id++;
+		last_id %= macid_ctl->num;
+	}
+
+	_exit_critical_bh(&macid_ctl->lock, &irqL);
+
+	if (i >= macid_ctl->num) {
+		psta->mac_id = macid_ctl->num;
+		RTW_ERR(FUNC_ADPT_FMT" if%u, hwaddr:"MAC_FMT" no available macid\n"
+			, FUNC_ADPT_ARG(padapter), padapter->iface_id + 1, MAC_ARG(psta->hwaddr));
+		rtw_warn_on(1);
+		goto exit;
+	} else
+		goto assigned;
+
+assigned:
+	psta->mac_id = i;
+	RTW_INFO(FUNC_ADPT_FMT" if%u, hwaddr:"MAC_FMT" macid:%u\n"
+		, FUNC_ADPT_ARG(padapter), padapter->iface_id + 1, MAC_ARG(psta->hwaddr), psta->mac_id);
+
+exit:
+	return;
+}
+
+void rtw_release_macid(_adapter *padapter, struct sta_info *psta)
+{
+	unsigned long irqL;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
+	u8 is_bc_sta = false;
+
+	if (!memcmp(psta->hwaddr, adapter_mac_addr(padapter), ETH_ALEN))
+		return;
+
+	if (!memcmp(psta->hwaddr, bc_addr, ETH_ALEN))
+		is_bc_sta = true;
+
+#ifdef CONFIG_SHARED_BMC_MACID
+	if (is_bc_sta
+#ifdef CONFIG_CONCURRENT_MODE
+	    && (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE) || check_fwstate(&padapter->mlmepriv, WIFI_NULL_STATE))
+#endif
+	   )
+		return;
+
+	if (psta->mac_id == 1) {
+		RTW_ERR(FUNC_ADPT_FMT" if%u, hwaddr:"MAC_FMT" with macid:%u\n"
+			, FUNC_ADPT_ARG(padapter), padapter->iface_id + 1, MAC_ARG(psta->hwaddr), psta->mac_id);
+		if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE) || check_fwstate(&padapter->mlmepriv, WIFI_NULL_STATE))
+			rtw_warn_on(1);
+		return;
+	}
+#endif
+
+	_enter_critical_bh(&macid_ctl->lock, &irqL);
+
+	if (psta->mac_id < macid_ctl->num) {
+		int i;
+
+		if (!rtw_macid_is_used(macid_ctl, psta->mac_id)) {
+			RTW_ERR(FUNC_ADPT_FMT" if%u, hwaddr:"MAC_FMT" macid:%u not used\n"
+				, FUNC_ADPT_ARG(padapter), padapter->iface_id + 1, MAC_ARG(psta->hwaddr), psta->mac_id);
+			rtw_warn_on(1);
+		}
+
+		rtw_macid_map_clr(&macid_ctl->used, psta->mac_id);
+		rtw_macid_map_clr(&macid_ctl->bmc, psta->mac_id);
+
+		if (is_bc_sta) {
+			struct cam_ctl_t *cam_ctl = dvobj_to_sec_camctl(dvobj);
+			u8 id = rtw_iface_bcmc_id_get(padapter);
+
+			if ((id != INVALID_SEC_MAC_CAM_ID) && (id < cam_ctl->num))
+				rtw_sec_cam_map_clr(&cam_ctl->used, id);
+
+			rtw_iface_bcmc_id_set(padapter, INVALID_SEC_MAC_CAM_ID);
+		}
+
+		for (i = 0; i < CONFIG_IFACE_NUMBER; i++)
+			rtw_macid_map_clr(&macid_ctl->if_g[i], psta->mac_id);
+		for (i = 0; i < 2; i++)
+			rtw_macid_map_clr(&macid_ctl->ch_g[i], psta->mac_id);
+		macid_ctl->sta[psta->mac_id] = NULL;
+	}
+
+	_exit_critical_bh(&macid_ctl->lock, &irqL);
+
+	psta->mac_id = macid_ctl->num;
+}
+
+/* For 8188E RA */
+u8 rtw_search_max_mac_id(_adapter *padapter)
+{
+	u8 max_mac_id = 0;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
+	int i;
+	unsigned long irqL;
+
+	/* TODO: Only search for connected macid? */
+
+	_enter_critical_bh(&macid_ctl->lock, &irqL);
+	for (i = (macid_ctl->num - 1); i > 0 ; i--) {
+		if (rtw_macid_is_used(macid_ctl, i))
+			break;
+	}
+	_exit_critical_bh(&macid_ctl->lock, &irqL);
+	max_mac_id = i;
+
+	return max_mac_id;
+}
+
+inline void rtw_macid_ctl_set_h2c_msr(struct macid_ctl_t *macid_ctl, u8 id, u8 h2c_msr)
+{
+	if (id >= macid_ctl->num) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	macid_ctl->h2c_msr[id] = h2c_msr;
+	if (0)
+		RTW_INFO("macid:%u, h2c_msr:"H2C_MSR_FMT"\n", id, H2C_MSR_ARG(&macid_ctl->h2c_msr[id]));
+}
+
+inline void rtw_macid_ctl_set_bw(struct macid_ctl_t *macid_ctl, u8 id, u8 bw)
+{
+	if (id >= macid_ctl->num) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	macid_ctl->bw[id] = bw;
+	if (0)
+		RTW_INFO("macid:%u, bw:%s\n", id, ch_width_str(macid_ctl->bw[id]));
+}
+
+inline void rtw_macid_ctl_set_vht_en(struct macid_ctl_t *macid_ctl, u8 id, u8 en)
+{
+	if (id >= macid_ctl->num) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	macid_ctl->vht_en[id] = en;
+	if (0)
+		RTW_INFO("macid:%u, vht_en:%u\n", id, macid_ctl->vht_en[id]);
+}
+
+inline void rtw_macid_ctl_set_rate_bmp0(struct macid_ctl_t *macid_ctl, u8 id, u32 bmp)
+{
+	if (id >= macid_ctl->num) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	macid_ctl->rate_bmp0[id] = bmp;
+	if (0)
+		RTW_INFO("macid:%u, rate_bmp0:0x%08X\n", id, macid_ctl->rate_bmp0[id]);
+}
+
+inline void rtw_macid_ctl_set_rate_bmp1(struct macid_ctl_t *macid_ctl, u8 id, u32 bmp)
+{
+	if (id >= macid_ctl->num) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	macid_ctl->rate_bmp1[id] = bmp;
+	if (0)
+		RTW_INFO("macid:%u, rate_bmp1:0x%08X\n", id, macid_ctl->rate_bmp1[id]);
+}
+
+inline void rtw_macid_ctl_init(struct macid_ctl_t *macid_ctl)
+{
+	spin_lock_init(&macid_ctl->lock);
+}
+
+inline void rtw_macid_ctl_deinit(struct macid_ctl_t *macid_ctl)
+{
+}
+
+_adapter *dvobj_get_port0_adapter(struct dvobj_priv *dvobj)
+{
+	_adapter *port0_iface = NULL;
+	int i;
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		if (get_hw_port(dvobj->padapters[i]) == HW_PORT0)
+			break;
+	}
+
+	if (i < 0 || i >= dvobj->iface_nums)
+		rtw_warn_on(1);
+	else
+		port0_iface = dvobj->padapters[i];
+
+	return port0_iface;
+}
+
+_adapter *dvobj_get_unregisterd_adapter(struct dvobj_priv *dvobj)
+{
+	_adapter *adapter = NULL;
+	int i;
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		if (dvobj->padapters[i]->registered == 0)
+			break;
+	}
+
+	if (i < dvobj->iface_nums)
+		adapter = dvobj->padapters[i];
+
+	return adapter;
+}
+
+_adapter *dvobj_get_adapter_by_addr(struct dvobj_priv *dvobj, u8 *addr)
+{
+	_adapter *adapter = NULL;
+	int i;
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		if (!memcmp(dvobj->padapters[i]->mac_addr, addr, ETH_ALEN))
+			break;
+	}
+
+	if (i < dvobj->iface_nums)
+		adapter = dvobj->padapters[i];
+
+	return adapter;
+}
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+void rtw_get_current_ip_address(PADAPTER padapter, u8 *pcurrentip)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct in_device *my_ip_ptr = padapter->pnetdev->ip_ptr;
+	u8 ipaddress[4];
+
+	if ((pmlmeinfo->state & WIFI_FW_LINKING_STATE) ||
+	    pmlmeinfo->state & WIFI_FW_AP_STATE) {
+		if (my_ip_ptr != NULL) {
+			struct in_ifaddr *my_ifa_list  = my_ip_ptr->ifa_list ;
+			if (my_ifa_list != NULL) {
+				ipaddress[0] = my_ifa_list->ifa_address & 0xFF;
+				ipaddress[1] = (my_ifa_list->ifa_address >> 8) & 0xFF;
+				ipaddress[2] = (my_ifa_list->ifa_address >> 16) & 0xFF;
+				ipaddress[3] = my_ifa_list->ifa_address >> 24;
+				RTW_INFO("%s: %d.%d.%d.%d ==========\n", __func__,
+					ipaddress[0], ipaddress[1], ipaddress[2], ipaddress[3]);
+				memcpy(pcurrentip, ipaddress, 4);
+			}
+		}
+	}
+}
+#endif
+#ifdef CONFIG_WOWLAN
+bool rtw_wowlan_parser_pattern_cmd(u8 *input, char *pattern,
+				   int *pattern_len, char *bit_mask)
+{
+	char *cp = NULL, *end = NULL;
+	size_t len = 0;
+	int pos = 0, mask_pos = 0, res = 0;
+	u8 member[2] = {0};
+
+	cp = strchr(input, '=');
+	if (cp) {
+		*cp = 0;
+		cp++;
+		input = cp;
+	}
+
+	while (1) {
+		cp = strchr(input, ':');
+
+		if (cp) {
+			len = strlen(input) - strlen(cp);
+			*cp = 0;
+			cp++;
+		} else
+			len = 2;
+
+		if (bit_mask && (strcmp(input, "-") == 0 ||
+				 strcmp(input, "xx") == 0 ||
+				 strcmp(input, "--") == 0)) {
+			/* skip this byte and leave mask bit unset */
+		} else {
+			u8 hex;
+
+			strncpy(member, input, len);
+			if (!rtw_check_pattern_valid(member, sizeof(member))) {
+				RTW_INFO("%s:[ERROR] pattern is invalid!!\n",
+					 __func__);
+				goto error;
+			}
+
+			res = sscanf(member, "%02hhx", &hex);
+			pattern[pos] = hex;
+			mask_pos = pos / 8;
+			if (bit_mask)
+				bit_mask[mask_pos] |= 1 << (pos % 8);
+		}
+
+		pos++;
+		if (!cp)
+			break;
+		input = cp;
+	}
+
+	(*pattern_len) = pos;
+
+	return true;
+error:
+	return false;
+}
+
+bool rtw_check_pattern_valid(u8 *input, u8 len)
+{
+	int i = 0;
+	bool res = false;
+
+	if (len != 2)
+		goto exit;
+
+	for (i = 0 ; i < len ; i++)
+		if (IsHexDigit(input[i]) == false)
+			goto exit;
+
+	res = _SUCCESS;
+
+exit:
+	return res;
+}
+void rtw_wow_pattern_sw_reset(_adapter *adapter)
+{
+	int i;
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(adapter);
+
+	pwrctrlpriv->wowlan_pattern_idx = DEFAULT_PATTERN_NUM;
+
+	for (i = 0 ; i < MAX_WKFM_CAM_NUM; i++) {
+		memset(pwrctrlpriv->patterns[i].content, '\0', sizeof(pwrctrlpriv->patterns[i].content));
+		memset(pwrctrlpriv->patterns[i].mask, '\0', sizeof(pwrctrlpriv->patterns[i].mask));
+		pwrctrlpriv->patterns[i].len = 0;
+	}
+}
+
+u8 rtw_set_default_pattern(_adapter *adapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter);
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	u8 index = 0;
+	u8 currentip[4];
+	u8 multicast_addr[3] = {0x01, 0x00, 0x5e};
+	u8 multicast_ip[4] = {0xe0, 0x28, 0x28, 0x2a};
+	u8 unicast_mask[5] = {0x3f, 0x70, 0x80, 0xc0, 0x03};
+	u8 multicast_mask[5] = {0x07, 0x70, 0x80, 0xc0, 0x03};
+	u8 ip_protocol[3] = {0x08, 0x00, 0x45};
+	u8 icmp_protocol[1] = {0x01};
+	u8 tcp_protocol[1] = {0x06};
+	u8 udp_protocol[1] = {0x11};
+
+	if (pregistrypriv->default_patterns_en == false)
+		return 0;
+
+	for (index = 0 ; index < DEFAULT_PATTERN_NUM ; index++) {
+		memset(pwrpriv->patterns[index].content, 0,
+			    sizeof(pwrpriv->patterns[index].content));
+		memset(pwrpriv->patterns[index].mask, 0,
+			    sizeof(pwrpriv->patterns[index].mask));
+		pwrpriv->patterns[index].len = 0;
+	}
+
+	rtw_get_current_ip_address(adapter, currentip);
+
+	/*TCP/ICMP unicast*/
+	for (index = 0 ; index < DEFAULT_PATTERN_NUM ; index++) {
+		switch (index) {
+		case 0:
+			memcpy(pwrpriv->patterns[index].content,
+				    adapter_mac_addr(adapter),
+				    ETH_ALEN);
+			memcpy(pwrpriv->patterns[index].content + ETH_TYPE_OFFSET,
+				    &ip_protocol, sizeof(ip_protocol));
+			memcpy(pwrpriv->patterns[index].content + PROTOCOL_OFFSET,
+				    &tcp_protocol, sizeof(tcp_protocol));
+			memcpy(pwrpriv->patterns[index].content + IP_OFFSET,
+				    &currentip, sizeof(currentip));
+			memcpy(pwrpriv->patterns[index].mask,
+				    &unicast_mask, sizeof(unicast_mask));
+			pwrpriv->patterns[index].len = IP_OFFSET + sizeof(currentip);
+			break;
+		case 1:
+			memcpy(pwrpriv->patterns[index].content,
+				    adapter_mac_addr(adapter),
+				    ETH_ALEN);
+			memcpy(pwrpriv->patterns[index].content + ETH_TYPE_OFFSET,
+				    &ip_protocol, sizeof(ip_protocol));
+			memcpy(pwrpriv->patterns[index].content + PROTOCOL_OFFSET,
+				    &icmp_protocol, sizeof(icmp_protocol));
+			memcpy(pwrpriv->patterns[index].content + IP_OFFSET,
+				    &currentip, sizeof(currentip));
+			memcpy(pwrpriv->patterns[index].mask,
+				    &unicast_mask, sizeof(unicast_mask));
+			pwrpriv->patterns[index].len = IP_OFFSET + sizeof(currentip);
+			break;
+		case 2:
+			memcpy(pwrpriv->patterns[index].content, &multicast_addr,
+				    sizeof(multicast_addr));
+			memcpy(pwrpriv->patterns[index].content + ETH_TYPE_OFFSET,
+				    &ip_protocol, sizeof(ip_protocol));
+			memcpy(pwrpriv->patterns[index].content + PROTOCOL_OFFSET,
+				    &udp_protocol, sizeof(udp_protocol));
+			memcpy(pwrpriv->patterns[index].content + IP_OFFSET,
+				    &multicast_ip, sizeof(multicast_ip));
+			memcpy(pwrpriv->patterns[index].mask,
+				    &multicast_mask, sizeof(multicast_mask));
+			pwrpriv->patterns[index].len =
+				IP_OFFSET + sizeof(multicast_ip);
+			break;
+		}
+	}
+
+	return index;
+}
+
+
+
+void rtw_dump_priv_pattern(_adapter *adapter, u8 idx)
+{
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(adapter);
+	char str_1[128];
+	char *p_str;
+	u8 val8 = 0;
+	int i = 0, j = 0, len = 0, max_len = 0;
+
+	RTW_INFO("=========[%d]========\n", idx);
+
+	RTW_INFO(">>>priv_pattern_content:\n");
+	p_str = str_1;
+	max_len = sizeof(str_1);
+	for (i = 0 ; i < MAX_WKFM_PATTERN_SIZE / 8 ; i++) {
+		memset(p_str, 0, max_len);
+		len = 0;
+		for (j = 0 ; j < 8 ; j++) {
+			val8 = pwrctl->patterns[idx].content[i * 8 + j];
+			len += snprintf(p_str + len, max_len - len,
+					"%02x ", val8);
+		}
+		RTW_INFO("%s\n", p_str);
+	}
+
+	RTW_INFO(">>>priv_pattern_mask:\n");
+	for (i = 0 ; i < MAX_WKFM_SIZE / 8 ; i++) {
+		memset(p_str, 0, max_len);
+		len = 0;
+		for (j = 0 ; j < 8 ; j++) {
+			val8 = pwrctl->patterns[idx].mask[i * 8 + j];
+			len += snprintf(p_str + len, max_len - len,
+					"%02x ", val8);
+		}
+		RTW_INFO("%s\n", p_str);
+	}
+
+	RTW_INFO(">>>priv_pattern_len:\n");
+	RTW_INFO("%s: len: %d\n", __func__, pwrctl->patterns[idx].len);
+}
+
+void rtw_wow_pattern_sw_dump(_adapter *adapter)
+{
+	int i;
+
+	RTW_INFO("********[RTK priv-patterns]*********\n");
+	for (i = 0 ; i < MAX_WKFM_CAM_NUM; i++)
+		rtw_dump_priv_pattern(adapter, i);
+}
+
+void rtw_get_sec_iv(PADAPTER padapter, u8 *pcur_dot11txpn, u8 *StaAddr)
+{
+	struct sta_info		*psta;
+	struct security_priv *psecpriv = &padapter->securitypriv;
+
+	memset(pcur_dot11txpn, 0, 8);
+	if (NULL == StaAddr)
+		return;
+	psta = rtw_get_stainfo(&padapter->stapriv, StaAddr);
+	RTW_INFO("%s(): StaAddr: %02x %02x %02x %02x %02x %02x\n",
+		 __func__, StaAddr[0], StaAddr[1], StaAddr[2],
+		 StaAddr[3], StaAddr[4], StaAddr[5]);
+
+	if (psta) {
+		if (psecpriv->dot11PrivacyAlgrthm == _AES_)
+			AES_IV(pcur_dot11txpn, psta->dot11txpn, 0);
+		else if (psecpriv->dot11PrivacyAlgrthm == _TKIP_)
+			TKIP_IV(pcur_dot11txpn, psta->dot11txpn, 0);
+
+		RTW_INFO("%s(): CurrentIV: %02x %02x %02x %02x %02x %02x %02x %02x\n"
+			 , __func__, pcur_dot11txpn[0], pcur_dot11txpn[1],
+			pcur_dot11txpn[2], pcur_dot11txpn[3], pcur_dot11txpn[4],
+			pcur_dot11txpn[5], pcur_dot11txpn[6], pcur_dot11txpn[7]);
+	}
+}
+#endif /* CONFIG_WOWLAN */
+
+#ifdef CONFIG_PNO_SUPPORT
+#define	CSCAN_TLV_TYPE_SSID_IE	'S'
+#define CIPHER_IE "key_mgmt="
+#define CIPHER_NONE "NONE"
+#define CIPHER_WPA_PSK "WPA-PSK"
+#define CIPHER_WPA_EAP "WPA-EAP IEEE8021X"
+/*
+ *  SSIDs list parsing from cscan tlv list
+ */
+int rtw_parse_ssid_list_tlv(char **list_str, pno_ssid_t *ssid,
+			    int max, int *bytes_left)
+{
+	char *str;
+
+	int idx = 0;
+
+	if ((list_str == NULL) || (*list_str == NULL) || (*bytes_left < 0)) {
+		RTW_INFO("%s error paramters\n", __func__);
+		return -1;
+	}
+
+	str = *list_str;
+	while (*bytes_left > 0) {
+
+		if (str[0] != CSCAN_TLV_TYPE_SSID_IE) {
+			*list_str = str;
+			RTW_INFO("nssid=%d left_parse=%d %d\n", idx, *bytes_left, str[0]);
+			return idx;
+		}
+
+		/* Get proper CSCAN_TLV_TYPE_SSID_IE */
+		*bytes_left -= 1;
+		str += 1;
+
+		if (str[0] == 0) {
+			/* Broadcast SSID */
+			ssid[idx].SSID_len = 0;
+			memset((char *)ssid[idx].SSID, 0x0, WLAN_SSID_MAXLEN);
+			*bytes_left -= 1;
+			str += 1;
+
+			RTW_INFO("BROADCAST SCAN  left=%d\n", *bytes_left);
+		} else if (str[0] <= WLAN_SSID_MAXLEN) {
+			/* Get proper SSID size */
+			ssid[idx].SSID_len = str[0];
+			*bytes_left -= 1;
+			str += 1;
+
+			/* Get SSID */
+			if (ssid[idx].SSID_len > *bytes_left) {
+				RTW_INFO("%s out of memory range len=%d but left=%d\n",
+					__func__, ssid[idx].SSID_len, *bytes_left);
+				return -1;
+			}
+
+			memcpy((char *)ssid[idx].SSID, str, ssid[idx].SSID_len);
+
+			*bytes_left -= ssid[idx].SSID_len;
+			str += ssid[idx].SSID_len;
+
+			RTW_INFO("%s :size=%d left=%d\n",
+				(char *)ssid[idx].SSID, ssid[idx].SSID_len, *bytes_left);
+		} else {
+			RTW_INFO("### SSID size more that %d\n", str[0]);
+			return -1;
+		}
+
+		if (idx++ >  max) {
+			RTW_INFO("%s number of SSIDs more that %d\n", __func__, idx);
+			return -1;
+		}
+	}
+
+	*list_str = str;
+	return idx;
+}
+
+int rtw_parse_cipher_list(struct pno_nlo_info *nlo_info, char *list_str)
+{
+
+	char *pch, *pnext, *pend;
+	u8 key_len = 0, index = 0;
+
+	pch = list_str;
+
+	if (nlo_info == NULL || list_str == NULL) {
+		RTW_INFO("%s error paramters\n", __func__);
+		return -1;
+	}
+
+	while (strlen(pch) != 0) {
+		pnext = strstr(pch, "key_mgmt=");
+		if (pnext != NULL) {
+			pch = pnext + strlen(CIPHER_IE);
+			pend = strstr(pch, "}");
+			if (strncmp(pch, CIPHER_NONE,
+				    strlen(CIPHER_NONE)) == 0)
+				nlo_info->ssid_cipher_info[index] = 0x00;
+			else if (strncmp(pch, CIPHER_WPA_PSK,
+					 strlen(CIPHER_WPA_PSK)) == 0)
+				nlo_info->ssid_cipher_info[index] = 0x66;
+			else if (strncmp(pch, CIPHER_WPA_EAP,
+					 strlen(CIPHER_WPA_EAP)) == 0)
+				nlo_info->ssid_cipher_info[index] = 0x01;
+			index++;
+			pch = pend + 1;
+		} else
+			break;
+	}
+	return 0;
+}
+
+int rtw_dev_nlo_info_set(struct pno_nlo_info *nlo_info, pno_ssid_t *ssid,
+		 int num, int pno_time, int pno_repeat, int pno_freq_expo_max)
+{
+
+	int i = 0;
+	struct file *fp;
+	mm_segment_t fs;
+	loff_t pos = 0;
+	u8 *source = NULL;
+	long len = 0;
+
+	RTW_INFO("+%s+\n", __func__);
+
+	nlo_info->fast_scan_period = pno_time;
+	nlo_info->ssid_num = num & BIT_LEN_MASK_32(8);
+	nlo_info->hidden_ssid_num = num & BIT_LEN_MASK_32(8);
+	nlo_info->slow_scan_period = (pno_time * 2);
+	nlo_info->fast_scan_iterations = 5;
+
+	if (nlo_info->hidden_ssid_num > 8)
+		nlo_info->hidden_ssid_num = 8;
+
+	/* TODO: channel list and probe index is all empty. */
+	for (i = 0 ; i < num ; i++) {
+		nlo_info->ssid_length[i]
+			= ssid[i].SSID_len;
+	}
+
+	/* cipher array */
+	fp = filp_open("/data/misc/wifi/wpa_supplicant.conf", O_RDONLY,  0644);
+	if (IS_ERR(fp)) {
+		RTW_INFO("Error, wpa_supplicant.conf doesn't exist.\n");
+		RTW_INFO("Error, cipher array using default value.\n");
+		return 0;
+	}
+
+	len = i_size_read(fp->f_path.dentry->d_inode);
+	if (len < 0 || len > 2048) {
+		RTW_INFO("Error, file size is bigger than 2048.\n");
+		RTW_INFO("Error, cipher array using default value.\n");
+		return 0;
+	}
+
+	fs = get_fs();
+	set_fs(KERNEL_DS);
+
+	source = rtw_zmalloc(2048);
+
+	if (source != NULL) {
+		len = vfs_read(fp, source, len, &pos);
+		rtw_parse_cipher_list(nlo_info, source);
+		rtw_mfree(source, 2048);
+	}
+
+	set_fs(fs);
+	filp_close(fp, NULL);
+
+	RTW_INFO("-%s-\n", __func__);
+	return 0;
+}
+
+int rtw_dev_ssid_list_set(struct pno_ssid_list *pno_ssid_list,
+			  pno_ssid_t *ssid, u8 num)
+{
+
+	int i = 0;
+	if (num > MAX_PNO_LIST_COUNT)
+		num = MAX_PNO_LIST_COUNT;
+
+	for (i = 0 ; i < num ; i++) {
+		memcpy(&pno_ssid_list->node[i].SSID,
+			    ssid[i].SSID, ssid[i].SSID_len);
+		pno_ssid_list->node[i].SSID_len = ssid[i].SSID_len;
+	}
+	return 0;
+}
+
+int rtw_dev_scan_info_set(_adapter *padapter, pno_ssid_t *ssid,
+	  unsigned char ch, unsigned char ch_offset, unsigned short bw_mode)
+{
+
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+	struct pno_scan_info *scan_info = pwrctl->pscan_info;
+	int i;
+
+	scan_info->channel_num = MAX_SCAN_LIST_COUNT;
+	scan_info->orig_ch = ch;
+	scan_info->orig_bw = bw_mode;
+	scan_info->orig_40_offset = ch_offset;
+
+	for (i = 0 ; i < scan_info->channel_num ; i++) {
+		if (i < 11)
+			scan_info->ssid_channel_info[i].active = 1;
+		else
+			scan_info->ssid_channel_info[i].active = 0;
+
+		scan_info->ssid_channel_info[i].timeout = 100;
+
+		scan_info->ssid_channel_info[i].tx_power =
+			phy_get_tx_power_index(padapter, 0, 0x02, bw_mode, i + 1);
+
+		scan_info->ssid_channel_info[i].channel = i + 1;
+	}
+
+	RTW_INFO("%s, channel_num: %d, orig_ch: %d, orig_bw: %d orig_40_offset: %d\n",
+		 __func__, scan_info->channel_num, scan_info->orig_ch,
+		 scan_info->orig_bw, scan_info->orig_40_offset);
+	return 0;
+}
+
+int rtw_dev_pno_set(struct net_device *net, pno_ssid_t *ssid, int num,
+		    int pno_time, int pno_repeat, int pno_freq_expo_max)
+{
+
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(net);
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	int ret = -1;
+
+	if (num == 0) {
+		RTW_INFO("%s, nssid is zero, no need to setup pno ssid list\n", __func__);
+		return 0;
+	}
+
+	if (pwrctl == NULL) {
+		RTW_INFO("%s, ERROR: pwrctl is NULL\n", __func__);
+		return -1;
+	} else {
+		pwrctl->pnlo_info =
+			(pno_nlo_info_t *)rtw_zmalloc(sizeof(pno_nlo_info_t));
+		pwrctl->pno_ssid_list =
+			(pno_ssid_list_t *)rtw_zmalloc(sizeof(pno_ssid_list_t));
+		pwrctl->pscan_info =
+			(pno_scan_info_t *)rtw_zmalloc(sizeof(pno_scan_info_t));
+	}
+
+	if (pwrctl->pnlo_info == NULL ||
+	    pwrctl->pscan_info == NULL ||
+	    pwrctl->pno_ssid_list == NULL) {
+		RTW_INFO("%s, ERROR: alloc nlo_info, ssid_list, scan_info fail\n", __func__);
+		goto failing;
+	}
+
+	pwrctl->wowlan_in_resume = false;
+
+	pwrctl->pno_inited = true;
+	/* NLO Info */
+	ret = rtw_dev_nlo_info_set(pwrctl->pnlo_info, ssid, num,
+				   pno_time, pno_repeat, pno_freq_expo_max);
+
+	/* SSID Info */
+	ret = rtw_dev_ssid_list_set(pwrctl->pno_ssid_list, ssid, num);
+
+	/* SCAN Info */
+	ret = rtw_dev_scan_info_set(padapter, ssid, pmlmeext->cur_channel,
+			    pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	RTW_INFO("+%s num: %d, pno_time: %d, pno_repeat:%d, pno_freq_expo_max:%d+\n",
+		 __func__, num, pno_time, pno_repeat, pno_freq_expo_max);
+
+	return 0;
+
+failing:
+	if (pwrctl->pnlo_info) {
+		rtw_mfree((u8 *)pwrctl->pnlo_info, sizeof(pno_nlo_info_t));
+		pwrctl->pnlo_info = NULL;
+	}
+	if (pwrctl->pno_ssid_list) {
+		rtw_mfree((u8 *)pwrctl->pno_ssid_list, sizeof(pno_ssid_list_t));
+		pwrctl->pno_ssid_list = NULL;
+	}
+	if (pwrctl->pscan_info) {
+		rtw_mfree((u8 *)pwrctl->pscan_info, sizeof(pno_scan_info_t));
+		pwrctl->pscan_info = NULL;
+	}
+
+	return -1;
+}
+
+#ifdef CONFIG_PNO_SET_DEBUG
+void rtw_dev_pno_debug(struct net_device *net)
+{
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(net);
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+	int i = 0, j = 0;
+
+	RTW_INFO("*******NLO_INFO********\n");
+	RTW_INFO("ssid_num: %d\n", pwrctl->pnlo_info->ssid_num);
+	RTW_INFO("fast_scan_iterations: %d\n",
+		 pwrctl->pnlo_info->fast_scan_iterations);
+	RTW_INFO("fast_scan_period: %d\n", pwrctl->pnlo_info->fast_scan_period);
+	RTW_INFO("slow_scan_period: %d\n", pwrctl->pnlo_info->slow_scan_period);
+
+
+
+	for (i = 0 ; i < MAX_PNO_LIST_COUNT ; i++) {
+		RTW_INFO("%d SSID (%s) length (%d) cipher(%x) channel(%d)\n",
+			i, pwrctl->pno_ssid_list->node[i].SSID, pwrctl->pnlo_info->ssid_length[i],
+			pwrctl->pnlo_info->ssid_cipher_info[i], pwrctl->pnlo_info->ssid_channel_info[i]);
+	}
+
+	RTW_INFO("******SCAN_INFO******\n");
+	RTW_INFO("ch_num: %d\n", pwrctl->pscan_info->channel_num);
+	RTW_INFO("orig_ch: %d\n", pwrctl->pscan_info->orig_ch);
+	RTW_INFO("orig bw: %d\n", pwrctl->pscan_info->orig_bw);
+	RTW_INFO("orig 40 offset: %d\n", pwrctl->pscan_info->orig_40_offset);
+	for (i = 0 ; i < MAX_SCAN_LIST_COUNT ; i++) {
+		RTW_INFO("[%02d] avtive:%d, timeout:%d, tx_power:%d, ch:%02d\n",
+			 i, pwrctl->pscan_info->ssid_channel_info[i].active,
+			 pwrctl->pscan_info->ssid_channel_info[i].timeout,
+			 pwrctl->pscan_info->ssid_channel_info[i].tx_power,
+			 pwrctl->pscan_info->ssid_channel_info[i].channel);
+	}
+	RTW_INFO("*****************\n");
+}
+#endif /* CONFIG_PNO_SET_DEBUG */
+#endif /* CONFIG_PNO_SUPPORT */
diff --git a/drivers/staging/rtl8188eu/core/rtw_xmit.c b/drivers/staging/rtl8188eu/core/rtw_xmit.c
new file mode 100644
index 000000000000..1e918748d1ef
--- /dev/null
+++ b/drivers/staging/rtl8188eu/core/rtw_xmit.c
@@ -0,0 +1,5070 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved. */
+
+#define _RTW_XMIT_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+#include <usb_osintf.h>
+
+static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
+static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
+
+static void _init_txservq(struct tx_servq *ptxservq)
+{
+	INIT_LIST_HEAD(&ptxservq->tx_pending);
+	_rtw_init_queue(&ptxservq->sta_pending);
+	ptxservq->qcnt = 0;
+}
+
+void	_rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv)
+{
+
+	memset((unsigned char *)psta_xmitpriv, 0, sizeof(struct sta_xmit_priv));
+
+	spin_lock_init(&psta_xmitpriv->lock);
+
+	/* for(i = 0 ; i < MAX_NUMBLKS; i++) */
+	/*	_init_txservq(&(psta_xmitpriv->blk_q[i])); */
+
+	_init_txservq(&psta_xmitpriv->be_q);
+	_init_txservq(&psta_xmitpriv->bk_q);
+	_init_txservq(&psta_xmitpriv->vi_q);
+	_init_txservq(&psta_xmitpriv->vo_q);
+	INIT_LIST_HEAD(&psta_xmitpriv->legacy_dz);
+	INIT_LIST_HEAD(&psta_xmitpriv->apsd);
+
+}
+
+s32	_rtw_init_xmit_priv(struct xmit_priv *pxmitpriv, _adapter *padapter)
+{
+	int i;
+	struct xmit_buf *pxmitbuf;
+	struct xmit_frame *pxframe;
+	sint	res = _SUCCESS;
+
+	/* We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */
+	/* memset((unsigned char *)pxmitpriv, 0, sizeof(struct xmit_priv)); */
+
+	spin_lock_init(&pxmitpriv->lock);
+	spin_lock_init(&pxmitpriv->lock_sctx);
+	sema_init(&pxmitpriv->xmit_sema, 0);
+	sema_init(&pxmitpriv->terminate_xmitthread_sema, 0);
+
+	/*
+	Please insert all the queue initializaiton using _rtw_init_queue below
+	*/
+
+	pxmitpriv->adapter = padapter;
+
+	/* for(i = 0 ; i < MAX_NUMBLKS; i++) */
+	/*	_rtw_init_queue(&pxmitpriv->blk_strms[i]); */
+
+	_rtw_init_queue(&pxmitpriv->be_pending);
+	_rtw_init_queue(&pxmitpriv->bk_pending);
+	_rtw_init_queue(&pxmitpriv->vi_pending);
+	_rtw_init_queue(&pxmitpriv->vo_pending);
+	_rtw_init_queue(&pxmitpriv->bm_pending);
+
+	/* _rtw_init_queue(&pxmitpriv->legacy_dz_queue); */
+	/* _rtw_init_queue(&pxmitpriv->apsd_queue); */
+
+	_rtw_init_queue(&pxmitpriv->free_xmit_queue);
+
+	/*
+	Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME,
+	and initialize free_xmit_frame below.
+	Please also apply  free_txobj to link_up all the xmit_frames...
+	*/
+
+	pxmitpriv->pallocated_frame_buf = rtw_zvmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
+
+	if (pxmitpriv->pallocated_frame_buf  == NULL) {
+		pxmitpriv->pxmit_frame_buf = NULL;
+		res = _FAIL;
+		goto exit;
+	}
+	pxmitpriv->pxmit_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_frame_buf), 4);
+	/* pxmitpriv->pxmit_frame_buf = pxmitpriv->pallocated_frame_buf + 4 - */
+	/*						((SIZE_PTR) (pxmitpriv->pallocated_frame_buf) &3); */
+
+	pxframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf;
+
+	for (i = 0; i < NR_XMITFRAME; i++) {
+		INIT_LIST_HEAD(&(pxframe->list));
+
+		pxframe->padapter = padapter;
+		pxframe->frame_tag = NULL_FRAMETAG;
+
+		pxframe->pkt = NULL;
+
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+
+		list_add_tail(&(pxframe->list), &(pxmitpriv->free_xmit_queue.queue));
+
+		pxframe++;
+	}
+
+	pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME;
+
+	pxmitpriv->frag_len = MAX_FRAG_THRESHOLD;
+
+	/* init xmit_buf */
+	_rtw_init_queue(&pxmitpriv->free_xmitbuf_queue);
+	_rtw_init_queue(&pxmitpriv->pending_xmitbuf_queue);
+
+	pxmitpriv->pallocated_xmitbuf = rtw_zvmalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
+
+	if (pxmitpriv->pallocated_xmitbuf  == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pxmitpriv->pxmitbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmitbuf), 4);
+	/* pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 - */
+	/*						((SIZE_PTR) (pxmitpriv->pallocated_xmitbuf) &3); */
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+
+	for (i = 0; i < NR_XMITBUFF; i++) {
+		INIT_LIST_HEAD(&pxmitbuf->list);
+
+		pxmitbuf->priv_data = NULL;
+		pxmitbuf->padapter = padapter;
+		pxmitbuf->buf_tag = XMITBUF_DATA;
+
+		/* Tx buf allocation may fail sometimes, so sleep and retry. */
+		res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true);
+		if (res == _FAIL) {
+			rtw_msleep_os(10);
+			res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true);
+			if (res == _FAIL)
+				goto exit;
+		}
+		pxmitbuf->flags = XMIT_VO_QUEUE;
+
+		list_add_tail(&pxmitbuf->list, &(pxmitpriv->free_xmitbuf_queue.queue));
+#ifdef DBG_XMIT_BUF
+		pxmitbuf->no = i;
+#endif
+
+		pxmitbuf++;
+
+	}
+
+	pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF;
+
+	/* init xframe_ext queue,  the same count as extbuf */
+	_rtw_init_queue(&pxmitpriv->free_xframe_ext_queue);
+
+	pxmitpriv->xframe_ext_alloc_addr = rtw_zvmalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_frame) + 4);
+
+	if (pxmitpriv->xframe_ext_alloc_addr  == NULL) {
+		pxmitpriv->xframe_ext = NULL;
+		res = _FAIL;
+		goto exit;
+	}
+	pxmitpriv->xframe_ext = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->xframe_ext_alloc_addr), 4);
+	pxframe = (struct xmit_frame *)pxmitpriv->xframe_ext;
+
+	for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
+		INIT_LIST_HEAD(&(pxframe->list));
+
+		pxframe->padapter = padapter;
+		pxframe->frame_tag = NULL_FRAMETAG;
+
+		pxframe->pkt = NULL;
+
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+
+		pxframe->ext_tag = 1;
+
+		list_add_tail(&(pxframe->list), &(pxmitpriv->free_xframe_ext_queue.queue));
+
+		pxframe++;
+	}
+	pxmitpriv->free_xframe_ext_cnt = NR_XMIT_EXTBUFF;
+
+	/* Init xmit extension buff */
+	_rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue);
+
+	pxmitpriv->pallocated_xmit_extbuf = rtw_zvmalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_buf) + 4);
+
+	if (pxmitpriv->pallocated_xmit_extbuf  == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmit_extbuf), 4);
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+
+	for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
+		INIT_LIST_HEAD(&pxmitbuf->list);
+
+		pxmitbuf->priv_data = NULL;
+		pxmitbuf->padapter = padapter;
+		pxmitbuf->buf_tag = XMITBUF_MGNT;
+
+		res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, MAX_XMIT_EXTBUF_SZ + XMITBUF_ALIGN_SZ, true);
+		if (res == _FAIL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		list_add_tail(&pxmitbuf->list, &(pxmitpriv->free_xmit_extbuf_queue.queue));
+#ifdef DBG_XMIT_BUF_EXT
+		pxmitbuf->no = i;
+#endif
+		pxmitbuf++;
+
+	}
+
+	pxmitpriv->free_xmit_extbuf_cnt = NR_XMIT_EXTBUFF;
+
+	for (i = 0; i < CMDBUF_MAX; i++) {
+		pxmitbuf = &pxmitpriv->pcmd_xmitbuf[i];
+		if (pxmitbuf) {
+			INIT_LIST_HEAD(&pxmitbuf->list);
+
+			pxmitbuf->priv_data = NULL;
+			pxmitbuf->padapter = padapter;
+			pxmitbuf->buf_tag = XMITBUF_CMD;
+
+			res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, MAX_CMDBUF_SZ + XMITBUF_ALIGN_SZ, true);
+			if (res == _FAIL) {
+				res = _FAIL;
+				goto exit;
+			}
+			pxmitbuf->alloc_sz = MAX_CMDBUF_SZ + XMITBUF_ALIGN_SZ;
+		}
+	}
+
+	rtw_alloc_hwxmits(padapter);
+	rtw_init_hwxmits(pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
+
+	for (i = 0; i < 4; i++)
+		pxmitpriv->wmm_para_seq[i] = i;
+
+	pxmitpriv->txirp_cnt = 1;
+
+	sema_init(&(pxmitpriv->tx_retevt), 0);
+
+	/* per AC pending irp */
+	pxmitpriv->beq_cnt = 0;
+	pxmitpriv->bkq_cnt = 0;
+	pxmitpriv->viq_cnt = 0;
+	pxmitpriv->voq_cnt = 0;
+
+#ifdef CONFIG_XMIT_ACK
+	pxmitpriv->ack_tx = false;
+	_rtw_mutex_init(&pxmitpriv->ack_tx_mutex);
+	rtw_sctx_init(&pxmitpriv->ack_tx_ops, 0);
+#endif
+
+#ifdef CONFIG_TX_AMSDU
+	_init_timer(&(pxmitpriv->amsdu_vo_timer), padapter->pnetdev,
+		rtw_amsdu_vo_timeout_handler, padapter);
+	pxmitpriv->amsdu_vo_timeout = RTW_AMSDU_TIMER_UNSET;
+
+	_init_timer(&(pxmitpriv->amsdu_vi_timer), padapter->pnetdev,
+		rtw_amsdu_vi_timeout_handler, padapter);
+	pxmitpriv->amsdu_vi_timeout = RTW_AMSDU_TIMER_UNSET;
+
+        _init_timer(&(pxmitpriv->amsdu_be_timer), padapter->pnetdev,
+		rtw_amsdu_be_timeout_handler, padapter);
+	pxmitpriv->amsdu_be_timeout = RTW_AMSDU_TIMER_UNSET;
+
+	_init_timer(&(pxmitpriv->amsdu_bk_timer), padapter->pnetdev,
+		rtw_amsdu_bk_timeout_handler, padapter);
+	pxmitpriv->amsdu_bk_timeout = RTW_AMSDU_TIMER_UNSET;
+
+	pxmitpriv->amsdu_debug_set_timer = 0;
+	pxmitpriv->amsdu_debug_timeout = 0;
+	pxmitpriv->amsdu_debug_coalesce_one = 0;
+	pxmitpriv->amsdu_debug_coalesce_two = 0;
+#endif
+	rtw_hal_init_xmit_priv(padapter);
+
+exit:
+
+	return res;
+}
+
+void _rtw_free_xmit_priv(struct xmit_priv *pxmitpriv)
+{
+	int i;
+	_adapter *padapter = pxmitpriv->adapter;
+	struct xmit_frame	*pxmitframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf;
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+
+	rtw_hal_free_xmit_priv(padapter);
+
+	if (pxmitpriv->pxmit_frame_buf == NULL)
+		goto out;
+
+	for (i = 0; i < NR_XMITFRAME; i++) {
+		rtw_os_xmit_complete(padapter, pxmitframe);
+
+		pxmitframe++;
+	}
+
+	for (i = 0; i < NR_XMITBUFF; i++) {
+		rtw_os_xmit_resource_free(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true);
+
+		pxmitbuf++;
+	}
+
+	if (pxmitpriv->pallocated_frame_buf)
+		rtw_vmfree(pxmitpriv->pallocated_frame_buf, NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
+
+	if (pxmitpriv->pallocated_xmitbuf)
+		rtw_vmfree(pxmitpriv->pallocated_xmitbuf, NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
+
+	/* free xframe_ext queue,  the same count as extbuf */
+	if ((pxmitframe = (struct xmit_frame *)pxmitpriv->xframe_ext)) {
+		for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
+			rtw_os_xmit_complete(padapter, pxmitframe);
+			pxmitframe++;
+		}
+	}
+	if (pxmitpriv->xframe_ext_alloc_addr)
+		rtw_vmfree(pxmitpriv->xframe_ext_alloc_addr, NR_XMIT_EXTBUFF * sizeof(struct xmit_frame) + 4);
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+	for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
+		rtw_os_xmit_resource_free(padapter, pxmitbuf, (MAX_XMIT_EXTBUF_SZ + XMITBUF_ALIGN_SZ), true);
+
+		pxmitbuf++;
+	}
+
+	if (pxmitpriv->pallocated_xmit_extbuf)
+		rtw_vmfree(pxmitpriv->pallocated_xmit_extbuf, NR_XMIT_EXTBUFF * sizeof(struct xmit_buf) + 4);
+
+	for (i = 0; i < CMDBUF_MAX; i++) {
+		pxmitbuf = &pxmitpriv->pcmd_xmitbuf[i];
+		if (pxmitbuf != NULL)
+			rtw_os_xmit_resource_free(padapter, pxmitbuf, MAX_CMDBUF_SZ + XMITBUF_ALIGN_SZ , true);
+	}
+
+	rtw_free_hwxmits(padapter);
+
+#ifdef CONFIG_XMIT_ACK
+	_rtw_mutex_free(&pxmitpriv->ack_tx_mutex);
+#endif
+
+out:
+	return;
+}
+
+u8 rtw_get_tx_bw_mode(_adapter *adapter, struct sta_info *sta)
+{
+	u8 bw;
+
+	bw = sta->bw_mode;
+	if (MLME_STATE(adapter) & WIFI_ASOC_STATE) {
+		if (adapter->mlmeextpriv.cur_channel <= 14)
+			bw = rtw_min(bw, ADAPTER_TX_BW_2G(adapter));
+		else
+			bw = rtw_min(bw, ADAPTER_TX_BW_5G(adapter));
+	}
+
+	return bw;
+}
+
+void rtw_get_adapter_tx_rate_bmp_by_bw(_adapter *adapter, u8 bw, u16 *r_bmp_cck_ofdm, u32 *r_bmp_ht, u32 *r_bmp_vht)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
+	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
+	u8 fix_bw = 0xFF;
+	u16 bmp_cck_ofdm = 0;
+	u32 bmp_ht = 0;
+	u32 bmp_vht = 0;
+	int i;
+
+	if (adapter->fix_rate != 0xFF && adapter->fix_bw != 0xFF)
+		fix_bw = adapter->fix_bw;
+
+	/* TODO: adapter->fix_rate */
+
+	for (i = 0; i < macid_ctl->num; i++) {
+		if (!rtw_macid_is_used(macid_ctl, i))
+			continue;
+		if (rtw_macid_get_if_g(macid_ctl, i) != adapter->iface_id)
+			continue;
+
+		if (bw == CHANNEL_WIDTH_20) /* CCK, OFDM always 20MHz */
+			bmp_cck_ofdm |= macid_ctl->rate_bmp0[i] & 0x00000FFF;
+
+		/* bypass mismatch bandwidth for HT, VHT */
+		if ((fix_bw != 0xFF && fix_bw != bw) || (fix_bw == 0xFF && macid_ctl->bw[i] != bw))
+			continue;
+
+		if (macid_ctl->vht_en[i])
+			bmp_vht |= (macid_ctl->rate_bmp0[i] >> 12) | (macid_ctl->rate_bmp1[i] << 20);
+		else
+			bmp_ht |= (macid_ctl->rate_bmp0[i] >> 12) | (macid_ctl->rate_bmp1[i] << 20);
+	}
+
+	/* TODO: mlmeext->tx_rate*/
+
+exit:
+	if (r_bmp_cck_ofdm)
+		*r_bmp_cck_ofdm = bmp_cck_ofdm;
+	if (r_bmp_ht)
+		*r_bmp_ht = bmp_ht;
+	if (r_bmp_vht)
+		*r_bmp_vht = bmp_vht;
+}
+
+static void rtw_get_shared_macid_tx_rate_bmp_by_bw(struct dvobj_priv *dvobj, u8 bw, u16 *r_bmp_cck_ofdm, u32 *r_bmp_ht, u32 *r_bmp_vht)
+{
+	struct macid_ctl_t *macid_ctl = dvobj_to_macidctl(dvobj);
+	u16 bmp_cck_ofdm = 0;
+	u32 bmp_ht = 0;
+	u32 bmp_vht = 0;
+	int i;
+
+	for (i = 0; i < macid_ctl->num; i++) {
+		if (!rtw_macid_is_used(macid_ctl, i))
+			continue;
+		if (rtw_macid_get_if_g(macid_ctl, i) != -1)
+			continue;
+
+		if (bw == CHANNEL_WIDTH_20) /* CCK, OFDM always 20MHz */
+			bmp_cck_ofdm |= macid_ctl->rate_bmp0[i] & 0x00000FFF;
+
+		/* bypass mismatch bandwidth for HT, VHT */
+		if (macid_ctl->bw[i] != bw)
+			continue;
+
+		if (macid_ctl->vht_en[i])
+			bmp_vht |= (macid_ctl->rate_bmp0[i] >> 12) | (macid_ctl->rate_bmp1[i] << 20);
+		else
+			bmp_ht |= (macid_ctl->rate_bmp0[i] >> 12) | (macid_ctl->rate_bmp1[i] << 20);
+	}
+
+	if (r_bmp_cck_ofdm)
+		*r_bmp_cck_ofdm = bmp_cck_ofdm;
+	if (r_bmp_ht)
+		*r_bmp_ht = bmp_ht;
+	if (r_bmp_vht)
+		*r_bmp_vht = bmp_vht;
+}
+
+void rtw_update_tx_rate_bmp(struct dvobj_priv *dvobj)
+{
+	struct rf_ctl_t *rf_ctl = dvobj_to_rfctl(dvobj);
+	_adapter *adapter = dvobj_get_primary_adapter(dvobj);
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+	u8 bw;
+	u16 bmp_cck_ofdm, tmp_cck_ofdm;
+	u32 bmp_ht, tmp_ht, ori_bmp_ht[2];
+	u8 ori_highest_ht_rate_bw_bmp;
+	u32 bmp_vht, tmp_vht, ori_bmp_vht[4];
+	u8 ori_highest_vht_rate_bw_bmp;
+	int i;
+
+	/* backup the original ht & vht highest bw bmp */
+	ori_highest_ht_rate_bw_bmp = rf_ctl->highest_ht_rate_bw_bmp;
+	ori_highest_vht_rate_bw_bmp = rf_ctl->highest_vht_rate_bw_bmp;
+
+	for (bw = CHANNEL_WIDTH_20; bw <= CHANNEL_WIDTH_160; bw++) {
+		/* backup the original ht & vht bmp */
+		if (bw <= CHANNEL_WIDTH_40)
+			ori_bmp_ht[bw] = rf_ctl->rate_bmp_ht_by_bw[bw];
+		if (bw <= CHANNEL_WIDTH_160)
+			ori_bmp_vht[bw] = rf_ctl->rate_bmp_vht_by_bw[bw];
+
+		bmp_cck_ofdm = bmp_ht = bmp_vht = 0;
+		if (hal_is_bw_support(dvobj_get_primary_adapter(dvobj), bw)) {
+			for (i = 0; i < dvobj->iface_nums; i++) {
+				if (!dvobj->padapters[i])
+					continue;
+				rtw_get_adapter_tx_rate_bmp_by_bw(dvobj->padapters[i], bw, &tmp_cck_ofdm, &tmp_ht, &tmp_vht);
+				bmp_cck_ofdm |= tmp_cck_ofdm;
+				bmp_ht |= tmp_ht;
+				bmp_vht |= tmp_vht;
+			}
+			rtw_get_shared_macid_tx_rate_bmp_by_bw(dvobj, bw, &tmp_cck_ofdm, &tmp_ht, &tmp_vht);
+			bmp_cck_ofdm |= tmp_cck_ofdm;
+			bmp_ht |= tmp_ht;
+			bmp_vht |= tmp_vht;
+		}
+		if (bw == CHANNEL_WIDTH_20)
+			rf_ctl->rate_bmp_cck_ofdm = bmp_cck_ofdm;
+		if (bw <= CHANNEL_WIDTH_40)
+			rf_ctl->rate_bmp_ht_by_bw[bw] = bmp_ht;
+		if (bw <= CHANNEL_WIDTH_160)
+			rf_ctl->rate_bmp_vht_by_bw[bw] = bmp_vht;
+	}
+
+#ifndef DBG_HIGHEST_RATE_BMP_BW_CHANGE
+#define DBG_HIGHEST_RATE_BMP_BW_CHANGE 0
+#endif
+
+	{
+		u8 highest_rate_bw;
+		u8 highest_rate_bw_bmp;
+		u8 update_ht_rs = false;
+		u8 update_vht_rs = false;
+
+		highest_rate_bw_bmp = BW_CAP_20M;
+		highest_rate_bw = CHANNEL_WIDTH_20;
+		for (bw = CHANNEL_WIDTH_20; bw <= CHANNEL_WIDTH_40; bw++) {
+			if (rf_ctl->rate_bmp_ht_by_bw[highest_rate_bw] < rf_ctl->rate_bmp_ht_by_bw[bw]) {
+				highest_rate_bw_bmp = ch_width_to_bw_cap(bw);
+				highest_rate_bw = bw;
+			} else if (rf_ctl->rate_bmp_ht_by_bw[highest_rate_bw] == rf_ctl->rate_bmp_ht_by_bw[bw])
+				highest_rate_bw_bmp |= ch_width_to_bw_cap(bw);
+		}
+		rf_ctl->highest_ht_rate_bw_bmp = highest_rate_bw_bmp;
+
+		if (ori_highest_ht_rate_bw_bmp != rf_ctl->highest_ht_rate_bw_bmp
+			|| largest_bit(ori_bmp_ht[highest_rate_bw]) != largest_bit(rf_ctl->rate_bmp_ht_by_bw[highest_rate_bw])
+		) {
+			if (DBG_HIGHEST_RATE_BMP_BW_CHANGE) {
+				RTW_INFO("highest_ht_rate_bw_bmp:0x%02x=>0x%02x\n", ori_highest_ht_rate_bw_bmp, rf_ctl->highest_ht_rate_bw_bmp);
+				RTW_INFO("rate_bmp_ht_by_bw[%u]:0x%08x=>0x%08x\n", highest_rate_bw, ori_bmp_ht[highest_rate_bw], rf_ctl->rate_bmp_ht_by_bw[highest_rate_bw]);
+			}
+			update_ht_rs = true;
+		}
+
+		highest_rate_bw_bmp = BW_CAP_20M;
+		highest_rate_bw = CHANNEL_WIDTH_20;
+		for (bw = CHANNEL_WIDTH_20; bw <= CHANNEL_WIDTH_160; bw++) {
+			if (rf_ctl->rate_bmp_vht_by_bw[highest_rate_bw] < rf_ctl->rate_bmp_vht_by_bw[bw]) {
+				highest_rate_bw_bmp = ch_width_to_bw_cap(bw);
+				highest_rate_bw = bw;
+			} else if (rf_ctl->rate_bmp_vht_by_bw[highest_rate_bw] == rf_ctl->rate_bmp_vht_by_bw[bw])
+				highest_rate_bw_bmp |= ch_width_to_bw_cap(bw);
+		}
+		rf_ctl->highest_vht_rate_bw_bmp = highest_rate_bw_bmp;
+
+		if (ori_highest_vht_rate_bw_bmp != rf_ctl->highest_vht_rate_bw_bmp
+			|| largest_bit(ori_bmp_vht[highest_rate_bw]) != largest_bit(rf_ctl->rate_bmp_vht_by_bw[highest_rate_bw])
+		) {
+			if (DBG_HIGHEST_RATE_BMP_BW_CHANGE) {
+				RTW_INFO("highest_vht_rate_bw_bmp:0x%02x=>0x%02x\n", ori_highest_vht_rate_bw_bmp, rf_ctl->highest_vht_rate_bw_bmp);
+				RTW_INFO("rate_bmp_vht_by_bw[%u]:0x%08x=>0x%08x\n", highest_rate_bw, ori_bmp_vht[highest_rate_bw], rf_ctl->rate_bmp_vht_by_bw[highest_rate_bw]);
+			}
+			update_vht_rs = true;
+		}
+
+		/* TODO: per rfpath and rate section handling? */
+		if (update_ht_rs || update_vht_rs == true)
+			rtw_hal_set_tx_power_level(dvobj_get_primary_adapter(dvobj), hal_data->current_channel);
+	}
+}
+
+inline u16 rtw_get_tx_rate_bmp_cck_ofdm(struct dvobj_priv *dvobj)
+{
+	struct rf_ctl_t *rf_ctl = dvobj_to_rfctl(dvobj);
+
+	return rf_ctl->rate_bmp_cck_ofdm;
+}
+
+inline u32 rtw_get_tx_rate_bmp_ht_by_bw(struct dvobj_priv *dvobj, u8 bw)
+{
+	struct rf_ctl_t *rf_ctl = dvobj_to_rfctl(dvobj);
+
+	return rf_ctl->rate_bmp_ht_by_bw[bw];
+}
+
+inline u32 rtw_get_tx_rate_bmp_vht_by_bw(struct dvobj_priv *dvobj, u8 bw)
+{
+	struct rf_ctl_t *rf_ctl = dvobj_to_rfctl(dvobj);
+
+	return rf_ctl->rate_bmp_vht_by_bw[bw];
+}
+
+u8 rtw_get_tx_bw_bmp_of_ht_rate(struct dvobj_priv *dvobj, u8 rate, u8 max_bw)
+{
+	struct rf_ctl_t *rf_ctl = dvobj_to_rfctl(dvobj);
+	u8 bw;
+	u8 bw_bmp = 0;
+	u32 rate_bmp;
+
+	if (!IS_HT_RATE(rate)) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	rate_bmp = 1 << (rate - MGN_MCS0);
+
+	if (max_bw > CHANNEL_WIDTH_40)
+		max_bw = CHANNEL_WIDTH_40;
+
+	for (bw = CHANNEL_WIDTH_20; bw <= max_bw; bw++) {
+		/* RA may use lower rate for retry */
+		if (rf_ctl->rate_bmp_ht_by_bw[bw] >= rate_bmp)
+			bw_bmp |= ch_width_to_bw_cap(bw);
+	}
+
+exit:
+	return bw_bmp;
+}
+
+u8 rtw_get_tx_bw_bmp_of_vht_rate(struct dvobj_priv *dvobj, u8 rate, u8 max_bw)
+{
+	struct rf_ctl_t *rf_ctl = dvobj_to_rfctl(dvobj);
+	u8 bw;
+	u8 bw_bmp = 0;
+	u32 rate_bmp;
+
+	if (!IS_VHT_RATE(rate)) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	rate_bmp = 1 << (rate - MGN_VHT1SS_MCS0);
+
+	if (max_bw > CHANNEL_WIDTH_160)
+		max_bw = CHANNEL_WIDTH_160;
+
+	for (bw = CHANNEL_WIDTH_20; bw <= max_bw; bw++) {
+		/* RA may use lower rate for retry */
+		if (rf_ctl->rate_bmp_vht_by_bw[bw] >= rate_bmp)
+			bw_bmp |= ch_width_to_bw_cap(bw);
+	}
+
+exit:
+	return bw_bmp;
+}
+
+u8 query_ra_short_GI(struct sta_info *psta, u8 bw)
+{
+	u8	sgi = false, sgi_20m = false, sgi_40m = false, sgi_80m = false;
+
+	sgi_20m = psta->htpriv.sgi_20m;
+	sgi_40m = psta->htpriv.sgi_40m;
+
+	switch (bw) {
+	case CHANNEL_WIDTH_80:
+		sgi = sgi_80m;
+		break;
+	case CHANNEL_WIDTH_40:
+		sgi = sgi_40m;
+		break;
+	case CHANNEL_WIDTH_20:
+	default:
+		sgi = sgi_20m;
+		break;
+	}
+
+	return sgi;
+}
+
+static void update_attrib_vcs_info(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	u32	sz;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	/* struct sta_info	*psta = pattrib->psta; */
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/*
+		if(pattrib->psta)
+		{
+			psta = pattrib->psta;
+		}
+		else
+		{
+			RTW_INFO("%s, call rtw_get_stainfo()\n", __func__);
+			psta=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] );
+		}
+
+		if(psta==NULL)
+		{
+			RTW_INFO("%s, psta==NUL\n", __func__);
+			return;
+		}
+
+		if(!(psta->state &_FW_LINKED))
+		{
+			RTW_INFO("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+			return;
+		}
+	*/
+
+	if (pattrib->nr_frags != 1)
+		sz = padapter->xmitpriv.frag_len;
+	else /* no frag */
+		sz = pattrib->last_txcmdsz;
+
+	/* (1) RTS_Threshold is compared to the MPDU, not MSDU. */
+	/* (2) If there are more than one frag in  this MSDU, only the first frag uses protection frame. */
+	/*		Other fragments are protected by previous fragment. */
+	/*		So we only need to check the length of first fragment. */
+	if (pmlmeext->cur_wireless_mode < WIRELESS_11_24N  || padapter->registrypriv.wifi_spec) {
+		if (sz > padapter->registrypriv.rts_thresh)
+			pattrib->vcs_mode = RTS_CTS;
+		else {
+			if (pattrib->rtsen)
+				pattrib->vcs_mode = RTS_CTS;
+			else if (pattrib->cts2self)
+				pattrib->vcs_mode = CTS_TO_SELF;
+			else
+				pattrib->vcs_mode = NONE_VCS;
+		}
+	} else {
+		while (true) {
+			/* IOT action */
+			if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS) && (pattrib->ampdu_en) &&
+			    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+				pattrib->vcs_mode = CTS_TO_SELF;
+				break;
+			}
+
+			/* check ERP protection */
+			if (pattrib->rtsen || pattrib->cts2self) {
+				if (pattrib->rtsen)
+					pattrib->vcs_mode = RTS_CTS;
+				else if (pattrib->cts2self)
+					pattrib->vcs_mode = CTS_TO_SELF;
+
+				break;
+			}
+
+			/* check HT op mode */
+			if (pattrib->ht_en) {
+				u8 HTOpMode = pmlmeinfo->HT_protection;
+				if ((pmlmeext->cur_bwmode && (HTOpMode == 2 || HTOpMode == 3)) ||
+				    (!pmlmeext->cur_bwmode && HTOpMode == 3)) {
+					pattrib->vcs_mode = RTS_CTS;
+					break;
+				}
+			}
+
+			/* check rts */
+			if (sz > padapter->registrypriv.rts_thresh) {
+				pattrib->vcs_mode = RTS_CTS;
+				break;
+			}
+
+			/* to do list: check MIMO power save condition. */
+
+			/* check AMPDU aggregation for TXOP */
+			if ((pattrib->ampdu_en)) {
+				pattrib->vcs_mode = RTS_CTS;
+				break;
+			}
+
+			pattrib->vcs_mode = NONE_VCS;
+			break;
+		}
+	}
+
+	/* for debug : force driver control vrtl_carrier_sense. */
+	if (padapter->driver_vcs_en == 1) {
+		/* u8 driver_vcs_en; */ /* Enable=1, Disable=0 driver control vrtl_carrier_sense. */
+		/* u8 driver_vcs_type; */ /* force 0:disable VCS, 1:RTS-CTS, 2:CTS-to-self when vcs_en=1. */
+		pattrib->vcs_mode = padapter->driver_vcs_type;
+	}
+
+}
+
+static void update_attrib_phy_info(_adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)
+{
+	struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
+	u8 bw;
+
+	pattrib->rtsen = psta->rtsen;
+	pattrib->cts2self = psta->cts2self;
+
+	pattrib->mdata = 0;
+	pattrib->eosp = 0;
+	pattrib->triggered = 0;
+	pattrib->ampdu_spacing = 0;
+
+	/* qos_en, ht_en, init rate, ,bw, ch_offset, sgi */
+	pattrib->qos_en = psta->qos_option;
+
+	pattrib->raid = psta->raid;
+
+	bw = rtw_get_tx_bw_mode(padapter, psta);
+	pattrib->bwmode = rtw_min(bw, mlmeext->cur_bwmode);
+	pattrib->sgi = query_ra_short_GI(psta, pattrib->bwmode);
+
+	pattrib->ldpc = psta->ldpc;
+	pattrib->stbc = psta->stbc;
+
+	pattrib->ht_en = psta->htpriv.ht_option;
+	pattrib->ch_offset = psta->htpriv.ch_offset;
+	pattrib->ampdu_en = false;
+
+	if (padapter->driver_ampdu_spacing != 0xFF) /* driver control AMPDU Density for peer sta's rx */
+		pattrib->ampdu_spacing = padapter->driver_ampdu_spacing;
+	else
+		pattrib->ampdu_spacing = psta->htpriv.rx_ampdu_min_spacing;
+
+	/* check if enable ampdu */
+	if (pattrib->ht_en && psta->htpriv.ampdu_enable) {
+		if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority)) {
+			pattrib->ampdu_en = true;
+			if (psta->htpriv.tx_amsdu_enable)
+				pattrib->amsdu_ampdu_en = true;
+			else
+				pattrib->amsdu_ampdu_en = false;
+		}
+	}
+	/* if(pattrib->ht_en && psta->htpriv.ampdu_enable) */
+	/* { */
+	/*	if(psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority)) */
+	/*		pattrib->ampdu_en = true; */
+	/* }	 */
+
+#ifdef CONFIG_TDLS
+	if (pattrib->direct_link) {
+		psta = pattrib->ptdls_sta;
+
+		pattrib->raid = psta->raid;
+		pattrib->bwmode = rtw_get_tx_bw_mode(padapter, psta);
+		pattrib->ht_en = psta->htpriv.ht_option;
+		pattrib->ch_offset = psta->htpriv.ch_offset;
+		pattrib->sgi = query_ra_short_GI(psta, pattrib->bwmode);
+	}
+#endif /* CONFIG_TDLS */
+
+	pattrib->retry_ctrl = false;
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (psta->isrc && psta->pid > 0)
+		pattrib->pctrl = true;
+#endif
+
+}
+
+static s32 update_attrib_sec_info(_adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)
+{
+	sint res = _SUCCESS;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	sint bmcast = IS_MCAST(pattrib->ra);
+
+	memset(pattrib->dot118021x_UncstKey.skey,  0, 16);
+	memset(pattrib->dot11tkiptxmickey.skey,  0, 16);
+	pattrib->mac_id = psta->mac_id;
+
+	if (psta->ieee8021x_blocked) {
+
+		pattrib->encrypt = 0;
+
+		if ((pattrib->ether_type != 0x888e) && (check_fwstate(pmlmepriv, WIFI_MP_STATE) == false)) {
+#ifdef DBG_TX_DROP_FRAME
+			RTW_INFO("DBG_TX_DROP_FRAME %s psta->ieee8021x_blocked,  pattrib->ether_type(%04x) != 0x888e\n", __func__, pattrib->ether_type);
+#endif
+			res = _FAIL;
+			goto exit;
+		}
+	} else {
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast);
+
+#ifdef CONFIG_WAPI_SUPPORT
+		if (pattrib->ether_type == 0x88B4)
+			pattrib->encrypt = _NO_PRIVACY_;
+#endif
+
+		switch (psecuritypriv->dot11AuthAlgrthm) {
+		case dot11AuthAlgrthm_Open:
+		case dot11AuthAlgrthm_Shared:
+		case dot11AuthAlgrthm_Auto:
+			pattrib->key_idx = (u8)psecuritypriv->dot11PrivacyKeyIndex;
+			break;
+		case dot11AuthAlgrthm_8021X:
+			if (bmcast)
+				pattrib->key_idx = (u8)psecuritypriv->dot118021XGrpKeyid;
+			else
+				pattrib->key_idx = 0;
+			break;
+		default:
+			pattrib->key_idx = 0;
+			break;
+		}
+
+		/* For WPS 1.0 WEP, driver should not encrypt EAPOL Packet for WPS handshake. */
+		if (((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) && (pattrib->ether_type == 0x888e))
+			pattrib->encrypt = _NO_PRIVACY_;
+
+	}
+
+#ifdef CONFIG_TDLS
+	if (pattrib->direct_link) {
+		if (pattrib->encrypt > 0)
+			pattrib->encrypt = _AES_;
+	}
+#endif
+
+	switch (pattrib->encrypt) {
+	case _WEP40_:
+	case _WEP104_:
+		pattrib->iv_len = 4;
+		pattrib->icv_len = 4;
+		WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+		break;
+
+	case _TKIP_:
+		pattrib->iv_len = 8;
+		pattrib->icv_len = 4;
+
+		if (psecuritypriv->busetkipkey == _FAIL) {
+#ifdef DBG_TX_DROP_FRAME
+			RTW_INFO("DBG_TX_DROP_FRAME %s psecuritypriv->busetkipkey(%d)==_FAIL drop packet\n", __func__, psecuritypriv->busetkipkey);
+#endif
+			res = _FAIL;
+			goto exit;
+		}
+
+		if (bmcast)
+			TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+		else
+			TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
+
+		memcpy(pattrib->dot11tkiptxmickey.skey, psta->dot11tkiptxmickey.skey, 16);
+
+		break;
+
+	case _AES_:
+
+		pattrib->iv_len = 8;
+		pattrib->icv_len = 8;
+
+		if (bmcast)
+			AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+		else
+			AES_IV(pattrib->iv, psta->dot11txpn, 0);
+
+		break;
+
+#ifdef CONFIG_WAPI_SUPPORT
+	case _SMS4_:
+		pattrib->iv_len = 18;
+		pattrib->icv_len = 16;
+		rtw_wapi_get_iv(padapter, pattrib->ra, pattrib->iv);
+		break;
+#endif
+	default:
+		pattrib->iv_len = 0;
+		pattrib->icv_len = 0;
+		break;
+	}
+
+	if (pattrib->encrypt > 0)
+		memcpy(pattrib->dot118021x_UncstKey.skey, psta->dot118021x_UncstKey.skey, 16);
+
+	if (pattrib->encrypt &&
+	    ((padapter->securitypriv.sw_encrypt) || (psecuritypriv->hw_decrypted == false))) {
+		pattrib->bswenc = true;
+	} else {
+		pattrib->bswenc = false;
+	}
+
+#if defined(CONFIG_CONCURRENT_MODE)
+	pattrib->bmc_camid = padapter->securitypriv.dot118021x_bmc_cam_id;
+#endif
+
+	if (pattrib->encrypt && bmcast && _rtw_camctl_chk_flags(padapter, SEC_STATUS_STA_PK_GK_CONFLICT_DIS_BMC_SEARCH))
+		pattrib->bswenc = true;
+
+#ifdef CONFIG_WAPI_SUPPORT
+	if (pattrib->encrypt == _SMS4_)
+		pattrib->bswenc = false;
+#endif
+
+exit:
+
+	return res;
+
+}
+
+u8	qos_acm(u8 acm_mask, u8 priority)
+{
+	u8	change_priority = priority;
+
+	switch (priority) {
+	case 0:
+	case 3:
+		if (acm_mask & BIT(1))
+			change_priority = 1;
+		break;
+	case 1:
+	case 2:
+		break;
+	case 4:
+	case 5:
+		if (acm_mask & BIT(2))
+			change_priority = 0;
+		break;
+	case 6:
+	case 7:
+		if (acm_mask & BIT(3))
+			change_priority = 5;
+		break;
+	default:
+		RTW_INFO("qos_acm(): invalid pattrib->priority: %d!!!\n", priority);
+		break;
+	}
+
+	return change_priority;
+}
+
+static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
+{
+	struct ethhdr etherhdr;
+	struct iphdr ip_hdr;
+	s32 UserPriority = 0;
+
+	_rtw_open_pktfile(ppktfile->pkt, ppktfile);
+	_rtw_pktfile_read(ppktfile, (unsigned char *)&etherhdr, ETH_HLEN);
+
+	/* get UserPriority from IP hdr */
+	if (pattrib->ether_type == 0x0800) {
+		_rtw_pktfile_read(ppktfile, (u8 *)&ip_hdr, sizeof(ip_hdr));
+		/*		UserPriority = (ntohs(ip_hdr.tos) >> 5) & 0x3; */
+		UserPriority = ip_hdr.tos >> 5;
+	}
+	/*
+		else if (pattrib->ether_type == 0x888e) {
+
+			UserPriority = 7;
+		}
+	*/
+	pattrib->priority = UserPriority;
+	pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN;
+	pattrib->subtype = WIFI_QOS_DATA_TYPE;
+}
+
+#ifdef CONFIG_TDLS
+u8 rtw_check_tdls_established(_adapter *padapter, struct pkt_attrib *pattrib)
+{
+	pattrib->ptdls_sta = NULL;
+
+	pattrib->direct_link = false;
+	if (padapter->tdlsinfo.link_established) {
+		pattrib->ptdls_sta = rtw_get_stainfo(&padapter->stapriv, pattrib->dst);
+		if ((pattrib->ptdls_sta != NULL) &&
+		    (pattrib->ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) &&
+		    (pattrib->ether_type != 0x0806)) {
+			pattrib->direct_link = true;
+		}
+	}
+
+	return pattrib->direct_link;
+}
+
+s32 update_tdls_attrib(_adapter *padapter, struct pkt_attrib *pattrib)
+{
+
+	struct sta_info *psta = NULL;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv		*pqospriv = &pmlmepriv->qospriv;
+
+	s32 res = _SUCCESS;
+
+	psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+	if (psta == NULL)	{
+		res = _FAIL;
+		goto exit;
+	}
+
+	pattrib->mac_id = psta->mac_id;
+	pattrib->psta = psta;
+	pattrib->ack_policy = 0;
+	/* get ether_hdr_len */
+	pattrib->pkt_hdrlen = ETH_HLEN;
+
+	/* [TDLS] TODO: setup req/rsp should be AC_BK */
+	if (pqospriv->qos_option &&  psta->qos_option) {
+		pattrib->priority = 4;	/* tdls management frame should be AC_VI */
+		pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN;
+		pattrib->subtype = WIFI_QOS_DATA_TYPE;
+	} else {
+		pattrib->priority = 0;
+		pattrib->hdrlen = WLAN_HDR_A3_LEN;
+		pattrib->subtype = WIFI_DATA_TYPE;
+	}
+
+	/* TODO:_lock */
+	if (update_attrib_sec_info(padapter, pattrib, psta) == _FAIL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	update_attrib_phy_info(padapter, pattrib, psta);
+
+exit:
+
+	return res;
+}
+
+#endif /* CONFIG_TDLS */
+
+/*get non-qos hw_ssn control register,mapping to REG_HW_SEQ0,1,2,3*/
+inline u8 rtw_get_hwseq_no(_adapter *padapter)
+{
+	u8 hwseq_num = 0;
+#ifdef CONFIG_CONCURRENT_MODE
+	if (padapter->adapter_type != PRIMARY_ADAPTER)
+		hwseq_num = 1;
+	/* else */
+	/*	hwseq_num = 2; */
+#endif /* CONFIG_CONCURRENT_MODE */
+	return hwseq_num;
+}
+static s32 update_attrib(_adapter *padapter, _pkt *pkt, struct pkt_attrib *pattrib)
+{
+	uint i;
+	struct pkt_file pktfile;
+	struct sta_info *psta = NULL;
+	struct ethhdr etherhdr;
+
+	sint bmcast;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv		*pqospriv = &pmlmepriv->qospriv;
+	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	sint res = _SUCCESS;
+
+	DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib);
+
+	_rtw_open_pktfile(pkt, &pktfile);
+	i = _rtw_pktfile_read(&pktfile, (u8 *)&etherhdr, ETH_HLEN);
+
+	pattrib->ether_type = ntohs(etherhdr.h_proto);
+
+	memcpy(pattrib->dst, &etherhdr.h_dest, ETH_ALEN);
+	memcpy(pattrib->src, &etherhdr.h_source, ETH_ALEN);
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) ||
+	    (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))) {
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, adapter_mac_addr(padapter), ETH_ALEN);
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_adhoc);
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+#ifdef CONFIG_TDLS
+		if (rtw_check_tdls_established(padapter, pattrib))
+			memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);	/* For TDLS direct link Tx, set ra to be same to dst */
+		else
+#endif
+			memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+		memcpy(pattrib->ta, adapter_mac_addr(padapter), ETH_ALEN);
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_sta);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN);
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_ap);
+	} else
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_unknown);
+
+	bmcast = IS_MCAST(pattrib->ra);
+	if (bmcast) {
+		psta = rtw_get_bcmc_stainfo(padapter);
+		if (psta == NULL) { /* if we cannot get psta => drop the pkt */
+			DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sta);
+#ifdef DBG_TX_DROP_FRAME
+			RTW_INFO("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra));
+#endif
+			res = _FAIL;
+			goto exit;
+		}
+	} else {
+		psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+		if (psta == NULL) { /* if we cannot get psta => drop the pkt */
+			DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_sta);
+#ifdef DBG_TX_DROP_FRAME
+			RTW_INFO("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra));
+#endif
+			res = _FAIL;
+			goto exit;
+		} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) && !(psta->state & _FW_LINKED)) {
+			DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_ap_link);
+			res = _FAIL;
+			goto exit;
+		}
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_link);
+		RTW_INFO("%s-"ADPT_FMT" psta("MAC_FMT")->state(0x%x) != _FW_LINKED\n", __func__, ADPT_ARG(padapter), MAC_ARG(psta->hwaddr), psta->state);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pattrib->pktlen = pktfile.pkt_len;
+
+	/* TODO: 802.1Q VLAN header */
+	/* TODO: IPV6 */
+
+	if (ETH_P_IP == pattrib->ether_type) {
+		u8 ip[20];
+
+		_rtw_pktfile_read(&pktfile, ip, 20);
+
+		if (GET_IPV4_IHL(ip) * 4 > 20)
+			_rtw_pktfile_read(&pktfile, NULL, GET_IPV4_IHL(ip) - 20);
+
+		pattrib->icmp_pkt = 0;
+		pattrib->dhcp_pkt = 0;
+
+		if (GET_IPV4_PROTOCOL(ip) == 0x01) { /* ICMP */
+			pattrib->icmp_pkt = 1;
+			DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_icmp);
+
+		} else if (GET_IPV4_PROTOCOL(ip) == 0x11) { /* UDP */
+			u8 udp[8];
+
+			_rtw_pktfile_read(&pktfile, udp, 8);
+
+			if ((GET_UDP_SRC(udp) == 68 && GET_UDP_DST(udp) == 67)
+				|| (GET_UDP_SRC(udp) == 67 && GET_UDP_DST(udp) == 68)
+			) {
+				/* 67 : UDP BOOTP server, 68 : UDP BOOTP client */
+				if (pattrib->pktlen > 282) { /* MINIMUM_DHCP_PACKET_SIZE */
+					pattrib->dhcp_pkt = 1;
+					DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_dhcp);
+					if (0)
+						RTW_INFO("send DHCP packet\n");
+				}
+			}
+
+		} else if (GET_IPV4_PROTOCOL(ip) == 0x06 /* TCP */
+			&& rtw_st_ctl_chk_reg_s_proto(&psta->st_ctl, 0x06)
+		) {
+			u8 tcp[20];
+
+			_rtw_pktfile_read(&pktfile, tcp, 20);
+
+			if (rtw_st_ctl_chk_reg_rule(&psta->st_ctl, padapter, IPV4_SRC(ip), TCP_SRC(tcp), IPV4_DST(ip), TCP_DST(tcp))) {
+				if (GET_TCP_SYN(tcp) && GET_TCP_ACK(tcp)) {
+					session_tracker_add_cmd(padapter, psta
+						, IPV4_SRC(ip), TCP_SRC(tcp)
+						, IPV4_SRC(ip), TCP_DST(tcp));
+					if (DBG_SESSION_TRACKER)
+						RTW_INFO(FUNC_ADPT_FMT" local:"IP_FMT":"PORT_FMT", remote:"IP_FMT":"PORT_FMT" SYN-ACK\n"
+							, FUNC_ADPT_ARG(padapter)
+							, IP_ARG(IPV4_SRC(ip)), PORT_ARG(TCP_SRC(tcp))
+							, IP_ARG(IPV4_DST(ip)), PORT_ARG(TCP_DST(tcp)));
+				}
+				if (GET_TCP_FIN(tcp)) {
+					session_tracker_del_cmd(padapter, psta
+						, IPV4_SRC(ip), TCP_SRC(tcp)
+						, IPV4_SRC(ip), TCP_DST(tcp));
+					if (DBG_SESSION_TRACKER)
+						RTW_INFO(FUNC_ADPT_FMT" local:"IP_FMT":"PORT_FMT", remote:"IP_FMT":"PORT_FMT" FIN\n"
+							, FUNC_ADPT_ARG(padapter)
+							, IP_ARG(IPV4_SRC(ip)), PORT_ARG(TCP_SRC(tcp))
+							, IP_ARG(IPV4_DST(ip)), PORT_ARG(TCP_DST(tcp)));
+				}
+			}
+		}
+
+	} else if (0x888e == pattrib->ether_type)
+		RTW_PRINT("send eapol packet\n");
+
+	if ((pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1))
+		rtw_mi_set_scan_deny(padapter, 3000);
+
+#ifdef CONFIG_LPS
+	/* If EAPOL , ARP , OR DHCP packet, driver must be in active mode. */
+#ifdef CONFIG_WAPI_SUPPORT
+	if ((pattrib->ether_type == 0x88B4) || (pattrib->ether_type == 0x0806) || (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1))
+#else /* !CONFIG_WAPI_SUPPORT */
+	if (pattrib->icmp_pkt == 1)
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1);
+	else if (pattrib->dhcp_pkt == 1)
+#endif
+	{
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_active);
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SPECIAL_PACKET, 1);
+	}
+#endif /* CONFIG_LPS */
+
+#ifdef CONFIG_BEAMFORMING
+	update_attrib_txbf_info(padapter, pattrib, psta);
+#endif
+
+	/* TODO:_lock */
+	if (update_attrib_sec_info(padapter, pattrib, psta) == _FAIL) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sec);
+		res = _FAIL;
+		goto exit;
+	}
+
+	update_attrib_phy_info(padapter, pattrib, psta);
+
+	/* RTW_INFO("%s ==> mac_id(%d)\n",__func__,pattrib->mac_id ); */
+
+	pattrib->psta = psta;
+	/* TODO:_unlock */
+
+	pattrib->pctrl = 0;
+
+	pattrib->ack_policy = 0;
+	/* get ether_hdr_len */
+	pattrib->pkt_hdrlen = ETH_HLEN;/* (pattrib->ether_type == 0x8100) ? (14 + 4 ): 14; */ /* vlan tag */
+
+	pattrib->hdrlen = WLAN_HDR_A3_LEN;
+	pattrib->subtype = WIFI_DATA_TYPE;
+	pattrib->priority = 0;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE | WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE)) {
+		if (pattrib->qos_en)
+			set_qos(&pktfile, pattrib);
+	} else {
+#ifdef CONFIG_TDLS
+		if (pattrib->direct_link) {
+			if (pattrib->qos_en)
+				set_qos(&pktfile, pattrib);
+		} else
+#endif
+		{
+			if (pqospriv->qos_option) {
+				set_qos(&pktfile, pattrib);
+
+				if (pmlmepriv->acm_mask != 0)
+					pattrib->priority = qos_acm(pmlmepriv->acm_mask, pattrib->priority);
+			}
+		}
+	}
+
+	/* pattrib->priority = 5; */ /* force to used VI queue, for testing */
+	pattrib->hw_ssn_sel = pxmitpriv->hw_ssn_seq_no;
+	rtw_set_tx_chksum_offload(pkt, pattrib);
+
+exit:
+
+	return res;
+}
+
+static s32 xmitframe_addmic(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	sint			curfragnum, length;
+	u8	*pframe, *payload, mic[8];
+	struct	mic_data		micdata;
+	/* struct	sta_info		*stainfo; */
+	struct	qos_priv   *pqospriv = &(padapter->mlmepriv.qospriv);
+	struct	pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct	xmit_priv		*pxmitpriv = &padapter->xmitpriv;
+	u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
+	u8 hw_hdr_offset = 0;
+	sint bmcst = IS_MCAST(pattrib->ra);
+
+	/*
+		if(pattrib->psta)
+		{
+			stainfo = pattrib->psta;
+		}
+		else
+		{
+			RTW_INFO("%s, call rtw_get_stainfo()\n", __func__);
+			stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]);
+		}
+
+		if(stainfo==NULL)
+		{
+			RTW_INFO("%s, psta==NUL\n", __func__);
+			return _FAIL;
+		}
+
+		if(!(stainfo->state &_FW_LINKED))
+		{
+			RTW_INFO("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
+			return _FAIL;
+		}
+	*/
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+	hw_hdr_offset = TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);;
+#else
+#ifdef CONFIG_TX_EARLY_MODE
+	hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE;
+#else
+	hw_hdr_offset = TXDESC_OFFSET;
+#endif
+#endif
+
+	if (pattrib->encrypt == _TKIP_) { /* if(psecuritypriv->dot11PrivacyAlgrthm==_TKIP_PRIVACY_) */
+		/* encode mic code */
+		/* if(stainfo!= NULL) */
+		{
+			u8 null_key[16] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+
+			pframe = pxmitframe->buf_addr + hw_hdr_offset;
+
+			if (bmcst) {
+				if (!memcmp(psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey, null_key, 16)) {
+					/* DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey==0\n"); */
+					/* rtw_msleep_os(10); */
+					return _FAIL;
+				}
+				/* start to calculate the mic code */
+				rtw_secmicsetkey(&micdata, psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey);
+			} else {
+				if (!memcmp(&pattrib->dot11tkiptxmickey.skey[0], null_key, 16)) {
+					/* DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey==0\n"); */
+					/* rtw_msleep_os(10); */
+					return _FAIL;
+				}
+				/* start to calculate the mic code */
+				rtw_secmicsetkey(&micdata, &pattrib->dot11tkiptxmickey.skey[0]);
+			}
+
+			if (pframe[1] & 1) { /* ToDS==1 */
+				rtw_secmicappend(&micdata, &pframe[16], 6);  /* DA */
+				if (pframe[1] & 2) /* From Ds==1 */
+					rtw_secmicappend(&micdata, &pframe[24], 6);
+				else
+					rtw_secmicappend(&micdata, &pframe[10], 6);
+			} else {	/* ToDS==0 */
+				rtw_secmicappend(&micdata, &pframe[4], 6);   /* DA */
+				if (pframe[1] & 2) /* From Ds==1 */
+					rtw_secmicappend(&micdata, &pframe[16], 6);
+				else
+					rtw_secmicappend(&micdata, &pframe[10], 6);
+
+			}
+
+			/* if(pqospriv->qos_option==1) */
+			if (pattrib->qos_en)
+				priority[0] = (u8)pxmitframe->attrib.priority;
+
+			rtw_secmicappend(&micdata, &priority[0], 4);
+
+			payload = pframe;
+
+			for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
+				payload = (u8 *)RND4((SIZE_PTR)(payload));
+
+				payload = payload + pattrib->hdrlen + pattrib->iv_len;
+				if ((curfragnum + 1) == pattrib->nr_frags) {
+					length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - ((pattrib->bswenc) ? pattrib->icv_len : 0);
+					rtw_secmicappend(&micdata, payload, length);
+					payload = payload + length;
+				} else {
+					length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - ((pattrib->bswenc) ? pattrib->icv_len : 0);
+					rtw_secmicappend(&micdata, payload, length);
+					payload = payload + length + pattrib->icv_len;
+				}
+			}
+			rtw_secgetmic(&micdata, &(mic[0]));
+			/* add mic code  and add the mic code length in last_txcmdsz */
+
+			memcpy(payload, &(mic[0]), 8);
+			pattrib->last_txcmdsz += 8;
+
+			payload = payload - pattrib->last_txcmdsz + 8;
+		}
+	}
+
+	return _SUCCESS;
+}
+
+/*#define DBG_TX_SW_ENCRYPTOR*/
+
+static s32 xmitframe_swencrypt(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+
+	struct	pkt_attrib	*pattrib = &pxmitframe->attrib;
+	/* struct 	security_priv	*psecuritypriv=&padapter->securitypriv; */
+
+	/* if((psecuritypriv->sw_encrypt)||(pattrib->bswenc))	 */
+	if (pattrib->bswenc) {
+#ifdef DBG_TX_SW_ENCRYPTOR
+		RTW_INFO(ADPT_FMT" - sec_type:%s DO SW encryption\n",
+			ADPT_ARG(padapter), security_type_str(pattrib->encrypt));
+#endif
+
+		switch (pattrib->encrypt) {
+		case _WEP40_:
+		case _WEP104_:
+			rtw_wep_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		case _TKIP_:
+			rtw_tkip_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		case _AES_:
+			rtw_aes_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+#ifdef CONFIG_WAPI_SUPPORT
+		case _SMS4_:
+			rtw_sms4_encrypt(padapter, (u8 *)pxmitframe);
+#endif
+		default:
+			break;
+		}
+
+	}
+
+	return _SUCCESS;
+}
+
+s32 rtw_make_wlanhdr(_adapter *padapter , u8 *hdr, struct pkt_attrib *pattrib)
+{
+	u16 *qc;
+
+	struct rtw_ieee80211_hdr *pwlanhdr = (struct rtw_ieee80211_hdr *)hdr;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	u8 qos_option = false;
+	sint res = _SUCCESS;
+	__le16 *fctrl = &pwlanhdr->frame_ctl;
+
+	/* struct sta_info *psta; */
+
+	/* sint bmcst = IS_MCAST(pattrib->ra); */
+
+	/*
+		psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+		if(pattrib->psta != psta)
+		{
+			RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+			return;
+		}
+
+		if(psta==NULL)
+		{
+			RTW_INFO("%s, psta==NUL\n", __func__);
+			return _FAIL;
+		}
+
+		if(!(psta->state &_FW_LINKED))
+		{
+			RTW_INFO("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+			return _FAIL;
+		}
+	*/
+
+	memset(hdr, 0, WLANHDR_OFFSET);
+
+	set_frame_sub_type(fctrl, pattrib->subtype);
+
+	if (pattrib->subtype & WIFI_DATA_TYPE) {
+		if ((check_fwstate(pmlmepriv,  WIFI_STATION_STATE))) {
+#ifdef CONFIG_TDLS
+			if (pattrib->direct_link) {
+				/* TDLS data transfer, ToDS=0, FrDs=0 */
+				memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+				memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+				memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+
+				if (pattrib->qos_en)
+					qos_option = true;
+			} else
+#endif /* CONFIG_TDLS */
+			{
+				/* to_ds = 1, fr_ds = 0; */
+				/* 1.Data transfer to AP */
+				/* 2.Arp pkt will relayed by AP */
+				SetToDs(fctrl);
+				memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
+				memcpy(pwlanhdr->addr2, pattrib->ta, ETH_ALEN);
+				memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+
+				if (pqospriv->qos_option)
+					qos_option = true;
+			}
+		} else if ((check_fwstate(pmlmepriv,  WIFI_AP_STATE))) {
+			/* to_ds = 0, fr_ds = 1; */
+			SetFrDs(fctrl);
+			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+			memcpy(pwlanhdr->addr2, get_bssid(pmlmepriv), ETH_ALEN);
+			memcpy(pwlanhdr->addr3, pattrib->src, ETH_ALEN);
+
+			if (pattrib->qos_en)
+				qos_option = true;
+		} else if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) ||
+			(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))) {
+			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+			memcpy(pwlanhdr->addr2, pattrib->ta, ETH_ALEN);
+			memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+
+			if (pattrib->qos_en)
+				qos_option = true;
+		} else {
+			res = _FAIL;
+			goto exit;
+		}
+
+		if (pattrib->mdata)
+			SetMData(fctrl);
+
+		if (pattrib->encrypt)
+			SetPrivacy(fctrl);
+
+		if (qos_option) {
+			qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
+
+			if (pattrib->priority)
+				SetPriority(qc, pattrib->priority);
+
+			SetEOSP(qc, pattrib->eosp);
+
+			SetAckpolicy(qc, pattrib->ack_policy);
+
+			if(pattrib->amsdu)
+				SetAMsdu(qc, pattrib->amsdu);
+		}
+
+		/* TODO: fill HT Control Field */
+
+		/* Update Seq Num will be handled by f/w */
+		{
+			struct sta_info *psta;
+			psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+			if (pattrib->psta != psta) {
+				RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+				return _FAIL;
+			}
+
+			if (psta == NULL) {
+				RTW_INFO("%s, psta==NUL\n", __func__);
+				return _FAIL;
+			}
+
+			if (!(psta->state & _FW_LINKED)) {
+				RTW_INFO("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+				return _FAIL;
+			}
+
+			if (psta) {
+				psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
+				psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
+				pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
+
+				SetSeqNum(hdr, pattrib->seqnum);
+
+				/* re-check if enable ampdu by BA_starting_seqctrl */
+				if (pattrib->ampdu_en) {
+					u16 tx_seq;
+
+					tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f];
+
+					/* check BA_starting_seqctrl */
+					if (SN_LESS(pattrib->seqnum, tx_seq)) {
+						/* RTW_INFO("tx ampdu seqnum(%d) < tx_seq(%d)\n", pattrib->seqnum, tx_seq); */
+						pattrib->ampdu_en = false;/* AGG BK */
+					} else if (SN_EQUAL(pattrib->seqnum, tx_seq)) {
+						psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (tx_seq + 1) & 0xfff;
+
+						pattrib->ampdu_en = true;/* AGG EN */
+					} else {
+						/* RTW_INFO("tx ampdu over run\n"); */
+						psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum + 1) & 0xfff;
+						pattrib->ampdu_en = true;/* AGG EN */
+					}
+
+				}
+			}
+		}
+	} else {
+
+	}
+
+exit:
+
+	return res;
+}
+
+s32 rtw_txframes_pending(_adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	return ((_rtw_queue_empty(&pxmitpriv->be_pending) == false) ||
+		(_rtw_queue_empty(&pxmitpriv->bk_pending) == false) ||
+		(_rtw_queue_empty(&pxmitpriv->vi_pending) == false) ||
+		(_rtw_queue_empty(&pxmitpriv->vo_pending) == false));
+}
+
+s32 rtw_txframes_sta_ac_pending(_adapter *padapter, struct pkt_attrib *pattrib)
+{
+	struct sta_info *psta;
+	struct tx_servq *ptxservq;
+	int priority = pattrib->priority;
+	/*
+		if(pattrib->psta)
+		{
+			psta = pattrib->psta;
+		}
+		else
+		{
+			RTW_INFO("%s, call rtw_get_stainfo()\n", __func__);
+			psta=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]);
+		}
+	*/
+	psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	if (pattrib->psta != psta) {
+		RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+		return 0;
+	}
+
+	if (psta == NULL) {
+		RTW_INFO("%s, psta==NUL\n", __func__);
+		return 0;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		RTW_INFO("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return 0;
+	}
+
+	switch (priority) {
+	case 1:
+	case 2:
+		ptxservq = &(psta->sta_xmitpriv.bk_q);
+		break;
+	case 4:
+	case 5:
+		ptxservq = &(psta->sta_xmitpriv.vi_q);
+		break;
+	case 6:
+	case 7:
+		ptxservq = &(psta->sta_xmitpriv.vo_q);
+		break;
+	case 0:
+	case 3:
+	default:
+		ptxservq = &(psta->sta_xmitpriv.be_q);
+		break;
+
+	}
+
+	return ptxservq->qcnt;
+}
+
+#ifdef CONFIG_TDLS
+
+int rtw_build_tdls_ies(_adapter *padapter, struct xmit_frame *pxmitframe, u8 *pframe, struct tdls_txmgmt *ptxmgmt)
+{
+	int res = _SUCCESS;
+
+	switch (ptxmgmt->action_code) {
+	case TDLS_SETUP_REQUEST:
+		rtw_build_tdls_setup_req_ies(padapter, pxmitframe, pframe, ptxmgmt);
+		break;
+	case TDLS_SETUP_RESPONSE:
+		rtw_build_tdls_setup_rsp_ies(padapter, pxmitframe, pframe, ptxmgmt);
+		break;
+	case TDLS_SETUP_CONFIRM:
+		rtw_build_tdls_setup_cfm_ies(padapter, pxmitframe, pframe, ptxmgmt);
+		break;
+	case TDLS_TEARDOWN:
+		rtw_build_tdls_teardown_ies(padapter, pxmitframe, pframe, ptxmgmt);
+		break;
+	case TDLS_DISCOVERY_REQUEST:
+		rtw_build_tdls_dis_req_ies(padapter, pxmitframe, pframe, ptxmgmt);
+		break;
+	case TDLS_PEER_TRAFFIC_INDICATION:
+		rtw_build_tdls_peer_traffic_indication_ies(padapter, pxmitframe, pframe, ptxmgmt);
+		break;
+#ifdef CONFIG_TDLS_CH_SW
+	case TDLS_CHANNEL_SWITCH_REQUEST:
+		rtw_build_tdls_ch_switch_req_ies(padapter, pxmitframe, pframe, ptxmgmt);
+		break;
+	case TDLS_CHANNEL_SWITCH_RESPONSE:
+		rtw_build_tdls_ch_switch_rsp_ies(padapter, pxmitframe, pframe, ptxmgmt);
+		break;
+#endif
+	case TDLS_PEER_TRAFFIC_RESPONSE:
+		rtw_build_tdls_peer_traffic_rsp_ies(padapter, pxmitframe, pframe, ptxmgmt);
+		break;
+#ifdef CONFIG_WFD
+	case TUNNELED_PROBE_REQ:
+		rtw_build_tunneled_probe_req_ies(padapter, pxmitframe, pframe);
+		break;
+	case TUNNELED_PROBE_RSP:
+		rtw_build_tunneled_probe_rsp_ies(padapter, pxmitframe, pframe);
+		break;
+#endif /* CONFIG_WFD */
+	default:
+		res = _FAIL;
+		break;
+	}
+
+	return res;
+}
+
+s32 rtw_make_tdls_wlanhdr(_adapter *padapter , u8 *hdr, struct pkt_attrib *pattrib, struct tdls_txmgmt *ptxmgmt)
+{
+	u16 *qc;
+	struct rtw_ieee80211_hdr *pwlanhdr = (struct rtw_ieee80211_hdr *)hdr;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL, *ptdls_sta = NULL;
+	u8 tdls_seq = 0, baddr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+
+	sint res = _SUCCESS;
+	u16 *fctrl = &pwlanhdr->frame_ctl;
+
+	memset(hdr, 0, WLANHDR_OFFSET);
+
+	set_frame_sub_type(fctrl, pattrib->subtype);
+
+	switch (ptxmgmt->action_code) {
+	case TDLS_SETUP_REQUEST:
+	case TDLS_SETUP_RESPONSE:
+	case TDLS_SETUP_CONFIRM:
+	case TDLS_PEER_TRAFFIC_INDICATION:
+	case TDLS_PEER_PSM_REQUEST:
+	case TUNNELED_PROBE_REQ:
+	case TUNNELED_PROBE_RSP:
+	case TDLS_DISCOVERY_REQUEST:
+		SetToDs(fctrl);
+		memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
+		memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+		break;
+	case TDLS_CHANNEL_SWITCH_REQUEST:
+	case TDLS_CHANNEL_SWITCH_RESPONSE:
+	case TDLS_PEER_PSM_RESPONSE:
+	case TDLS_PEER_TRAFFIC_RESPONSE:
+		memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+		tdls_seq = 1;
+		break;
+	case TDLS_TEARDOWN:
+		if (ptxmgmt->status_code == _RSON_TDLS_TEAR_UN_RSN_) {
+			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
+			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+			memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+			tdls_seq = 1;
+		} else {
+			SetToDs(fctrl);
+			memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
+			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
+			memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+		}
+		break;
+	}
+
+	if (pattrib->encrypt)
+		SetPrivacy(fctrl);
+
+	if (ptxmgmt->action_code == TDLS_PEER_TRAFFIC_RESPONSE)
+		SetPwrMgt(fctrl);
+
+	if (pqospriv->qos_option) {
+		qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
+		if (pattrib->priority)
+			SetPriority(qc, pattrib->priority);
+		SetAckpolicy(qc, pattrib->ack_policy);
+	}
+
+	psta = pattrib->psta;
+
+	/* 1. update seq_num per link by sta_info */
+	/* 2. rewrite encrypt to _AES_, also rewrite iv_len, icv_len */
+	if (tdls_seq == 1) {
+		ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst);
+		if (ptdls_sta) {
+			ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
+			ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
+			pattrib->seqnum = ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority];
+			SetSeqNum(hdr, pattrib->seqnum);
+
+			if (pattrib->encrypt) {
+				pattrib->encrypt = _AES_;
+				pattrib->iv_len = 8;
+				pattrib->icv_len = 8;
+				pattrib->bswenc = false;
+			}
+			pattrib->mac_id = ptdls_sta->mac_id;
+		} else {
+			res = _FAIL;
+			goto exit;
+		}
+	} else if (psta) {
+		psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
+		psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
+		pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
+		SetSeqNum(hdr, pattrib->seqnum);
+	}
+
+exit:
+
+	return res;
+}
+
+s32 rtw_xmit_tdls_coalesce(_adapter *padapter, struct xmit_frame *pxmitframe, struct tdls_txmgmt *ptxmgmt)
+{
+	s32 llc_sz;
+
+	u8 *pframe, *mem_start;
+
+	struct sta_info		*psta;
+	struct sta_priv		*pstapriv = &padapter->stapriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	u8 *pbuf_start;
+	s32 bmcst = IS_MCAST(pattrib->ra);
+	s32 res = _SUCCESS;
+
+	if (pattrib->psta)
+		psta = pattrib->psta;
+	else {
+		if (bmcst)
+			psta = rtw_get_bcmc_stainfo(padapter);
+		else
+			psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	}
+
+	if (psta == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	if (pxmitframe->buf_addr == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pbuf_start = pxmitframe->buf_addr;
+	mem_start = pbuf_start + TXDESC_OFFSET;
+
+	if (rtw_make_tdls_wlanhdr(padapter, mem_start, pattrib, ptxmgmt) == _FAIL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pframe = mem_start;
+	pframe += pattrib->hdrlen;
+
+	/* adding icv, if necessary... */
+	if (pattrib->iv_len) {
+		if (psta != NULL) {
+			switch (pattrib->encrypt) {
+			case _WEP40_:
+			case _WEP104_:
+				WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+				break;
+			case _TKIP_:
+				if (bmcst)
+					TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+				else
+					TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
+				break;
+			case _AES_:
+				if (bmcst)
+					AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+				else
+					AES_IV(pattrib->iv, psta->dot11txpn, 0);
+				break;
+			}
+		}
+
+		memcpy(pframe, pattrib->iv, pattrib->iv_len);
+		pframe += pattrib->iv_len;
+
+	}
+
+	llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
+	pframe += llc_sz;
+
+	/* pattrib->pktlen will be counted in rtw_build_tdls_ies */
+	pattrib->pktlen = 0;
+
+	rtw_build_tdls_ies(padapter, pxmitframe, pframe, ptxmgmt);
+
+	if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+		pframe += pattrib->pktlen;
+		memcpy(pframe, pattrib->icv, pattrib->icv_len);
+		pframe += pattrib->icv_len;
+	}
+
+	pattrib->nr_frags = 1;
+	pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + llc_sz +
+		((pattrib->bswenc) ? pattrib->icv_len : 0) + pattrib->pktlen;
+
+	if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	xmitframe_swencrypt(padapter, pxmitframe);
+
+	update_attrib_vcs_info(padapter, pxmitframe);
+
+exit:
+
+	return res;
+}
+#endif /* CONFIG_TDLS */
+
+/*
+ * Calculate wlan 802.11 packet MAX size from pkt_attrib
+ * This function doesn't consider fragment case
+ */
+u32 rtw_calculate_wlan_pkt_size_by_attribue(struct pkt_attrib *pattrib)
+{
+	u32	len = 0;
+
+	len = pattrib->hdrlen + pattrib->iv_len; /* WLAN Header and IV */
+	len += SNAP_SIZE + sizeof(u16); /* LLC */
+	len += pattrib->pktlen;
+	if (pattrib->encrypt == _TKIP_)
+		len += 8; /* MIC */
+	len += ((pattrib->bswenc) ? pattrib->icv_len : 0); /* ICV */
+
+	return len;
+}
+
+#ifdef CONFIG_TX_AMSDU
+s32 check_amsdu(struct xmit_frame *pxmitframe)
+{
+	struct pkt_attrib *pattrib;
+	s32 ret = true;
+
+	if (!pxmitframe)
+		ret = false;
+
+	pattrib = &pxmitframe->attrib;
+
+	if (IS_MCAST(pattrib->ra))
+		ret = false;
+
+	if ((pattrib->ether_type == 0x888e) ||
+		(pattrib->ether_type == 0x0806) ||
+		(pattrib->ether_type == 0x88b4) ||
+		(pattrib->dhcp_pkt == 1))
+		ret = false;
+
+	if ((pattrib->encrypt == _WEP40_) ||
+	    (pattrib->encrypt == _WEP104_) ||
+	    (pattrib->encrypt == _TKIP_))
+		ret = false;
+
+	if (!pattrib->qos_en)
+		ret = false;
+
+	return ret;
+}
+
+s32 check_amsdu_tx_support(_adapter *padapter)
+{
+	struct dvobj_priv *pdvobjpriv;
+	int tx_amsdu;
+	int tx_amsdu_rate;
+	int current_tx_rate;
+	s32 ret = false;
+
+	pdvobjpriv = adapter_to_dvobj(padapter);
+	tx_amsdu = padapter->tx_amsdu;
+	tx_amsdu_rate = padapter->tx_amsdu_rate;
+	current_tx_rate = pdvobjpriv->traffic_stat.cur_tx_tp;
+
+	if (tx_amsdu == 1)
+		ret = true;
+	else if (tx_amsdu == 2 && (tx_amsdu_rate == 0 || current_tx_rate > tx_amsdu_rate))
+		ret = true;
+	else
+		ret = false;
+
+	return ret;
+}
+
+s32 rtw_xmitframe_coalesce_amsdu(_adapter *padapter, struct xmit_frame *pxmitframe, struct xmit_frame *pxmitframe_queue)
+{
+
+	struct pkt_file pktfile;
+	struct pkt_attrib *pattrib;
+	_pkt *pkt;
+
+	struct pkt_file pktfile_queue;
+	struct pkt_attrib *pattrib_queue;
+	_pkt *pkt_queue;
+
+	s32 llc_sz, mem_sz;
+
+	s32 padding = 0;
+
+	u8 *pframe, *mem_start;
+	u8 hw_hdr_offset;
+
+	u16* len;
+	u8 *pbuf_start;
+	s32 res = _SUCCESS;
+
+	if (pxmitframe->buf_addr == NULL) {
+		RTW_INFO("==> %s buf_addr==NULL\n", __func__);
+		return _FAIL;
+	}
+
+	pbuf_start = pxmitframe->buf_addr;
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+	hw_hdr_offset =  TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
+#else
+#ifdef CONFIG_TX_EARLY_MODE /* for SDIO && Tx Agg */
+	hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE;
+#else
+	hw_hdr_offset = TXDESC_OFFSET;
+#endif
+#endif
+
+	mem_start = pbuf_start + hw_hdr_offset; //for DMA
+
+	pattrib = &pxmitframe->attrib;
+
+	pattrib->amsdu = 1;
+
+	if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n"));
+		RTW_INFO("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n");
+		res = _FAIL;
+		goto exit;
+	}
+
+	llc_sz = 0;
+
+	pframe = mem_start;
+
+	//SetMFrag(mem_start);
+	ClearMFrag(mem_start);
+
+	pframe += pattrib->hdrlen;
+
+	/* adding icv, if necessary... */
+	if (pattrib->iv_len) {
+		memcpy(pframe, pattrib->iv, pattrib->iv_len); // queue or new?
+
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
+			("rtw_xmitframe_coalesce: keyid=%d pattrib->iv[3]=%.2x pframe=%.2x %.2x %.2x %.2x\n",
+			padapter->securitypriv.dot11PrivacyKeyIndex, pattrib->iv[3], *pframe, *(pframe + 1), *(pframe + 2), *(pframe + 3)));
+
+		pframe += pattrib->iv_len;
+	}
+
+	pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len;
+
+	if(pxmitframe_queue)
+	{
+		pattrib_queue = &pxmitframe_queue->attrib;
+		pkt_queue = pxmitframe_queue->pkt;
+
+		_rtw_open_pktfile(pkt_queue, &pktfile_queue);
+		_rtw_pktfile_read(&pktfile_queue, NULL, pattrib_queue->pkt_hdrlen);
+
+		/* 802.3 MAC Header DA(6)  SA(6)  Len(2)*/
+
+		memcpy(pframe, pattrib_queue->dst, ETH_ALEN);
+		pframe += ETH_ALEN;
+
+		memcpy(pframe, pattrib_queue->src, ETH_ALEN);
+		pframe += ETH_ALEN;
+
+		len = (u16*) pframe;
+		pframe += 2;
+
+		llc_sz = rtw_put_snap(pframe, pattrib_queue->ether_type);
+		pframe += llc_sz;
+
+		mem_sz = _rtw_pktfile_read(&pktfile_queue, pframe, pattrib_queue->pktlen);
+		pframe += mem_sz;
+
+		*len = htons(llc_sz + mem_sz);
+
+		//calc padding
+		padding = 4 - ((ETH_HLEN + llc_sz + mem_sz) & (4-1));
+		if(padding == 4)
+			padding = 0;
+
+		//memset(pframe,0xaa, padding);
+		pframe += padding;
+
+		pattrib->last_txcmdsz += ETH_HLEN + llc_sz + mem_sz + padding ;
+	}
+
+	//2nd mpdu
+
+	pkt = pxmitframe->pkt;
+	_rtw_open_pktfile(pkt, &pktfile);
+	_rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen);
+
+	/* 802.3 MAC Header  DA(6)  SA(6)  Len(2) */
+
+	memcpy(pframe, pattrib->dst, ETH_ALEN);
+	pframe += ETH_ALEN;
+
+	memcpy(pframe, pattrib->src, ETH_ALEN);
+	pframe += ETH_ALEN;
+
+	len = (u16*) pframe;
+	pframe += 2;
+
+	llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
+	pframe += llc_sz;
+
+	mem_sz = _rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen);
+
+	pframe += mem_sz;
+
+	*len = htons(llc_sz + mem_sz);
+
+	//the last ampdu has no padding
+	padding = 0;
+
+	pattrib->nr_frags = 1;
+
+	pattrib->last_txcmdsz += ETH_HLEN + llc_sz + mem_sz + padding +
+		((pattrib->bswenc) ? pattrib->icv_len : 0) ;
+
+	if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+		memcpy(pframe, pattrib->icv, pattrib->icv_len);
+		pframe += pattrib->icv_len;
+	}
+
+	if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic(padapter, pxmitframe)==_FAIL\n"));
+		RTW_INFO("xmitframe_addmic(padapter, pxmitframe)==_FAIL\n");
+		res = _FAIL;
+		goto exit;
+	}
+
+	xmitframe_swencrypt(padapter, pxmitframe);
+
+	pattrib->vcs_mode = NONE_VCS;
+
+exit:
+	return res;
+}
+#endif /* CONFIG_TX_AMSDU */
+
+/*
+
+This sub-routine will perform all the following:
+
+1. remove 802.3 header.
+2. create wlan_header, based on the info in pxmitframe
+3. append sta's iv/ext-iv
+4. append LLC
+5. move frag chunk from pframe to pxmitframe->mem
+6. apply sw-encrypt, if necessary.
+
+*/
+s32 rtw_xmitframe_coalesce(_adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe)
+{
+	struct pkt_file pktfile;
+
+	s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
+
+	SIZE_PTR addr;
+
+	u8 *pframe, *mem_start;
+	u8 hw_hdr_offset;
+
+	/* struct sta_info		*psta; */
+	/* struct sta_priv		*pstapriv = &padapter->stapriv; */
+	/* struct mlme_priv	*pmlmepriv = &padapter->mlmepriv; */
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+
+	u8 *pbuf_start;
+
+	s32 bmcst = IS_MCAST(pattrib->ra);
+	s32 res = _SUCCESS;
+
+	/*
+		if (pattrib->psta)
+		{
+			psta = pattrib->psta;
+		} else
+		{
+			RTW_INFO("%s, call rtw_get_stainfo()\n", __func__);
+			psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+		}
+
+		if(psta==NULL)
+		{
+
+			RTW_INFO("%s, psta==NUL\n", __func__);
+			return _FAIL;
+		}
+
+		if(!(psta->state &_FW_LINKED))
+		{
+			RTW_INFO("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+			return _FAIL;
+		}
+	*/
+	if (pxmitframe->buf_addr == NULL) {
+		RTW_INFO("==> %s buf_addr==NULL\n", __func__);
+		return _FAIL;
+	}
+
+	pbuf_start = pxmitframe->buf_addr;
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+	hw_hdr_offset =  TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
+#else
+#ifdef CONFIG_TX_EARLY_MODE /* for SDIO && Tx Agg */
+	hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE;
+#else
+	hw_hdr_offset = TXDESC_OFFSET;
+#endif
+#endif
+
+	mem_start = pbuf_start +	hw_hdr_offset;
+
+	if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
+		RTW_INFO("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n");
+		res = _FAIL;
+		goto exit;
+	}
+
+	_rtw_open_pktfile(pkt, &pktfile);
+	_rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen);
+
+	frg_inx = 0;
+	frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */
+
+	while (1) {
+		llc_sz = 0;
+
+		mpdu_len = frg_len;
+
+		pframe = mem_start;
+
+		SetMFrag(mem_start);
+
+		pframe += pattrib->hdrlen;
+		mpdu_len -= pattrib->hdrlen;
+
+		/* adding icv, if necessary... */
+		if (pattrib->iv_len) {
+			memcpy(pframe, pattrib->iv, pattrib->iv_len);
+
+			pframe += pattrib->iv_len;
+
+			mpdu_len -= pattrib->iv_len;
+		}
+
+		if (frg_inx == 0) {
+			llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
+			pframe += llc_sz;
+			mpdu_len -= llc_sz;
+		}
+
+		if ((pattrib->icv_len > 0) && (pattrib->bswenc))
+			mpdu_len -= pattrib->icv_len;
+
+		if (bmcst) {
+			/* don't do fragment to broadcat/multicast packets */
+			mem_sz = _rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen);
+		} else
+			mem_sz = _rtw_pktfile_read(&pktfile, pframe, mpdu_len);
+
+		pframe += mem_sz;
+
+		if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+			memcpy(pframe, pattrib->icv, pattrib->icv_len);
+			pframe += pattrib->icv_len;
+		}
+
+		frg_inx++;
+
+		if (bmcst || (rtw_endofpktfile(&pktfile))) {
+			pattrib->nr_frags = frg_inx;
+
+			pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + ((pattrib->nr_frags == 1) ? llc_sz : 0) +
+				((pattrib->bswenc) ? pattrib->icv_len : 0) + mem_sz;
+
+			ClearMFrag(mem_start);
+
+			break;
+		}
+
+		addr = (SIZE_PTR)(pframe);
+
+		mem_start = (unsigned char *)RND4(addr) + hw_hdr_offset;
+		memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
+
+	}
+
+	if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
+		RTW_INFO("xmitframe_addmic(padapter, pxmitframe)==_FAIL\n");
+		res = _FAIL;
+		goto exit;
+	}
+
+	xmitframe_swencrypt(padapter, pxmitframe);
+
+	if (bmcst == false)
+		update_attrib_vcs_info(padapter, pxmitframe);
+	else
+		pattrib->vcs_mode = NONE_VCS;
+
+exit:
+
+	return res;
+}
+
+#ifdef CONFIG_IEEE80211W
+/* broadcast or multicast management pkt use BIP, unicast management pkt use CCMP encryption */
+s32 rtw_mgmt_xmitframe_coalesce(_adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe)
+{
+	struct pkt_file pktfile;
+	s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
+	SIZE_PTR addr;
+	u8 *pframe, *mem_start = NULL, *tmp_buf = NULL;
+	u8 hw_hdr_offset, subtype ;
+	struct sta_info		*psta = NULL;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	u8 *pbuf_start;
+	s32 bmcst = IS_MCAST(pattrib->ra);
+	s32 res = _FAIL;
+	u8 *BIP_AAD = NULL;
+	u8 *MGMT_body = NULL;
+
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct rtw_ieee80211_hdr	*pwlanhdr;
+	u8 MME[_MME_IE_LENGTH_];
+
+	unsigned long irqL;
+	u32	ori_len;
+	mem_start = pframe = (u8 *)(pxmitframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+	ori_len = BIP_AAD_SIZE + pattrib->pktlen;
+	tmp_buf = BIP_AAD = rtw_zmalloc(ori_len);
+	subtype = get_frame_sub_type(pframe); /* bit(7)~bit(2) */
+
+	if (BIP_AAD == NULL)
+		return _FAIL;
+
+	_enter_critical_bh(&padapter->security_key_mutex, &irqL);
+
+	/* IGTK key is not install, it may not support 802.11w */
+	if (padapter->securitypriv.binstallBIPkey != true) {
+		RTW_INFO("no instll BIP key\n");
+		goto xmitframe_coalesce_success;
+	}
+	/* station mode doesn't need TX BIP, just ready the code */
+	if (bmcst) {
+		int frame_body_len;
+		u8 mic[16];
+
+		memset(MME, 0, _MME_IE_LENGTH_);
+
+		/* other types doesn't need the BIP */
+		if (get_frame_sub_type(pframe) != WIFI_DEAUTH && get_frame_sub_type(pframe) != WIFI_DISASSOC)
+			goto xmitframe_coalesce_fail;
+
+		MGMT_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+		pframe += pattrib->pktlen;
+
+		/* octent 0 and 1 is key index ,BIP keyid is 4 or 5, LSB only need octent 0 */
+		MME[0] = padapter->securitypriv.dot11wBIPKeyid;
+		/* copy packet number */
+		memcpy(&MME[2], &pmlmeext->mgnt_80211w_IPN, 6);
+		/* increase the packet number */
+		pmlmeext->mgnt_80211w_IPN++;
+
+		/* add MME IE with MIC all zero, MME string doesn't include element id and length */
+		pframe = rtw_set_ie(pframe, _MME_IE_ , 16 , MME, &(pattrib->pktlen));
+		pattrib->last_txcmdsz = pattrib->pktlen;
+		/* total frame length - header length */
+		frame_body_len = pattrib->pktlen - sizeof(struct rtw_ieee80211_hdr_3addr);
+
+		/* conscruct AAD, copy frame control field */
+		memcpy(BIP_AAD, &pwlanhdr->frame_ctl, 2);
+		ClearRetry(BIP_AAD);
+		ClearPwrMgt(BIP_AAD);
+		ClearMData(BIP_AAD);
+		/* conscruct AAD, copy address 1 to address 3 */
+		memcpy(BIP_AAD + 2, pwlanhdr->addr1, 18);
+		/* copy management fram body */
+		memcpy(BIP_AAD + BIP_AAD_SIZE, MGMT_body, frame_body_len);
+		/* calculate mic */
+		if (omac1_aes_128(padapter->securitypriv.dot11wBIPKey[padapter->securitypriv.dot11wBIPKeyid].skey
+			  , BIP_AAD, BIP_AAD_SIZE + frame_body_len, mic))
+			goto xmitframe_coalesce_fail;
+
+		/* copy right BIP mic value, total is 128bits, we use the 0~63 bits */
+		memcpy(pframe - 8, mic, 8);
+	} else { /* unicast mgmt frame TX */
+		/* start to encrypt mgmt frame */
+		if (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC ||
+		    subtype == WIFI_REASSOCREQ || subtype == WIFI_ACTION) {
+			if (pattrib->psta)
+				psta = pattrib->psta;
+			else
+				psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+
+			if (psta == NULL) {
+
+				RTW_INFO("%s, psta==NUL\n", __func__);
+				goto xmitframe_coalesce_fail;
+			}
+
+			if (pxmitframe->buf_addr == NULL) {
+				RTW_INFO("%s, pxmitframe->buf_addr\n", __func__);
+				goto xmitframe_coalesce_fail;
+			}
+
+			/* RTW_INFO("%s, action frame category=%d\n", __func__, pframe[WLAN_HDR_A3_LEN]); */
+			/* according 802.11-2012 standard, these five types are not robust types */
+			if (subtype == WIFI_ACTION &&
+			    (pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_PUBLIC ||
+			     pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_HT ||
+			     pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_UNPROTECTED_WNM ||
+			     pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_SELF_PROTECTED  ||
+			     pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_P2P))
+				goto xmitframe_coalesce_fail;
+			/* before encrypt dump the management packet content */
+			/*{
+				int i;
+				RTW_INFO("Management pkt: ");
+				for(i=0; i<pattrib->pktlen; i++)
+				RTW_INFO(" %02x ", pframe[i]);
+				RTW_INFO("=======\n");
+			}*/
+			if (pattrib->encrypt > 0)
+				memcpy(pattrib->dot118021x_UncstKey.skey, psta->dot118021x_UncstKey.skey, 16);
+
+			/* To use wrong key */
+			if (pattrib->key_type == IEEE80211W_WRONG_KEY) {
+				RTW_INFO("use wrong key\n");
+				pattrib->dot118021x_UncstKey.skey[0] = 0xff;
+			}
+
+			/* bakeup original management packet */
+			memcpy(tmp_buf, pframe, pattrib->pktlen);
+			/* move to data portion */
+			pframe += pattrib->hdrlen;
+
+			/* 802.11w unicast management packet must be _AES_ */
+			pattrib->iv_len = 8;
+			/* it's MIC of AES */
+			pattrib->icv_len = 8;
+
+			switch (pattrib->encrypt) {
+			case _AES_:
+				/* set AES IV header */
+				AES_IV(pattrib->iv, psta->dot11wtxpn, 0);
+				break;
+			default:
+				goto xmitframe_coalesce_fail;
+			}
+			/* insert iv header into management frame */
+			memcpy(pframe, pattrib->iv, pattrib->iv_len);
+			pframe += pattrib->iv_len;
+			/* copy mgmt data portion after CCMP header */
+			memcpy(pframe, tmp_buf + pattrib->hdrlen, pattrib->pktlen - pattrib->hdrlen);
+			/* move pframe to end of mgmt pkt */
+			pframe += pattrib->pktlen - pattrib->hdrlen;
+			/* add 8 bytes CCMP IV header to length */
+			pattrib->pktlen += pattrib->iv_len;
+
+			if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+				memcpy(pframe, pattrib->icv, pattrib->icv_len);
+				pframe += pattrib->icv_len;
+			}
+			/* add 8 bytes MIC */
+			pattrib->pktlen += pattrib->icv_len;
+			/* set final tx command size */
+			pattrib->last_txcmdsz = pattrib->pktlen;
+
+			/* set protected bit must be beofre SW encrypt */
+			SetPrivacy(mem_start);
+			/* software encrypt */
+			xmitframe_swencrypt(padapter, pxmitframe);
+		}
+	}
+
+xmitframe_coalesce_success:
+	_exit_critical_bh(&padapter->security_key_mutex, &irqL);
+	rtw_mfree(BIP_AAD, ori_len);
+	return _SUCCESS;
+
+xmitframe_coalesce_fail:
+	_exit_critical_bh(&padapter->security_key_mutex, &irqL);
+	rtw_mfree(BIP_AAD, ori_len);
+
+	return _FAIL;
+}
+#endif /* CONFIG_IEEE80211W */
+
+/* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
+ * IEEE LLC/SNAP header contains 8 octets
+ * First 3 octets comprise the LLC portion
+ * SNAP portion, 5 octets, is divided into two fields:
+ *	Organizationally Unique Identifier(OUI), 3 octets,
+ *	type, defined by that organization, 2 octets.
+ */
+s32 rtw_put_snap(u8 *data, u16 h_proto)
+{
+	struct ieee80211_snap_hdr *snap;
+	u8 *oui;
+
+	snap = (struct ieee80211_snap_hdr *)data;
+	snap->dsap = 0xaa;
+	snap->ssap = 0xaa;
+	snap->ctrl = 0x03;
+
+	if (h_proto == 0x8137 || h_proto == 0x80f3)
+		oui = P802_1H_OUI;
+	else
+		oui = RFC1042_OUI;
+
+	snap->oui[0] = oui[0];
+	snap->oui[1] = oui[1];
+	snap->oui[2] = oui[2];
+
+	*(__be16 *)(data + SNAP_SIZE) = htons(h_proto);
+
+	return SNAP_SIZE + sizeof(u16);
+}
+
+void rtw_update_protection(_adapter *padapter, u8 *ie, uint ie_len)
+{
+
+	uint	protection;
+	u8	*perp;
+	sint	 erp_len;
+	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct	registry_priv *pregistrypriv = &padapter->registrypriv;
+
+	switch (pxmitpriv->vcs_setting) {
+	case DISABLE_VCS:
+		pxmitpriv->vcs = NONE_VCS;
+		break;
+
+	case ENABLE_VCS:
+		break;
+
+	case AUTO_VCS:
+	default:
+		perp = rtw_get_ie(ie, _ERPINFO_IE_, &erp_len, ie_len);
+		if (perp == NULL)
+			pxmitpriv->vcs = NONE_VCS;
+		else {
+			protection = (*(perp + 2)) & BIT(1);
+			if (protection) {
+				if (pregistrypriv->vcs_type == RTS_CTS)
+					pxmitpriv->vcs = RTS_CTS;
+				else
+					pxmitpriv->vcs = CTS_TO_SELF;
+			} else
+				pxmitpriv->vcs = NONE_VCS;
+		}
+
+		break;
+
+	}
+
+}
+
+void rtw_count_tx_stats(PADAPTER padapter, struct xmit_frame *pxmitframe, int sz)
+{
+	struct sta_info *psta = NULL;
+	struct stainfo_stats *pstats = NULL;
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	u8	pkt_num = 1;
+
+	if ((pxmitframe->frame_tag & 0x0f) == DATA_FRAMETAG) {
+#if defined(CONFIG_USB_TX_AGGREGATION)
+		pkt_num = pxmitframe->agg_num;
+#endif
+		pmlmepriv->LinkDetectInfo.NumTxOkInPeriod += pkt_num;
+
+		pxmitpriv->tx_pkts += pkt_num;
+
+		pxmitpriv->tx_bytes += sz;
+
+		psta = pxmitframe->attrib.psta;
+		if (psta) {
+			pstats = &psta->sta_stats;
+
+			pstats->tx_pkts += pkt_num;
+
+			pstats->tx_bytes += sz;
+#ifdef CONFIG_TDLS
+			if (pxmitframe->attrib.ptdls_sta != NULL) {
+				pstats = &(pxmitframe->attrib.ptdls_sta->sta_stats);
+				pstats->tx_pkts += pkt_num;
+				pstats->tx_bytes += sz;
+			}
+#endif /* CONFIG_TDLS */
+		}
+
+#ifdef CONFIG_CHECK_LEAVE_LPS
+		/* traffic_check_for_leave_lps(padapter, true); */
+#endif /* CONFIG_LPS */
+
+	}
+}
+
+static struct xmit_buf *__rtw_alloc_cmd_xmitbuf(struct xmit_priv *pxmitpriv,
+		enum cmdbuf_type buf_type)
+{
+	struct xmit_buf *pxmitbuf =  NULL;
+
+	pxmitbuf = &pxmitpriv->pcmd_xmitbuf[buf_type];
+	if (pxmitbuf !=  NULL) {
+		pxmitbuf->priv_data = NULL;
+		if (pxmitbuf->sctx) {
+			RTW_INFO("%s pxmitbuf->sctx is not NULL\n", __func__);
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+		}
+	} else
+		RTW_INFO("%s fail, no xmitbuf available !!!\n", __func__);
+
+exit:
+
+	return pxmitbuf;
+}
+
+struct xmit_frame *__rtw_alloc_cmdxmitframe(struct xmit_priv *pxmitpriv,
+		enum cmdbuf_type buf_type)
+{
+	struct xmit_frame		*pcmdframe;
+	struct xmit_buf		*pxmitbuf;
+
+	pcmdframe = rtw_alloc_xmitframe(pxmitpriv);
+	if (pcmdframe == NULL) {
+		RTW_INFO("%s, alloc xmitframe fail\n", __func__);
+		return NULL;
+	}
+
+	pxmitbuf = __rtw_alloc_cmd_xmitbuf(pxmitpriv, buf_type);
+	if (pxmitbuf == NULL) {
+		RTW_INFO("%s, alloc xmitbuf fail\n", __func__);
+		rtw_free_xmitframe(pxmitpriv, pcmdframe);
+		return NULL;
+	}
+
+	pcmdframe->frame_tag = MGNT_FRAMETAG;
+
+	pcmdframe->pxmitbuf = pxmitbuf;
+
+	pcmdframe->buf_addr = pxmitbuf->pbuf;
+
+	/* initial memory to zero */
+	memset(pcmdframe->buf_addr, 0, pxmitbuf->alloc_sz);
+
+	pxmitbuf->priv_data = pcmdframe;
+
+	return pcmdframe;
+
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv)
+{
+	unsigned long irqL;
+	struct xmit_buf *pxmitbuf =  NULL;
+	_list *plist, *phead;
+	_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+
+	_enter_critical(&pfree_queue->lock, &irqL);
+
+	if (_rtw_queue_empty(pfree_queue))
+		pxmitbuf = NULL;
+	else {
+
+		phead = get_list_head(pfree_queue);
+
+		plist = get_next(phead);
+
+		pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
+
+		list_del_init(&(pxmitbuf->list));
+	}
+
+	if (pxmitbuf !=  NULL) {
+		pxmitpriv->free_xmit_extbuf_cnt--;
+#ifdef DBG_XMIT_BUF_EXT
+		RTW_INFO("DBG_XMIT_BUF_EXT ALLOC no=%d,  free_xmit_extbuf_cnt=%d\n", pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt);
+#endif
+
+		pxmitbuf->priv_data = NULL;
+
+		if (pxmitbuf->sctx) {
+			RTW_INFO("%s pxmitbuf->sctx is not NULL\n", __func__);
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+		}
+
+	}
+
+	_exit_critical(&pfree_queue->lock, &irqL);
+
+	return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	unsigned long irqL;
+	_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+
+	if (pxmitbuf == NULL)
+		return _FAIL;
+
+	_enter_critical(&pfree_queue->lock, &irqL);
+
+	list_del_init(&pxmitbuf->list);
+
+	list_add_tail(&(pxmitbuf->list), get_list_head(pfree_queue));
+	pxmitpriv->free_xmit_extbuf_cnt++;
+#ifdef DBG_XMIT_BUF_EXT
+	RTW_INFO("DBG_XMIT_BUF_EXT FREE no=%d, free_xmit_extbuf_cnt=%d\n", pxmitbuf->no , pxmitpriv->free_xmit_extbuf_cnt);
+#endif
+
+	_exit_critical(&pfree_queue->lock, &irqL);
+
+	return _SUCCESS;
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv)
+{
+	unsigned long irqL;
+	struct xmit_buf *pxmitbuf =  NULL;
+	_list *plist, *phead;
+	_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+	/* RTW_INFO("+rtw_alloc_xmitbuf\n"); */
+
+	_enter_critical(&pfree_xmitbuf_queue->lock, &irqL);
+
+	if (_rtw_queue_empty(pfree_xmitbuf_queue))
+		pxmitbuf = NULL;
+	else {
+
+		phead = get_list_head(pfree_xmitbuf_queue);
+
+		plist = get_next(phead);
+
+		pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
+
+		list_del_init(&(pxmitbuf->list));
+	}
+
+	if (pxmitbuf !=  NULL) {
+		pxmitpriv->free_xmitbuf_cnt--;
+#ifdef DBG_XMIT_BUF
+		RTW_INFO("DBG_XMIT_BUF ALLOC no=%d,  free_xmitbuf_cnt=%d\n", pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt);
+#endif
+		/* RTW_INFO("alloc, free_xmitbuf_cnt=%d\n", pxmitpriv->free_xmitbuf_cnt); */
+
+		pxmitbuf->priv_data = NULL;
+
+		if (pxmitbuf->sctx) {
+			RTW_INFO("%s pxmitbuf->sctx is not NULL\n", __func__);
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+		}
+	}
+#ifdef DBG_XMIT_BUF
+	else
+		RTW_INFO("DBG_XMIT_BUF rtw_alloc_xmitbuf return NULL\n");
+#endif
+
+	_exit_critical(&pfree_xmitbuf_queue->lock, &irqL);
+
+	return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	unsigned long irqL;
+	_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+	/* RTW_INFO("+rtw_free_xmitbuf\n"); */
+
+	if (pxmitbuf == NULL)
+		return _FAIL;
+
+	if (pxmitbuf->sctx) {
+		RTW_INFO("%s pxmitbuf->sctx is not NULL\n", __func__);
+		rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE);
+	}
+
+	if (pxmitbuf->buf_tag == XMITBUF_CMD) {
+	} else if (pxmitbuf->buf_tag == XMITBUF_MGNT)
+		rtw_free_xmitbuf_ext(pxmitpriv, pxmitbuf);
+	else {
+		_enter_critical(&pfree_xmitbuf_queue->lock, &irqL);
+
+		list_del_init(&pxmitbuf->list);
+
+		list_add_tail(&(pxmitbuf->list), get_list_head(pfree_xmitbuf_queue));
+
+		pxmitpriv->free_xmitbuf_cnt++;
+		/* RTW_INFO("FREE, free_xmitbuf_cnt=%d\n", pxmitpriv->free_xmitbuf_cnt); */
+#ifdef DBG_XMIT_BUF
+		RTW_INFO("DBG_XMIT_BUF FREE no=%d, free_xmitbuf_cnt=%d\n", pxmitbuf->no , pxmitpriv->free_xmitbuf_cnt);
+#endif
+		_exit_critical(&pfree_xmitbuf_queue->lock, &irqL);
+	}
+
+	return _SUCCESS;
+}
+
+static void rtw_init_xmitframe(struct xmit_frame *pxframe)
+{
+	if (pxframe !=  NULL) { /* default value setting */
+		pxframe->buf_addr = NULL;
+		pxframe->pxmitbuf = NULL;
+
+		memset(&pxframe->attrib, 0, sizeof(struct pkt_attrib));
+		/* pxframe->attrib.psta = NULL; */
+
+		pxframe->frame_tag = DATA_FRAMETAG;
+
+		pxframe->pkt = NULL;
+#ifdef USB_PACKET_OFFSET_SZ
+		pxframe->pkt_offset = (PACKET_OFFSET_SZ / 8);
+#else
+		pxframe->pkt_offset = 1;/* default use pkt_offset to fill tx desc */
+#endif
+
+#ifdef CONFIG_USB_TX_AGGREGATION
+		pxframe->agg_num = 1;
+#endif
+
+#ifdef CONFIG_XMIT_ACK
+		pxframe->ack_report = 0;
+#endif
+
+	}
+}
+
+/*
+Calling context:
+1. OS_TXENTRY
+2. RXENTRY (rx_thread or RX_ISR/RX_CallBack)
+
+If we turn on USE_RXTHREAD, then, no need for critical section.
+Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
+
+Must be very very cautious...
+
+*/
+struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)/* (_queue *pfree_xmit_queue) */
+{
+	/*
+		Please remember to use all the osdep_service api,
+		and lock/unlock or _enter/_exit critical to protect
+		pfree_xmit_queue
+	*/
+
+	unsigned long irqL;
+	struct xmit_frame *pxframe = NULL;
+	_list *plist, *phead;
+	_queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
+
+	_enter_critical_bh(&pfree_xmit_queue->lock, &irqL);
+
+	if (_rtw_queue_empty(pfree_xmit_queue)) {
+		pxframe =  NULL;
+	} else {
+		phead = get_list_head(pfree_xmit_queue);
+
+		plist = get_next(phead);
+
+		pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		list_del_init(&(pxframe->list));
+		pxmitpriv->free_xmitframe_cnt--;
+	}
+
+	_exit_critical_bh(&pfree_xmit_queue->lock, &irqL);
+
+	rtw_init_xmitframe(pxframe);
+
+	return pxframe;
+}
+
+struct xmit_frame *rtw_alloc_xmitframe_ext(struct xmit_priv *pxmitpriv)
+{
+	unsigned long irqL;
+	struct xmit_frame *pxframe = NULL;
+	_list *plist, *phead;
+	_queue *queue = &pxmitpriv->free_xframe_ext_queue;
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	if (_rtw_queue_empty(queue)) {
+		pxframe =  NULL;
+	} else {
+		phead = get_list_head(queue);
+		plist = get_next(phead);
+		pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		list_del_init(&(pxframe->list));
+		pxmitpriv->free_xframe_ext_cnt--;
+	}
+
+	_exit_critical_bh(&queue->lock, &irqL);
+
+	rtw_init_xmitframe(pxframe);
+
+	return pxframe;
+}
+
+struct xmit_frame *rtw_alloc_xmitframe_once(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_frame *pxframe = NULL;
+	u8 *alloc_addr;
+
+	alloc_addr = rtw_zmalloc(sizeof(struct xmit_frame) + 4);
+
+	if (alloc_addr == NULL)
+		goto exit;
+
+	pxframe = (struct xmit_frame *)N_BYTE_ALIGMENT((SIZE_PTR)(alloc_addr), 4);
+	pxframe->alloc_addr = alloc_addr;
+
+	pxframe->padapter = pxmitpriv->adapter;
+	pxframe->frame_tag = NULL_FRAMETAG;
+
+	pxframe->pkt = NULL;
+
+	pxframe->buf_addr = NULL;
+	pxframe->pxmitbuf = NULL;
+
+	rtw_init_xmitframe(pxframe);
+
+	RTW_INFO("################## %s ##################\n", __func__);
+
+exit:
+	return pxframe;
+}
+
+s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe)
+{
+	unsigned long irqL;
+	_queue *queue = NULL;
+	_adapter *padapter = pxmitpriv->adapter;
+	_pkt *pndis_pkt = NULL;
+
+	if (pxmitframe == NULL) {
+		goto exit;
+	}
+
+	if (pxmitframe->pkt) {
+		pndis_pkt = pxmitframe->pkt;
+		pxmitframe->pkt = NULL;
+	}
+
+	if (pxmitframe->alloc_addr) {
+		RTW_INFO("################## %s with alloc_addr ##################\n", __func__);
+		rtw_mfree(pxmitframe->alloc_addr, sizeof(struct xmit_frame) + 4);
+		goto check_pkt_complete;
+	}
+
+	if (pxmitframe->ext_tag == 0)
+		queue = &pxmitpriv->free_xmit_queue;
+	else if (pxmitframe->ext_tag == 1)
+		queue = &pxmitpriv->free_xframe_ext_queue;
+	else
+		rtw_warn_on(1);
+
+	_enter_critical_bh(&queue->lock, &irqL);
+
+	list_del_init(&pxmitframe->list);
+	list_add_tail(&pxmitframe->list, get_list_head(queue));
+	if (pxmitframe->ext_tag == 0) {
+		pxmitpriv->free_xmitframe_cnt++;
+	} else if (pxmitframe->ext_tag == 1) {
+		pxmitpriv->free_xframe_ext_cnt++;
+	} else {
+	}
+
+	_exit_critical_bh(&queue->lock, &irqL);
+
+check_pkt_complete:
+
+	if (pndis_pkt)
+		rtw_os_pkt_complete(padapter, pndis_pkt);
+
+exit:
+
+	return _SUCCESS;
+}
+
+void rtw_free_xmitframe_queue(struct xmit_priv *pxmitpriv, _queue *pframequeue)
+{
+	unsigned long irqL;
+	_list	*plist, *phead;
+	struct	xmit_frame	*pxmitframe;
+
+	_enter_critical_bh(&(pframequeue->lock), &irqL);
+
+	phead = get_list_head(pframequeue);
+	plist = get_next(phead);
+
+	while (rtw_end_of_queue_search(phead, plist) == false) {
+
+		pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		plist = get_next(plist);
+
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+
+	}
+	_exit_critical_bh(&(pframequeue->lock), &irqL);
+
+}
+
+s32 rtw_xmitframe_enqueue(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	DBG_COUNTER(padapter->tx_logs.core_tx_enqueue);
+	if (rtw_xmit_classifier(padapter, pxmitframe) == _FAIL) {
+		/*		pxmitframe->pkt = NULL; */
+		return _FAIL;
+	}
+
+	return _SUCCESS;
+}
+
+static struct xmit_frame *dequeue_one_xmitframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit, struct tx_servq *ptxservq, _queue *pframe_queue)
+{
+	_list	*xmitframe_plist, *xmitframe_phead;
+	struct	xmit_frame	*pxmitframe = NULL;
+
+	xmitframe_phead = get_list_head(pframe_queue);
+	xmitframe_plist = get_next(xmitframe_phead);
+
+	while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == false) {
+		pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+		list_del_init(&pxmitframe->list);
+
+		ptxservq->qcnt--;
+		break;
+	}
+
+	return pxmitframe;
+}
+
+static struct xmit_frame *get_one_xmitframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit, struct tx_servq *ptxservq, _queue *pframe_queue)
+{
+	_list	*xmitframe_plist, *xmitframe_phead;
+	struct	xmit_frame	*pxmitframe = NULL;
+
+	xmitframe_phead = get_list_head(pframe_queue);
+	xmitframe_plist = get_next(xmitframe_phead);
+
+	while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == false) {
+		pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+		break;
+	}
+
+	return pxmitframe;
+}
+
+static struct xmit_frame *rtw_get_xframe(struct xmit_priv *pxmitpriv, int *num_frame)
+{
+	unsigned long irqL0;
+	_list *sta_plist, *sta_phead;
+	struct hw_xmit *phwxmit_i = pxmitpriv->hwxmits;
+	sint entry =  pxmitpriv->hwxmit_entry;
+
+	struct hw_xmit *phwxmit;
+	struct tx_servq *ptxservq = NULL;
+	_queue *pframe_queue = NULL;
+	struct xmit_frame *pxmitframe = NULL;
+	_adapter *padapter = pxmitpriv->adapter;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	int i, inx[4];
+
+	inx[0] = 0;
+	inx[1] = 1;
+	inx[2] = 2;
+	inx[3] = 3;
+
+	*num_frame = 0;
+
+	/*No amsdu when wifi_spec on*/
+	if (pregpriv->wifi_spec == 1) {
+		return NULL;
+	}
+
+	_enter_critical_bh(&pxmitpriv->lock, &irqL0);
+
+	for (i = 0; i < entry; i++) {
+		phwxmit = phwxmit_i + inx[i];
+
+		sta_phead = get_list_head(phwxmit->sta_queue);
+		sta_plist = get_next(sta_phead);
+
+		while ((rtw_end_of_queue_search(sta_phead, sta_plist)) == false) {
+
+			ptxservq = LIST_CONTAINOR(sta_plist, struct tx_servq, tx_pending);
+			pframe_queue = &ptxservq->sta_pending;
+
+			if(ptxservq->qcnt)
+			{
+				*num_frame = ptxservq->qcnt;
+				pxmitframe = get_one_xmitframe(pxmitpriv, phwxmit, ptxservq, pframe_queue);
+				goto exit;
+			}
+			sta_plist = get_next(sta_plist);
+		}
+	}
+
+exit:
+
+	_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+
+	return pxmitframe;
+}
+
+struct xmit_frame *rtw_dequeue_xframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit_i, sint entry)
+{
+	unsigned long irqL0;
+	_list *sta_plist, *sta_phead;
+	struct hw_xmit *phwxmit;
+	struct tx_servq *ptxservq = NULL;
+	_queue *pframe_queue = NULL;
+	struct xmit_frame *pxmitframe = NULL;
+	_adapter *padapter = pxmitpriv->adapter;
+	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	int i, inx[4];
+
+	inx[0] = 0;
+	inx[1] = 1;
+	inx[2] = 2;
+	inx[3] = 3;
+
+	if (pregpriv->wifi_spec == 1) {
+		int j, tmp, acirp_cnt[4];
+
+		for (j = 0; j < 4; j++)
+			inx[j] = pxmitpriv->wmm_para_seq[j];
+	}
+
+	_enter_critical_bh(&pxmitpriv->lock, &irqL0);
+
+	for (i = 0; i < entry; i++) {
+		phwxmit = phwxmit_i + inx[i];
+
+		/* _enter_critical_ex(&phwxmit->sta_queue->lock, &irqL0); */
+
+		sta_phead = get_list_head(phwxmit->sta_queue);
+		sta_plist = get_next(sta_phead);
+
+		while ((rtw_end_of_queue_search(sta_phead, sta_plist)) == false) {
+
+			ptxservq = LIST_CONTAINOR(sta_plist, struct tx_servq, tx_pending);
+
+			pframe_queue = &ptxservq->sta_pending;
+
+			pxmitframe = dequeue_one_xmitframe(pxmitpriv, phwxmit, ptxservq, pframe_queue);
+
+			if (pxmitframe) {
+				phwxmit->accnt--;
+
+				/* Remove sta node when there is no pending packets. */
+				if (_rtw_queue_empty(pframe_queue)) /* must be done after get_next and before break */
+					list_del_init(&ptxservq->tx_pending);
+
+				/* _exit_critical_ex(&phwxmit->sta_queue->lock, &irqL0); */
+
+				goto exit;
+			}
+
+			sta_plist = get_next(sta_plist);
+
+		}
+
+		/* _exit_critical_ex(&phwxmit->sta_queue->lock, &irqL0); */
+
+	}
+
+exit:
+
+	_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+
+	return pxmitframe;
+}
+
+struct tx_servq *rtw_get_sta_pending(_adapter *padapter, struct sta_info *psta, sint up, u8 *ac)
+{
+	struct tx_servq *ptxservq = NULL;
+
+	switch (up) {
+	case 1:
+	case 2:
+		ptxservq = &(psta->sta_xmitpriv.bk_q);
+		*(ac) = 3;
+		break;
+
+	case 4:
+	case 5:
+		ptxservq = &(psta->sta_xmitpriv.vi_q);
+		*(ac) = 1;
+		break;
+
+	case 6:
+	case 7:
+		ptxservq = &(psta->sta_xmitpriv.vo_q);
+		*(ac) = 0;
+		break;
+
+	case 0:
+	case 3:
+	default:
+		ptxservq = &(psta->sta_xmitpriv.be_q);
+		*(ac) = 2;
+		break;
+
+	}
+
+	return ptxservq;
+}
+
+/*
+ * Will enqueue pxmitframe to the proper queue,
+ * and indicate it to xx_pending list.....
+ */
+s32 rtw_xmit_classifier(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	/* unsigned long irqL0; */
+	u8	ac_index;
+	struct sta_info	*psta;
+	struct tx_servq	*ptxservq;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	struct hw_xmit	*phwxmits =  padapter->xmitpriv.hwxmits;
+	sint res = _SUCCESS;
+
+	DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class);
+
+	/*
+		if (pattrib->psta) {
+			psta = pattrib->psta;
+		} else {
+			RTW_INFO("%s, call rtw_get_stainfo()\n", __func__);
+			psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+		}
+	*/
+
+	psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	if (pattrib->psta != psta) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_sta);
+		RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+		return _FAIL;
+	}
+
+	if (psta == NULL) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_nosta);
+		res = _FAIL;
+		RTW_INFO("rtw_xmit_classifier: psta == NULL\n");
+		goto exit;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_fwlink);
+		RTW_INFO("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return _FAIL;
+	}
+
+	ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+	/* _enter_critical(&pstapending->lock, &irqL0); */
+
+	if (list_empty(&ptxservq->tx_pending))
+		list_add_tail(&ptxservq->tx_pending, get_list_head(phwxmits[ac_index].sta_queue));
+
+	/* _enter_critical(&ptxservq->sta_pending.lock, &irqL1); */
+
+	list_add_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending));
+	ptxservq->qcnt++;
+	phwxmits[ac_index].accnt++;
+
+	/* _exit_critical(&ptxservq->sta_pending.lock, &irqL1); */
+
+	/* _exit_critical(&pstapending->lock, &irqL0); */
+
+exit:
+
+	return res;
+}
+
+void rtw_alloc_hwxmits(_adapter *padapter)
+{
+	struct hw_xmit *hwxmits;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
+
+	pxmitpriv->hwxmits = NULL;
+
+	pxmitpriv->hwxmits = (struct hw_xmit *)rtw_zmalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry);
+
+	if (pxmitpriv->hwxmits == NULL) {
+		RTW_INFO("alloc hwxmits fail!...\n");
+		return;
+	}
+
+	hwxmits = pxmitpriv->hwxmits;
+
+	if (pxmitpriv->hwxmit_entry == 5) {
+		/* pxmitpriv->bmc_txqueue.head = 0; */
+		/* hwxmits[0] .phwtxqueue = &pxmitpriv->bmc_txqueue; */
+		hwxmits[0] .sta_queue = &pxmitpriv->bm_pending;
+
+		/* pxmitpriv->vo_txqueue.head = 0; */
+		/* hwxmits[1] .phwtxqueue = &pxmitpriv->vo_txqueue; */
+		hwxmits[1] .sta_queue = &pxmitpriv->vo_pending;
+
+		/* pxmitpriv->vi_txqueue.head = 0; */
+		/* hwxmits[2] .phwtxqueue = &pxmitpriv->vi_txqueue; */
+		hwxmits[2] .sta_queue = &pxmitpriv->vi_pending;
+
+		/* pxmitpriv->bk_txqueue.head = 0; */
+		/* hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue; */
+		hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
+
+		/* pxmitpriv->be_txqueue.head = 0; */
+		/* hwxmits[4] .phwtxqueue = &pxmitpriv->be_txqueue; */
+		hwxmits[4] .sta_queue = &pxmitpriv->be_pending;
+
+	} else if (pxmitpriv->hwxmit_entry == 4) {
+
+		/* pxmitpriv->vo_txqueue.head = 0; */
+		/* hwxmits[0] .phwtxqueue = &pxmitpriv->vo_txqueue; */
+		hwxmits[0] .sta_queue = &pxmitpriv->vo_pending;
+
+		/* pxmitpriv->vi_txqueue.head = 0; */
+		/* hwxmits[1] .phwtxqueue = &pxmitpriv->vi_txqueue; */
+		hwxmits[1] .sta_queue = &pxmitpriv->vi_pending;
+
+		/* pxmitpriv->be_txqueue.head = 0; */
+		/* hwxmits[2] .phwtxqueue = &pxmitpriv->be_txqueue; */
+		hwxmits[2] .sta_queue = &pxmitpriv->be_pending;
+
+		/* pxmitpriv->bk_txqueue.head = 0; */
+		/* hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue; */
+		hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
+	} else {
+
+	}
+
+}
+
+void rtw_free_hwxmits(_adapter *padapter)
+{
+	struct hw_xmit *hwxmits;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	hwxmits = pxmitpriv->hwxmits;
+	if (hwxmits)
+		rtw_mfree((u8 *)hwxmits, (sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry));
+}
+
+void rtw_init_hwxmits(struct hw_xmit *phwxmit, sint entry)
+{
+	sint i;
+	for (i = 0; i < entry; i++, phwxmit++) {
+		/* spin_lock_init(&phwxmit->xmit_lock); */
+		/* INIT_LIST_HEAD(&phwxmit->pending);		 */
+		/* phwxmit->txcmdcnt = 0; */
+		phwxmit->accnt = 0;
+	}
+}
+
+#ifdef CONFIG_BR_EXT
+static int rtw_br_client_tx(_adapter *padapter, struct sk_buff **pskb)
+{
+	struct sk_buff *skb = *pskb;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	unsigned long irqL;
+	/* if(check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE)) */
+	{
+		int res, is_vlan_tag = 0, i, do_nat25 = 1;
+		unsigned short vlan_hdr = 0;
+		void *br_port = NULL;
+
+		/* mac_clone_handle_frame(priv, skb); */
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+		br_port = padapter->pnetdev->br_port;
+#else   /* (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) */
+		rcu_read_lock();
+		br_port = rcu_dereference(padapter->pnetdev->rx_handler_data);
+		rcu_read_unlock();
+#endif /* (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) */
+		_enter_critical_bh(&padapter->br_ext_lock, &irqL);
+		if (!(skb->data[0] & 1) &&
+		    br_port &&
+		    memcmp(skb->data + MACADDRLEN, padapter->br_mac, MACADDRLEN) &&
+		    *((__be16 *)(skb->data + MACADDRLEN * 2)) != __constant_htons(ETH_P_8021Q) &&
+		    *((__be16 *)(skb->data + MACADDRLEN * 2)) == __constant_htons(ETH_P_IP) &&
+		    !memcmp(padapter->scdb_mac, skb->data + MACADDRLEN, MACADDRLEN) && padapter->scdb_entry) {
+			memcpy(skb->data + MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
+			padapter->scdb_entry->ageing_timer = jiffies;
+			_exit_critical_bh(&padapter->br_ext_lock, &irqL);
+		} else
+			/* if (!priv->pmib->ethBrExtInfo.nat25_disable)		 */
+		{
+			/*			if (priv->dev->br_port &&
+			 *				 !memcmp(skb->data+MACADDRLEN, priv->br_mac, MACADDRLEN)) { */
+			if (*((__be16 *)(skb->data + MACADDRLEN * 2)) == __constant_htons(ETH_P_8021Q)) {
+				is_vlan_tag = 1;
+				vlan_hdr = *((unsigned short *)(skb->data + MACADDRLEN * 2 + 2));
+				for (i = 0; i < 6; i++)
+					*((unsigned short *)(skb->data + MACADDRLEN * 2 + 2 - i * 2)) = *((unsigned short *)(skb->data + MACADDRLEN * 2 - 2 - i * 2));
+				skb_pull(skb, 4);
+			}
+			/* if SA == br_mac && skb== IP  => copy SIP to br_ip ?? why */
+			if (!memcmp(skb->data + MACADDRLEN, padapter->br_mac, MACADDRLEN) &&
+			    (*((__be16 *)(skb->data + MACADDRLEN * 2)) == __constant_htons(ETH_P_IP)))
+				memcpy(padapter->br_ip, skb->data + WLAN_ETHHDR_LEN + 12, 4);
+
+			if (*((__be16 *)(skb->data + MACADDRLEN * 2)) == __constant_htons(ETH_P_IP)) {
+				if (memcmp(padapter->scdb_mac, skb->data + MACADDRLEN, MACADDRLEN)) {
+
+					padapter->scdb_entry = (struct nat25_network_db_entry *)scdb_findEntry(padapter,
+						skb->data + MACADDRLEN, skb->data + WLAN_ETHHDR_LEN + 12);
+					if (padapter->scdb_entry != NULL) {
+						memcpy(padapter->scdb_mac, skb->data + MACADDRLEN, MACADDRLEN);
+						memcpy(padapter->scdb_ip, skb->data + WLAN_ETHHDR_LEN + 12, 4);
+						padapter->scdb_entry->ageing_timer = jiffies;
+						do_nat25 = 0;
+					}
+				} else {
+					if (padapter->scdb_entry) {
+						padapter->scdb_entry->ageing_timer = jiffies;
+						do_nat25 = 0;
+					} else {
+						memset(padapter->scdb_mac, 0, MACADDRLEN);
+						memset(padapter->scdb_ip, 0, 4);
+					}
+				}
+			}
+			_exit_critical_bh(&padapter->br_ext_lock, &irqL);
+			if (do_nat25) {
+				if (nat25_db_handle(padapter, skb, NAT25_CHECK) == 0) {
+					struct sk_buff *newskb;
+
+					if (is_vlan_tag) {
+						skb_push(skb, 4);
+						for (i = 0; i < 6; i++)
+							*((unsigned short *)(skb->data + i * 2)) = *((unsigned short *)(skb->data + 4 + i * 2));
+						*((__be16 *)(skb->data + MACADDRLEN * 2)) = __constant_htons(ETH_P_8021Q);
+						*((unsigned short *)(skb->data + MACADDRLEN * 2 + 2)) = vlan_hdr;
+					}
+
+					newskb = rtw_skb_copy(skb);
+					if (newskb == NULL) {
+						/* priv->ext_stats.tx_drops++; */
+						DEBUG_ERR("TX DROP: rtw_skb_copy fail!\n");
+						/* goto stop_proc; */
+						return -1;
+					}
+					rtw_skb_free(skb);
+
+					*pskb = skb = newskb;
+					if (is_vlan_tag) {
+						vlan_hdr = *((unsigned short *)(skb->data + MACADDRLEN * 2 + 2));
+						for (i = 0; i < 6; i++)
+							*((unsigned short *)(skb->data + MACADDRLEN * 2 + 2 - i * 2)) = *((unsigned short *)(skb->data + MACADDRLEN * 2 - 2 - i * 2));
+						skb_pull(skb, 4);
+					}
+				}
+
+				if (skb_is_nonlinear(skb))
+					DEBUG_ERR("%s(): skb_is_nonlinear!!\n", __func__);
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
+				res = skb_linearize(skb, GFP_ATOMIC);
+#else	/* (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18)) */
+				res = skb_linearize(skb);
+#endif /* (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18)) */
+				if (res < 0) {
+					DEBUG_ERR("TX DROP: skb_linearize fail!\n");
+					/* goto free_and_stop; */
+					return -1;
+				}
+
+				res = nat25_db_handle(padapter, skb, NAT25_INSERT);
+				if (res < 0) {
+					if (res == -2) {
+						/* priv->ext_stats.tx_drops++; */
+						DEBUG_ERR("TX DROP: nat25_db_handle fail!\n");
+						/* goto free_and_stop; */
+						return -1;
+
+					}
+					/* we just print warning message and let it go */
+					/* DEBUG_WARN("%s()-%d: nat25_db_handle INSERT Warning!\n", __func__, __LINE__); */
+					/* return -1; */ /* return -1 will cause system crash on 2011/08/30! */
+					return 0;
+				}
+			}
+
+			memcpy(skb->data + MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
+
+			dhcp_flag_bcast(padapter, skb);
+
+			if (is_vlan_tag) {
+				skb_push(skb, 4);
+				for (i = 0; i < 6; i++)
+					*((unsigned short *)(skb->data + i * 2)) = *((unsigned short *)(skb->data + 4 + i * 2));
+				*((__be16 *)(skb->data + MACADDRLEN * 2)) = __constant_htons(ETH_P_8021Q);
+				*((unsigned short *)(skb->data + MACADDRLEN * 2 + 2)) = vlan_hdr;
+			}
+		}
+		/* check if SA is equal to our MAC */
+		if (memcmp(skb->data + MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN)) {
+			/* priv->ext_stats.tx_drops++; */
+			DEBUG_ERR("TX DROP: untransformed frame SA:%02X%02X%02X%02X%02X%02X!\n",
+				skb->data[6], skb->data[7], skb->data[8], skb->data[9], skb->data[10], skb->data[11]);
+			/* goto free_and_stop; */
+			return -1;
+		}
+	}
+	return 0;
+}
+#endif /* CONFIG_BR_EXT */
+
+u32 rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe)
+{
+	u32 addr;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+	switch (pattrib->qsel) {
+	case 0:
+	case 3:
+		addr = BE_QUEUE_INX;
+		break;
+	case 1:
+	case 2:
+		addr = BK_QUEUE_INX;
+		break;
+	case 4:
+	case 5:
+		addr = VI_QUEUE_INX;
+		break;
+	case 6:
+	case 7:
+		addr = VO_QUEUE_INX;
+		break;
+	case 0x10:
+		addr = BCN_QUEUE_INX;
+		break;
+	case 0x11: /* BC/MC in PS (HIQ) */
+		addr = HIGH_QUEUE_INX;
+		break;
+	case 0x13:
+		addr = TXCMD_QUEUE_INX;
+		break;
+	case 0x12:
+	default:
+		addr = MGT_QUEUE_INX;
+		break;
+
+	}
+
+	return addr;
+
+}
+
+static void do_queue_select(_adapter	*padapter, struct pkt_attrib *pattrib)
+{
+	u8 qsel;
+
+	qsel = pattrib->priority;
+
+#ifdef CONFIG_CONCURRENT_MODE
+	/*	if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE))
+	 *		qsel = 7; */
+#endif
+
+#ifdef CONFIG_MCC_MODE
+	if (MCC_EN(padapter)) {
+		/* Under MCC */
+		if (rtw_hal_check_mcc_status(padapter, MCC_STATUS_NEED_MCC)) {
+			if (padapter->mcc_adapterpriv.role == MCC_ROLE_GO
+			    || padapter->mcc_adapterpriv.role == MCC_ROLE_AP) {
+				pattrib->qsel = QSLT_VO; /* AP interface VO queue */
+			} else {
+				pattrib->qsel = QSLT_BE; /* STA interface BE queue */
+			}
+		} else
+			/* Not Under MCC */
+			pattrib->qsel = qsel;
+	} else
+		/* Not enable MCC */
+		pattrib->qsel = qsel;
+#else /* !CONFIG_MCC_MODE */
+	pattrib->qsel = qsel;
+#endif /* CONFIG_MCC_MODE */
+}
+
+/*
+ * The main transmit(tx) entry
+ *
+ * Return
+ *	1	enqueue
+ *	0	success, hardware will handle this xmit frame(packet)
+ *	<0	fail
+ */
+ #if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24))
+s32 rtw_monitor_xmit_entry(struct sk_buff *skb, struct net_device *ndev)
+{
+	int ret = 0;
+	int rtap_len;
+	int qos_len = 0;
+	int dot11_hdr_len = 24;
+	int snap_len = 6;
+	unsigned char *pdata;
+	u16 frame_ctl;
+	unsigned char src_mac_addr[6];
+	unsigned char dst_mac_addr[6];
+	struct rtw_ieee80211_hdr *dot11_hdr;
+	struct ieee80211_radiotap_header *rtap_hdr;
+	_adapter *padapter = (_adapter *)rtw_netdev_priv(ndev);
+
+	if (skb)
+		rtw_mstat_update(MSTAT_TYPE_SKB, MSTAT_ALLOC_SUCCESS, skb->truesize);
+
+	if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
+		goto fail;
+
+	rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
+	if (unlikely(rtap_hdr->it_version))
+		goto fail;
+
+	rtap_len = ieee80211_get_radiotap_len(skb->data);
+	if (unlikely(skb->len < rtap_len))
+		goto fail;
+
+	if (rtap_len != 12) {
+		RTW_INFO("radiotap len (should be 14): %d\n", rtap_len);
+		goto fail;
+	}
+
+	/* Skip the ratio tap header */
+	skb_pull(skb, rtap_len);
+
+	dot11_hdr = (struct rtw_ieee80211_hdr *)skb->data;
+	frame_ctl = le16_to_cpu(dot11_hdr->frame_ctl);
+	/* Check if the QoS bit is set */
+
+	if ((frame_ctl & RTW_IEEE80211_FCTL_FTYPE) == RTW_IEEE80211_FTYPE_DATA) {
+
+		struct xmit_frame		*pmgntframe;
+		struct pkt_attrib	*pattrib;
+		unsigned char	*pframe;
+		struct rtw_ieee80211_hdr *pwlanhdr;
+		struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
+		struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+		u8 *buf = skb->data;
+		u32 len = skb->len;
+		u8 category, action;
+		int type = -1;
+
+		pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+		if (pmgntframe == NULL) {
+			rtw_udelay_os(500);
+			goto fail;
+		}
+		pattrib = &pmgntframe->attrib;
+
+		update_monitor_frame_attrib(padapter, pattrib);
+
+		pattrib->retry_ctrl = false;
+
+		memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+		pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+
+		memcpy(pframe, (void *)buf, len);
+
+		pattrib->pktlen = len;
+
+		pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+		if (is_broadcast_mac_addr(pwlanhdr->addr3) || is_broadcast_mac_addr(pwlanhdr->addr1))
+			pattrib->rate = MGN_24M;
+
+		pmlmeext->mgnt_seq = GetSequence(pwlanhdr);
+		pattrib->seqnum = pmlmeext->mgnt_seq;
+		pmlmeext->mgnt_seq++;
+
+		pattrib->last_txcmdsz = pattrib->pktlen;
+
+		dump_mgntframe(padapter, pmgntframe);
+
+	} else {
+		struct xmit_frame		*pmgntframe;
+		struct pkt_attrib	*pattrib;
+		unsigned char	*pframe;
+		struct rtw_ieee80211_hdr *pwlanhdr;
+		struct xmit_priv	*pxmitpriv = &(padapter->xmitpriv);
+		struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+		u8 *buf = skb->data;
+		u32 len = skb->len;
+		u8 category, action;
+		int type = -1;
+
+		pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+		if (pmgntframe == NULL)
+			goto fail;
+
+		pattrib = &pmgntframe->attrib;
+		update_mgntframe_attrib(padapter, pattrib);
+		pattrib->retry_ctrl = false;
+
+		memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+		pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+
+		memcpy(pframe, (void *)buf, len);
+
+		pattrib->pktlen = len;
+
+		pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
+
+		pmlmeext->mgnt_seq = GetSequence(pwlanhdr);
+		pattrib->seqnum = pmlmeext->mgnt_seq;
+		pmlmeext->mgnt_seq++;
+
+		pattrib->last_txcmdsz = pattrib->pktlen;
+
+		dump_mgntframe(padapter, pmgntframe);
+
+	}
+
+fail:
+
+	rtw_skb_free(skb);
+
+	return 0;
+}
+#endif
+/*
+ * The main transmit(tx) entry
+ *
+ * Return
+ *	1	enqueue
+ *	0	success, hardware will handle this xmit frame(packet)
+ *	<0	fail
+ */
+s32 rtw_xmit(_adapter *padapter, _pkt **ppkt)
+{
+	static u32 start = 0;
+	static u32 drop_cnt = 0;
+#ifdef CONFIG_AP_MODE
+	unsigned long irqL0;
+#endif
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_frame *pxmitframe = NULL;
+#ifdef CONFIG_BR_EXT
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	void *br_port = NULL;
+#endif /* CONFIG_BR_EXT */
+
+	s32 res;
+
+	DBG_COUNTER(padapter->tx_logs.core_tx);
+
+	if (start == 0)
+		start = jiffies;
+
+	pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
+
+	if (rtw_get_passing_time_ms(start) > 2000) {
+		if (drop_cnt)
+			RTW_INFO("DBG_TX_DROP_FRAME %s no more pxmitframe, drop_cnt:%u\n", __func__, drop_cnt);
+		start = jiffies;
+		drop_cnt = 0;
+	}
+
+	if (pxmitframe == NULL) {
+		drop_cnt++;
+		/*RTW_INFO("%s-"ADPT_FMT" no more xmitframe\n", __func__, ADPT_ARG(padapter));*/
+		DBG_COUNTER(padapter->tx_logs.core_tx_err_pxmitframe);
+		return -1;
+	}
+
+#ifdef CONFIG_BR_EXT
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
+	br_port = padapter->pnetdev->br_port;
+#else   /* (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) */
+	rcu_read_lock();
+	br_port = rcu_dereference(padapter->pnetdev->rx_handler_data);
+	rcu_read_unlock();
+#endif /* (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35)) */
+
+	if (br_port && check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_ADHOC_STATE)) {
+		res = rtw_br_client_tx(padapter, ppkt);
+		if (res == -1) {
+			rtw_free_xmitframe(pxmitpriv, pxmitframe);
+			DBG_COUNTER(padapter->tx_logs.core_tx_err_brtx);
+			return -1;
+		}
+	}
+
+#endif /* CONFIG_BR_EXT */
+
+	res = update_attrib(padapter, *ppkt, &pxmitframe->attrib);
+
+#ifdef CONFIG_MCC_MODE
+	/* record data kernel TX to driver to check MCC concurrent TX */
+	rtw_hal_mcc_calc_tx_bytes_from_kernel(padapter, pxmitframe->attrib.pktlen);
+#endif /* CONFIG_MCC_MODE */
+
+#ifdef CONFIG_WAPI_SUPPORT
+	if (pxmitframe->attrib.ether_type != 0x88B4) {
+		if (rtw_wapi_drop_for_key_absent(padapter, pxmitframe->attrib.ra)) {
+			WAPI_TRACE(WAPI_RX, "drop for key absend when tx\n");
+			res = _FAIL;
+		}
+	}
+#endif
+	if (res == _FAIL) {
+		/*RTW_INFO("%s-"ADPT_FMT" update attrib fail\n", __func__, ADPT_ARG(padapter));*/
+#ifdef DBG_TX_DROP_FRAME
+		RTW_INFO("DBG_TX_DROP_FRAME %s update attrib fail\n", __func__);
+#endif
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+		return -1;
+	}
+	pxmitframe->pkt = *ppkt;
+
+	rtw_led_control(padapter, LED_CTL_TX);
+
+	do_queue_select(padapter, &pxmitframe->attrib);
+
+#ifdef CONFIG_AP_MODE
+	_enter_critical_bh(&pxmitpriv->lock, &irqL0);
+	if (xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe)) {
+		_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue);
+		return 1;
+	}
+	_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+#endif
+
+	/* pre_xmitframe */
+	if (rtw_hal_xmit(padapter, pxmitframe) == false)
+		return 1;
+
+	return 0;
+}
+
+#ifdef CONFIG_TDLS
+sint xmitframe_enqueue_for_tdls_sleeping_sta(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	sint ret = false;
+
+	unsigned long irqL;
+	struct sta_info *ptdls_sta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	int i;
+
+	ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst);
+	if (ptdls_sta == NULL)
+		return ret;
+	else if (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) {
+
+		if (pattrib->triggered == 1) {
+			ret = true;
+			return ret;
+		}
+
+		_enter_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);
+
+		if (ptdls_sta->state & WIFI_SLEEP_STATE) {
+			list_del_init(&pxmitframe->list);
+
+			/* _enter_critical_bh(&psta->sleep_q.lock, &irqL);	 */
+
+			list_add_tail(&pxmitframe->list, get_list_head(&ptdls_sta->sleep_q));
+
+			ptdls_sta->sleepq_len++;
+			ptdls_sta->sleepq_ac_len++;
+
+			/* indicate 4-AC queue bit in TDLS peer traffic indication */
+			switch (pattrib->priority) {
+			case 1:
+			case 2:
+				ptdls_sta->uapsd_bk |= BIT(1);
+				break;
+			case 4:
+			case 5:
+				ptdls_sta->uapsd_vi |= BIT(1);
+				break;
+			case 6:
+			case 7:
+				ptdls_sta->uapsd_vo |= BIT(1);
+				break;
+			case 0:
+			case 3:
+			default:
+				ptdls_sta->uapsd_be |= BIT(1);
+				break;
+			}
+
+			/* Transmit TDLS PTI via AP */
+			if (ptdls_sta->sleepq_len == 1)
+				rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_ISSUE_PTI);
+
+			ret = true;
+		}
+
+		_exit_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);
+	}
+
+	return ret;
+
+}
+#endif /* CONFIG_TDLS */
+
+#define RTW_HIQ_FILTER_ALLOW_ALL 0
+#define RTW_HIQ_FILTER_ALLOW_SPECIAL 1
+#define RTW_HIQ_FILTER_DENY_ALL 2
+
+inline bool xmitframe_hiq_filter(struct xmit_frame *xmitframe)
+{
+	bool allow = false;
+	_adapter *adapter = xmitframe->padapter;
+	struct registry_priv *registry = &adapter->registrypriv;
+
+	if (rtw_get_intf_type(adapter) != RTW_PCIE) {
+
+		if (adapter->registrypriv.wifi_spec == 1)
+			allow = true;
+		else if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_SPECIAL) {
+
+			struct pkt_attrib *attrib = &xmitframe->attrib;
+
+			if (attrib->ether_type == 0x0806
+			    || attrib->ether_type == 0x888e
+#ifdef CONFIG_WAPI_SUPPORT
+			    || attrib->ether_type == 0x88B4
+#endif
+			    || attrib->dhcp_pkt
+			   ) {
+				if (0)
+					RTW_INFO(FUNC_ADPT_FMT" ether_type:0x%04x%s\n", FUNC_ADPT_ARG(xmitframe->padapter)
+						, attrib->ether_type, attrib->dhcp_pkt ? " DHCP" : "");
+				allow = true;
+			}
+		} else if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_ALL)
+			allow = true;
+		else if (registry->hiq_filter == RTW_HIQ_FILTER_DENY_ALL) {
+		} else
+			rtw_warn_on(1);
+	}
+	return allow;
+}
+
+#if defined(CONFIG_AP_MODE) || defined(CONFIG_TDLS)
+
+sint xmitframe_enqueue_for_sleeping_sta(_adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	unsigned long irqL;
+	sint ret = false;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	sint bmcst = IS_MCAST(pattrib->ra);
+	bool update_tim = false;
+#ifdef CONFIG_TDLS
+
+	if (padapter->tdlsinfo.link_established)
+		ret = xmitframe_enqueue_for_tdls_sleeping_sta(padapter, pxmitframe);
+#endif /* CONFIG_TDLS */
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == false) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_fwstate);
+		return ret;
+	}
+	/*
+		if(pattrib->psta)
+		{
+			psta = pattrib->psta;
+		}
+		else
+		{
+			RTW_INFO("%s, call rtw_get_stainfo()\n", __func__);
+			psta=rtw_get_stainfo(pstapriv, pattrib->ra);
+		}
+	*/
+	psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	if (pattrib->psta != psta) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_sta);
+		RTW_INFO("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+		return false;
+	}
+
+	if (psta == NULL) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_nosta);
+		RTW_INFO("%s, psta==NUL\n", __func__);
+		return false;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_link);
+		RTW_INFO("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return false;
+	}
+
+	if (pattrib->triggered == 1) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_trigger);
+		/* RTW_INFO("directly xmit pspoll_triggered packet\n"); */
+
+		/* pattrib->triggered=0; */
+		if (bmcst && xmitframe_hiq_filter(pxmitframe))
+			pattrib->qsel = QSLT_HIGH;/* HIQ */
+
+		return ret;
+	}
+
+	if (bmcst) {
+		_enter_critical_bh(&psta->sleep_q.lock, &irqL);
+
+		if (pstapriv->sta_dz_bitmap) { /* if anyone sta is in ps mode */
+			/* pattrib->qsel = QSLT_HIGH; */ /* HIQ */
+
+			list_del_init(&pxmitframe->list);
+
+			/*_enter_critical_bh(&psta->sleep_q.lock, &irqL);*/
+
+			list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
+
+			psta->sleepq_len++;
+
+			if (!(pstapriv->tim_bitmap & BIT(0)))
+				update_tim = true;
+
+			pstapriv->tim_bitmap |= BIT(0);
+			pstapriv->sta_dz_bitmap |= BIT(0);
+
+			/* RTW_INFO("enqueue, sq_len=%d, tim=%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
+			if (update_tim) {
+				if (is_broadcast_mac_addr(pattrib->ra))
+					_update_beacon(padapter, _TIM_IE_, NULL, true, "buffer BC");
+				else
+					_update_beacon(padapter, _TIM_IE_, NULL, true, "buffer MC");
+			} else
+				chk_bmc_sleepq_cmd(padapter);
+
+			/*_exit_critical_bh(&psta->sleep_q.lock, &irqL);*/
+
+			ret = true;
+
+			DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_mcast);
+
+		}
+
+		_exit_critical_bh(&psta->sleep_q.lock, &irqL);
+
+		return ret;
+
+	}
+
+	_enter_critical_bh(&psta->sleep_q.lock, &irqL);
+
+	if (psta->state & WIFI_SLEEP_STATE) {
+		u8 wmmps_ac = 0;
+
+		if (pstapriv->sta_dz_bitmap & BIT(psta->aid)) {
+			list_del_init(&pxmitframe->list);
+
+			/* _enter_critical_bh(&psta->sleep_q.lock, &irqL);	 */
+
+			list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
+
+			psta->sleepq_len++;
+
+			switch (pattrib->priority) {
+			case 1:
+			case 2:
+				wmmps_ac = psta->uapsd_bk & BIT(0);
+				break;
+			case 4:
+			case 5:
+				wmmps_ac = psta->uapsd_vi & BIT(0);
+				break;
+			case 6:
+			case 7:
+				wmmps_ac = psta->uapsd_vo & BIT(0);
+				break;
+			case 0:
+			case 3:
+			default:
+				wmmps_ac = psta->uapsd_be & BIT(0);
+				break;
+			}
+
+			if (wmmps_ac)
+				psta->sleepq_ac_len++;
+
+			if (((psta->has_legacy_ac) && (!wmmps_ac)) || ((!psta->has_legacy_ac) && (wmmps_ac))) {
+				if (!(pstapriv->tim_bitmap & BIT(psta->aid)))
+					update_tim = true;
+
+				pstapriv->tim_bitmap |= BIT(psta->aid);
+
+				/* RTW_INFO("enqueue, sq_len=%d, tim=%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
+
+				if (update_tim) {
+					/* RTW_INFO("sleepq_len==1, update BCNTIM\n"); */
+					/* upate BCN for TIM IE */
+					_update_beacon(padapter, _TIM_IE_, NULL, true, "buffer UC");
+				}
+			}
+
+			/* _exit_critical_bh(&psta->sleep_q.lock, &irqL);			 */
+
+			/* if(psta->sleepq_len > (NR_XMITFRAME>>3)) */
+			/* { */
+			/*	wakeup_sta_to_xmit(padapter, psta); */
+			/* }	 */
+
+			ret = true;
+
+			DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_ucast);
+		}
+
+	}
+
+	_exit_critical_bh(&psta->sleep_q.lock, &irqL);
+
+	return ret;
+
+}
+
+static void dequeue_xmitframes_to_sleeping_queue(_adapter *padapter, struct sta_info *psta, _queue *pframequeue)
+{
+	sint ret;
+	_list	*plist, *phead;
+	u8	ac_index;
+	struct tx_servq	*ptxservq;
+	struct pkt_attrib	*pattrib;
+	struct xmit_frame	*pxmitframe;
+	struct hw_xmit *phwxmits =  padapter->xmitpriv.hwxmits;
+
+	phead = get_list_head(pframequeue);
+	plist = get_next(phead);
+
+	while (rtw_end_of_queue_search(phead, plist) == false) {
+		pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		plist = get_next(plist);
+
+		pattrib = &pxmitframe->attrib;
+
+		pattrib->triggered = 0;
+
+		ret = xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe);
+
+		if (ret) {
+			ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+			ptxservq->qcnt--;
+			phwxmits[ac_index].accnt--;
+		} else {
+			/* RTW_INFO("xmitframe_enqueue_for_sleeping_sta return false\n"); */
+		}
+
+	}
+
+}
+
+void stop_sta_xmit(_adapter *padapter, struct sta_info *psta)
+{
+	unsigned long irqL0;
+	struct sta_info *psta_bmc;
+	struct sta_xmit_priv *pstaxmitpriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	pstaxmitpriv = &psta->sta_xmitpriv;
+
+	/* for BC/MC Frames */
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+
+	_enter_critical_bh(&pxmitpriv->lock, &irqL0);
+
+	psta->state |= WIFI_SLEEP_STATE;
+
+#ifdef CONFIG_TDLS
+	if (!(psta->tdls_sta_state & TDLS_LINKED_STATE))
+#endif /* CONFIG_TDLS */
+		pstapriv->sta_dz_bitmap |= BIT(psta->aid);
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vo_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->vo_q.tx_pending));
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vi_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->vi_q.tx_pending));
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->be_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->be_q.tx_pending));
+
+	dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->bk_q.sta_pending);
+	list_del_init(&(pstaxmitpriv->bk_q.tx_pending));
+
+#ifdef CONFIG_TDLS
+	if (!(psta->tdls_sta_state & TDLS_LINKED_STATE) && (psta_bmc != NULL)) {
+#endif /* CONFIG_TDLS */
+
+		/* for BC/MC Frames */
+		pstaxmitpriv = &psta_bmc->sta_xmitpriv;
+		dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->be_q.sta_pending);
+		list_del_init(&(pstaxmitpriv->be_q.tx_pending));
+
+#ifdef CONFIG_TDLS
+	}
+#endif /* CONFIG_TDLS	 */
+	_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+
+}
+
+void wakeup_sta_to_xmit(_adapter *padapter, struct sta_info *psta)
+{
+	unsigned long irqL;
+	u8 update_mask = 0, wmmps_ac = 0;
+	struct sta_info *psta_bmc;
+	_list	*xmitframe_plist, *xmitframe_phead;
+	struct xmit_frame *pxmitframe = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+
+	/* _enter_critical_bh(&psta->sleep_q.lock, &irqL); */
+	_enter_critical_bh(&pxmitpriv->lock, &irqL);
+
+	xmitframe_phead = get_list_head(&psta->sleep_q);
+	xmitframe_plist = get_next(xmitframe_phead);
+
+	while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == false) {
+		pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+		xmitframe_plist = get_next(xmitframe_plist);
+
+		list_del_init(&pxmitframe->list);
+
+		switch (pxmitframe->attrib.priority) {
+		case 1:
+		case 2:
+			wmmps_ac = psta->uapsd_bk & BIT(1);
+			break;
+		case 4:
+		case 5:
+			wmmps_ac = psta->uapsd_vi & BIT(1);
+			break;
+		case 6:
+		case 7:
+			wmmps_ac = psta->uapsd_vo & BIT(1);
+			break;
+		case 0:
+		case 3:
+		default:
+			wmmps_ac = psta->uapsd_be & BIT(1);
+			break;
+		}
+
+		psta->sleepq_len--;
+		if (psta->sleepq_len > 0)
+			pxmitframe->attrib.mdata = 1;
+		else
+			pxmitframe->attrib.mdata = 0;
+
+		if (wmmps_ac) {
+			psta->sleepq_ac_len--;
+			if (psta->sleepq_ac_len > 0) {
+				pxmitframe->attrib.mdata = 1;
+				pxmitframe->attrib.eosp = 0;
+			} else {
+				pxmitframe->attrib.mdata = 0;
+				pxmitframe->attrib.eosp = 1;
+			}
+		}
+
+		pxmitframe->attrib.triggered = 1;
+
+		/*
+				_exit_critical_bh(&psta->sleep_q.lock, &irqL);
+				if(rtw_hal_xmit(padapter, pxmitframe))
+				{
+					rtw_os_xmit_complete(padapter, pxmitframe);
+				}
+				_enter_critical_bh(&psta->sleep_q.lock, &irqL);
+		*/
+		rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+	}
+
+	if (psta->sleepq_len == 0) {
+#ifdef CONFIG_TDLS
+		if (psta->tdls_sta_state & TDLS_LINKED_STATE) {
+			if (psta->state & WIFI_SLEEP_STATE)
+				psta->state ^= WIFI_SLEEP_STATE;
+
+			_exit_critical_bh(&pxmitpriv->lock, &irqL);
+			return;
+		}
+#endif /* CONFIG_TDLS */
+
+		if (pstapriv->tim_bitmap & BIT(psta->aid)) {
+			/* RTW_INFO("wakeup to xmit, qlen==0, update_BCNTIM, tim=%x\n", pstapriv->tim_bitmap); */
+			/* upate BCN for TIM IE */
+			/* update_BCNTIM(padapter); */
+			update_mask = BIT(0);
+		}
+
+		pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+		if (psta->state & WIFI_SLEEP_STATE)
+			psta->state ^= WIFI_SLEEP_STATE;
+
+		if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+			RTW_INFO("%s alive check\n", __func__);
+			psta->expire_to = pstapriv->expire_to;
+			psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+		}
+
+		pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+	}
+
+	/* for BC/MC Frames */
+	if (!psta_bmc)
+		goto _exit;
+
+	if ((pstapriv->sta_dz_bitmap & 0xfffe) == 0x0) { /* no any sta in ps mode */
+		xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+		xmitframe_plist = get_next(xmitframe_phead);
+
+		while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == false) {
+			pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+			xmitframe_plist = get_next(xmitframe_plist);
+
+			list_del_init(&pxmitframe->list);
+
+			psta_bmc->sleepq_len--;
+			if (psta_bmc->sleepq_len > 0)
+				pxmitframe->attrib.mdata = 1;
+			else
+				pxmitframe->attrib.mdata = 0;
+
+			pxmitframe->attrib.triggered = 1;
+			/*
+						_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
+						if(rtw_hal_xmit(padapter, pxmitframe))
+						{
+							rtw_os_xmit_complete(padapter, pxmitframe);
+						}
+						_enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
+
+			*/
+			rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+		}
+
+		if (psta_bmc->sleepq_len == 0) {
+			if (pstapriv->tim_bitmap & BIT(0)) {
+				/* RTW_INFO("wakeup to xmit, qlen==0, update_BCNTIM, tim=%x\n", pstapriv->tim_bitmap); */
+				/* upate BCN for TIM IE */
+				/* update_BCNTIM(padapter); */
+				update_mask |= BIT(1);
+			}
+			pstapriv->tim_bitmap &= ~BIT(0);
+			pstapriv->sta_dz_bitmap &= ~BIT(0);
+		}
+
+	}
+
+_exit:
+
+	/* _exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);	 */
+	_exit_critical_bh(&pxmitpriv->lock, &irqL);
+
+	if (update_mask) {
+		/* update_BCNTIM(padapter); */
+		if ((update_mask & (BIT(0) | BIT(1))) == (BIT(0) | BIT(1)))
+			_update_beacon(padapter, _TIM_IE_, NULL, true, "clear UC&BMC");
+		else if ((update_mask & BIT(1)) == BIT(1))
+			_update_beacon(padapter, _TIM_IE_, NULL, true, "clear BMC");
+		else
+			_update_beacon(padapter, _TIM_IE_, NULL, true, "clear UC");
+	}
+
+}
+
+void xmit_delivery_enabled_frames(_adapter *padapter, struct sta_info *psta)
+{
+	unsigned long irqL;
+	u8 wmmps_ac = 0;
+	_list	*xmitframe_plist, *xmitframe_phead;
+	struct xmit_frame *pxmitframe = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	/* _enter_critical_bh(&psta->sleep_q.lock, &irqL); */
+	_enter_critical_bh(&pxmitpriv->lock, &irqL);
+
+	xmitframe_phead = get_list_head(&psta->sleep_q);
+	xmitframe_plist = get_next(xmitframe_phead);
+
+	while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == false) {
+		pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+		xmitframe_plist = get_next(xmitframe_plist);
+
+		switch (pxmitframe->attrib.priority) {
+		case 1:
+		case 2:
+			wmmps_ac = psta->uapsd_bk & BIT(1);
+			break;
+		case 4:
+		case 5:
+			wmmps_ac = psta->uapsd_vi & BIT(1);
+			break;
+		case 6:
+		case 7:
+			wmmps_ac = psta->uapsd_vo & BIT(1);
+			break;
+		case 0:
+		case 3:
+		default:
+			wmmps_ac = psta->uapsd_be & BIT(1);
+			break;
+		}
+
+		if (!wmmps_ac)
+			continue;
+
+		list_del_init(&pxmitframe->list);
+
+		psta->sleepq_len--;
+		psta->sleepq_ac_len--;
+
+		if (psta->sleepq_ac_len > 0) {
+			pxmitframe->attrib.mdata = 1;
+			pxmitframe->attrib.eosp = 0;
+		} else {
+			pxmitframe->attrib.mdata = 0;
+			pxmitframe->attrib.eosp = 1;
+		}
+
+		pxmitframe->attrib.triggered = 1;
+		rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+		if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) && (wmmps_ac)) {
+#ifdef CONFIG_TDLS
+			if (psta->tdls_sta_state & TDLS_LINKED_STATE) {
+				/* _exit_critical_bh(&psta->sleep_q.lock, &irqL); */
+				goto exit;
+			}
+#endif /* CONFIG_TDLS */
+			pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+			/* RTW_INFO("wakeup to xmit, qlen==0, update_BCNTIM, tim=%x\n", pstapriv->tim_bitmap); */
+			/* upate BCN for TIM IE */
+			/* update_BCNTIM(padapter); */
+			update_beacon(padapter, _TIM_IE_, NULL, true);
+			/* update_mask = BIT(0); */
+		}
+
+	}
+
+exit:
+	/* _exit_critical_bh(&psta->sleep_q.lock, &irqL);	 */
+	_exit_critical_bh(&pxmitpriv->lock, &irqL);
+
+	return;
+}
+
+#endif /* defined(CONFIG_AP_MODE) || defined(CONFIG_TDLS) */
+
+#ifdef CONFIG_XMIT_THREAD_MODE
+void enqueue_pending_xmitbuf(
+	struct xmit_priv *pxmitpriv,
+	struct xmit_buf *pxmitbuf)
+{
+	unsigned long irql;
+	_queue *pqueue;
+	_adapter *pri_adapter = pxmitpriv->adapter;
+
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	_enter_critical_bh(&pqueue->lock, &irql);
+	list_del_init(&pxmitbuf->list);
+	list_add_tail(&pxmitbuf->list, get_list_head(pqueue));
+	_exit_critical_bh(&pqueue->lock, &irql);
+	up(&(pri_adapter->xmitpriv.xmit_sema));
+}
+
+void enqueue_pending_xmitbuf_to_head(
+	struct xmit_priv *pxmitpriv,
+	struct xmit_buf *pxmitbuf)
+{
+	unsigned long irql;
+	_queue *pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	_enter_critical_bh(&pqueue->lock, &irql);
+	list_del_init(&pxmitbuf->list);
+	list_add(&pxmitbuf->list, get_list_head(pqueue));
+	_exit_critical_bh(&pqueue->lock, &irql);
+}
+
+struct xmit_buf *dequeue_pending_xmitbuf(
+	struct xmit_priv *pxmitpriv)
+{
+	unsigned long irql;
+	struct xmit_buf *pxmitbuf;
+	_queue *pqueue;
+
+	pxmitbuf = NULL;
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	_enter_critical_bh(&pqueue->lock, &irql);
+
+	if (_rtw_queue_empty(pqueue) == false) {
+		_list *plist, *phead;
+
+		phead = get_list_head(pqueue);
+		plist = get_next(phead);
+		pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
+		list_del_init(&pxmitbuf->list);
+	}
+
+	_exit_critical_bh(&pqueue->lock, &irql);
+
+	return pxmitbuf;
+}
+
+static struct xmit_buf *dequeue_pending_xmitbuf_under_survey(
+	struct xmit_priv *pxmitpriv)
+{
+	unsigned long irql;
+	struct xmit_buf *pxmitbuf;
+	struct xmit_frame *pxmitframe;
+	_queue *pqueue;
+
+	pxmitbuf = NULL;
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	_enter_critical_bh(&pqueue->lock, &irql);
+
+	if (_rtw_queue_empty(pqueue) == false) {
+		_list *plist, *phead;
+		u8 type = 0;
+
+		phead = get_list_head(pqueue);
+		plist = phead;
+		do {
+			plist = get_next(plist);
+			if (plist == phead)
+				break;
+
+			pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
+
+			pxmitframe = (struct xmit_frame *)pxmitbuf->priv_data;
+			if (pxmitframe)
+				type = get_frame_sub_type(pxmitbuf->pbuf + TXDESC_SIZE + pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
+			else
+				RTW_INFO("%s, !!!ERROR!!! For USB, TODO ITEM\n", __func__);
+
+			if ((type == WIFI_PROBEREQ) ||
+			    (type == WIFI_DATA_NULL) ||
+			    (type == WIFI_QOS_DATA_NULL)) {
+				list_del_init(&pxmitbuf->list);
+				break;
+			}
+			pxmitbuf = NULL;
+		} while (1);
+	}
+
+	_exit_critical_bh(&pqueue->lock, &irql);
+
+	return pxmitbuf;
+}
+
+static struct xmit_buf *dequeue_pending_xmitbuf_ext(
+	struct xmit_priv *pxmitpriv)
+{
+	unsigned long irql;
+	struct xmit_buf *pxmitbuf;
+	_queue *pqueue;
+
+	pxmitbuf = NULL;
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	_enter_critical_bh(&pqueue->lock, &irql);
+
+	if (_rtw_queue_empty(pqueue) == false) {
+		_list *plist, *phead;
+		u8 type = 0;
+
+		phead = get_list_head(pqueue);
+		plist = phead;
+		do {
+			plist = get_next(plist);
+			if (plist == phead)
+				break;
+
+			pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
+
+			if (pxmitbuf->buf_tag == XMITBUF_MGNT) {
+				list_del_init(&pxmitbuf->list);
+				break;
+			}
+			pxmitbuf = NULL;
+		} while (1);
+	}
+
+	_exit_critical_bh(&pqueue->lock, &irql);
+
+	return pxmitbuf;
+}
+
+struct xmit_buf *select_and_dequeue_pending_xmitbuf(_adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_buf *pxmitbuf = NULL;
+
+	if (rtw_xmit_ac_blocked(padapter))
+		pxmitbuf = dequeue_pending_xmitbuf_under_survey(pxmitpriv);
+	else {
+		pxmitbuf = dequeue_pending_xmitbuf_ext(pxmitpriv);
+		if (pxmitbuf == NULL)
+			pxmitbuf = dequeue_pending_xmitbuf(pxmitpriv);
+	}
+
+	return pxmitbuf;
+}
+
+sint check_pending_xmitbuf(
+	struct xmit_priv *pxmitpriv)
+{
+	unsigned long irql;
+	_queue *pqueue;
+	sint	ret = false;
+
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	_enter_critical_bh(&pqueue->lock, &irql);
+
+	if (_rtw_queue_empty(pqueue) == false)
+		ret = true;
+
+	_exit_critical_bh(&pqueue->lock, &irql);
+
+	return ret;
+}
+
+thread_return rtw_xmit_thread(thread_context context)
+{
+	s32 err;
+	PADAPTER padapter;
+
+	err = _SUCCESS;
+	padapter = (PADAPTER)context;
+
+	thread_enter("RTW_XMIT_THREAD");
+
+	do {
+		err = rtw_hal_xmit_thread_handler(padapter);
+		flush_signals_thread();
+	} while (_SUCCESS == err);
+
+	up(&padapter->xmitpriv.terminate_xmitthread_sema);
+
+	thread_exit();
+}
+#endif
+
+bool rtw_xmit_ac_blocked(_adapter *adapter)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	_adapter *iface;
+	struct mlme_ext_priv *mlmeext;
+	struct mlme_ext_info *mlmeextinfo;
+	bool blocked = false;
+	int i;
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		iface = dvobj->padapters[i];
+		mlmeext = &iface->mlmeextpriv;
+
+		/* check scan state */
+		if (mlmeext_scan_state(mlmeext) != SCAN_DISABLE
+			&& mlmeext_scan_state(mlmeext) != SCAN_BACK_OP
+		) {
+			blocked = true;
+			goto exit;
+		}
+
+		if (mlmeext_scan_state(mlmeext) == SCAN_BACK_OP
+			&& !mlmeext_chk_scan_backop_flags(mlmeext, SS_BACKOP_TX_RESUME)
+		) {
+			blocked = true;
+			goto exit;
+		}
+	}
+
+#ifdef CONFIG_MCC_MODE
+	if (MCC_EN(adapter)) {
+		if (rtw_hal_check_mcc_status(adapter, MCC_STATUS_DOING_MCC)) {
+			if (MCC_STOP(adapter)) {
+				blocked = true;
+				goto exit;
+			}
+		}
+	}
+#endif /*  CONFIG_MCC_MODE */
+
+exit:
+	return blocked;
+}
+
+#ifdef CONFIG_TX_AMSDU
+void rtw_amsdu_vo_timeout_handler(void *FunctionContext)
+{
+	_adapter *adapter = (_adapter *)FunctionContext;
+
+	adapter->xmitpriv.amsdu_vo_timeout = RTW_AMSDU_TIMER_TIMEOUT;
+
+	tasklet_hi_schedule(&adapter->xmitpriv.xmit_tasklet);
+}
+
+void rtw_amsdu_vi_timeout_handler(void *FunctionContext)
+{
+	_adapter *adapter = (_adapter *)FunctionContext;
+
+	adapter->xmitpriv.amsdu_vi_timeout = RTW_AMSDU_TIMER_TIMEOUT;
+
+	tasklet_hi_schedule(&adapter->xmitpriv.xmit_tasklet);
+}
+
+void rtw_amsdu_be_timeout_handler(void *FunctionContext)
+{
+	_adapter *adapter = (_adapter *)FunctionContext;
+
+	adapter->xmitpriv.amsdu_be_timeout = RTW_AMSDU_TIMER_TIMEOUT;
+
+	if (printk_ratelimit())
+		RTW_INFO("%s Timeout!\n",__func__);
+
+	tasklet_hi_schedule(&adapter->xmitpriv.xmit_tasklet);
+}
+
+void rtw_amsdu_bk_timeout_handler(void *FunctionContext)
+{
+	_adapter *adapter = (_adapter *)FunctionContext;
+
+	adapter->xmitpriv.amsdu_bk_timeout = RTW_AMSDU_TIMER_TIMEOUT;
+
+	tasklet_hi_schedule(&adapter->xmitpriv.xmit_tasklet);
+}
+
+u8 rtw_amsdu_get_timer_status(_adapter *padapter, u8 priority)
+{
+	struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
+
+	u8 status =  RTW_AMSDU_TIMER_UNSET;
+
+	switch(priority)
+	{
+		case 1:
+		case 2:
+			status = pxmitpriv->amsdu_bk_timeout;
+			break;
+		case 4:
+		case 5:
+			status = pxmitpriv->amsdu_vi_timeout;
+			break;
+		case 6:
+		case 7:
+			status = pxmitpriv->amsdu_vo_timeout;
+			break;
+		case 0:
+		case 3:
+		default:
+			status = pxmitpriv->amsdu_be_timeout;
+			break;
+	}
+	return status;
+}
+
+void rtw_amsdu_set_timer_status(_adapter *padapter, u8 priority, u8 status)
+{
+	struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
+
+	switch(priority)
+	{
+		case 1:
+		case 2:
+			pxmitpriv->amsdu_bk_timeout = status;
+			break;
+		case 4:
+		case 5:
+			pxmitpriv->amsdu_vi_timeout = status;
+			break;
+		case 6:
+		case 7:
+			pxmitpriv->amsdu_vo_timeout = status;
+			break;
+		case 0:
+		case 3:
+		default:
+			pxmitpriv->amsdu_be_timeout = status;
+			break;
+	}
+}
+
+void rtw_amsdu_set_timer(_adapter *padapter, u8 priority)
+{
+	struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
+
+	_timer* amsdu_timer = NULL;
+
+	switch(priority)
+	{
+		case 1:
+		case 2:
+			amsdu_timer = &pxmitpriv->amsdu_bk_timer;
+			break;
+		case 4:
+		case 5:
+			amsdu_timer = &pxmitpriv->amsdu_vi_timer;
+			break;
+		case 6:
+		case 7:
+			amsdu_timer = &pxmitpriv->amsdu_vo_timer;
+			break;
+		case 0:
+		case 3:
+		default:
+			amsdu_timer = &pxmitpriv->amsdu_be_timer;
+			break;
+	}
+	_set_timer(amsdu_timer, 1);
+}
+
+void rtw_amsdu_cancel_timer(_adapter *padapter, u8 priority)
+{
+	struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
+	_timer* amsdu_timer = NULL;
+	u8 cancel;
+
+	switch(priority)
+	{
+		case 1:
+		case 2:
+			amsdu_timer = &pxmitpriv->amsdu_bk_timer;
+			break;
+		case 4:
+		case 5:
+			amsdu_timer = &pxmitpriv->amsdu_vi_timer;
+			break;
+		case 6:
+		case 7:
+			amsdu_timer = &pxmitpriv->amsdu_vo_timer;
+			break;
+		case 0:
+		case 3:
+		default:
+			amsdu_timer = &pxmitpriv->amsdu_be_timer;
+			break;
+	}
+	_cancel_timer(amsdu_timer, &cancel);
+}
+#endif /* CONFIG_TX_AMSDU */
+
+void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms)
+{
+	sctx->timeout_ms = timeout_ms;
+	sctx->submit_time = jiffies;
+	init_completion(&sctx->done);
+	sctx->status = RTW_SCTX_SUBMITTED;
+}
+
+int rtw_sctx_wait(struct submit_ctx *sctx, const char *msg)
+{
+	int ret = _FAIL;
+	unsigned long expire;
+	int status = 0;
+
+	expire = sctx->timeout_ms ? msecs_to_jiffies(sctx->timeout_ms) : MAX_SCHEDULE_TIMEOUT;
+	if (!wait_for_completion_timeout(&sctx->done, expire)) {
+		/* timeout, do something?? */
+		status = RTW_SCTX_DONE_TIMEOUT;
+		RTW_INFO("%s timeout: %s\n", __func__, msg);
+	} else
+		status = sctx->status;
+
+	if (status == RTW_SCTX_DONE_SUCCESS)
+		ret = _SUCCESS;
+
+	return ret;
+}
+
+static bool rtw_sctx_chk_waring_status(int status)
+{
+	switch (status) {
+	case RTW_SCTX_DONE_UNKNOWN:
+	case RTW_SCTX_DONE_BUF_ALLOC:
+	case RTW_SCTX_DONE_BUF_FREE:
+
+	case RTW_SCTX_DONE_DRV_STOP:
+	case RTW_SCTX_DONE_DEV_REMOVE:
+		return true;
+	default:
+		return false;
+	}
+}
+
+void rtw_sctx_done_err(struct submit_ctx **sctx, int status)
+{
+	if (*sctx) {
+		if (rtw_sctx_chk_waring_status(status))
+			RTW_INFO("%s status:%d\n", __func__, status);
+		(*sctx)->status = status;
+		complete(&((*sctx)->done));
+		*sctx = NULL;
+	}
+}
+
+void rtw_sctx_done(struct submit_ctx **sctx)
+{
+	rtw_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS);
+}
+
+#ifdef CONFIG_XMIT_ACK
+int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms)
+{
+	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+
+	pack_tx_ops->submit_time = jiffies;
+	pack_tx_ops->timeout_ms = timeout_ms;
+	pack_tx_ops->status = RTW_SCTX_SUBMITTED;
+
+	return rtw_sctx_wait(pack_tx_ops, __func__);
+}
+
+void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status)
+{
+	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
+
+	if (pxmitpriv->ack_tx)
+		rtw_sctx_done_err(&pack_tx_ops, status);
+	else
+		RTW_INFO("%s ack_tx not set\n", __func__);
+}
+#endif /* CONFIG_XMIT_ACK */
-- 
2.31.1






[Index of Archives]     [Linux Driver Development]     [Linux Driver Backports]     [DMA Engine]     [Linux GPIO]     [Linux SPI]     [Video for Linux]     [Linux USB Devel]     [Linux Coverity]     [Linux Audio Users]     [Linux Kernel]     [Linux SCSI]     [Yosemite Backpacking]
  Powered by Linux