code:
From https://github.com/hadess/rtl8723bs

Tpomost commit:

commit b3def82d8cbd0e7011bfaa6b70cd74725863e833
Merge: aae04af 20822be
Author: lwfinger <Larry.Finger@lwfinger.net>
Date:   Thu Nov 3 10:33:33 2016 -0500

    Merge pull request #110 from colin4124/patch-1

    Add usage guide

Signed-off-by: Thomas Backlund <tmb@mageia.org>

 3rdparty/rtl8723bs/Kconfig                        |   11
 3rdparty/rtl8723bs/Makefile                       |  251
 3rdparty/rtl8723bs/core/rtw_ap.c                  | 2684 ++++++++
 3rdparty/rtl8723bs/core/rtw_btcoex.c              |  243
 3rdparty/rtl8723bs/core/rtw_cmd.c                 | 2244 +++++++
 3rdparty/rtl8723bs/core/rtw_debug.c               | 1453 ++++
 3rdparty/rtl8723bs/core/rtw_eeprom.c              |  369 +
 3rdparty/rtl8723bs/core/rtw_efuse.c               |  635 ++
 3rdparty/rtl8723bs/core/rtw_ieee80211.c           | 1433 ++++
 3rdparty/rtl8723bs/core/rtw_io.c                  |  203
 3rdparty/rtl8723bs/core/rtw_ioctl_set.c           |  696 ++
 3rdparty/rtl8723bs/core/rtw_mlme.c                | 3155 ++++++++++
 3rdparty/rtl8723bs/core/rtw_mlme_ext.c            | 6940 ++++++++++++++++++++++
 3rdparty/rtl8723bs/core/rtw_odm.c                 |  195
 3rdparty/rtl8723bs/core/rtw_pwrctrl.c             | 1421 ++++
 3rdparty/rtl8723bs/core/rtw_recv.c                | 2693 ++++++++
 3rdparty/rtl8723bs/core/rtw_rf.c                  |   64
 3rdparty/rtl8723bs/core/rtw_security.c            | 2448 +++++++
 3rdparty/rtl8723bs/core/rtw_sta_mgt.c             |  641 ++
 3rdparty/rtl8723bs/core/rtw_wlan_util.c           | 2328 +++++++
 3rdparty/rtl8723bs/core/rtw_xmit.c                | 3100 +++++++++
 3rdparty/rtl8723bs/hal/Hal8723BPwrSeq.c           |  138
 3rdparty/rtl8723bs/hal/Hal8723BReg.h              |  442 +
 3rdparty/rtl8723bs/hal/HalBtc8723b1Ant.c          | 3779 +++++++++++
 3rdparty/rtl8723bs/hal/HalBtc8723b1Ant.h          |  193
 3rdparty/rtl8723bs/hal/HalBtc8723b2Ant.c          | 3729 +++++++++++
 3rdparty/rtl8723bs/hal/HalBtc8723b2Ant.h          |  155
 3rdparty/rtl8723bs/hal/HalBtcOutSrc.h             |  565 +
 3rdparty/rtl8723bs/hal/HalHWImg8723B_BB.c         |  643 ++
 3rdparty/rtl8723bs/hal/HalHWImg8723B_BB.h         |   48
 3rdparty/rtl8723bs/hal/HalHWImg8723B_MAC.c        |  302
 3rdparty/rtl8723bs/hal/HalHWImg8723B_MAC.h        |   28
 3rdparty/rtl8723bs/hal/HalHWImg8723B_RF.c         |  799 ++
 3rdparty/rtl8723bs/hal/HalHWImg8723B_RF.h         |   49
 3rdparty/rtl8723bs/hal/HalPhyRf.c                 |  662 ++
 3rdparty/rtl8723bs/hal/HalPhyRf.h                 |   63
 3rdparty/rtl8723bs/hal/HalPhyRf_8723B.c           | 2091 ++++++
 3rdparty/rtl8723bs/hal/HalPhyRf_8723B.h           |   83
 3rdparty/rtl8723bs/hal/HalPwrSeqCmd.c             |  205
 3rdparty/rtl8723bs/hal/Mp_Precomp.h               |   33
 3rdparty/rtl8723bs/hal/hal_btcoex.c               | 1720 +++++
 3rdparty/rtl8723bs/hal/hal_com.c                  | 1753 +++++
 3rdparty/rtl8723bs/hal/hal_com_phycfg.c           | 3288 ++++++++++
 3rdparty/rtl8723bs/hal/hal_intf.c                 |  474 +
 3rdparty/rtl8723bs/hal/hal_phy.c                  |  224
 3rdparty/rtl8723bs/hal/hal_sdio.c                 |  115
 3rdparty/rtl8723bs/hal/odm.c                      | 1446 ++++
 3rdparty/rtl8723bs/hal/odm.h                      | 1465 ++++
 3rdparty/rtl8723bs/hal/odm_AntDiv.c               |   70
 3rdparty/rtl8723bs/hal/odm_AntDiv.h               |   38
 3rdparty/rtl8723bs/hal/odm_CfoTracking.c          |  338 +
 3rdparty/rtl8723bs/hal/odm_CfoTracking.h          |   47
 3rdparty/rtl8723bs/hal/odm_DIG.c                  | 1221 +++
 3rdparty/rtl8723bs/hal/odm_DIG.h                  |  195
 3rdparty/rtl8723bs/hal/odm_DynamicBBPowerSaving.c |   89
 3rdparty/rtl8723bs/hal/odm_DynamicBBPowerSaving.h |   39
 3rdparty/rtl8723bs/hal/odm_DynamicTxPower.c       |   30
 3rdparty/rtl8723bs/hal/odm_DynamicTxPower.h       |   37
 3rdparty/rtl8723bs/hal/odm_EdcaTurboCheck.c       |  186
 3rdparty/rtl8723bs/hal/odm_EdcaTurboCheck.h       |   31
 3rdparty/rtl8723bs/hal/odm_HWConfig.c             |  535 +
 3rdparty/rtl8723bs/hal/odm_HWConfig.h             |  162
 3rdparty/rtl8723bs/hal/odm_NoiseMonitor.c         |  175
 3rdparty/rtl8723bs/hal/odm_NoiseMonitor.h         |   47
 3rdparty/rtl8723bs/hal/odm_PathDiv.c              |   42
 3rdparty/rtl8723bs/hal/odm_PathDiv.h              |   29
 3rdparty/rtl8723bs/hal/odm_RTL8723B.c             |   45
 3rdparty/rtl8723bs/hal/odm_RTL8723B.h             |   22
 3rdparty/rtl8723bs/hal/odm_RegConfig8723B.c       |  257
 3rdparty/rtl8723bs/hal/odm_RegConfig8723B.h       |   65
 3rdparty/rtl8723bs/hal/odm_RegDefine11N.h         |  172
 3rdparty/rtl8723bs/hal/odm_debug.c                |   52
 3rdparty/rtl8723bs/hal/odm_debug.h                |  165
 3rdparty/rtl8723bs/hal/odm_interface.h            |   59
 3rdparty/rtl8723bs/hal/odm_precomp.h              |   60
 3rdparty/rtl8723bs/hal/odm_reg.h                  |  103
 3rdparty/rtl8723bs/hal/odm_types.h                |  102
 3rdparty/rtl8723bs/hal/rtl8723b_cmd.c             | 2358 +++++++
 3rdparty/rtl8723bs/hal/rtl8723b_dm.c              |  300
 3rdparty/rtl8723bs/hal/rtl8723b_hal_init.c        | 4546 ++++++++++++++
 3rdparty/rtl8723bs/hal/rtl8723b_phycfg.c          | 1060 +++
 3rdparty/rtl8723bs/hal/rtl8723b_rf6052.c          |  230
 3rdparty/rtl8723bs/hal/rtl8723b_rxdesc.c          |   86
 3rdparty/rtl8723bs/hal/rtl8723bs_recv.c           |  490 +
 3rdparty/rtl8723bs/hal/rtl8723bs_xmit.c           |  686 ++
 3rdparty/rtl8723bs/hal/sdio_halinit.c             | 1928 ++++++
 3rdparty/rtl8723bs/hal/sdio_ops.c                 | 1296 ++++
 3rdparty/rtl8723bs/include/Hal8192CPhyReg.h       | 1126 +++
 3rdparty/rtl8723bs/include/Hal8723BPhyCfg.h       |  128
 3rdparty/rtl8723bs/include/Hal8723BPhyReg.h       |   77
 3rdparty/rtl8723bs/include/Hal8723BPwrSeq.h       |  232
 3rdparty/rtl8723bs/include/HalPwrSeqCmd.h         |  132
 3rdparty/rtl8723bs/include/HalVerDef.h            |  127
 3rdparty/rtl8723bs/include/autoconf.h             |   71
 3rdparty/rtl8723bs/include/basic_types.h          |  209
 3rdparty/rtl8723bs/include/cmd_osdep.h            |   26
 3rdparty/rtl8723bs/include/drv_conf.h             |   37
 3rdparty/rtl8723bs/include/drv_types.h            |  720 ++
 3rdparty/rtl8723bs/include/drv_types_sdio.h       |   39
 3rdparty/rtl8723bs/include/ethernet.h             |   22
 3rdparty/rtl8723bs/include/hal_btcoex.h           |   68
 3rdparty/rtl8723bs/include/hal_com.h              |  309
 3rdparty/rtl8723bs/include/hal_com_h2c.h          |  293
 3rdparty/rtl8723bs/include/hal_com_phycfg.h       |  275
 3rdparty/rtl8723bs/include/hal_com_reg.h          | 1725 +++++
 3rdparty/rtl8723bs/include/hal_data.h             |  485 +
 3rdparty/rtl8723bs/include/hal_intf.h             |  410 +
 3rdparty/rtl8723bs/include/hal_pg.h               |   81
 3rdparty/rtl8723bs/include/hal_phy.h              |  183
 3rdparty/rtl8723bs/include/hal_phy_reg.h          |   25
 3rdparty/rtl8723bs/include/hal_sdio.h             |   26
 3rdparty/rtl8723bs/include/ieee80211.h            | 1345 ++++
 3rdparty/rtl8723bs/include/ioctl_cfg80211.h       |  132
 3rdparty/rtl8723bs/include/mlme_osdep.h           |   27
 3rdparty/rtl8723bs/include/osdep_intf.h           |   88
 3rdparty/rtl8723bs/include/osdep_service.h        |  281
 3rdparty/rtl8723bs/include/osdep_service_linux.h  |  178
 3rdparty/rtl8723bs/include/recv_osdep.h           |   48
 3rdparty/rtl8723bs/include/rtl8192c_recv.h        |   50
 3rdparty/rtl8723bs/include/rtl8192c_rf.h          |   39
 3rdparty/rtl8723bs/include/rtl8723b_cmd.h         |  199
 3rdparty/rtl8723bs/include/rtl8723b_dm.h          |   41
 3rdparty/rtl8723bs/include/rtl8723b_hal.h         |  279
 3rdparty/rtl8723bs/include/rtl8723b_recv.h        |  144
 3rdparty/rtl8723bs/include/rtl8723b_rf.h          |   26
 3rdparty/rtl8723bs/include/rtl8723b_spec.h        |  262
 3rdparty/rtl8723bs/include/rtl8723b_xmit.h        |  458 +
 3rdparty/rtl8723bs/include/rtw_ap.h               |   47
 3rdparty/rtl8723bs/include/rtw_beamforming.h      |  135
 3rdparty/rtl8723bs/include/rtw_br_ext.h           |   63
 3rdparty/rtl8723bs/include/rtw_btcoex.h           |   64
 3rdparty/rtl8723bs/include/rtw_byteorder.h        |   24
 3rdparty/rtl8723bs/include/rtw_cmd.h              |  980 +++
 3rdparty/rtl8723bs/include/rtw_debug.h            |  355 +
 3rdparty/rtl8723bs/include/rtw_eeprom.h           |  128
 3rdparty/rtl8723bs/include/rtw_efuse.h            |  132
 3rdparty/rtl8723bs/include/rtw_event.h            |  117
 3rdparty/rtl8723bs/include/rtw_ht.h               |  118
 3rdparty/rtl8723bs/include/rtw_io.h               |  373 +
 3rdparty/rtl8723bs/include/rtw_ioctl.h            |   80
 3rdparty/rtl8723bs/include/rtw_ioctl_set.h        |   41
 3rdparty/rtl8723bs/include/rtw_mlme.h             |  695 ++
 3rdparty/rtl8723bs/include/rtw_mlme_ext.h         |  888 ++
 3rdparty/rtl8723bs/include/rtw_mp.h               |  512 +
 3rdparty/rtl8723bs/include/rtw_odm.h              |   36
 3rdparty/rtl8723bs/include/rtw_pwrctrl.h          |  375 +
 3rdparty/rtl8723bs/include/rtw_qos.h              |   27
 3rdparty/rtl8723bs/include/rtw_recv.h             |  553 +
 3rdparty/rtl8723bs/include/rtw_rf.h               |  159
 3rdparty/rtl8723bs/include/rtw_security.h         |  440 +
 3rdparty/rtl8723bs/include/rtw_version.h          |    2
 3rdparty/rtl8723bs/include/rtw_wifi_regd.h        |   35
 3rdparty/rtl8723bs/include/rtw_xmit.h             |  528 +
 3rdparty/rtl8723bs/include/sdio_hal.h             |   28
 3rdparty/rtl8723bs/include/sdio_ops.h             |   49
 3rdparty/rtl8723bs/include/sdio_ops_linux.h       |   40
 3rdparty/rtl8723bs/include/sdio_osintf.h          |   24
 3rdparty/rtl8723bs/include/sta_info.h             |  392 +
 3rdparty/rtl8723bs/include/wifi.h                 | 1158 +++
 3rdparty/rtl8723bs/include/wlan_bssdef.h          |  278
 3rdparty/rtl8723bs/include/xmit_osdep.h           |   54
 3rdparty/rtl8723bs/os_dep/ioctl_cfg80211.c        | 3608 +++++++++++
 3rdparty/rtl8723bs/os_dep/ioctl_linux.c           | 5820 ++++++++++++++++++
 3rdparty/rtl8723bs/os_dep/mlme_linux.c            |  206
 3rdparty/rtl8723bs/os_dep/os_intfs.c              | 1916 ++++++
 3rdparty/rtl8723bs/os_dep/osdep_service.c         |  481 +
 3rdparty/rtl8723bs/os_dep/recv_linux.c            |  366 +
 3rdparty/rtl8723bs/os_dep/rtw_proc.c              |  786 ++
 3rdparty/rtl8723bs/os_dep/rtw_proc.h              |   45
 3rdparty/rtl8723bs/os_dep/sdio_intf.c             |  727 ++
 3rdparty/rtl8723bs/os_dep/sdio_ops_linux.c        |  599 +
 3rdparty/rtl8723bs/os_dep/wifi_regd.c             |  164
 3rdparty/rtl8723bs/os_dep/xmit_linux.c            |  298
 173 files changed, 109461 insertions(+)

diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_ap.c linux/3rdparty/rtl8723bs/core/rtw_ap.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_ap.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_ap.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,2684 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#define _RTW_AP_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+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(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+
+
+	spin_lock_init(&pmlmepriv->bcn_update_lock);
+
+	/* for ACL */
+	_rtw_init_queue(&pacl_list->acl_node_q);
+
+	/* pmlmeext->bstart_bss = false; */
+
+	start_ap_mode(padapter);
+}
+
+void free_mlme_ap_info(struct adapter *padapter)
+{
+	struct sta_info *psta = NULL;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* stop_ap_mode(padapter); */
+
+	pmlmepriv->update_bcn = false;
+	pmlmeext->bstart_bss = false;
+
+	rtw_sta_flush(padapter);
+
+	pmlmeinfo->state = _HW_STATE_NOLINK_;
+
+	/* free_assoc_sta_resources */
+	rtw_free_all_stainfo(padapter);
+
+	/* free bc/mc sta_info */
+	psta = rtw_get_bcmc_stainfo(padapter);
+	rtw_free_stainfo(padapter, psta);
+}
+
+static void update_BCNTIM(struct adapter *padapter)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *pnetwork_mlmeext = &(pmlmeinfo->network);
+	unsigned char *pie = pnetwork_mlmeext->IEs;
+
+	/* DBG_871X("%s\n", __func__); */
+
+	/* update TIM IE */
+	/* if (pstapriv->tim_bitmap) */
+	if (true) {
+
+		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;
+
+			/* calucate head_len */
+			offset = _FIXED_IE_LENGTH_;
+
+			/* get ssid_ie len */
+			p = rtw_get_ie(
+				pie + _BEACON_IE_OFFSET_,
+				_SSID_IE_,
+				&tmp_len,
+				(pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_)
+			);
+			if (p != NULL)
+				offset += tmp_len+2;
+
+			/*  get supported rates len */
+			p = rtw_get_ie(
+				pie + _BEACON_IE_OFFSET_,
+				_SUPPORTEDRATES_IE_, &tmp_len,
+				(pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_)
+			);
+			if (p !=  NULL)
+				offset += tmp_len+2;
+
+
+			/* DS Parameter Set IE, len =3 */
+			offset += 3;
+
+			premainder_ie = pie + offset;
+
+			remainder_ielen = pnetwork_mlmeext->IELength - offset - tim_ielen;
+
+			/* append TIM IE from offset */
+			dst_ie = pie + offset;
+
+		}
+
+
+		if (remainder_ielen > 0) {
+
+			pbackup_remainder_ie = rtw_malloc(remainder_ielen);
+			if (pbackup_remainder_ie && premainder_ie)
+				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 peroid */
+
+		if (pstapriv->tim_bitmap&BIT(0))/* for bc/mc frames */
+			*dst_ie++ = BIT(0);/* bitmap ctrl */
+		else
+			*dst_ie++ = 0;
+
+		if (tim_ielen == 4) {
+
+			__le16 pvb;
+
+			if (pstapriv->tim_bitmap&0xff00)
+				pvb = cpu_to_le16(pstapriv->tim_bitmap >> 8);
+			else
+				pvb = tim_bitmap_le;
+
+			*dst_ie++ = le16_to_cpu(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);
+
+			kfree(pbackup_remainder_ie);
+		}
+
+		offset =  (uint)(dst_ie - pie);
+		pnetwork_mlmeext->IELength = offset + remainder_ielen;
+
+	}
+}
+
+u8 chk_sta_is_alive(struct sta_info *psta);
+u8 chk_sta_is_alive(struct sta_info *psta)
+{
+	#ifdef DBG_EXPIRATION_CHK
+	DBG_871X(
+		"sta:"MAC_FMT", rssi:%d, rx:"STA_PKTS_FMT", expire_to:%u, %s%ssq_len:%u\n"
+		, MAC_ARG(psta->hwaddr)
+		, psta->rssi_stat.UndecoratedSmoothedPWDB
+		/*  STA_RX_PKTS_ARG(psta) */
+		, STA_RX_PKTS_DIFF_ARG(psta)
+		, psta->expire_to
+		, psta->state&WIFI_SLEEP_STATE?"PS, ":""
+		, psta->state&WIFI_STA_ALIVE_CHK_STATE?"SAC, ":""
+		, psta->sleepq_len
+	);
+	#endif
+
+	sta_update_last_rx_pkts(psta);
+
+	return true;
+}
+
+void expire_timeout_chk(struct adapter *padapter)
+{
+	struct list_head	*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;
+
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+
+	phead = &pstapriv->auth_list;
+	plist = get_next(phead);
+
+	/* check auth_queue */
+	#ifdef DBG_EXPIRATION_CHK
+	if (phead != plist) {
+		DBG_871X(FUNC_NDEV_FMT" auth_list, cnt:%u\n"
+			, FUNC_NDEV_ARG(padapter->pnetdev), pstapriv->auth_list_cnt);
+	}
+	#endif
+	while (phead != plist) {
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, auth_list);
+
+		plist = get_next(plist);
+
+		if (psta->expire_to > 0) {
+
+			psta->expire_to--;
+			if (psta->expire_to == 0) {
+
+				list_del_init(&psta->auth_list);
+				pstapriv->auth_list_cnt--;
+
+				DBG_871X(
+					"auth expire %02X%02X%02X%02X%02X%02X\n",
+					psta->hwaddr[0],
+					psta->hwaddr[1],
+					psta->hwaddr[2],
+					psta->hwaddr[3],
+					psta->hwaddr[4],
+					psta->hwaddr[5]
+				);
+
+				spin_unlock_bh(&pstapriv->auth_list_lock);
+
+				rtw_free_stainfo(padapter, psta);
+
+				spin_lock_bh(&pstapriv->auth_list_lock);
+			}
+		}
+
+	}
+
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+	psta = NULL;
+
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+
+	phead = &pstapriv->asoc_list;
+	plist = get_next(phead);
+
+	/* check asoc_queue */
+	#ifdef DBG_EXPIRATION_CHK
+	if (phead != plist) {
+		DBG_871X(FUNC_NDEV_FMT" asoc_list, cnt:%u\n"
+			, FUNC_NDEV_ARG(padapter->pnetdev), pstapriv->asoc_list_cnt);
+	}
+	#endif
+	while (phead != plist) {
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+		plist = get_next(plist);
+#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;
+			psta->under_exist_checking = 0;
+		} else {
+			if (psta->expire_to > 0)
+				psta->expire_to--;
+		}
+
+		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;
+			}
+
+			if (psta->state & WIFI_SLEEP_STATE) {
+				if (!(psta->state & WIFI_STA_ALIVE_CHK_STATE)) {
+					/* to check if alive by another methods if staion is at ps mode. */
+					psta->expire_to = pstapriv->expire_to;
+					psta->state |= WIFI_STA_ALIVE_CHK_STATE;
+
+					/* DBG_871X("alive chk, sta:" MAC_FMT " is at ps mode!\n", MAC_ARG(psta->hwaddr)); */
+
+					/* to update bcn with tim_bitmap for this station */
+					pstapriv->tim_bitmap |= BIT(psta->aid);
+					update_beacon(padapter, _TIM_IE_, NULL, true);
+
+					if (!pmlmeext->active_keep_alive_check)
+						continue;
+				}
+			}
+			if (pmlmeext->active_keep_alive_check) {
+				int stainfo_offset;
+
+				stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
+				if (stainfo_offset_valid(stainfo_offset))
+					chk_alive_list[chk_alive_num++] = stainfo_offset;
+
+
+				continue;
+			}
+			list_del_init(&psta->asoc_list);
+			pstapriv->asoc_list_cnt--;
+			DBG_871X(
+				"asoc expire "MAC_FMT", state = 0x%x\n",
+				MAC_ARG(psta->hwaddr),
+				psta->state
+			);
+			updated = ap_free_sta(padapter, psta, false, WLAN_REASON_DEAUTH_LEAVING);
+		} else{
+
+
+			/* TODO: Aging mechanism to digest frames in sleep_q to avoid running out of xmitframe */
+			if (psta->sleepq_len > (NR_XMITFRAME/pstapriv->asoc_list_cnt)
+				&& padapter->xmitpriv.free_xmitframe_cnt < ((
+					NR_XMITFRAME/pstapriv->asoc_list_cnt
+				)/2)
+			) {
+				DBG_871X(
+					"%s sta:"MAC_FMT", sleepq_len:%u, free_xmitframe_cnt:%u, asoc_list_cnt:%u, clear sleep_q\n",
+					__func__,
+					MAC_ARG(psta->hwaddr),
+					psta->sleepq_len,
+					padapter->xmitpriv.free_xmitframe_cnt,
+					pstapriv->asoc_list_cnt
+				);
+				wakeup_sta_to_xmit(padapter, psta);
+			}
+		}
+	}
+
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+if (chk_alive_num) {
+
+	u8 backup_oper_channel = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	/* switch to correct channel of current network  before issue keep-alive frames */
+	if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
+		backup_oper_channel = rtw_get_oper_ch(padapter);
+		SelectChannel(padapter, pmlmeext->cur_channel);
+	}
+
+	/* issue null data to check sta alive*/
+	for (i = 0; i < chk_alive_num; i++) {
+		int ret = _FAIL;
+
+		psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);
+		if (!(psta->state & _FW_LINKED))
+			continue;
+
+		if (psta->state & WIFI_SLEEP_STATE)
+			ret = issue_nulldata(padapter, psta->hwaddr, 0, 1, 50);
+		else
+			ret = issue_nulldata(padapter, psta->hwaddr, 0, 3, 50);
+
+		psta->keep_alive_trycnt++;
+		if (ret == _SUCCESS) {
+
+			DBG_871X(
+				"asoc check, sta(" MAC_FMT ") is alive\n",
+				MAC_ARG(psta->hwaddr)
+			);
+			psta->expire_to = pstapriv->expire_to;
+			psta->keep_alive_trycnt = 0;
+			continue;
+		} else if (psta->keep_alive_trycnt <= 3) {
+
+
+			DBG_871X(
+				"ack check for asoc expire, keep_alive_trycnt =%d\n",
+				psta->keep_alive_trycnt
+			);
+			psta->expire_to = 1;
+			continue;
+		}
+
+		psta->keep_alive_trycnt = 0;
+		DBG_871X(
+			"asoc expire "MAC_FMT", state = 0x%x\n",
+			MAC_ARG(psta->hwaddr),
+			psta->state
+		);
+		spin_lock_bh(&pstapriv->asoc_list_lock);
+		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_DEAUTH_LEAVING);
+		}
+		spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	}
+
+	if (backup_oper_channel > 0) /* back to the original operation channel */
+		SelectChannel(padapter, backup_oper_channel);
+}
+
+	associated_clients_update(padapter, updated);
+}
+
+void add_RATid(struct adapter *padapter, struct sta_info *psta, u8 rssi_level)
+{
+	unsigned char sta_band = 0, shortGIrate = false;
+	unsigned int tx_ra_bitmap = 0;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct wlan_bssid_ex
+		*pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+
+	if (!psta)
+		return;
+
+	if (!(psta->state & _FW_LINKED))
+		return;
+
+	rtw_hal_update_sta_rate_mask(padapter, psta);
+	tx_ra_bitmap = psta->ra_mask;
+
+	shortGIrate = query_ra_short_GI(psta);
+
+	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 {
+		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) {
+
+		u8 arg[4] = {0};
+
+		arg[0] = psta->mac_id;
+		arg[1] = psta->raid;
+		arg[2] = shortGIrate;
+		arg[3] = psta->init_rate;
+
+		DBG_871X("%s => mac_id:%d , raid:%d , shortGIrate =%d, bitmap = 0x%x\n",
+			__func__, psta->mac_id, psta->raid, shortGIrate, tx_ra_bitmap);
+
+		rtw_hal_add_ra_tid(padapter, tx_ra_bitmap, arg, rssi_level);
+	} else{
+
+
+		DBG_871X("station aid %d exceed the max number\n", psta->aid);
+	}
+
+}
+
+void update_bmc_sta(struct adapter *padapter)
+{
+	unsigned char network_type;
+	int supportRateNum = 0;
+	unsigned int tx_ra_bitmap = 0;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex
+		*pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct sta_info *psta = rtw_get_bcmc_stainfo(padapter);
+
+	if (psta) {
+
+		psta->aid = 0;/* default set to 0 */
+		/* psta->mac_id = psta->aid+4; */
+		psta->mac_id = psta->aid + 1;/* mac_id = 1 for bc/mc stainfo */
+
+		pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+		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);
+		tx_ra_bitmap = psta->ra_mask;
+
+		psta->raid = rtw_hal_networktype_to_raid(padapter, psta);
+
+		/* ap mode */
+		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
+
+		/* if (pHalData->fw_ractrl == true) */
+		{
+			u8 arg[4] = {0};
+
+			arg[0] = psta->mac_id;
+			arg[1] = psta->raid;
+			arg[2] = 0;
+			arg[3] = psta->init_rate;
+
+			DBG_871X("%s => mac_id:%d , raid:%d , bitmap = 0x%x\n",
+				__func__, psta->mac_id, psta->raid, tx_ra_bitmap);
+
+			rtw_hal_add_ra_tid(padapter, tx_ra_bitmap, arg, 0);
+		}
+
+		rtw_sta_media_status_rpt(padapter, psta, 1);
+
+		spin_lock_bh(&psta->lock);
+		psta->state = _FW_LINKED;
+		spin_unlock_bh(&psta->lock);
+
+	} else{
+
+
+		DBG_871X("add_RATid_bmc_sta error!\n");
+	}
+
+}
+
+/* notes: */
+/* AID: 1~MAX for sta and 0 for bc/mc in ap/adhoc mode */
+/* MAC_ID = AID+1 for sta in ap/adhoc mode */
+/* MAC_ID = 1 for bc/mc for sta/ap/adhoc */
+/* MAC_ID = 0 for bssid for sta/ap/adhoc */
+/* CAM_ID = 0~3 for default key, cmd_id =macid + 3, macid =aid+1; */
+
+void update_sta_info_apmode(struct adapter *padapter, struct sta_info *psta)
+{
+	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; */
+
+	DBG_871X("%s\n", __func__);
+
+	/* psta->mac_id = psta->aid+4; */
+	/* psta->mac_id = psta->aid+1;//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 (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_CAPABILITY_ELE_LDPC_CAP((u8 *)(&phtpriv_sta->ht_cap))) {
+
+			SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX));
+			DBG_871X("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_CAPABILITY_ELE_RX_STBC((u8 *)(&phtpriv_sta->ht_cap))) {
+
+			SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX));
+			DBG_871X("Enable HT Tx STBC for STA(%d)\n", psta->aid);
+		}
+	} 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));
+
+
+	/* add ratid */
+	/* add_RATid(padapter, psta);//move to ap_sta_info_defer_update() */
+
+
+	spin_lock_bh(&psta->lock);
+	psta->state |= _FW_LINKED;
+	spin_unlock_bh(&psta->lock);
+
+
+}
+
+static void update_ap_info(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct wlan_bssid_ex
+		*pnetwork = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	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));
+}
+
+static void update_hw_ht_param(struct adapter *padapter)
+{
+	unsigned char max_AMPDU_len;
+	unsigned char min_MPDU_spacing;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	DBG_871X("%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)
+		DBG_871X("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__);
+
+	/*  */
+	/*  Config current HT Protection mode. */
+	/*  */
+	/* pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3; */
+
+}
+
+void start_bss_network(struct adapter *padapter, u8 *pbuf)
+{
+	u8 *p;
+	u8 val8, cur_channel, cur_bwmode, cur_ch_offset;
+	u16 bcn_interval;
+	u32 acparm;
+	int	ie_len;
+	struct registry_priv  *pregpriv = &padapter->registrypriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	struct wlan_bssid_ex
+		*pnetwork = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *pnetwork_mlmeext = &(pmlmeinfo->network);
+	struct HT_info_element *pht_info = NULL;
+	u8 cbw40_enable = 0;
+
+	/* DBG_871X("%s\n", __func__); */
+
+	bcn_interval = (u16)pnetwork->Configuration.BeaconPeriod;
+	cur_channel = pnetwork->Configuration.DSConfig;
+	cur_bwmode = CHANNEL_WIDTH_20;
+	cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+
+	/* check if there is wps ie, */
+	/* if there is wpsie in beacon, the hostapd will update beacon twice when stating hostapd, */
+	/* and at first time the security ie (RSN/WPA IE) will not include in beacon. */
+	if (NULL == rtw_get_wps_ie(
+		pnetwork->IEs+_FIXED_IE_LENGTH_,
+		pnetwork->IELength-_FIXED_IE_LENGTH_,
+		NULL,
+		NULL
+	))
+		pmlmeext->bstart_bss = true;
+
+
+	/* todo: update wmm, ht cap */
+	/* pmlmeinfo->WMM_enable; */
+	/* pmlmeinfo->HT_enable; */
+	if (pmlmepriv->qospriv.qos_option)
+		pmlmeinfo->WMM_enable = true;
+	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 */
+	acparm = 0x002F3217; /*  VO */
+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
+	acparm = 0x005E4317; /*  VI */
+	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
+	/* acparm = 0x00105320; // BE */
+	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));
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
+
+	if (pmlmepriv->cur_network.join_res != true) { /* setting only at  first time */
+
+		/* u32 initialgain; */
+
+		/* initialgain = 0x1e; */
+
+
+		/* disable dynamic functions, such as high power, DIG */
+		/* Save_DM_Func_Flag(padapter); */
+		/* Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false); */
+
+		/* turn on all dynamic functions */
+		Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true);
+
+		/* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
+
+	}
+
+	/* set channel, bwmode */
+	p = rtw_get_ie(
+		(pnetwork->IEs + sizeof(struct ndis_802_11_fix_ie)),
+		_HT_ADD_INFO_IE_,
+		&ie_len,
+		(pnetwork->IELength - sizeof(struct ndis_802_11_fix_ie))
+	);
+	if (p && ie_len) {
+
+		pht_info = (struct HT_info_element *)(p+2);
+
+		if (cur_channel > 14) {
+			if ((pregpriv->bw_mode & 0xf0) > 0)
+				cbw40_enable = 1;
+		} else {
+			if ((pregpriv->bw_mode & 0x0f) > 0)
+				cbw40_enable = 1;
+		}
+
+		if ((cbw40_enable) &&	 (pht_info->infos[0] & BIT(2))) {
+
+			/* switch to the 40M Hz mode */
+			/* pmlmeext->cur_bwmode = CHANNEL_WIDTH_40; */
+			cur_bwmode = CHANNEL_WIDTH_40;
+			switch (pht_info->infos[0] & 0x3) {
+
+			case 1:
+				/* pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER; */
+				cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+				break;
+
+			case 3:
+				/* pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER; */
+				cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+				break;
+
+			default:
+				/* pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE; */
+				cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+				break;
+			}
+
+		}
+
+	}
+
+	set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
+	DBG_871X(
+		"CH =%d, BW =%d, offset =%d\n",
+		cur_channel,
+		cur_bwmode,
+		cur_ch_offset
+	);
+	pmlmeext->cur_channel = cur_channel;
+	pmlmeext->cur_bwmode = cur_bwmode;
+	pmlmeext->cur_ch_offset = cur_ch_offset;
+	pmlmeext->cur_wireless_mode = pmlmepriv->cur_network.network_type;
+
+	/* let pnetwork_mlmeext == pnetwork_mlme. */
+	memcpy(pnetwork_mlmeext, pnetwork, pnetwork->Length);
+
+	/* update cur_wireless_mode */
+	update_wireless_mode(padapter);
+
+	/* update RRSR after set channel and bandwidth */
+	UpdateBrateTbl(padapter, pnetwork->SupportedRates);
+	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
+
+	/* udpate capability after cur_wireless_mode updated */
+	update_capinfo(
+		padapter,
+		rtw_get_capability((struct wlan_bssid_ex *)pnetwork)
+	);
+
+
+	if (true == pmlmeext->bstart_bss) {
+
+		update_beacon(padapter, _TIM_IE_, NULL, true);
+
+#ifndef CONFIG_INTERRUPT_BASED_TXBCN /* other case will  tx beacon when bcn interrupt coming in. */
+		/* issue beacon frame */
+		if (send_beacon(padapter) == _FAIL)
+			DBG_871X("issue_beacon, fail!\n");
+
+#endif /* CONFIG_INTERRUPT_BASED_TXBCN */
+
+	}
+
+
+	/* update bc/mc sta_info */
+	update_bmc_sta(padapter);
+
+	/* pmlmeext->bstart_bss = true; */
+
+}
+
+int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
+{
+	int ret = _SUCCESS;
+	u8 *p;
+	u8 *pHT_caps_ie = NULL;
+	u8 *pHT_info_ie = NULL;
+	struct sta_info *psta = NULL;
+	u16 cap, ht_cap = false;
+	uint ie_len = 0;
+	int group_cipher, pairwise_cipher;
+	u8 channel, network_type, supportRate[NDIS_802_11_LENGTH_RATES_EX];
+	int supportRateNum = 0;
+	u8 OUI1[] = {0x00, 0x50, 0xf2, 0x01};
+	u8 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);
+	struct wlan_bssid_ex
+		*pbss_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	u8 *ie = pbss_network->IEs;
+
+	/* SSID */
+	/* Supported rates */
+	/* DS Params */
+	/* WLAN_EID_COUNTRY */
+	/* ERP Information element */
+	/* Extended supported rates */
+	/* WPA/WPA2 */
+	/* Wi-Fi Wireless Multimedia Extensions */
+	/* ht_capab, ht_oper */
+	/* WPS IE */
+
+	DBG_871X("%s, len =%d\n", __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;
+
+	pbss_network->Rssi = 0;
+
+	memcpy(pbss_network->MacAddress, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+	/* beacon interval */
+	p = rtw_get_beacon_interval_from_ie(ie);/* ie + 8;	8: TimeStamp, 2: Beacon Interval 2:Capability */
+	/* pbss_network->Configuration.BeaconPeriod = le16_to_cpu(*(unsigned short*)p); */
+	pbss_network->Configuration.BeaconPeriod = RTW_GET_LE16(p);
+
+	/* capability */
+	/* cap = *(unsigned short *)rtw_get_capability_from_ie(ie); */
+	/* cap = le16_to_cpu(cap); */
+	cap = RTW_GET_LE16(ie);
+
+	/* SSID */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		_SSID_IE_,
+		&ie_len,
+		(pbss_network->IELength - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0) {
+
+		memset(&pbss_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
+		memcpy(pbss_network->Ssid.Ssid, (p + 2), ie_len);
+		pbss_network->Ssid.SsidLength = ie_len;
+	}
+
+	/* chnnel */
+	channel = 0;
+	pbss_network->Configuration.Length = 0;
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		_DSSET_IE_, &ie_len,
+		(pbss_network->IELength - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0)
+		channel = *(p + 2);
+
+	pbss_network->Configuration.DSConfig = channel;
+
+
+	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, (struct ndis_80211_var_ie *)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;
+		u8 max_rx_ampdu_factor = 0;
+		struct rtw_ieee80211_ht_cap *pht_cap = (struct rtw_ieee80211_ht_cap *)(p+2);
+
+		pHT_caps_ie = p;
+
+		ht_cap = true;
+		network_type |= WIRELESS_11_24N;
+
+		rtw_ht_use_default_setting(padapter);
+
+		if (pmlmepriv->htpriv.sgi_20m == false)
+			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_SGI_20));
+
+		if (pmlmepriv->htpriv.sgi_40m == false)
+			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));
+
+
+		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 */
+
+		rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+		if (rf_type == RF_1T1R) {
+
+			pht_cap->supp_mcs_set[0] = 0xff;
+			pht_cap->supp_mcs_set[1] = 0x0;
+		}
+
+		memcpy(&pmlmepriv->htpriv.ht_cap, p+2, ie_len);
+
+	}
+
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		_HT_ADD_INFO_IE_,
+		&ie_len,
+		(pbss_network->IELength - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0)
+		pHT_info_ie = p;
+
+
+	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 == true) {
+
+		pmlmepriv->htpriv.ht_option = true;
+		pmlmepriv->qospriv.qos_option = 1;
+
+		if (pregistrypriv->ampdu_enable == 1)
+			pmlmepriv->htpriv.ampdu_enable = true;
+
+
+		HT_caps_handler(padapter, (struct ndis_80211_var_ie *)pHT_caps_ie);
+
+		HT_info_handler(padapter, (struct ndis_80211_var_ie *)pHT_info_ie);
+	}
+
+	pbss_network->Length = get_wlan_bssid_ex_sz(
+		(struct wlan_bssid_ex  *)pbss_network
+	);
+
+	/* issue beacon to start bss network */
+	/* start_bss_network(padapter, (u8 *)pbss_network); */
+	rtw_startbss_cmd(padapter, RTW_CMDF_WAIT_ACK);
+
+
+	/* alloc sta_info for ap itself */
+	psta = rtw_get_stainfo(&padapter->stapriv, pbss_network->MacAddress);
+	if (!psta) {
+
+		psta = rtw_alloc_stainfo(&padapter->stapriv, pbss_network->MacAddress);
+		if (psta == NULL)
+			return _FAIL;
+
+	}
+
+	/*  update AP's sta info */
+	update_ap_info(padapter, psta);
+
+	psta->state |= WIFI_AP_STATE;		/* Aries, add, fix bug of flush_cam_entry at STOP AP mode , 0724 */
+	rtw_indicate_connect(padapter);
+
+	pmlmepriv->cur_network.join_res = true;/* for check if already set beacon */
+
+	/* update bc/mc sta_info */
+	/* update_bmc_sta(padapter); */
+
+	return ret;
+
+}
+
+void rtw_set_macaddr_acl(struct adapter *padapter, int mode)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+
+	DBG_871X("%s, mode =%d\n", __func__, mode);
+
+	pacl_list->mode = mode;
+}
+
+int rtw_acl_add_sta(struct adapter *padapter, u8 *addr)
+{
+	struct list_head	*plist, *phead;
+	u8 added = false;
+	int i, ret = 0;
+	struct rtw_wlan_acl_node *paclnode;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+	struct __queue	*pacl_node_q = &pacl_list->acl_node_q;
+
+	DBG_871X(
+		"%s(acl_num =%d) =" MAC_FMT "\n",
+		__func__,
+		pacl_list->num,
+		MAC_ARG(addr)
+	);
+
+	if ((NUM_ACL-1) < pacl_list->num)
+		return (-1);
+
+
+	spin_lock_bh(&(pacl_node_q->lock));
+
+	phead = get_list_head(pacl_node_q);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+
+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);
+		plist = get_next(plist);
+
+		if (!memcmp(paclnode->addr, addr, ETH_ALEN)) {
+
+			if (paclnode->valid == true) {
+
+				added = true;
+				DBG_871X("%s, sta has been added\n", __func__);
+				break;
+			}
+		}
+	}
+
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+
+	if (added == true)
+		return ret;
+
+
+	spin_lock_bh(&(pacl_node_q->lock));
+
+	for (i = 0; i < NUM_ACL; i++) {
+
+		paclnode = &pacl_list->aclnode[i];
+
+		if (paclnode->valid == false) {
+
+			INIT_LIST_HEAD(&paclnode->list);
+
+			memcpy(paclnode->addr, addr, ETH_ALEN);
+
+			paclnode->valid = true;
+
+			list_add_tail(&paclnode->list, get_list_head(pacl_node_q));
+
+			pacl_list->num++;
+
+			break;
+		}
+	}
+
+	DBG_871X("%s, acl_num =%d\n", __func__, pacl_list->num);
+
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+	return ret;
+}
+
+int rtw_acl_remove_sta(struct adapter *padapter, u8 *addr)
+{
+	struct list_head	*plist, *phead;
+	int ret = 0;
+	struct rtw_wlan_acl_node *paclnode;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+	struct __queue	*pacl_node_q = &pacl_list->acl_node_q;
+	u8 baddr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };	/* Baddr is used for clearing acl_list */
+
+	DBG_871X(
+		"%s(acl_num =%d) =" MAC_FMT "\n",
+		__func__,
+		pacl_list->num,
+		MAC_ARG(addr)
+	);
+
+	spin_lock_bh(&(pacl_node_q->lock));
+
+	phead = get_list_head(pacl_node_q);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+
+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);
+		plist = get_next(plist);
+
+		if (
+			!memcmp(paclnode->addr, addr, ETH_ALEN) ||
+			!memcmp(baddr, addr, ETH_ALEN)
+		) {
+
+			if (paclnode->valid == true) {
+
+				paclnode->valid = false;
+
+				list_del_init(&paclnode->list);
+
+				pacl_list->num--;
+			}
+		}
+	}
+
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+	DBG_871X("%s, acl_num =%d\n", __func__, pacl_list->num);
+
+	return ret;
+
+}
+
+u8 rtw_ap_set_pairwise_key(struct 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) {
+		kfree((u8 *) ph2c);
+		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(
+	struct 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;
+
+	/* DBG_871X("%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) {
+		kfree((unsigned char *)pcmd);
+		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(struct adapter *padapter, u8 *key, u8 alg, int keyid)
+{
+	DBG_871X("%s\n", __func__);
+
+	return rtw_ap_set_key(padapter, key, alg, keyid, 1);
+}
+
+int rtw_ap_set_wep_key(
+	struct 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_;
+	}
+
+	DBG_871X("%s\n", __func__);
+
+	return rtw_ap_set_key(padapter, key, alg, keyid, set_tx);
+}
+
+static void update_bcn_fixed_ie(struct adapter *padapter)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+static void update_bcn_erpinfo_ie(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+	unsigned char *p, *ie = pnetwork->IEs;
+	u32 len = 0;
+
+	DBG_871X("%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) {
+
+		struct ndis_80211_var_ie *pIE = (struct ndis_80211_var_ie *)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(struct adapter *padapter)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+static void update_bcn_htinfo_ie(struct adapter *padapter)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+static void update_bcn_rsn_ie(struct adapter *padapter)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+static void update_bcn_wpa_ie(struct adapter *padapter)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+static void update_bcn_wmm_ie(struct adapter *padapter)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+static void update_bcn_wps_ie(struct adapter *padapter)
+{
+	u8 *pwps_ie = NULL;
+	u8 *pwps_ie_src;
+	u8 *premainder_ie;
+	u8 *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);
+	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+	unsigned char *ie = pnetwork->IEs;
+	u32 ielen = pnetwork->IELength;
+
+
+	DBG_871X("%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)
+		kfree(pbackup_remainder_ie);
+
+	/*  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);
+			DBG_871X("%s, set WIFI_UNDER_WPS\n", __func__);
+		}
+	}
+#endif
+}
+
+static void update_bcn_p2p_ie(struct adapter *padapter)
+{
+
+}
+
+static void update_bcn_vendor_spec_ie(struct adapter *padapter, u8 *oui)
+{
+	DBG_871X("%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
+		DBG_871X("unknown OUI type!\n");
+
+
+
+}
+
+void update_beacon(struct adapter *padapter, u8 ie_id, u8 *oui, u8 tx)
+{
+	struct mlme_priv *pmlmepriv;
+	struct mlme_ext_priv *pmlmeext;
+	/* struct mlme_ext_info *pmlmeinfo; */
+
+	/* DBG_871X("%s\n", __func__); */
+
+	if (!padapter)
+		return;
+
+	pmlmepriv = &(padapter->mlmepriv);
+	pmlmeext = &(padapter->mlmeextpriv);
+	/* pmlmeinfo = &(pmlmeext->mlmext_info); */
+
+	if (false == pmlmeext->bstart_bss)
+		return;
+
+	spin_lock_bh(&pmlmepriv->bcn_update_lock);
+
+	switch (ie_id) {
+
+	case 0xFF:
+
+		update_bcn_fixed_ie(padapter);/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
+
+		break;
+
+	case _TIM_IE_:
+
+		update_BCNTIM(padapter);
+
+		break;
+
+	case _ERPINFO_IE_:
+
+		update_bcn_erpinfo_ie(padapter);
+
+		break;
+
+	case _HT_CAPABILITY_IE_:
+
+		update_bcn_htcap_ie(padapter);
+
+		break;
+
+	case _RSN_IE_2_:
+
+		update_bcn_rsn_ie(padapter);
+
+		break;
+
+	case _HT_ADD_INFO_IE_:
+
+		update_bcn_htinfo_ie(padapter);
+
+		break;
+
+	case _VENDOR_SPECIFIC_IE_:
+
+		update_bcn_vendor_spec_ie(padapter, oui);
+
+		break;
+
+	default:
+		break;
+	}
+
+	pmlmepriv->update_bcn = true;
+
+	spin_unlock_bh(&pmlmepriv->bcn_update_lock);
+
+#ifndef CONFIG_INTERRUPT_BASED_TXBCN
+	if (tx) {
+
+		/* send_beacon(padapter);//send_beacon must execute on TSR level */
+		set_tx_beacon_cmd(padapter);
+	}
+#endif /* CONFIG_INTERRUPT_BASED_TXBCN */
+
+}
+
+/*
+op_mode
+Set to 0 (HT pure) under the followign conditions
+	- all STAs in the BSS are 20/40 MHz HT in 20/40 MHz BSS or
+	- all STAs in the BSS are 20 MHz HT in 20 MHz BSS
+Set to 1 (HT non-member protection) if there may be non-HT STAs
+	in both the primary and the secondary channel
+Set to 2 if only HT STAs are associated in BSS,
+	however and at least one 20 MHz HT STA is associated
+Set to 3 (HT mixed mode) when one or more non-HT STAs are associated
+	(currently non-GF HT station is considered as non-HT STA also)
+*/
+static int rtw_ht_operation_update(struct adapter *padapter)
+{
+	u16 cur_op_mode, new_op_mode;
+	int op_mode_changes = 0;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct ht_priv *phtpriv_ap = &pmlmepriv->htpriv;
+
+	if (pmlmepriv->htpriv.ht_option == true)
+		return 0;
+
+	/* if (!iface->conf->ieee80211n || iface->conf->ht_op_mode_fixed) */
+	/*  return 0; */
+
+	DBG_871X("%s current operation mode = 0x%X\n",
+		   __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 || pmlmepriv->olbc_ht)) {
+		pmlmepriv->ht_op_mode |= HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+		op_mode_changes++;
+	} else if ((pmlmepriv->ht_op_mode &
+		    HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&
+		   (pmlmepriv->num_sta_no_ht == 0 && !pmlmepriv->olbc_ht)) {
+		pmlmepriv->ht_op_mode &=
+			~HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+		op_mode_changes++;
+	}
+
+	/* Note: currently we switch to the MIXED op mode if HT non-greenfield
+	 * station is associated. Probably it's a theoretical case, since
+	 * it looks like all known HT STAs support greenfield.
+	 */
+	new_op_mode = 0;
+	if (pmlmepriv->num_sta_no_ht ||
+	    (pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT))
+		new_op_mode = OP_MODE_MIXED;
+	else if (
+		(le16_to_cpu(phtpriv_ap->ht_cap.cap_info) & IEEE80211_HT_CAP_SUP_WIDTH)
+		&& pmlmepriv->num_sta_ht_20mhz)
+		new_op_mode = OP_MODE_20MHZ_HT_STA_ASSOCED;
+	else if (pmlmepriv->olbc_ht)
+		new_op_mode = OP_MODE_MAY_BE_LEGACY_STAS;
+	else
+		new_op_mode = OP_MODE_PURE;
+
+	cur_op_mode = pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+	if (cur_op_mode != new_op_mode) {
+		pmlmepriv->ht_op_mode &= ~HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+		pmlmepriv->ht_op_mode |= new_op_mode;
+		op_mode_changes++;
+	}
+
+	DBG_871X("%s new operation mode = 0x%X changes =%d\n",
+		   __func__, pmlmepriv->ht_op_mode, op_mode_changes);
+
+	return op_mode_changes;
+
+}
+
+void associated_clients_update(struct adapter *padapter, u8 updated)
+{
+	/* update associcated stations cap. */
+	if (updated == true) {
+
+		struct list_head	*phead, *plist;
+		struct sta_info *psta = NULL;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		spin_lock_bh(&pstapriv->asoc_list_lock);
+
+		phead = &pstapriv->asoc_list;
+		plist = get_next(phead);
+
+		/* check asoc_queue */
+		while (phead != plist) {
+
+			psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+
+			plist = get_next(plist);
+
+			VCS_update(padapter, psta);
+		}
+
+		spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	}
+
+}
+
+/* called > TSR LEVEL for USB or SDIO Interface*/
+void bss_cap_update_on_sta_join(struct 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);
+
+		DBG_871X("HT: STA " MAC_FMT " HT Capabilities "
+			   "Info: 0x%04x\n", MAC_ARG(psta->hwaddr), ht_capab);
+
+		if (psta->no_ht_set) {
+			psta->no_ht_set = 0;
+			pmlmepriv->num_sta_no_ht--;
+		}
+
+		if ((ht_capab & IEEE80211_HT_CAP_GRN_FLD) == 0) {
+			if (!psta->no_ht_gf_set) {
+				psta->no_ht_gf_set = 1;
+				pmlmepriv->num_sta_ht_no_gf++;
+			}
+			DBG_871X("%s STA " MAC_FMT " - no "
+				   "greenfield, num of non-gf stations %d\n",
+				   __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++;
+			}
+			DBG_871X("%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 == true) {
+			DBG_871X("%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);
+	}
+
+	/* update associcated stations cap. */
+	associated_clients_update(padapter,  beacon_updated);
+
+	DBG_871X("%s, updated =%d\n", __func__, beacon_updated);
+
+}
+
+u8 bss_cap_update_on_sta_leave(struct 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);
+	}
+
+	/* update associcated stations cap. */
+	/* associated_clients_update(padapter,  beacon_updated); //move it to avoid deadlock */
+
+	DBG_871X("%s, updated =%d\n", __func__, beacon_updated);
+
+	return beacon_updated;
+
+}
+
+u8 ap_free_sta(
+	struct adapter *padapter,
+	struct sta_info *psta,
+	bool active,
+	u16 reason
+)
+{
+	u8 beacon_updated = false;
+
+	if (!psta)
+		return beacon_updated;
+
+	if (active == true) {
+
+		/* 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);
+	}
+
+	psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+	psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+
+
+	/* report_del_sta_event(padapter, psta->hwaddr, reason); */
+
+	/* clear cam entry / key */
+	rtw_clearstakey_cmd(padapter, psta, true);
+
+
+	spin_lock_bh(&psta->lock);
+	psta->state &= ~_FW_LINKED;
+	spin_unlock_bh(&psta->lock);
+
+	rtw_cfg80211_indicate_sta_disassoc(padapter, psta->hwaddr, reason);
+
+	report_del_sta_event(padapter, psta->hwaddr, reason);
+
+	beacon_updated = bss_cap_update_on_sta_leave(padapter, psta);
+
+	rtw_free_stainfo(padapter, psta);
+
+
+	return beacon_updated;
+
+}
+
+int rtw_sta_flush(struct adapter *padapter)
+{
+	struct list_head	*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};
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		return ret;
+
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	phead = &pstapriv->asoc_list;
+	plist = get_next(phead);
+
+	/* free sta asoc_queue */
+	while (phead != plist) {
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+
+		plist = get_next(plist);
+
+		list_del_init(&psta->asoc_list);
+		pstapriv->asoc_list_cnt--;
+
+		/* spin_unlock_bh(&pstapriv->asoc_list_lock); */
+		ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+		/* spin_lock_bh(&pstapriv->asoc_list_lock); */
+	}
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+
+	issue_deauth(padapter, bc_addr, WLAN_REASON_DEAUTH_LEAVING);
+
+	associated_clients_update(padapter, true);
+
+	return ret;
+
+}
+
+/* called > TSR LEVEL for USB or SDIO Interface*/
+void sta_info_update(struct 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;
+	} 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(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (psta->state & _FW_LINKED) {
+
+		pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+		/* add ratid */
+		add_RATid(padapter, psta, 0);/* DM_RATR_STA_INIT */
+	}
+}
+/* restore hw setting from sw data structures */
+void rtw_ap_restore_network(struct adapter *padapter)
+{
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta;
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	struct list_head	*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
+	);
+
+	start_bss_network(padapter, (u8 *)&mlmepriv->cur_network.network);
+
+	if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
+		(padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+
+		/* restore group key, WEP keys is restored in ips_leave() */
+		rtw_set_key(
+			padapter,
+			psecuritypriv,
+			psecuritypriv->dot118021XGrpKeyid,
+			0,
+			false
+		);
+	}
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+
+	phead = &pstapriv->asoc_list;
+	plist = get_next(phead);
+
+	while (phead != plist) {
+		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;
+
+	}
+
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	for (i = 0; i < chk_alive_num; i++) {
+		psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);
+
+		if (psta == NULL) {
+			DBG_871X(FUNC_ADPT_FMT" sta_info is null\n", FUNC_ADPT_ARG(padapter));
+		} else if (psta->state & _FW_LINKED) {
+			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, true, false);
+			}
+		}
+	}
+
+}
+
+void start_ap_mode(struct adapter *padapter)
+{
+	int i;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+
+	pmlmepriv->update_bcn = false;
+
+	/* init_mlme_ap_info(padapter); */
+	pmlmeext->bstart_bss = false;
+
+	pmlmepriv->num_sta_non_erp = 0;
+
+	pmlmepriv->num_sta_no_short_slot_time = 0;
+
+	pmlmepriv->num_sta_no_short_preamble = 0;
+
+	pmlmepriv->num_sta_ht_no_gf = 0;
+	pmlmepriv->num_sta_no_ht = 0;
+	pmlmepriv->num_sta_ht_20mhz = 0;
+
+	pmlmepriv->olbc = false;
+
+	pmlmepriv->olbc_ht = false;
+
+	pmlmepriv->ht_op_mode = 0;
+
+	for (i = 0; i < NUM_STA; i++)
+		pstapriv->sta_aid[i] = NULL;
+
+	pmlmepriv->wps_beacon_ie = NULL;
+	pmlmepriv->wps_probe_resp_ie = NULL;
+	pmlmepriv->wps_assoc_resp_ie = NULL;
+
+	pmlmepriv->p2p_beacon_ie = NULL;
+	pmlmepriv->p2p_probe_resp_ie = NULL;
+
+
+	/* for ACL */
+	INIT_LIST_HEAD(&(pacl_list->acl_node_q.queue));
+	pacl_list->num = 0;
+	pacl_list->mode = 0;
+	for (i = 0; i < NUM_ACL; i++) {
+		INIT_LIST_HEAD(&pacl_list->aclnode[i].list);
+		pacl_list->aclnode[i].valid = false;
+	}
+
+}
+
+void stop_ap_mode(struct adapter *padapter)
+{
+	struct list_head	*phead, *plist;
+	struct rtw_wlan_acl_node *paclnode;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+	struct __queue	*pacl_node_q = &pacl_list->acl_node_q;
+
+	pmlmepriv->update_bcn = false;
+	pmlmeext->bstart_bss = 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;
+
+	/* for ACL */
+	spin_lock_bh(&(pacl_node_q->lock));
+	phead = get_list_head(pacl_node_q);
+	plist = get_next(phead);
+	while (phead != plist) {
+
+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);
+		plist = get_next(plist);
+
+		if (paclnode->valid == true) {
+
+			paclnode->valid = false;
+
+			list_del_init(&paclnode->list);
+
+			pacl_list->num--;
+		}
+	}
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+	DBG_871X("%s, free acl_node_queue, num =%d\n", __func__, pacl_list->num);
+
+	rtw_sta_flush(padapter);
+
+	/* free_assoc_sta_resources */
+	rtw_free_all_stainfo(padapter);
+
+	psta = rtw_get_bcmc_stainfo(padapter);
+	rtw_free_stainfo(padapter, psta);
+
+	rtw_init_bcmc_stainfo(padapter);
+
+	rtw_free_mlme_priv_ie_data(pmlmepriv);
+
+	rtw_btcoex_MediaStatusNotify(padapter, 0); /* disconnect */
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_btcoex.c linux/3rdparty/rtl8723bs/core/rtw_btcoex.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_btcoex.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_btcoex.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,243 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 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.
+ *
+ ******************************************************************************/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtw_btcoex.h>
+#include <hal_btcoex.h>
+
+
+void rtw_btcoex_Initialize(struct adapter *padapter)
+{
+	hal_btcoex_Initialize(padapter);
+}
+
+void rtw_btcoex_PowerOnSetting(struct adapter *padapter)
+{
+	hal_btcoex_PowerOnSetting(padapter);
+}
+
+void rtw_btcoex_HAL_Initialize(struct adapter *padapter, u8 bWifiOnly)
+{
+	hal_btcoex_InitHwConfig(padapter, bWifiOnly);
+}
+
+void rtw_btcoex_IpsNotify(struct adapter *padapter, u8 type)
+{
+	hal_btcoex_IpsNotify(padapter, type);
+}
+
+void rtw_btcoex_LpsNotify(struct adapter *padapter, u8 type)
+{
+	hal_btcoex_LpsNotify(padapter, type);
+}
+
+void rtw_btcoex_ScanNotify(struct adapter *padapter, u8 type)
+{
+	hal_btcoex_ScanNotify(padapter, type);
+}
+
+void rtw_btcoex_ConnectNotify(struct adapter *padapter, u8 action)
+{
+	hal_btcoex_ConnectNotify(padapter, action);
+}
+
+void rtw_btcoex_MediaStatusNotify(struct adapter *padapter, u8 mediaStatus)
+{
+	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(struct adapter *padapter, u8 pktType)
+{
+	hal_btcoex_SpecialPacketNotify(padapter, pktType);
+}
+
+void rtw_btcoex_IQKNotify(struct adapter *padapter, u8 state)
+{
+	hal_btcoex_IQKNotify(padapter, state);
+}
+
+void rtw_btcoex_BtInfoNotify(struct adapter *padapter, u8 length, u8 *tmpBuf)
+{
+	hal_btcoex_BtInfoNotify(padapter, length, tmpBuf);
+}
+
+void rtw_btcoex_SuspendNotify(struct adapter *padapter, u8 state)
+{
+	hal_btcoex_SuspendNotify(padapter, state);
+}
+
+void rtw_btcoex_HaltNotify(struct adapter *padapter)
+{
+	if (false == padapter->bup) {
+		DBG_871X(FUNC_ADPT_FMT ": bup =%d Skip!\n",
+			FUNC_ADPT_ARG(padapter), padapter->bup);
+
+		return;
+	}
+
+	if (true == padapter->bSurpriseRemoved) {
+		DBG_871X(FUNC_ADPT_FMT ": bSurpriseRemoved =%d Skip!\n",
+			FUNC_ADPT_ARG(padapter), padapter->bSurpriseRemoved);
+
+		return;
+	}
+
+	hal_btcoex_HaltNotify(padapter);
+}
+
+u8 rtw_btcoex_IsBtDisabled(struct adapter *padapter)
+{
+	return hal_btcoex_IsBtDisabled(padapter);
+}
+
+void rtw_btcoex_Handler(struct adapter *padapter)
+{
+	hal_btcoex_Hanlder(padapter);
+}
+
+s32 rtw_btcoex_IsBTCoexCtrlAMPDUSize(struct adapter *padapter)
+{
+	s32 coexctrl;
+
+	coexctrl = hal_btcoex_IsBTCoexCtrlAMPDUSize(padapter);
+
+	return coexctrl;
+}
+
+void rtw_btcoex_SetManualControl(struct adapter *padapter, u8 manual)
+{
+	if (true == manual) {
+		hal_btcoex_SetManualControl(padapter, true);
+	} else{
+		hal_btcoex_SetManualControl(padapter, false);
+	}
+}
+
+u8 rtw_btcoex_IsBtControlLps(struct adapter *padapter)
+{
+	return hal_btcoex_IsBtControlLps(padapter);
+}
+
+u8 rtw_btcoex_IsLpsOn(struct adapter *padapter)
+{
+	return hal_btcoex_IsLpsOn(padapter);
+}
+
+u8 rtw_btcoex_RpwmVal(struct adapter *padapter)
+{
+	return hal_btcoex_RpwmVal(padapter);
+}
+
+u8 rtw_btcoex_LpsVal(struct adapter *padapter)
+{
+	return hal_btcoex_LpsVal(padapter);
+}
+
+void rtw_btcoex_SetBTCoexist(struct adapter *padapter, u8 bBtExist)
+{
+	hal_btcoex_SetBTCoexist(padapter, bBtExist);
+}
+
+void rtw_btcoex_SetChipType(struct adapter *padapter, u8 chipType)
+{
+	hal_btcoex_SetChipType(padapter, chipType);
+}
+
+void rtw_btcoex_SetPGAntNum(struct adapter *padapter, u8 antNum)
+{
+	hal_btcoex_SetPgAntNum(padapter, antNum);
+}
+
+void rtw_btcoex_SetSingleAntPath(struct adapter *padapter, u8 singleAntPath)
+{
+	hal_btcoex_SetSingleAntPath(padapter, singleAntPath);
+}
+
+u32 rtw_btcoex_GetRaMask(struct adapter *padapter)
+{
+	return hal_btcoex_GetRaMask(padapter);
+}
+
+void rtw_btcoex_RecordPwrMode(struct adapter *padapter, u8 *pCmdBuf, u8 cmdLen)
+{
+	hal_btcoex_RecordPwrMode(padapter, pCmdBuf, cmdLen);
+}
+
+void rtw_btcoex_DisplayBtCoexInfo(struct adapter *padapter, u8 *pbuf, u32 bufsize)
+{
+	hal_btcoex_DisplayBtCoexInfo(padapter, pbuf, bufsize);
+}
+
+void rtw_btcoex_SetDBG(struct adapter *padapter, u32 *pDbgModule)
+{
+	hal_btcoex_SetDBG(padapter, pDbgModule);
+}
+
+u32 rtw_btcoex_GetDBG(struct adapter *padapter, u8 *pStrBuf, u32 bufSize)
+{
+	return hal_btcoex_GetDBG(padapter, pStrBuf, bufSize);
+}
+
+/*  ================================================== */
+/*  Below Functions are called by BT-Coex */
+/*  ================================================== */
+void rtw_btcoex_RejectApAggregatedPacket(struct adapter *padapter, u8 enable)
+{
+	struct mlme_ext_info *pmlmeinfo;
+	struct sta_info *psta;
+
+	pmlmeinfo = &padapter->mlmeextpriv.mlmext_info;
+	psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(&padapter->mlmepriv));
+
+	if (true == enable) {
+		pmlmeinfo->bAcceptAddbaReq = false;
+		if (psta)
+			send_delba(padapter, 0, psta->hwaddr);
+	} else{
+		pmlmeinfo->bAcceptAddbaReq = true;
+	}
+}
+
+void rtw_btcoex_LPS_Enter(struct adapter *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(struct adapter *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;
+	}
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_cmd.c linux/3rdparty/rtl8723bs/core/rtw_cmd.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_cmd.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_cmd.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,2244 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#define _RTW_CMD_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <linux/jiffies.h>
+
+static struct _cmd_callback rtw_cmd_callback[] = {
+	{GEN_CMD_CODE(_Read_MACREG), NULL}, /*0*/
+	{GEN_CMD_CODE(_Write_MACREG), NULL},
+	{GEN_CMD_CODE(_Read_BBREG), &rtw_getbbrfreg_cmdrsp_callback},
+	{GEN_CMD_CODE(_Write_BBREG), NULL},
+	{GEN_CMD_CODE(_Read_RFREG), &rtw_getbbrfreg_cmdrsp_callback},
+	{GEN_CMD_CODE(_Write_RFREG), NULL}, /*5*/
+	{GEN_CMD_CODE(_Read_EEPROM), NULL},
+	{GEN_CMD_CODE(_Write_EEPROM), NULL},
+	{GEN_CMD_CODE(_Read_EFUSE), NULL},
+	{GEN_CMD_CODE(_Write_EFUSE), NULL},
+
+	{GEN_CMD_CODE(_Read_CAM),	NULL},	/*10*/
+	{GEN_CMD_CODE(_Write_CAM),	 NULL},
+	{GEN_CMD_CODE(_setBCNITV), NULL},
+	{GEN_CMD_CODE(_setMBIDCFG), NULL},
+	{GEN_CMD_CODE(_JoinBss), &rtw_joinbss_cmd_callback},  /*14*/
+	{GEN_CMD_CODE(_DisConnect), &rtw_disassoc_cmd_callback}, /*15*/
+	{GEN_CMD_CODE(_CreateBss), &rtw_createbss_cmd_callback},
+	{GEN_CMD_CODE(_SetOpMode), NULL},
+	{GEN_CMD_CODE(_SiteSurvey), &rtw_survey_cmd_callback}, /*18*/
+	{GEN_CMD_CODE(_SetAuth), NULL},
+
+	{GEN_CMD_CODE(_SetKey), NULL},	/*20*/
+	{GEN_CMD_CODE(_SetStaKey), &rtw_setstaKey_cmdrsp_callback},
+	{GEN_CMD_CODE(_SetAssocSta), &rtw_setassocsta_cmdrsp_callback},
+	{GEN_CMD_CODE(_DelAssocSta), NULL},
+	{GEN_CMD_CODE(_SetStaPwrState), NULL},
+	{GEN_CMD_CODE(_SetBasicRate), NULL}, /*25*/
+	{GEN_CMD_CODE(_GetBasicRate), NULL},
+	{GEN_CMD_CODE(_SetDataRate), NULL},
+	{GEN_CMD_CODE(_GetDataRate), NULL},
+	{GEN_CMD_CODE(_SetPhyInfo), NULL},
+
+	{GEN_CMD_CODE(_GetPhyInfo), NULL}, /*30*/
+	{GEN_CMD_CODE(_SetPhy), NULL},
+	{GEN_CMD_CODE(_GetPhy), NULL},
+	{GEN_CMD_CODE(_readRssi), NULL},
+	{GEN_CMD_CODE(_readGain), NULL},
+	{GEN_CMD_CODE(_SetAtim), NULL}, /*35*/
+	{GEN_CMD_CODE(_SetPwrMode), NULL},
+	{GEN_CMD_CODE(_JoinbssRpt), NULL},
+	{GEN_CMD_CODE(_SetRaTable), NULL},
+	{GEN_CMD_CODE(_GetRaTable), NULL},
+
+	{GEN_CMD_CODE(_GetCCXReport), NULL}, /*40*/
+	{GEN_CMD_CODE(_GetDTMReport),	NULL},
+	{GEN_CMD_CODE(_GetTXRateStatistics), NULL},
+	{GEN_CMD_CODE(_SetUsbSuspend), NULL},
+	{GEN_CMD_CODE(_SetH2cLbk), NULL},
+	{GEN_CMD_CODE(_AddBAReq), NULL}, /*45*/
+	{GEN_CMD_CODE(_SetChannel), NULL},		/*46*/
+	{GEN_CMD_CODE(_SetTxPower), NULL},
+	{GEN_CMD_CODE(_SwitchAntenna), NULL},
+	{GEN_CMD_CODE(_SetCrystalCap), NULL},
+	{GEN_CMD_CODE(_SetSingleCarrierTx), NULL},	/*50*/
+
+	{GEN_CMD_CODE(_SetSingleToneTx), NULL}, /*51*/
+	{GEN_CMD_CODE(_SetCarrierSuppressionTx), NULL},
+	{GEN_CMD_CODE(_SetContinuousTx), NULL},
+	{GEN_CMD_CODE(_SwitchBandwidth), NULL},		/*54*/
+	{GEN_CMD_CODE(_TX_Beacon), NULL},/*55*/
+
+	{GEN_CMD_CODE(_Set_MLME_EVT), NULL},/*56*/
+	{GEN_CMD_CODE(_Set_Drv_Extra), NULL},/*57*/
+	{GEN_CMD_CODE(_Set_H2C_MSG), NULL},/*58*/
+	{GEN_CMD_CODE(_SetChannelPlan), NULL},/*59*/
+	{GEN_CMD_CODE(_LedBlink), NULL},/*60*/
+
+	{GEN_CMD_CODE(_SetChannelSwitch), NULL},/*61*/
+	{GEN_CMD_CODE(_TDLS), NULL},/*62*/
+	{GEN_CMD_CODE(_ChkBMCSleepq), NULL}, /*63*/
+
+	{GEN_CMD_CODE(_RunInThreadCMD), NULL},/*64*/
+};
+
+static struct cmd_hdl wlancmds[] = {
+	GEN_DRV_CMD_HANDLER(0, NULL) /*0*/
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_DRV_CMD_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL) /*10*/
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct joinbss_parm), join_cmd_hdl) /*14*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct disconnect_parm), disconnect_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct createbss_parm), createbss_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setopmode_parm), setopmode_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct sitesurvey_parm), sitesurvey_cmd_hdl) /*18*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct setauth_parm), setauth_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setkey_parm), setkey_hdl) /*20*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct set_stakey_parm), set_stakey_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct set_assocsta_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct del_assocsta_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setstapwrstate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setbasicrate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getbasicrate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setdatarate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getdatarate_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct setphyinfo_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getphyinfo_parm), NULL)  /*30*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct setphy_parm), NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct getphy_parm), NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)	/*40*/
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct addBaReq_parm), add_ba_hdl)
+	GEN_MLME_EXT_HANDLER(sizeof(struct set_ch_parm), set_ch_hdl) /* 46 */
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL) /*50*/
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(0, NULL)
+	GEN_MLME_EXT_HANDLER(sizeof(struct Tx_Beacon_param), tx_beacon_hdl) /*55*/
+
+	GEN_MLME_EXT_HANDLER(0, mlme_evt_hdl) /*56*/
+	GEN_MLME_EXT_HANDLER(0, rtw_drvextra_cmd_hdl) /*57*/
+
+	GEN_MLME_EXT_HANDLER(0, h2c_msg_hdl) /*58*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelPlan_param), set_chplan_hdl) /*59*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct LedBlink_param), led_blink_hdl) /*60*/
+
+	GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelSwitch_param), set_csa_hdl) /*61*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct TDLSoption_param), tdls_hdl) /*62*/
+	GEN_MLME_EXT_HANDLER(0, chk_bmc_sleepq_hdl) /*63*/
+	GEN_MLME_EXT_HANDLER(sizeof(struct RunInThread_param), run_in_thread_hdl) /*63*/
+};
+
+/*
+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->cmd_done_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 == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf  +  CMDBUFF_ALIGN_SZ - ((SIZE_PTR)(pcmdpriv->cmd_allocated_buf) & (CMDBUFF_ALIGN_SZ-1));
+
+	pcmdpriv->rsp_allocated_buf = rtw_zmalloc(MAX_RSPSZ + 4);
+
+	if (pcmdpriv->rsp_allocated_buf == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf  +  4 - ((SIZE_PTR)(pcmdpriv->rsp_allocated_buf) & 3);
+
+	pcmdpriv->cmd_issued_cnt = pcmdpriv->cmd_done_cnt = pcmdpriv->rsp_cnt = 0;
+
+	mutex_init(&pcmdpriv->sctx_mutex);
+exit:
+	return res;
+}
+
+static void c2h_wk_callback(_workitem *work);
+sint _rtw_init_evt_priv(struct evt_priv *pevtpriv)
+{
+	sint res = _SUCCESS;
+
+	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
+	atomic_set(&pevtpriv->event_seq, 0);
+	pevtpriv->evt_done_cnt = 0;
+
+	_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);
+
+	return res;
+}
+
+void _rtw_free_evt_priv(struct	evt_priv *pevtpriv)
+{
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+_rtw_free_evt_priv\n"));
+
+	_cancel_workitem_sync(&pevtpriv->c2h_wk);
+	while (pevtpriv->c2h_wk_alive)
+		msleep(10);
+
+	while (!rtw_cbuf_empty(pevtpriv->c2h_queue)) {
+		void *c2h = rtw_cbuf_pop(pevtpriv->c2h_queue);
+		if (c2h != NULL && c2h != (void *)pevtpriv) {
+			kfree(c2h);
+		}
+	}
+	kfree(pevtpriv->c2h_queue);
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("-_rtw_free_evt_priv\n"));
+}
+
+void _rtw_free_cmd_priv(struct	cmd_priv *pcmdpriv)
+{
+	if (pcmdpriv) {
+		if (pcmdpriv->cmd_allocated_buf)
+			kfree(pcmdpriv->cmd_allocated_buf);
+
+		if (pcmdpriv->rsp_allocated_buf)
+			kfree(pcmdpriv->rsp_allocated_buf);
+
+		mutex_destroy(&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.
+
+*/
+
+sint	_rtw_enqueue_cmd(struct __queue *queue, struct cmd_obj *obj)
+{
+	_irqL irqL;
+
+	if (obj == NULL)
+		goto exit;
+
+	/* spin_lock_bh(&queue->lock); */
+	spin_lock_irqsave(&queue->lock, irqL);
+
+	list_add_tail(&obj->list, &queue->queue);
+
+	/* spin_unlock_bh(&queue->lock); */
+	spin_unlock_irqrestore(&queue->lock, irqL);
+
+exit:
+	return _SUCCESS;
+}
+
+struct	cmd_obj	*_rtw_dequeue_cmd(struct __queue *queue)
+{
+	_irqL irqL;
+	struct cmd_obj *obj;
+
+	/* spin_lock_bh(&(queue->lock)); */
+	spin_lock_irqsave(&queue->lock, irqL);
+	if (list_empty(&(queue->queue)))
+		obj = NULL;
+	else{
+		obj = LIST_CONTAINOR(get_next(&(queue->queue)), struct cmd_obj, list);
+		list_del_init(&obj->list);
+	}
+
+	/* spin_unlock_bh(&(queue->lock)); */
+	spin_unlock_irqrestore(&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)
+{
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("rtw_free_evt_priv\n"));
+	_rtw_free_evt_priv(pevtpriv);
+}
+
+void rtw_free_cmd_priv(struct	cmd_priv *pcmdpriv)
+{
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("rtw_free_cmd_priv\n"));
+	_rtw_free_cmd_priv(pcmdpriv);
+}
+
+int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj);
+int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+	u8 bAllow = false; /* set to true to allow enqueuing cmd when hw_init_completed is false */
+
+	if (cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan))
+		bAllow = true;
+
+	if ((pcmdpriv->padapter->hw_init_completed == false && bAllow == false)
+		|| atomic_read(&(pcmdpriv->cmdthd_running)) == false	/* com_thread not running */
+	) {
+		/* DBG_871X("%s:%s: drop cmdcode:%u, hw_init_completed:%u, cmdthd_running:%u\n", caller_func, __func__, */
+		/* 	cmd_obj->cmdcode, */
+		/* 	pcmdpriv->padapter->hw_init_completed, */
+		/* 	pcmdpriv->cmdthd_running */
+		/*  */
+
+		return _FAIL;
+	}
+	return _SUCCESS;
+}
+
+
+
+u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+{
+	int res = _FAIL;
+	struct adapter *padapter = pcmdpriv->padapter;
+
+	if (cmd_obj == NULL) {
+		goto exit;
+	}
+
+	cmd_obj->padapter = padapter;
+
+	res = rtw_cmd_filter(pcmdpriv, cmd_obj);
+	if (_FAIL == res) {
+		rtw_free_cmd_obj(cmd_obj);
+		goto exit;
+	}
+
+	res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj);
+
+	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_free_cmd_obj(struct cmd_obj *pcmd)
+{
+	if ((pcmd->cmdcode != _JoinBss_CMD_) &&
+	    (pcmd->cmdcode != _CreateBss_CMD_)) {
+		/* free parmbuf in cmd_obj */
+		kfree((unsigned char *)pcmd->parmbuf);
+	}
+
+	if (pcmd->rsp != NULL) {
+		if (pcmd->rspsz != 0) {
+			/* free rsp in cmd_obj */
+			kfree((unsigned char *)pcmd->rsp);
+		}
+	}
+
+	/* free cmd_obj */
+	kfree((unsigned char *)pcmd);
+}
+
+
+void rtw_stop_cmd_thread(struct 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);
+		down(&adapter->cmdpriv.terminate_cmdthread_sema);
+	}
+}
+
+int rtw_cmd_thread(void *context)
+{
+	u8 ret;
+	struct cmd_obj *pcmd;
+	u8 *pcmdbuf, *prspbuf;
+	unsigned long cmd_start_time;
+	unsigned long cmd_process_time;
+	u8 (*cmd_hdl)(struct adapter *padapter, u8 *pbuf);
+	void (*pcmd_callback)(struct adapter *dev, struct cmd_obj *pcmd);
+	struct adapter *padapter = (struct adapter *)context;
+	struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
+	struct drvextra_cmd_parm *extra_parm = NULL;
+
+	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);
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("start r871x rtw_cmd_thread !!!!\n"));
+
+	while (1) {
+		if (down_interruptible(&pcmdpriv->cmd_queue_sema)) {
+			DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" down_interruptible(&pcmdpriv->cmd_queue_sema) return != 0, break\n", FUNC_ADPT_ARG(padapter));
+			break;
+		}
+
+		if ((padapter->bDriverStopped == true) || (padapter->bSurpriseRemoved == true)) {
+			DBG_871X_LEVEL(_drv_always_, "%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
+				__func__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
+			break;
+		}
+
+		if (pcmdpriv->stop_req) {
+			DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" stop_req:%u, break\n", FUNC_ADPT_ARG(padapter), pcmdpriv->stop_req);
+			break;
+		}
+
+		if (list_empty(&(pcmdpriv->cmd_queue.queue))) {
+			/* DBG_871X("%s: cmd queue is empty!\n", __func__); */
+			continue;
+		}
+
+		if (rtw_register_cmd_alive(padapter) != _SUCCESS) {
+			RT_TRACE(_module_hal_xmit_c_, _drv_notice_,
+					 ("%s: wait to leave LPS_LCLK\n", __func__));
+			continue;
+		}
+
+_next:
+		if ((padapter->bDriverStopped == true) || (padapter->bSurpriseRemoved == true)) {
+			DBG_871X_LEVEL(_drv_always_, "%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
+				__func__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
+			break;
+		}
+
+		pcmd = rtw_dequeue_cmd(pcmdpriv);
+		if (!pcmd) {
+			rtw_unregister_cmd_alive(padapter);
+			continue;
+		}
+
+		cmd_start_time = jiffies;
+
+		if (_FAIL == rtw_cmd_filter(pcmdpriv, pcmd)) {
+			pcmd->res = H2C_DROPPED;
+			goto post_process;
+		}
+
+		pcmdpriv->cmd_issued_cnt++;
+
+		pcmd->cmdsz = _RND4((pcmd->cmdsz));/* _RND4 */
+
+		memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
+
+		if (pcmd->cmdcode < ARRAY_SIZE(wlancmds)) {
+			cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
+
+			if (cmd_hdl) {
+				ret = cmd_hdl(pcmd->padapter, pcmdbuf);
+				pcmd->res = ret;
+			}
+
+			pcmdpriv->cmd_seq++;
+		} else{
+			pcmd->res = H2C_PARAMETERS_ERROR;
+		}
+
+		cmd_hdl = NULL;
+
+post_process:
+
+		if (mutex_lock_interruptible(&(pcmd->padapter->cmdpriv.sctx_mutex)) == 0) {
+			if (pcmd->sctx) {
+				DBG_871X_LEVEL(_drv_always_, 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);
+			}
+			mutex_unlock(&(pcmd->padapter->cmdpriv.sctx_mutex));
+		}
+
+		cmd_process_time = jiffies_to_msecs(jiffies - cmd_start_time);
+		if (cmd_process_time > 1000) {
+			if (pcmd->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
+				DBG_871X(ADPT_FMT" cmd =%d process_time =%lu > 1 sec\n",
+					ADPT_ARG(pcmd->padapter), pcmd->cmdcode, cmd_process_time);
+				/* rtw_warn_on(1); */
+			} else if (pcmd->cmdcode == GEN_CMD_CODE(_Set_MLME_EVT)) {
+				DBG_871X(ADPT_FMT" cmd =%d, process_time =%lu > 1 sec\n",
+					ADPT_ARG(pcmd->padapter), pcmd->cmdcode, cmd_process_time);
+				/* rtw_warn_on(1); */
+			} else {
+				DBG_871X(ADPT_FMT" cmd =%d, process_time =%lu > 1 sec\n",
+					ADPT_ARG(pcmd->padapter), pcmd->cmdcode, cmd_process_time);
+				/* rtw_warn_on(1); */
+			}
+		}
+
+		/* call callback function for post-processed */
+		if (pcmd->cmdcode < ARRAY_SIZE(rtw_cmd_callback)) {
+			pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback;
+			if (pcmd_callback == NULL) {
+				RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("mlme_cmd_hdl(): pcmd_callback = 0x%p, cmdcode = 0x%x\n", pcmd_callback, pcmd->cmdcode));
+				rtw_free_cmd_obj(pcmd);
+			} else{
+				/* todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!= NULL) */
+				pcmd_callback(pcmd->padapter, pcmd);/* need conider that free cmd_obj in rtw_cmd_callback */
+			}
+		} else{
+			RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("%s: cmdcode = 0x%x callback not defined!\n", __func__, pcmd->cmdcode));
+			rtw_free_cmd_obj(pcmd);
+		}
+
+		flush_signals_thread();
+
+		goto _next;
+
+	}
+
+	/*  free all cmd_obj resources */
+	do {
+		pcmd = rtw_dequeue_cmd(pcmdpriv);
+		if (pcmd == NULL) {
+			rtw_unregister_cmd_alive(padapter);
+			break;
+		}
+
+		/* DBG_871X("%s: leaving... drop cmdcode:%u size:%d\n", __func__, pcmd->cmdcode, pcmd->cmdsz); */
+
+		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) {
+				kfree(extra_parm->pbuf);
+			}
+		}
+
+		rtw_free_cmd_obj(pcmd);
+	} while (1);
+
+	up(&pcmdpriv->terminate_cmdthread_sema);
+	atomic_set(&(pcmdpriv->cmdthd_running), false);
+
+	thread_exit();
+}
+
+/*
+rtw_sitesurvey_cmd(~)
+	### NOTE:#### (!!!!)
+	MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
+*/
+u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct 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;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 1);
+	}
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL)
+		return _FAIL;
+
+	psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm));
+	if (psurveyPara == NULL) {
+		kfree((unsigned char *) ph2c);
+		return _FAIL;
+	}
+
+	rtw_free_network_queue(padapter, false);
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("%s: flush network queue\n", __func__));
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+
+	/* psurveyPara->bsslimit = 48; */
+	psurveyPara->scan_mode = pmlmepriv->scan_mode;
+
+	/* prepare ssid list */
+	if (ssid) {
+		int i;
+		for (i = 0; i < ssid_num && i < RTW_SSID_SCAN_AMOUNT; i++) {
+			if (ssid[i].SsidLength) {
+				memcpy(&psurveyPara->ssid[i], &ssid[i], sizeof(struct ndis_802_11_ssid));
+				psurveyPara->ssid_num++;
+
+				DBG_871X(FUNC_ADPT_FMT" ssid:(%s, %d)\n", FUNC_ADPT_ARG(padapter),
+					psurveyPara->ssid[i].Ssid, psurveyPara->ssid[i].SsidLength);
+			}
+		}
+	}
+
+	/* prepare channel list */
+	if (ch) {
+		int i;
+		for (i = 0; i < ch_num && i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
+			if (ch[i].hw_value && !(ch[i].flags & RTW_IEEE80211_CHAN_DISABLED)) {
+				memcpy(&psurveyPara->ch[i], &ch[i], sizeof(struct rtw_ieee80211_channel));
+				psurveyPara->ch_num++;
+
+				DBG_871X(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter),
+					psurveyPara->ch[i].hw_value);
+			}
+		}
+	}
+
+	set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+	if (res == _SUCCESS) {
+
+		pmlmepriv->scan_start_time = jiffies;
+		_set_timer(&pmlmepriv->scan_to_timer, SCANNING_TIMEOUT);
+	} else {
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	}
+	return res;
+}
+
+u8 rtw_setdatarate_cmd(struct 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 == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pbsetdataratepara = (struct setdatarate_parm *)rtw_zmalloc(sizeof(struct setdatarate_parm));
+	if (pbsetdataratepara == NULL) {
+		kfree((u8 *) ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pbsetdataratepara, GEN_CMD_CODE(_SetDataRate));
+	pbsetdataratepara->mac_id = 5;
+	memcpy(pbsetdataratepara->datarates, rateset, NumRates);
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+exit:
+	return res;
+}
+
+void rtw_getbbrfreg_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	/* rtw_free_cmd_obj(pcmd); */
+	kfree((unsigned char *) pcmd->parmbuf);
+	kfree((unsigned char *) pcmd);
+}
+
+u8 rtw_createbss_cmd(struct adapter  *padapter)
+{
+	struct cmd_obj *pcmd;
+	struct cmd_priv 			*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_priv 		*pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex		*pdev_network = &padapter->registrypriv.dev_network;
+	u8 res = _SUCCESS;
+
+	if (pmlmepriv->assoc_ssid.SsidLength == 0) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for Any SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
+	} else {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
+	}
+
+	pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	INIT_LIST_HEAD(&pcmd->list);
+	pcmd->cmdcode = _CreateBss_CMD_;
+	pcmd->parmbuf = (unsigned char *)pdev_network;
+	pcmd->cmdsz = get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pdev_network);
+	pcmd->rsp = NULL;
+	pcmd->rspsz = 0;
+
+	pdev_network->Length = pcmd->cmdsz;
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+	return res;
+}
+
+u8 rtw_startbss_cmd(struct adapter  *padapter, int flags)
+{
+	struct cmd_obj *pcmd;
+	struct cmd_priv  *pcmdpriv = &padapter->cmdpriv;
+	struct submit_ctx sctx;
+	u8 res = _SUCCESS;
+
+	if (flags & RTW_CMDF_DIRECTLY) {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		start_bss_network(padapter, (u8 *)&(padapter->mlmepriv.cur_network.network));
+	} else {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (pcmd == NULL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		INIT_LIST_HEAD(&pcmd->list);
+		pcmd->cmdcode = GEN_CMD_CODE(_CreateBss);
+		pcmd->parmbuf = NULL;
+		pcmd->cmdsz =  0;
+		pcmd->rsp = NULL;
+		pcmd->rspsz = 0;
+
+		if (flags & RTW_CMDF_WAIT_ACK) {
+			pcmd->sctx = &sctx;
+			rtw_sctx_init(&sctx, 2000);
+		}
+
+		res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+		if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) {
+			rtw_sctx_wait(&sctx, __func__);
+			if (mutex_lock_interruptible(&pcmdpriv->sctx_mutex) == 0) {
+				if (sctx.status == RTW_SCTX_SUBMITTED)
+					pcmd->sctx = NULL;
+				mutex_unlock(&pcmdpriv->sctx_mutex);
+			}
+		}
+	}
+
+exit:
+	return res;
+}
+
+u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
+{
+	u8 *auth, res = _SUCCESS;
+	uint	t_len = 0;
+	struct 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;
+	enum 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;
+
+	if (pmlmepriv->assoc_ssid.SsidLength == 0) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+Join cmd: Any SSid\n"));
+	} else {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+Join cmd: SSid =[%s]\n", pmlmepriv->assoc_ssid.Ssid));
+	}
+
+	pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd == NULL) {
+		res = _FAIL;
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd: memory allocate for cmd_obj fail!!!\n"));
+		goto exit;
+	}
+	/* for IEs is fix buf size */
+	t_len = sizeof(struct wlan_bssid_ex);
+
+
+	/* for hidden ap to set fw_state here */
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) != true) {
+		switch (ndis_network_mode) {
+		case Ndis802_11IBSS:
+			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			break;
+
+		case Ndis802_11Infrastructure:
+			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+			break;
+
+		case Ndis802_11APMode:
+		case Ndis802_11AutoUnknown:
+		case Ndis802_11InfrastructureMax:
+			break;
+
+		}
+	}
+
+	psecnetwork = (struct wlan_bssid_ex *)&psecuritypriv->sec_bss;
+	if (psecnetwork == NULL) {
+		if (pcmd != NULL)
+			kfree((unsigned char *)pcmd);
+
+		res = _FAIL;
+
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd :psecnetwork == NULL!!!\n"));
+
+		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;
+	/*  Added by Albert 2009/02/18 */
+	/*  If the the driver wants to use the bssid to create the connection. */
+	/*  If not,  we have to copy the connecting AP's MAC address to it so that */
+	/*  the driver just has the bssid information for PMKIDList searching. */
+
+	if (pmlmepriv->assoc_by_bssid == false) {
+		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) {
+		/* 	Added by Albert 2010/06/23 */
+		/* 	For the WEP mode, we will use the bg mode to do the connection to avoid some IOT issue. */
+		/* 	Especially for Realtek 8192u SoftAP. */
+		if ((padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_) &&
+			(padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_) &&
+			(padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_)) {
+			rtw_ht_use_default_setting(padapter);
+
+			rtw_build_wmm_ie_ht(padapter, &psecnetwork->IEs[12], &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);
+
+	pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.IEs, pnetwork->network.IELength);
+
+	pcmd->cmdsz = get_wlan_bssid_ex_sz(psecnetwork);/* get cmdsz before endian conversion */
+
+	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(struct 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;
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_disassoc_cmd\n"));
+
+	/* prepare cmd parameter */
+	param = (struct disconnect_parm *)rtw_zmalloc(sizeof(*param));
+	if (param == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	param->deauth_timeout_ms = deauth_timeout_ms;
+
+	if (enqueue) {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj));
+		if (cmdobj == NULL) {
+			res = _FAIL;
+			kfree((u8 *)param);
+			goto exit;
+		}
+		init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_);
+		res = rtw_enqueue_cmd(cmdpriv, cmdobj);
+	} else {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (H2C_SUCCESS != disconnect_hdl(padapter, (u8 *)param))
+			res = _FAIL;
+		kfree((u8 *)param);
+	}
+
+exit:
+	return res;
+}
+
+u8 rtw_setopmode_cmd(struct adapter  *padapter, enum 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 == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	psetop->mode = (u8)networktype;
+
+	if (enqueue) {
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL) {
+			kfree((u8 *)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);
+		kfree((u8 *)psetop);
+	}
+exit:
+	return res;
+}
+
+u8 rtw_setstakey_cmd(struct adapter *padapter, struct sta_info *sta, u8 unicast_key, 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 == NULL) {
+		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 (unicast_key == true) {
+		memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
+	} else{
+		memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, 16);
+	}
+
+	/* 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 == NULL) {
+			kfree((u8 *) psetstakey_para);
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp));
+		if (psetstakey_rsp == NULL) {
+			kfree((u8 *) ph2c);
+			kfree((u8 *) psetstakey_para);
+			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);
+		kfree((u8 *) psetstakey_para);
+	}
+exit:
+	return res;
+}
+
+u8 rtw_clearstakey_cmd(struct 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;
+	s16 cam_id = 0;
+	u8 res = _SUCCESS;
+
+	if (!enqueue) {
+		while ((cam_id = rtw_camid_search(padapter, sta->hwaddr, -1)) >= 0) {
+			DBG_871X_LEVEL(_drv_always_, "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 == NULL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm));
+		if (psetstakey_para == NULL) {
+			kfree((u8 *) ph2c);
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp));
+		if (psetstakey_rsp == NULL) {
+			kfree((u8 *) ph2c);
+			kfree((u8 *) psetstakey_para);
+			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_addbareq_cmd(struct 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 == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	paddbareq_parm = (struct addBaReq_parm *)rtw_zmalloc(sizeof(struct addBaReq_parm));
+	if (paddbareq_parm == NULL) {
+		kfree((unsigned char *)ph2c);
+		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));
+
+	/* DBG_871X("rtw_addbareq_cmd, tid =%d\n", tid); */
+
+	/* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+/* add for CONFIG_IEEE80211W, none 11w can use it */
+u8 rtw_reset_securitypriv_cmd(struct 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) {
+		kfree((unsigned char *)ph2c);
+		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));
+
+
+	/* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+u8 rtw_free_assoc_resources_cmd(struct 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) {
+		kfree((unsigned char *)ph2c);
+		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));
+
+
+	/* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+u8 rtw_dynamic_chk_wk_cmd(struct 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 == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		kfree((unsigned char *)ph2c);
+		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));
+
+
+	/* rtw_enqueue_cmd(pcmdpriv, ph2c); */
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan, u8 enqueue, u8 swconfig)
+{
+	struct	cmd_obj *pcmdobj;
+	struct	SetChannelPlan_param *setChannelPlan_param;
+	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
+
+	u8 res = _SUCCESS;
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_chplan_cmd\n"));
+
+	/*  check if allow software config */
+	if (swconfig && rtw_hal_is_disable_sw_channel_plan(padapter) == true) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	/* check input parameter */
+	if (!rtw_is_channel_plan_valid(chplan)) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	/* prepare cmd parameter */
+	setChannelPlan_param = (struct	SetChannelPlan_param *)rtw_zmalloc(sizeof(struct SetChannelPlan_param));
+	if (setChannelPlan_param == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	setChannelPlan_param->channel_plan = chplan;
+
+	if (enqueue) {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		pcmdobj = (struct	cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
+		if (pcmdobj == NULL) {
+			kfree((u8 *)setChannelPlan_param);
+			res = _FAIL;
+			goto exit;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelPlan_param, GEN_CMD_CODE(_SetChannelPlan));
+		res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
+	} else{
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (H2C_SUCCESS != set_chplan_hdl(padapter, (unsigned char *)setChannelPlan_param))
+			res = _FAIL;
+
+		kfree((u8 *)setChannelPlan_param);
+	}
+
+	/* do something based on res... */
+	if (res == _SUCCESS)
+		padapter->mlmepriv.ChannelPlan = chplan;
+
+exit:
+	return res;
+}
+
+static void collect_traffic_statistics(struct adapter *padapter)
+{
+	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;
+
+	/*  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);
+}
+
+u8 traffic_status_watchdog(struct adapter *padapter, u8 from_timer)
+{
+	u8 bEnterPS = false;
+	u16 BusyThresholdHigh = 25;
+	u16 BusyThresholdLow = 10;
+	u16 BusyThreshold = BusyThresholdHigh;
+	u8 bBusyTraffic = false, bTxBusyTraffic = false, bRxBusyTraffic = false;
+	u8 bHigherBusyTraffic = false, bHigherBusyRxTraffic = false, bHigherBusyTxTraffic = false;
+
+	struct mlme_priv 	*pmlmepriv = &(padapter->mlmepriv);
+
+	RT_LINK_DETECT_T *link_detect = &pmlmepriv->LinkDetectInfo;
+
+	collect_traffic_statistics(padapter);
+
+	/*  */
+	/*  Determine if our traffic is busy now */
+	/*  */
+	if ((check_fwstate(pmlmepriv, _FW_LINKED) == true)
+		/*&& !MgntInitAdapterInProgress(pMgntInfo)*/) {
+		/*  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) {
+
+		DBG_871X_LEVEL(_drv_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);
+	}
+#endif
+
+		/*  check traffic for  powersaving. */
+		if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
+			(pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2)) {
+			/* DBG_871X("(-)Tx = %d, Rx = %d\n", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); */
+			bEnterPS = false;
+
+			if (bBusyTraffic == true) {
+				if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount <= 4)
+					pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 4;
+
+				pmlmepriv->LinkDetectInfo.TrafficTransitionCount++;
+
+				/* DBG_871X("Set TrafficTransitionCount to %d\n", pmlmepriv->LinkDetectInfo.TrafficTransitionCount); */
+
+				if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount > 30/*TrafficTransitionLevel*/) {
+					pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 30;
+				}
+			}
+		} else{
+			/* DBG_871X("(+)Tx = %d, Rx = %d\n", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); */
+
+			if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount >= 2)
+				pmlmepriv->LinkDetectInfo.TrafficTransitionCount -= 2;
+			else
+				pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
+
+			if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount == 0)
+				bEnterPS = true;
+		}
+
+		/*  LeisurePS only work in infra mode. */
+		if (bEnterPS) {
+			if (!from_timer)
+				LPS_Enter(padapter, "TRAFFIC_IDLE");
+		} else {
+			if (!from_timer)
+				LPS_Leave(padapter, "TRAFFIC_BUSY");
+			else
+				rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_TRAFFIC_BUSY, 1);
+		}
+	} else{
+		struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+		int n_assoc_iface = 0;
+
+		if (check_fwstate(&(dvobj->padapters->mlmepriv), WIFI_ASOC_STATE))
+			n_assoc_iface++;
+
+		if (!from_timer && n_assoc_iface == 0)
+			LPS_Leave(padapter, "NON_LINKED");
+	}
+
+	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;
+
+}
+
+static void dynamic_chk_wk_hdl(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv;
+	pmlmepriv = &(padapter->mlmepriv);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		expire_timeout_chk(padapter);
+	}
+
+	/* for debug purpose */
+	_linked_info_dump(padapter);
+
+
+	/* if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING|_FW_UNDER_SURVEY) ==false) */
+	{
+		linked_status_chk(padapter);
+		traffic_status_watchdog(padapter, 0);
+	}
+
+	rtw_hal_dm_watchdog(padapter);
+
+	/* check_hw_pbc(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type); */
+
+	/*  */
+	/*  BT-Coexist */
+	/*  */
+	rtw_btcoex_Handler(padapter);
+
+
+	/* always call rtw_ps_processor() at last one. */
+	if (is_primary_adapter(padapter))
+		rtw_ps_processor(padapter);
+}
+
+void lps_ctrl_wk_hdl(struct adapter *padapter, u8 lps_ctrl_type);
+void lps_ctrl_wk_hdl(struct 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:
+		/* DBG_871X("LPS_CTRL_SCAN\n"); */
+		rtw_btcoex_ScanNotify(padapter, true);
+
+		if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+			/*  connect */
+			LPS_Leave(padapter, "LPS_CTRL_SCAN");
+		}
+		break;
+	case LPS_CTRL_JOINBSS:
+		/* DBG_871X("LPS_CTRL_JOINBSS\n"); */
+		LPS_Leave(padapter, "LPS_CTRL_JOINBSS");
+		break;
+	case LPS_CTRL_CONNECT:
+		/* DBG_871X("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));
+		rtw_btcoex_MediaStatusNotify(padapter, mstatus);
+		break;
+	case LPS_CTRL_DISCONNECT:
+		/* DBG_871X("LPS_CTRL_DISCONNECT\n"); */
+		mstatus = 0;/* disconnect */
+		rtw_btcoex_MediaStatusNotify(padapter, mstatus);
+		LPS_Leave(padapter, "LPS_CTRL_DISCONNECT");
+		rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
+		break;
+	case LPS_CTRL_SPECIAL_PACKET:
+		/* DBG_871X("LPS_CTRL_SPECIAL_PACKET\n"); */
+		pwrpriv->DelayLPSLastTimeStamp = jiffies;
+		rtw_btcoex_SpecialPacketNotify(padapter, PACKET_DHCP);
+		LPS_Leave(padapter, "LPS_CTRL_SPECIAL_PACKET");
+		break;
+	case LPS_CTRL_LEAVE:
+		/* DBG_871X("LPS_CTRL_LEAVE\n"); */
+		LPS_Leave(padapter, "LPS_CTRL_LEAVE");
+		break;
+	case LPS_CTRL_TRAFFIC_BUSY:
+		LPS_Leave(padapter, "LPS_CTRL_TRAFFIC_BUSY");
+	default:
+		break;
+	}
+}
+
+u8 rtw_lps_ctrl_wk_cmd(struct 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) {
+			kfree((unsigned char *)ph2c);
+			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(struct adapter *padapter)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_DM_IN_LPS, NULL);
+}
+
+u8 rtw_dm_in_lps_wk_cmd(struct 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) {
+		kfree((unsigned char *)ph2c);
+		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(struct adapter *padapter, u8 dtim)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+	if (dtim <= 0 || dtim > 16)
+		return;
+
+	if (rtw_btcoex_IsBtControlLps(padapter) == true)
+		return;
+
+	down(&pwrpriv->lock);
+
+	if (pwrpriv->dtim != dtim) {
+		DBG_871X("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;
+
+		/* DBG_871X("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));
+	}
+
+	up(&pwrpriv->lock);
+}
+
+static void rtw_dm_ra_mask_hdl(struct adapter *padapter, struct sta_info *psta)
+{
+	if (psta) {
+		set_sta_rate(padapter, psta);
+	}
+}
+
+u8 rtw_dm_ra_mask_wk_cmd(struct 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) {
+		kfree((unsigned char *)ph2c);
+		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(struct adapter *padapter)
+{
+	 rtw_ps_processor(padapter);
+}
+
+/* add for CONFIG_IEEE80211W, none 11w can use it */
+static void reset_securitypriv_hdl(struct adapter *padapter)
+{
+	 rtw_reset_securitypriv(padapter);
+}
+
+static void free_assoc_resources_hdl(struct adapter *padapter)
+{
+	 rtw_free_assoc_resources(padapter, 1);
+}
+
+u8 rtw_ps_cmd(struct adapter *padapter)
+{
+	struct cmd_obj		*ppscmd;
+	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	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) {
+		kfree((unsigned char *)ppscmd);
+		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;
+}
+
+static void rtw_chk_hi_queue_hdl(struct adapter *padapter)
+{
+	struct sta_info *psta_bmc;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	unsigned long 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 && jiffies_to_msecs(jiffies - start) < g_wait_hiq_empty) {
+		msleep(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);
+		} else{/* re check again */
+			rtw_chk_hi_queue_cmd(padapter);
+		}
+
+	}
+
+}
+
+u8 rtw_chk_hi_queue_cmd(struct 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) {
+		kfree((unsigned char *)ph2c);
+		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;
+
+}
+
+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;
+};
+
+static void rtw_btinfo_hdl(struct adapter *adapter, u8 *buf, u16 buf_len)
+{
+	#define BTINFO_WIFI_FETCH 0x23
+	#define BTINFO_BT_AUTO_RPT 0x27
+	struct btinfo *info = (struct btinfo *)buf;
+	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
+	btinfo_evt_dump(RTW_DBGDUMP, info);
+#endif
+
+	/* 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;
+	rtw_btcoex_BtInfoNotify(adapter, len+1, &buf[1]);
+}
+
+u8 rtw_c2h_packet_wk_cmd(struct adapter *padapter, u8 *pbuf, u16 length)
+{
+	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) {
+		kfree((u8 *)ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size = length;
+	pdrvextra_cmd_parm->pbuf = pbuf;
+
+	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;
+}
+
+/* dont call R/W in this function, beucase SDIO interrupt have claim host */
+/* or deadlock will happen and cause special-systemserver-died in android */
+u8 rtw_c2h_wk_cmd(struct adapter *padapter, u8 *c2h_evt)
+{
+	struct cmd_obj *ph2c;
+	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	u8 res = _SUCCESS;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		kfree((u8 *)ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
+	pdrvextra_cmd_parm->type = 0;
+	pdrvextra_cmd_parm->size =  c2h_evt?16:0;
+	pdrvextra_cmd_parm->pbuf = c2h_evt;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+
+	return res;
+}
+
+static void c2h_wk_callback(_workitem *work)
+{
+	struct evt_priv *evtpriv = container_of(work, struct evt_priv, c2h_wk);
+	struct adapter *adapter = container_of(evtpriv, struct adapter, evtpriv);
+	u8 *c2h_evt;
+	c2h_id_filter ccx_id_filter = rtw_hal_c2h_id_filter_ccx(adapter);
+
+	evtpriv->c2h_wk_alive = true;
+
+	while (!rtw_cbuf_empty(evtpriv->c2h_queue)) {
+		c2h_evt = (u8 *)rtw_cbuf_pop(evtpriv->c2h_queue);
+		if (c2h_evt != NULL) {
+			/* This C2H event is read, clear it */
+			c2h_evt_clear(adapter);
+		} else{
+			c2h_evt = (u8 *)rtw_malloc(16);
+			if (c2h_evt != NULL) {
+				/* This C2H event is not read, read & clear now */
+				if (rtw_hal_c2h_evt_read(adapter, c2h_evt) != _SUCCESS) {
+					kfree(c2h_evt);
+					continue;
+				}
+			}
+		}
+
+		/* Special pointer to trigger c2h_evt_clear only */
+		if ((void *)c2h_evt == (void *)evtpriv)
+			continue;
+
+		if (!rtw_hal_c2h_valid(adapter, c2h_evt)) {
+			kfree(c2h_evt);
+			continue;
+		}
+
+		if (ccx_id_filter(c2h_evt) == true) {
+			/* Handle CCX report here */
+			rtw_hal_c2h_handler(adapter, c2h_evt);
+			kfree(c2h_evt);
+		} else{
+			/* Enqueue into cmd_thread for others */
+			rtw_c2h_wk_cmd(adapter, c2h_evt);
+		}
+	}
+
+	evtpriv->c2h_wk_alive = false;
+}
+
+u8 rtw_drvextra_cmd_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct drvextra_cmd_parm *pdrvextra_cmd;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	pdrvextra_cmd = (struct drvextra_cmd_parm *)pbuf;
+
+	switch (pdrvextra_cmd->ec_id) {
+	case DYNAMIC_CHK_WK_CID:/* only  primary padapter go to this cmd, but execute dynamic_chk_wk_hdl() for two interfaces */
+		dynamic_chk_wk_hdl(padapter);
+		break;
+	case POWER_SAVING_CTRL_WK_CID:
+		power_saving_wk_hdl(padapter);
+		break;
+	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;
+	case CHECK_HIQ_WK_CID:
+		rtw_chk_hi_queue_hdl(padapter);
+		break;
+#ifdef CONFIG_INTEL_WIDI
+	case INTEl_WIDI_WK_CID:
+		intel_widi_wk_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf);
+		break;
+#endif /* CONFIG_INTEL_WIDI */
+	/* 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:
+		rtw_hal_set_hwreg_with_buf(padapter, HW_VAR_C2H_HANDLE, pdrvextra_cmd->pbuf, pdrvextra_cmd->size);
+		break;
+	case DM_RA_MSK_WK_CID:
+		rtw_dm_ra_mask_hdl(padapter, (struct sta_info *)pdrvextra_cmd->pbuf);
+		break;
+	case BTINFO_WK_CID:
+		rtw_btinfo_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->size);
+		break;
+	default:
+		break;
+	}
+
+	if (pdrvextra_cmd->pbuf && pdrvextra_cmd->size > 0) {
+		kfree(pdrvextra_cmd->pbuf);
+	}
+
+	return H2C_SUCCESS;
+}
+
+void rtw_survey_cmd_callback(struct 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->scan_to_timer, 1);
+	} else if (pcmd->res != H2C_SUCCESS) {
+		_set_timer(&pmlmepriv->scan_to_timer, 1);
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: MgntActrtw_set_802_11_bssid_LIST_SCAN Fail ************\n\n."));
+	}
+
+	/*  free cmd */
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_disassoc_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	if (pcmd->res != H2C_SUCCESS) {
+		spin_lock_bh(&pmlmepriv->lock);
+		set_fwstate(pmlmepriv, _FW_LINKED);
+		spin_unlock_bh(&pmlmepriv->lock);
+
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ***Error: disconnect_cmd_callback Fail ***\n."));
+		return;
+	}
+	/*  free cmd */
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_joinbss_cmd_callback(struct 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_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+	u8 timer_cancelled;
+	struct sta_info *psta = NULL;
+	struct wlan_network *pwlan = NULL;
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)pcmd->parmbuf;
+	struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
+
+	if (pcmd->parmbuf == NULL)
+		goto exit;
+
+	if ((pcmd->res != H2C_SUCCESS)) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: rtw_createbss_cmd_callback  Fail ************\n\n."));
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+	}
+
+	_cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		psta = rtw_get_stainfo(&padapter->stapriv, pnetwork->MacAddress);
+		if (!psta) {
+			psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->MacAddress);
+			if (psta == NULL) {
+				RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nCan't alloc sta_info when createbss_cmd_callback\n"));
+				goto createbss_cmd_fail;
+			}
+		}
+
+		rtw_indicate_connect(padapter);
+	} else{
+		pwlan = _rtw_alloc_network(pmlmepriv);
+		spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+		if (pwlan == NULL) {
+			pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue);
+			if (pwlan == NULL) {
+				RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n Error:  can't get pwlan in rtw_joinbss_event_callback\n"));
+				spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+				goto createbss_cmd_fail;
+			}
+			pwlan->last_scanned = jiffies;
+		} else{
+			list_add_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue);
+		}
+
+		pnetwork->Length = get_wlan_bssid_ex_sz(pnetwork);
+		memcpy(&(pwlan->network), pnetwork, pnetwork->Length);
+		/* pwlan->fixed = true; */
+
+		/* list_add_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue); */
+
+		/*  copy pdev_network information to	pmlmepriv->cur_network */
+		memcpy(&tgt_network->network, pnetwork, (get_wlan_bssid_ex_sz(pnetwork)));
+
+		/*  reset DSConfig */
+		/* tgt_network->network.Configuration.DSConfig = (u32)rtw_ch2freq(pnetwork->Configuration.DSConfig); */
+
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+		/*  we will set _FW_LINKED when there is one more sat to join us (rtw_stassoc_event_callback) */
+
+	}
+
+createbss_cmd_fail:
+
+	spin_unlock_bh(&pmlmepriv->lock);
+exit:
+	rtw_free_cmd_obj(pcmd);
+}
+
+
+
+void rtw_setstaKey_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct set_stakey_rsp *psetstakey_rsp = (struct set_stakey_rsp *) (pcmd->rsp);
+	struct sta_info *psta = rtw_get_stainfo(pstapriv, psetstakey_rsp->addr);
+
+	if (psta == NULL) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: rtw_setstaKey_cmdrsp_callback => can't get sta_info\n\n"));
+		goto exit;
+	}
+exit:
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_setassocsta_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
+{
+	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 == NULL) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: setassocsta_cmdrsp_callbac => can't get sta_info\n\n"));
+		goto exit;
+	}
+
+	psta->aid = psta->mac_id = passocsta_rsp->cam_id;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) && (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true))
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+	set_fwstate(pmlmepriv, _FW_LINKED);
+	spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+	rtw_free_cmd_obj(pcmd);
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_debug.c linux/3rdparty/rtl8723bs/core/rtw_debug.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_debug.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_debug.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,1453 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#define _RTW_DEBUG_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+u32 GlobalDebugLevel = _drv_err_;
+
+#ifdef CONFIG_DEBUG_RTL871X
+
+	u64 GlobalDebugComponents = \
+			_module_rtl871x_xmit_c_ |
+			_module_xmit_osdep_c_ |
+			_module_rtl871x_recv_c_ |
+			_module_recv_osdep_c_ |
+			_module_rtl871x_mlme_c_ |
+			_module_mlme_osdep_c_ |
+			_module_rtl871x_sta_mgt_c_ |
+			_module_rtl871x_cmd_c_ |
+			_module_cmd_osdep_c_ |
+			_module_rtl871x_io_c_ |
+			_module_io_osdep_c_ |
+			_module_os_intfs_c_|
+			_module_rtl871x_security_c_|
+			_module_rtl871x_eeprom_c_|
+			_module_hal_init_c_|
+			_module_hci_hal_init_c_|
+			_module_rtl871x_ioctl_c_|
+			_module_rtl871x_ioctl_set_c_|
+			_module_rtl871x_ioctl_query_c_|
+			_module_rtl871x_pwrctrl_c_|
+			_module_hci_intfs_c_|
+			_module_hci_ops_c_|
+			_module_hci_ops_os_c_|
+			_module_rtl871x_ioctl_os_c|
+			_module_rtl8712_cmd_c_|
+			_module_hal_xmit_c_|
+			_module_rtl8712_recv_c_ |
+			_module_mp_ |
+			_module_efuse_;
+
+#endif /* CONFIG_DEBUG_RTL871X */
+
+#include <rtw_version.h>
+
+void dump_drv_version(void *sel)
+{
+	DBG_871X_SEL_NL(sel, "%s %s\n", "rtl8723bs", DRIVERVERSION);
+}
+
+void dump_log_level(void *sel)
+{
+	DBG_871X_SEL_NL(sel, "log_level:%d\n", GlobalDebugLevel);
+}
+
+void sd_f0_reg_dump(void *sel, struct adapter *adapter)
+{
+	int i;
+
+	for (i = 0x0; i <= 0xff; i++) {
+		if (i%16 == 0)
+			DBG_871X_SEL_NL(sel, "0x%02x ", i);
+
+		DBG_871X_SEL(sel, "%02x ", rtw_sd_f0_read8(adapter, i));
+
+		if (i%16 == 15)
+			DBG_871X_SEL(sel, "\n");
+		else if (i%8 == 7)
+			DBG_871X_SEL(sel, "\t");
+	}
+}
+
+void mac_reg_dump(void *sel, struct adapter *adapter)
+{
+	int i, j = 1;
+
+	DBG_871X_SEL_NL(sel, "======= MAC REG =======\n");
+
+	for (i = 0x0; i < 0x800; i += 4) {
+		if (j%4 == 1)
+			DBG_871X_SEL_NL(sel, "0x%03x", i);
+		DBG_871X_SEL(sel, " 0x%08x ", rtw_read32(adapter, i));
+		if ((j++)%4 == 0)
+			DBG_871X_SEL(sel, "\n");
+	}
+}
+
+void bb_reg_dump(void *sel, struct adapter *adapter)
+{
+	int i, j = 1;
+
+	DBG_871X_SEL_NL(sel, "======= BB REG =======\n");
+	for (i = 0x800; i < 0x1000 ; i += 4) {
+		if (j%4 == 1)
+			DBG_871X_SEL_NL(sel, "0x%03x", i);
+		DBG_871X_SEL(sel, " 0x%08x ", rtw_read32(adapter, i));
+		if ((j++)%4 == 0)
+			DBG_871X_SEL(sel, "\n");
+	}
+}
+
+void rf_reg_dump(void *sel, struct 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;
+
+	DBG_871X_SEL_NL(sel, "======= RF REG =======\n");
+
+	for (path = 0; path < path_nums; path++) {
+		DBG_871X_SEL_NL(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)
+				DBG_871X_SEL_NL(sel, "0x%02x ", i);
+			DBG_871X_SEL(sel, " 0x%08x ", value);
+			if ((j++)%4 == 0)
+				DBG_871X_SEL(sel, "\n");
+		}
+	}
+}
+
+#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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 addr, val, len;
+
+	if (count < 3) {
+		DBG_871X("argument size is less than 3\n");
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%x %x %x", &addr, &val, &len);
+
+		if (num !=  3) {
+			DBG_871X("invalid write_reg parameter!\n");
+			return count;
+		}
+
+		switch (len) {
+		case 1:
+			rtw_write8(padapter, addr, (u8)val);
+			break;
+		case 2:
+			rtw_write16(padapter, addr, (u16)val);
+			break;
+		case 4:
+			rtw_write32(padapter, addr, val);
+			break;
+		default:
+			DBG_871X("error write length =%d", len);
+			break;
+		}
+
+	}
+
+	return count;
+
+}
+
+static u32 proc_get_read_addr = 0xeeeeeeee;
+static u32 proc_get_read_len = 0x4;
+
+int proc_get_read_reg(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	if (proc_get_read_addr == 0xeeeeeeee) {
+		DBG_871X_SEL_NL(m, "address not initialized\n");
+		return 0;
+	}
+
+	switch (proc_get_read_len) {
+	case 1:
+		DBG_871X_SEL_NL(m, "rtw_read8(0x%x) = 0x%x\n", proc_get_read_addr, rtw_read8(padapter, proc_get_read_addr));
+		break;
+	case 2:
+		DBG_871X_SEL_NL(m, "rtw_read16(0x%x) = 0x%x\n", proc_get_read_addr, rtw_read16(padapter, proc_get_read_addr));
+		break;
+	case 4:
+		DBG_871X_SEL_NL(m, "rtw_read32(0x%x) = 0x%x\n", proc_get_read_addr, rtw_read32(padapter, proc_get_read_addr));
+		break;
+	default:
+		DBG_871X_SEL_NL(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)
+{
+	char tmp[16];
+	u32 addr, len;
+
+	if (count < 2) {
+		DBG_871X("argument size is less than 2\n");
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%x %x", &addr, &len);
+
+		if (num !=  2) {
+			DBG_871X("invalid read_reg parameter!\n");
+			return count;
+		}
+
+		proc_get_read_addr = addr;
+
+		proc_get_read_len = len;
+	}
+
+	return count;
+
+}
+
+int proc_get_fwstate(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	DBG_871X_SEL_NL(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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct security_priv *sec = &padapter->securitypriv;
+
+	DBG_871X_SEL_NL(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);
+
+	DBG_871X_SEL_NL(m, "hw_decrypted =%d\n", sec->hw_decrypted);
+
+#ifdef DBG_SW_SEC_CNT
+	DBG_871X_SEL_NL(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);
+	DBG_871X_SEL_NL(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);
+
+	DBG_871X_SEL_NL(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);
+	DBG_871X_SEL_NL(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);
+
+	DBG_871X_SEL_NL(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);
+	DBG_871X_SEL_NL(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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	DBG_871X_SEL_NL(m, "pmlmeinfo->state = 0x%x\n", pmlmeinfo->state);
+
+	return 0;
+}
+
+int proc_get_roam_flags(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	DBG_871X_SEL_NL(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;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	char tmp[32];
+	u8 flags;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		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;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+
+	DBG_871X_SEL_NL(m, "%12s %12s %11s\n", "rssi_diff_th", "scanr_exp_ms", "scan_int_ms");
+	DBG_871X_SEL_NL(m, "%-12u %-12u %-11u\n"
+		, mlme->roam_rssi_diff_th
+		, mlme->roam_scanr_exp_ms
+		, mlme->roam_scan_int_ms
+	);
+
+	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;
+	struct adapter *adapter = (struct 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;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%hhu %u %u", &rssi_diff_th, &scanr_exp_ms, &scan_int_ms);
+
+		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;
+	}
+
+	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;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	char tmp[32];
+	u8 addr[ETH_ALEN];
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		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);
+
+		DBG_871X("set roam_tgt_addr to "MAC_FMT"\n", MAC_ARG(adapter->mlmepriv.roam_tgt_addr));
+	}
+
+	return count;
+}
+
+int proc_get_qos_option(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	DBG_871X_SEL_NL(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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	DBG_871X_SEL_NL(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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	DBG_871X_SEL_NL(m, "cur_ch =%d, cur_bw =%d, cur_ch_offet =%d\n",
+					pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+
+	DBG_871X_SEL_NL(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_survey_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct __queue	*queue	= &(pmlmepriv->scanned_queue);
+	struct wlan_network	*pnetwork = NULL;
+	struct list_head	*plist, *phead;
+	s32 notify_signal;
+	s16 notify_noise = 0;
+	u16  index = 0;
+
+	spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+	phead = get_list_head(queue);
+	plist = phead ? get_next(phead) : NULL;
+	plist = get_next(phead);
+	if ((!phead) || (!plist)) {
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+		return 0;
+	}
+
+	DBG_871X_SEL_NL(m, "%5s  %-17s  %3s  %-3s  %-4s  %-4s  %5s  %s\n", "index", "bssid", "ch", "RSSI", "SdBm", "Noise", "age", "ssid");
+	while (1) {
+		if (phead == plist)
+			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
+
+		DBG_871X_SEL_NL(m, "%5d  "MAC_FMT"  %3d  %3d  %4d  %4d  %5d  %s\n",
+			++index,
+			MAC_ARG(pnetwork->network.MacAddress),
+			pnetwork->network.Configuration.DSConfig,
+			(int)pnetwork->network.Rssi,
+			notify_signal,
+			notify_noise,
+			jiffies_to_msecs(jiffies - pnetwork->last_scanned),
+			/*translate_percentage_to_dbm(pnetwork->network.PhyInfo.SignalStrength),*/
+			pnetwork->network.Ssid.Ssid);
+		plist = get_next(plist);
+	}
+	spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+
+	return 0;
+}
+
+int proc_get_ap_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct sta_info *psta;
+	struct adapter *padapter = (struct 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;
+
+		DBG_871X_SEL_NL(m, "SSID =%s\n", cur_network->network.Ssid.Ssid);
+		DBG_871X_SEL_NL(m, "sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->hwaddr));
+		DBG_871X_SEL_NL(m, "cur_channel =%d, cur_bwmode =%d, cur_ch_offset =%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+		DBG_871X_SEL_NL(m, "wireless_mode = 0x%x, rtsen =%d, cts2slef =%d\n", psta->wireless_mode, psta->rtsen, psta->cts2self);
+		DBG_871X_SEL_NL(m, "state = 0x%x, aid =%d, macid =%d, raid =%d\n", psta->state, psta->aid, psta->mac_id, psta->raid);
+		DBG_871X_SEL_NL(m, "qos_en =%d, ht_en =%d, init_rate =%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate);
+		DBG_871X_SEL_NL(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);
+		DBG_871X_SEL_NL(m, "ampdu_enable = %d\n", psta->htpriv.ampdu_enable);
+		DBG_871X_SEL_NL(m, "agg_enable_bitmap =%x, candidate_tid_bitmap =%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap);
+		DBG_871X_SEL_NL(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);
+
+		for (i = 0; i < 16; i++) {
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+			if (preorder_ctrl->enable) {
+				DBG_871X_SEL_NL(m, "tid =%d, indicate_seq =%d\n", i, preorder_ctrl->indicate_seq);
+			}
+		}
+
+	} else{
+		DBG_871X_SEL_NL(m, "can't get sta's macaddr, cur_network's macaddr:" MAC_FMT "\n", MAC_ARG(cur_network->network.MacAddress));
+	}
+
+	return 0;
+}
+
+int proc_get_adapter_state(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	DBG_871X_SEL_NL(m, "name =%s, bSurpriseRemoved =%d, bDriverStopped =%d\n",
+					dev->name, padapter->bSurpriseRemoved, padapter->bDriverStopped);
+
+	return 0;
+}
+
+int proc_get_trx_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	int i;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct recv_priv  *precvpriv = &padapter->recvpriv;
+	struct hw_xmit *phwxmit;
+
+	DBG_871X_SEL_NL(m, "free_xmitbuf_cnt =%d, free_xmitframe_cnt =%d\n"
+		, pxmitpriv->free_xmitbuf_cnt, pxmitpriv->free_xmitframe_cnt);
+	DBG_871X_SEL_NL(m, "free_ext_xmitbuf_cnt =%d, free_xframe_ext_cnt =%d\n"
+		, pxmitpriv->free_xmit_extbuf_cnt, pxmitpriv->free_xframe_ext_cnt);
+	DBG_871X_SEL_NL(m, "free_recvframe_cnt =%d\n"
+		, precvpriv->free_recvframe_cnt);
+
+	for (i = 0; i < 4; i++) {
+		phwxmit = pxmitpriv->hwxmits + i;
+		DBG_871X_SEL_NL(m, "%d, hwq.accnt =%d\n", i, phwxmit->accnt);
+	}
+
+	return 0;
+}
+
+int proc_get_rate_ctl(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	if (adapter->fix_rate != 0xff) {
+		DBG_871X_SEL_NL(m, "FIX\n");
+		DBG_871X_SEL_NL(m, "0x%02x\n", adapter->fix_rate);
+	} else {
+		DBG_871X_SEL_NL(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;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u8 fix_rate;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%hhx", &fix_rate);
+
+		if (num >= 1)
+			adapter->fix_rate = fix_rate;
+	}
+
+	return count;
+}
+
+u8 g_fwdl_chksum_fail = 0;
+u8 g_fwdl_wintint_rdy_fail = 0;
+
+ssize_t proc_set_fwdl_test_case(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	char tmp[32];
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%hhu %hhu", &g_fwdl_chksum_fail, &g_fwdl_wintint_rdy_fail);
+	}
+
+	return count;
+}
+
+u32 g_wait_hiq_empty = 0;
+
+ssize_t proc_set_wait_hiq_empty(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	char tmp[32];
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%u", &g_wait_hiq_empty);
+	}
+
+	return count;
+}
+
+int proc_get_suspend_resume_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct dvobj_priv *dvobj = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &dvobj->drv_dbg;
+
+	DBG_871X_SEL_NL(m, "dbg_sdio_alloc_irq_cnt =%d\n", pdbgpriv->dbg_sdio_alloc_irq_cnt);
+	DBG_871X_SEL_NL(m, "dbg_sdio_free_irq_cnt =%d\n", pdbgpriv->dbg_sdio_free_irq_cnt);
+	DBG_871X_SEL_NL(m, "dbg_sdio_alloc_irq_error_cnt =%d\n", pdbgpriv->dbg_sdio_alloc_irq_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_sdio_free_irq_error_cnt =%d\n", pdbgpriv->dbg_sdio_free_irq_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_sdio_init_error_cnt =%d\n", pdbgpriv->dbg_sdio_init_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_sdio_deinit_error_cnt =%d\n", pdbgpriv->dbg_sdio_deinit_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_suspend_error_cnt =%d\n", pdbgpriv->dbg_suspend_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_suspend_cnt =%d\n", pdbgpriv->dbg_suspend_cnt);
+	DBG_871X_SEL_NL(m, "dbg_resume_cnt =%d\n", pdbgpriv->dbg_resume_cnt);
+	DBG_871X_SEL_NL(m, "dbg_resume_error_cnt =%d\n", pdbgpriv->dbg_resume_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_deinit_fail_cnt =%d\n", pdbgpriv->dbg_deinit_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_carddisable_cnt =%d\n", pdbgpriv->dbg_carddisable_cnt);
+	DBG_871X_SEL_NL(m, "dbg_ps_insuspend_cnt =%d\n", pdbgpriv->dbg_ps_insuspend_cnt);
+	DBG_871X_SEL_NL(m, "dbg_dev_unload_inIPS_cnt =%d\n", pdbgpriv->dbg_dev_unload_inIPS_cnt);
+	DBG_871X_SEL_NL(m, "dbg_scan_pwr_state_cnt =%d\n", pdbgpriv->dbg_scan_pwr_state_cnt);
+	DBG_871X_SEL_NL(m, "dbg_downloadfw_pwr_state_cnt =%d\n", pdbgpriv->dbg_downloadfw_pwr_state_cnt);
+	DBG_871X_SEL_NL(m, "dbg_carddisable_error_cnt =%d\n", pdbgpriv->dbg_carddisable_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_fw_read_ps_state_fail_cnt =%d\n", pdbgpriv->dbg_fw_read_ps_state_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_leave_ips_fail_cnt =%d\n", pdbgpriv->dbg_leave_ips_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_leave_lps_fail_cnt =%d\n", pdbgpriv->dbg_leave_lps_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_h2c_leave32k_fail_cnt =%d\n", pdbgpriv->dbg_h2c_leave32k_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_diswow_dload_fw_fail_cnt =%d\n", pdbgpriv->dbg_diswow_dload_fw_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_enwow_dload_fw_fail_cnt =%d\n", pdbgpriv->dbg_enwow_dload_fw_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_ips_drvopen_fail_cnt =%d\n", pdbgpriv->dbg_ips_drvopen_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_poll_fail_cnt =%d\n", pdbgpriv->dbg_poll_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_rpwm_toogle_cnt =%d\n", pdbgpriv->dbg_rpwm_toogle_cnt);
+	DBG_871X_SEL_NL(m, "dbg_rpwm_timeout_fail_cnt =%d\n", pdbgpriv->dbg_rpwm_timeout_fail_cnt);
+
+	return 0;
+}
+
+#ifdef CONFIG_DBG_COUNTER
+
+int proc_get_rx_logs(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct rx_logs *rx_logs = &padapter->rx_logs;
+
+	DBG_871X_SEL_NL(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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct tx_logs *tx_logs = &padapter->tx_logs;
+
+	DBG_871X_SEL_NL(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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	DBG_871X_SEL_NL(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_rx_signal(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	DBG_871X_SEL_NL(m, "rssi:%d\n", padapter->recvpriv.rssi);
+	/*DBG_871X_SEL_NL(m, "rxpwdb:%d\n", padapter->recvpriv.rxpwdb);*/
+	DBG_871X_SEL_NL(m, "signal_strength:%u\n", padapter->recvpriv.signal_strength);
+	DBG_871X_SEL_NL(m, "signal_qual:%u\n", padapter->recvpriv.signal_qual);
+	DBG_871X_SEL_NL(m, "noise:%d\n", padapter->recvpriv.noise);
+	rtw_odm_get_perpkt_rssi(m, padapter);
+	#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+	rtw_get_raw_rssi_info(m, padapter);
+	#endif
+	return 0;
+}
+
+
+int proc_get_hw_status(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct dvobj_priv *dvobj = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &dvobj->drv_dbg;
+
+	DBG_871X_SEL_NL(m, "RX FIFO full count: last_time =%lld, current_time =%lld, differential =%lld\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_rx_signal(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 is_signal_dbg, signal_strength;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%u %u", &is_signal_dbg, &signal_strength);
+
+		is_signal_dbg = is_signal_dbg == 0?0:1;
+
+		if (is_signal_dbg && num != 2)
+			return count;
+
+		signal_strength = signal_strength > 100?100:signal_strength;
+
+		padapter->recvpriv.is_signal_dbg = is_signal_dbg;
+		padapter->recvpriv.signal_strength_dbg =  signal_strength;
+
+		if (is_signal_dbg)
+			DBG_871X("set %s %u\n", "DBG_SIGNAL_STRENGTH", signal_strength);
+		else
+			DBG_871X("set %s\n", "HW_SIGNAL_STRENGTH");
+
+	}
+
+	return count;
+
+}
+
+int proc_get_ht_enable(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		DBG_871X_SEL_NL(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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && mode >= 0 && mode < 2) {
+			pregpriv->ht_enable = mode;
+			printk("ht_enable =%d\n", pregpriv->ht_enable);
+		}
+	}
+
+	return count;
+
+}
+
+int proc_get_bw_mode(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		DBG_871X_SEL_NL(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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv &&  mode < 2) {
+
+			pregpriv->bw_mode = mode;
+			printk("bw_mode =%d\n", mode);
+
+		}
+	}
+
+	return count;
+
+}
+
+int proc_get_ampdu_enable(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		DBG_871X_SEL_NL(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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && mode < 3) {
+			pregpriv->ampdu_enable = mode;
+			printk("ampdu_enable =%d\n", mode);
+		}
+
+	}
+
+	return count;
+
+}
+
+int proc_get_rx_ampdu(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct 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)
+		DBG_871X_SEL_NL(m,
+			"bAcceptAddbaReq = %d , 0:Reject AP's Add BA req, 1:Accept AP's Add BA req.\n", pmlmeinfo->bAcceptAddbaReq
+			);
+
+	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;
+	struct adapter *padapter = (struct 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 (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && mode >= 0 && mode < 2) {
+			pmlmeinfo->bAcceptAddbaReq = mode;
+			DBG_871X("pmlmeinfo->bAcceptAddbaReq =%d\n", pmlmeinfo->bAcceptAddbaReq);
+			if (mode == 0) {
+				/*tear down Rx AMPDU*/
+				send_delba(padapter, 0, get_my_bssid(&(pmlmeinfo->network)));/* recipient*/
+			}
+		}
+
+	}
+
+	return count;
+}
+
+int proc_get_en_fwps(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		DBG_871X_SEL_NL(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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && mode >= 0 && mode < 2) {
+			pregpriv->check_fw_ps = mode;
+			DBG_871X("pregpriv->check_fw_ps =%d\n", pregpriv->check_fw_ps);
+		}
+	}
+	return count;
+}
+
+int proc_get_rx_stbc(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		DBG_871X_SEL_NL(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;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && (mode == 0 || mode == 1 ||
+		    mode == 2 || mode == 3)) {
+			pregpriv->rx_stbc = mode;
+			printk("rx_stbc =%d\n", mode);
+		}
+	}
+
+	return count;
+
+}
+
+int proc_get_rssi_disp(struct seq_file *m, void *v)
+{
+	return 0;
+}
+
+ssize_t proc_set_rssi_disp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 enable = 0;
+
+	if (count < 1) {
+		DBG_8192C("argument size is less than 1\n");
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		int num = sscanf(tmp, "%x", &enable);
+
+		if (num !=  1) {
+			DBG_8192C("invalid set_rssi_disp parameter!\n");
+			return count;
+		}
+
+		if (enable) {
+			DBG_8192C("Linked info Function Enable\n");
+			padapter->bLinkInfoDump = enable;
+		} else {
+			DBG_8192C("Linked info Function Disable\n");
+			padapter->bLinkInfoDump = 0;
+		}
+	}
+	return count;
+}
+
+int proc_get_all_sta_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct sta_info *psta;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	int i, j;
+	struct list_head	*plist, *phead;
+	struct recv_reorder_ctrl *preorder_ctrl;
+
+	DBG_871X_SEL_NL(m, "sta_dz_bitmap = 0x%x, tim_bitmap = 0x%x\n", pstapriv->sta_dz_bitmap, pstapriv->tim_bitmap);
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+
+	for (i = 0; i < NUM_STA; i++) {
+		phead = &(pstapriv->sta_hash[i]);
+		plist = get_next(phead);
+
+		while (phead != plist) {
+			psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+			plist = get_next(plist);
+
+			DBG_871X_SEL_NL(m, "==============================\n");
+			DBG_871X_SEL_NL(m, "sta's macaddr:" MAC_FMT "\n",
+					MAC_ARG(psta->hwaddr));
+			DBG_871X_SEL_NL(m, "rtsen =%d, cts2slef =%d\n",
+					psta->rtsen, psta->cts2self);
+			DBG_871X_SEL_NL(m, "state = 0x%x, aid =%d, macid =%d, raid =%d\n",
+					psta->state, psta->aid, psta->mac_id,
+					psta->raid);
+			DBG_871X_SEL_NL(m, "qos_en =%d, ht_en =%d, init_rate =%d\n",
+					psta->qos_option,
+					psta->htpriv.ht_option,
+					psta->init_rate);
+			DBG_871X_SEL_NL(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);
+			DBG_871X_SEL_NL(m, "ampdu_enable = %d\n",
+					psta->htpriv.ampdu_enable);
+			DBG_871X_SEL_NL(m, "agg_enable_bitmap =%x, candidate_tid_bitmap =%x\n",
+					psta->htpriv.agg_enable_bitmap,
+					psta->htpriv.candidate_tid_bitmap);
+			DBG_871X_SEL_NL(m, "sleepq_len =%d\n",
+					psta->sleepq_len);
+			DBG_871X_SEL_NL(m, "sta_xmitpriv.vo_q_qcnt =%d\n",
+					psta->sta_xmitpriv.vo_q.qcnt);
+			DBG_871X_SEL_NL(m, "sta_xmitpriv.vi_q_qcnt =%d\n",
+					psta->sta_xmitpriv.vi_q.qcnt);
+			DBG_871X_SEL_NL(m, "sta_xmitpriv.be_q_qcnt =%d\n",
+					psta->sta_xmitpriv.be_q.qcnt);
+			DBG_871X_SEL_NL(m, "sta_xmitpriv.bk_q_qcnt =%d\n",
+					psta->sta_xmitpriv.bk_q.qcnt);
+
+			DBG_871X_SEL_NL(m, "capability = 0x%x\n",
+					psta->capability);
+			DBG_871X_SEL_NL(m, "flags = 0x%x\n", psta->flags);
+			DBG_871X_SEL_NL(m, "wpa_psk = 0x%x\n", psta->wpa_psk);
+			DBG_871X_SEL_NL(m, "wpa2_group_cipher = 0x%x\n",
+					psta->wpa2_group_cipher);
+			DBG_871X_SEL_NL(m, "wpa2_pairwise_cipher = 0x%x\n",
+					psta->wpa2_pairwise_cipher);
+			DBG_871X_SEL_NL(m, "qos_info = 0x%x\n", psta->qos_info);
+			DBG_871X_SEL_NL(m, "dot118021XPrivacy = 0x%x\n",
+					psta->dot118021XPrivacy);
+
+			for (j = 0; j < 16; j++) {
+				preorder_ctrl = &psta->recvreorder_ctrl[j];
+				if (preorder_ctrl->enable)
+					DBG_871X_SEL_NL(m, "tid =%d, indicate_seq =%d\n",
+							j, preorder_ctrl->indicate_seq);
+			}
+			DBG_871X_SEL_NL(m, "==============================\n");
+		}
+	}
+
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+	return 0;
+}
+
+int proc_get_btcoex_dbg(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter;
+	char buf[512] = {0};
+	padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	rtw_btcoex_GetDBG(padapter, buf, 512);
+
+	DBG_871X_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;
+	struct adapter *padapter;
+	u8 tmp[80] = {0};
+	u32 module[2] = {0};
+	u32 num;
+
+	padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+/*	DBG_871X("+" FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(padapter));*/
+
+	if (NULL == buffer) {
+		DBG_871X(FUNC_ADPT_FMT ": input buffer is NULL!\n",
+			FUNC_ADPT_ARG(padapter));
+
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		DBG_871X(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)) {
+		DBG_871X(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) {
+		DBG_871X(FUNC_ADPT_FMT ": input(\"%s\") format incorrect!\n",
+			FUNC_ADPT_ARG(padapter), tmp);
+
+		if (0 == num)
+			return -EFAULT;
+	}
+
+	DBG_871X(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;
+	struct adapter *padapter;
+	const u32 bufsize = 30*100;
+	u8 *pbuf = NULL;
+
+	padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	pbuf = rtw_zmalloc(bufsize);
+	if (NULL == pbuf) {
+		return -ENOMEM;
+	}
+
+	rtw_btcoex_DisplayBtCoexInfo(padapter, pbuf, bufsize);
+
+	DBG_871X_SEL(m, "%s\n", pbuf);
+
+	kfree(pbuf);
+
+	return 0;
+}
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_eeprom.c linux/3rdparty/rtl8723bs/core/rtw_eeprom.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_eeprom.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_eeprom.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,369 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#define _RTW_EEPROM_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+
+void up_clk(_adapter *padapter,	 u16 *x)
+{
+_func_enter_;
+	*x = *x | _EESK;
+	rtw_write8(padapter, EE_9346CR, (u8)*x);
+	udelay(CLOCK_RATE);
+
+_func_exit_;
+
+}
+
+void down_clk(_adapter *padapter, u16 *x)
+{
+_func_enter_;
+	*x = *x & ~_EESK;
+	rtw_write8(padapter, EE_9346CR, (u8)*x);
+	udelay(CLOCK_RATE);
+_func_exit_;
+}
+
+void shift_out_bits(_adapter *padapter, u16 data, u16 count)
+{
+	u16 x, mask;
+_func_enter_;
+
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	mask = 0x01 << (count - 1);
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EEDO | _EEDI);
+
+	do {
+		x &= ~_EEDI;
+		if (data & mask)
+			x |= _EEDI;
+		if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+		}
+		rtw_write8(padapter, EE_9346CR, (u8)x);
+		udelay(CLOCK_RATE);
+		up_clk(padapter, &x);
+		down_clk(padapter, &x);
+		mask = mask >> 1;
+	} while (mask);
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	x &= ~_EEDI;
+	rtw_write8(padapter, EE_9346CR, (u8)x);
+out:
+_func_exit_;
+}
+
+u16 shift_in_bits(_adapter *padapter)
+{
+	u16 x, d = 0, i;
+_func_enter_;
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EEDO | _EEDI);
+	d = 0;
+
+	for (i = 0; i < 16; i++) {
+		d = d << 1;
+		up_clk(padapter, &x);
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+		x = rtw_read8(padapter, EE_9346CR);
+
+		x &= ~(_EEDI);
+		if (x & _EEDO)
+		d |= 1;
+
+		down_clk(padapter, &x);
+	}
+out:
+_func_exit_;
+
+	return d;
+}
+
+void standby(_adapter *padapter)
+{
+	u8   x;
+_func_enter_;
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EECS | _EESK);
+	rtw_write8(padapter, EE_9346CR, x);
+
+	udelay(CLOCK_RATE);
+	x |= _EECS;
+	rtw_write8(padapter, EE_9346CR, x);
+	udelay(CLOCK_RATE);
+_func_exit_;
+}
+
+u16 wait_eeprom_cmd_done(_adapter *padapter)
+{
+	u8 x;
+	u16 i, res = false;
+_func_enter_;
+	standby(padapter);
+	for (i = 0; i < 200; i++) {
+		x = rtw_read8(padapter, EE_9346CR);
+		if (x & _EEDO) {
+			res = true;
+			goto exit;
+			}
+		udelay(CLOCK_RATE);
+	}
+exit:
+_func_exit_;
+	return res;
+}
+
+void eeprom_clean(_adapter *padapter)
+{
+	u16 x;
+_func_enter_;
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	x = rtw_read8(padapter, EE_9346CR);
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	x &= ~(_EECS | _EEDI);
+	rtw_write8(padapter, EE_9346CR, (u8)x);
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	up_clk(padapter, &x);
+		if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	down_clk(padapter, &x);
+out:
+_func_exit_;
+}
+
+void eeprom_write16(_adapter *padapter, u16 reg, u16 data)
+{
+	u8 x;
+
+_func_enter_;
+
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_write8(padapter, EE_9346CR, x);
+
+	shift_out_bits(padapter, EEPROM_EWEN_OPCODE, 5);
+
+	if (padapter->EepromAddressSize == 8)	/*CF+ and SDIO*/
+		shift_out_bits(padapter, 0, 6);
+	else									/*USB*/
+		shift_out_bits(padapter, 0, 4);
+
+	standby(padapter);
+
+/* Commented out by rcnjko, 2004.0
+*	 Erase this particular word.  Write the erase opcode and register
+*	 number in that order. The opcode is 3bits in length; reg is 6 bits long.
+*	shift_out_bits(Adapter, EEPROM_ERASE_OPCODE, 3);
+*	shift_out_bits(Adapter, reg, Adapter->EepromAddressSize);
+*
+*	if (wait_eeprom_cmd_done(Adapter ) == false)
+*	{
+*		return;
+*	}
+*/
+
+	standby(padapter);
+
+	/* write the new word to the EEPROM*/
+
+	/* send the write opcode the EEPORM*/
+	shift_out_bits(padapter, EEPROM_WRITE_OPCODE, 3);
+
+	/* select which word in the EEPROM that we are writing to.*/
+	shift_out_bits(padapter, reg, padapter->EepromAddressSize);
+
+	/* write the data to the selected EEPROM word.*/
+	shift_out_bits(padapter, data, 16);
+
+	if (wait_eeprom_cmd_done(padapter) == false) {
+
+		goto exit;
+	}
+
+	standby(padapter);
+
+	shift_out_bits(padapter, EEPROM_EWDS_OPCODE, 5);
+	shift_out_bits(padapter, reg, 4);
+
+	eeprom_clean(padapter);
+exit:
+_func_exit_;
+	return;
+}
+
+u16 eeprom_read16(_adapter *padapter, u16 reg) /*ReadEEprom*/
+{
+
+	u16 x;
+	u16 data = 0;
+
+_func_enter_;
+
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	/* select EEPROM, reset bits, set _EECS*/
+	x = rtw_read8(padapter, EE_9346CR);
+
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_write8(padapter, EE_9346CR, (unsigned char)x);
+
+	/* write the read opcode and register number in that order*/
+	/* The opcode is 3bits in length, reg is 6 bits long*/
+	shift_out_bits(padapter, EEPROM_READ_OPCODE, 3);
+	shift_out_bits(padapter, reg, padapter->EepromAddressSize);
+
+	/* Now read the data (16 bits) in from the selected EEPROM word*/
+	data = shift_in_bits(padapter);
+
+	eeprom_clean(padapter);
+out:
+_func_exit_;
+	return data;
+
+
+}
+
+
+
+
+/*From even offset*/
+void eeprom_read_sz(_adapter *padapter, u16 reg, u8 *data, u32 sz)
+{
+
+	u16 x, data16;
+	u32 i;
+_func_enter_;
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	/* select EEPROM, reset bits, set _EECS*/
+	x = rtw_read8(padapter, EE_9346CR);
+
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_write8(padapter, EE_9346CR, (unsigned char)x);
+
+	/* write the read opcode and register number in that order*/
+	/* The opcode is 3bits in length, reg is 6 bits long*/
+	shift_out_bits(padapter, EEPROM_READ_OPCODE, 3);
+	shift_out_bits(padapter, reg, padapter->EepromAddressSize);
+
+
+	for (i = 0; i < sz; i += 2) {
+		data16 = shift_in_bits(padapter);
+		data[i] = data16 & 0xff;
+		data[i+1] = data16 >> 8;
+	}
+
+	eeprom_clean(padapter);
+out:
+_func_exit_;
+
+
+
+}
+
+
+/*addr_off : address offset of the entry in eeprom (not the tuple number of eeprom (reg); that is addr_off !=reg)*/
+u8 eeprom_read(_adapter *padapter, u32 addr_off, u8 sz, u8 *rbuf)
+{
+	u8 quotient, remainder, addr_2align_odd;
+	u16 reg, stmp, i = 0, idx = 0;
+_func_enter_;
+	reg = (u16)(addr_off >> 1);
+	addr_2align_odd = (u8)(addr_off & 0x1);
+
+	/*read that start at high part: e.g  1,3,5,7,9,...*/
+	if (addr_2align_odd) {
+		stmp = eeprom_read16(padapter, reg);
+		rbuf[idx++] = (u8) ((stmp>>8)&0xff); /*return hogh-part of the short*/
+		reg++; sz--;
+	}
+
+	quotient = sz >> 1;
+	remainder = sz & 0x1;
+
+	for (i = 0; i < quotient; i++) {
+		stmp = eeprom_read16(padapter, reg+i);
+		rbuf[idx++] = (u8) (stmp&0xff);
+		rbuf[idx++] = (u8) ((stmp>>8)&0xff);
+	}
+
+	reg = reg+i;
+	if (remainder) { /*end of read at lower part of short : 0,2,4,6,...*/
+		stmp = eeprom_read16(padapter, reg);
+		rbuf[idx] = (u8)(stmp & 0xff);
+	}
+_func_exit_;
+	return true;
+}
+
+
+
+void read_eeprom_content(_adapter *padapter)
+{
+
+_func_enter_;
+
+
+_func_exit_;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_efuse.c linux/3rdparty/rtl8723bs/core/rtw_efuse.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_efuse.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_efuse.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,635 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#define _RTW_EFUSE_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+#include <linux/jiffies.h>
+
+
+/*------------------------Define local variable------------------------------*/
+u8 fakeEfuseBank = 0;
+u32 fakeEfuseUsedBytes = 0;
+u8 fakeEfuseContent[EFUSE_MAX_HW_SIZE] = {0};
+u8 fakeEfuseInitMap[EFUSE_MAX_MAP_LEN] = {0};
+u8 fakeEfuseModifiedMap[EFUSE_MAX_MAP_LEN] = {0};
+
+u32 BTEfuseUsedBytes = 0;
+u8 BTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+u8 BTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+u8 BTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+
+u32 fakeBTEfuseUsedBytes = 0;
+u8 fakeBTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
+u8 fakeBTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+u8 fakeBTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0};
+/*------------------------Define local variable------------------------------*/
+
+/*  */
+#define REG_EFUSE_CTRL		0x0030
+#define EFUSE_CTRL			REG_EFUSE_CTRL		/*  E-Fuse Control. */
+/*  */
+
+bool
+Efuse_Read1ByteFromFakeContent(
+	struct adapter *padapter,
+	u16 	Offset,
+	u8 *Value);
+bool
+Efuse_Read1ByteFromFakeContent(
+	struct adapter *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(
+	struct adapter *padapter,
+	u16 	Offset,
+	u8 Value);
+bool
+Efuse_Write1ByteToFakeContent(
+	struct adapter *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(
+struct adapter *padapter,
+u8 bWrite,
+u8 PwrState)
+{
+	padapter->HalFunc.EfusePowerSwitch(padapter, bWrite, PwrState);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	Efuse_GetCurrentSize
+ *
+ * Overview:	Get current efuse size!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/16/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+u16
+Efuse_GetCurrentSize(
+	struct adapter *padapter,
+	u8 	efuseType,
+	bool		bPseudoTest)
+{
+	u16 ret = 0;
+
+	ret = padapter->HalFunc.EfuseGetCurrentSize(padapter, efuseType, bPseudoTest);
+
+	return ret;
+}
+
+/*  11/16/2008 MH Add description. Get current efuse area enabled word!!. */
+u8
+Efuse_CalculateWordCnts(u8 word_en)
+{
+	u8 word_cnts = 0;
+	if (!(word_en & BIT(0)))
+		word_cnts++; /*  0 : write enable */
+	if (!(word_en & BIT(1)))
+		word_cnts++;
+	if (!(word_en & BIT(2)))
+		word_cnts++;
+	if (!(word_en & BIT(3)))
+		word_cnts++;
+	return word_cnts;
+}
+
+/*  */
+/* 	Description: */
+/* 		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(
+	struct adapter *Adapter,
+	u8 efuseType,
+	u16 	_offset,
+	u16 	_size_byte,
+	u8 *pbuf,
+bool	bPseudoTest
+	);
+void
+efuse_ReadEFuse(
+	struct adapter *Adapter,
+	u8 efuseType,
+	u16 	_offset,
+	u16 	_size_byte,
+	u8 *pbuf,
+bool	bPseudoTest
+	)
+{
+	Adapter->HalFunc.ReadEFuse(Adapter, efuseType, _offset, _size_byte, pbuf, bPseudoTest);
+}
+
+void
+EFUSE_GetEfuseDefinition(
+	struct adapter *padapter,
+	u8 efuseType,
+	u8 type,
+	void 	*pOut,
+	bool		bPseudoTest
+	)
+{
+	padapter->HalFunc.EFUSEGetEfuseDefinition(padapter, efuseType, type, pOut, bPseudoTest);
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_Read1Byte
+ *
+ * Overview:	Copy from WMAC fot EFUSE read 1 byte.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 09/23/2008	MHC		Copy from WMAC.
+ *
+ *---------------------------------------------------------------------------*/
+u8
+EFUSE_Read1Byte(
+struct adapter *Adapter,
+u16 	Address)
+{
+	u8 data;
+	u8 Bytetemp = {0x00};
+	u8 temp = {0x00};
+	u32 k = 0;
+	u16 contentLen = 0;
+
+	EFUSE_GetEfuseDefinition(Adapter, EFUSE_WIFI, TYPE_EFUSE_REAL_CONTENT_LEN, (void *)&contentLen, false);
+
+	if (Address < contentLen) {/* E-fuse 512Byte */
+		/* Write E-fuse Register address bit0~7 */
+		temp = Address & 0xFF;
+		rtw_write8(Adapter, EFUSE_CTRL+1, temp);
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+2);
+		/* Write E-fuse Register address bit8~9 */
+		temp = ((Address >> 8) & 0x03) | (Bytetemp & 0xFC);
+		rtw_write8(Adapter, EFUSE_CTRL+2, temp);
+
+		/* Write 0x30[31]= 0 */
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		temp = Bytetemp & 0x7F;
+		rtw_write8(Adapter, EFUSE_CTRL+3, temp);
+
+		/* Wait Write-ready (0x30[31]= 1) */
+		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		while (!(Bytetemp & 0x80)) {
+			Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+			k++;
+			if (k == 1000) {
+				k = 0;
+				break;
+			}
+		}
+		data = rtw_read8(Adapter, EFUSE_CTRL);
+		return data;
+	} else
+		return 0xFF;
+
+} /* EFUSE_Read1Byte */
+
+/*  11/16/2008 MH Read one byte from real Efuse. */
+u8
+efuse_OneByteRead(
+struct adapter *padapter,
+u16 		addr,
+u8 	*data,
+bool		bPseudoTest)
+{
+	u32 tmpidx = 0;
+	u8 bResult;
+	u8 readbyte;
+
+	/* DBG_871X("===> EFUSE_OneByteRead(), addr = %x\n", addr); */
+	/* DBG_871X("===> EFUSE_OneByteRead() start, 0x34 = 0x%X\n", rtw_read32(padapter, EFUSE_TEST)); */
+
+	if (bPseudoTest) {
+		bResult = Efuse_Read1ByteFromFakeContent(padapter, addr, data);
+		return bResult;
+	}
+
+	/*  <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_SetMacReg(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)) {
+		mdelay(1);
+		tmpidx++;
+	}
+	if (tmpidx < 100) {
+		*data = rtw_read8(padapter, EFUSE_CTRL);
+		bResult = true;
+	} else{
+		*data = 0xff;
+		bResult = false;
+		DBG_871X("%s: [ERROR] addr = 0x%x bResult =%d time out 1s !!!\n", __func__, addr, bResult);
+		DBG_871X("%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(
+struct adapter *padapter,
+u16 		addr,
+u8 	data,
+bool		bPseudoTest)
+{
+	u8 tmpidx = 0;
+	u8 bResult = false;
+	u32 efuseValue = 0;
+
+	/* DBG_871X("===> EFUSE_OneByteWrite(), addr = %x data =%x\n", addr, data); */
+	/* DBG_871X("===> EFUSE_OneByteWrite() start, 0x34 = 0x%X\n", rtw_read32(padapter, EFUSE_TEST)); */
+
+	if (bPseudoTest) {
+		bResult = Efuse_Write1ByteToFakeContent(padapter, addr, data);
+		return bResult;
+	}
+
+
+	/*  -----------------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. */
+
+	/*  <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_SetMacReg(padapter, 0x34, BIT11, 1); */
+	rtw_write16(padapter, 0x34, rtw_read16(padapter, 0x34) | (BIT11));
+	rtw_write32(padapter, EFUSE_CTRL, 0x90600000|((addr<<8 | data)));
+
+	while ((0x80 &  rtw_read8(padapter, EFUSE_CTRL+3)) && (tmpidx < 100)) {
+		mdelay(1);
+		tmpidx++;
+	}
+
+	if (tmpidx < 100) {
+		bResult = true;
+	} else{
+		bResult = false;
+		DBG_871X("%s: [ERROR] addr = 0x%x , efuseValue = 0x%x , bResult =%d time out 1s !!!\n",
+					__func__, addr, efuseValue, bResult);
+		DBG_871X("%s: [ERROR] EFUSE_CTRL = 0x%08x !!!\n", __func__, rtw_read32(padapter, EFUSE_CTRL));
+	}
+
+	/*  disable Efuse program enable */
+	PHY_SetMacReg(padapter, EFUSE_TEST, BIT(11), 0);
+
+	return bResult;
+}
+
+int
+Efuse_PgPacketRead(struct adapter *padapter,
+				u8 	offset,
+				u8 	*data,
+				bool		bPseudoTest)
+{
+	int	ret = 0;
+
+	ret =  padapter->HalFunc.Efuse_PgPacketRead(padapter, offset, data, bPseudoTest);
+
+	return ret;
+}
+
+int
+Efuse_PgPacketWrite(struct adapter *padapter,
+				u8 	offset,
+				u8 	word_en,
+				u8 	*data,
+				bool		bPseudoTest)
+{
+	int ret;
+
+	ret =  padapter->HalFunc.Efuse_PgPacketWrite(padapter, offset, word_en, data, bPseudoTest);
+
+	return ret;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	efuse_WordEnableDataRead
+ *
+ * Overview:	Read allowed word in current efuse section data.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/16/2008	MHC		Create Version 0.
+ * 11/21/2008	MHC		Fix Write bug when we only enable late word.
+ *
+ *---------------------------------------------------------------------------*/
+void
+efuse_WordEnableDataRead(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];
+	}
+}
+
+
+u8
+Efuse_WordEnableDataWrite(struct adapter *padapter,
+						u16 	efuse_addr,
+						u8 word_en,
+						u8 *data,
+						bool		bPseudoTest)
+{
+	u8 ret = 0;
+
+	ret =  padapter->HalFunc.Efuse_WordEnableDataWrite(padapter, efuse_addr, word_en, data, bPseudoTest);
+
+	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(
+	struct adapter *padapter,
+	u8 efuseType,
+	u8 *Efuse,
+	bool		bPseudoTest);
+void
+Efuse_ReadAllMap(
+	struct adapter *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(
+struct adapter *padapter,
+u16 	Offset,
+	u8 *Value)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+
+	*Value = pEEPROM->efuse_eeprom_data[Offset];
+
+}	/*  EFUSE_ShadowRead1Byte */
+
+/* Read Two Bytes */
+static void
+efuse_ShadowRead2Byte(
+struct adapter *padapter,
+u16 	Offset,
+	u16 	*Value)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+
+	*Value = pEEPROM->efuse_eeprom_data[Offset];
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+1]<<8;
+
+}	/*  EFUSE_ShadowRead2Byte */
+
+/* Read Four Bytes */
+static void
+efuse_ShadowRead4Byte(
+struct adapter *padapter,
+u16 	Offset,
+	u32 	*Value)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+
+	*Value = pEEPROM->efuse_eeprom_data[Offset];
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+1]<<8;
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+2]<<16;
+	*Value |= pEEPROM->efuse_eeprom_data[Offset+3]<<24;
+
+}	/*  efuse_ShadowRead4Byte */
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_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(
+	struct adapter *padapter,
+	u8 efuseType,
+	bool	bPseudoTest)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+	u16 mapLen = 0;
+
+	EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_EFUSE_MAP_LEN, (void *)&mapLen, bPseudoTest);
+
+	if (pEEPROM->bautoload_fail_flag == true) {
+		memset(pEEPROM->efuse_eeprom_data, 0xFF, mapLen);
+	} else{
+		Efuse_ReadAllMap(padapter, efuseType, pEEPROM->efuse_eeprom_data, bPseudoTest);
+	}
+
+	/* PlatformMoveMemory((void *)&pHalData->EfuseMap[EFUSE_MODIFY_MAP][0], */
+	/* void *)&pHalData->EfuseMap[EFUSE_INIT_MAP][0], mapLen); */
+} /*  EFUSE_ShadowMapUpdate */
+
+
+/*-----------------------------------------------------------------------------
+ * Function:	EFUSE_ShadowRead
+ *
+ * Overview:	Read from efuse init map !!!!!
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/12/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void
+EFUSE_ShadowRead(
+	struct adapter *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*/
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_ieee80211.c linux/3rdparty/rtl8723bs/core/rtw_ieee80211.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_ieee80211.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_ieee80211.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,1433 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#define _IEEE80211_C
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+
+u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 };
+u16 RTW_WPA_VERSION = 1;
+u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_NONE[] = { 0x00, 0x50, 0xf2, 0 };
+u8 WPA_CIPHER_SUITE_WEP40[] = { 0x00, 0x50, 0xf2, 1 };
+u8 WPA_CIPHER_SUITE_TKIP[] = { 0x00, 0x50, 0xf2, 2 };
+u8 WPA_CIPHER_SUITE_WRAP[] = { 0x00, 0x50, 0xf2, 3 };
+u8 WPA_CIPHER_SUITE_CCMP[] = { 0x00, 0x50, 0xf2, 4 };
+u8 WPA_CIPHER_SUITE_WEP104[] = { 0x00, 0x50, 0xf2, 5 };
+
+u16 RSN_VERSION_BSD = 1;
+u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_NONE[] = { 0x00, 0x0f, 0xac, 0 };
+u8 RSN_CIPHER_SUITE_WEP40[] = { 0x00, 0x0f, 0xac, 1 };
+u8 RSN_CIPHER_SUITE_TKIP[] = { 0x00, 0x0f, 0xac, 2 };
+u8 RSN_CIPHER_SUITE_WRAP[] = { 0x00, 0x0f, 0xac, 3 };
+u8 RSN_CIPHER_SUITE_CCMP[] = { 0x00, 0x0f, 0xac, 4 };
+u8 RSN_CIPHER_SUITE_WEP104[] = { 0x00, 0x0f, 0xac, 5 };
+/*  */
+/*  for adhoc-master to generate ie and provide supported-rate to fw */
+/*  */
+
+static u8 WIFI_CCKRATES[] = {
+		(IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
+		(IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
+		(IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
+		(IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)
+};
+
+static u8 WIFI_OFDMRATES[] = {
+		(IEEE80211_OFDM_RATE_6MB),
+		(IEEE80211_OFDM_RATE_9MB),
+		(IEEE80211_OFDM_RATE_12MB),
+		(IEEE80211_OFDM_RATE_18MB),
+		(IEEE80211_OFDM_RATE_24MB),
+		IEEE80211_OFDM_RATE_36MB,
+		IEEE80211_OFDM_RATE_48MB,
+		IEEE80211_OFDM_RATE_54MB
+};
+
+
+int rtw_get_bit_value_from_ieee_value(u8 val)
+{
+	unsigned char dot11_rate_table[] = {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 0}; /*  last element must be zero!! */
+
+	int i = 0;
+	while (dot11_rate_table[i] != 0) {
+		if (dot11_rate_table[i] == val)
+			return BIT(i);
+		i++;
+	}
+	return 0;
+}
+
+uint	rtw_is_cckrates_included(u8 *rate)
+{
+		u32 i = 0;
+
+		while (rate[i] !=  0) {
+			if  ((((rate[i]) & 0x7f) == 2)	|| (((rate[i]) & 0x7f) == 4) ||
+			(((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22))
+			return true;
+			i++;
+		}
+
+		return false;
+}
+
+uint	rtw_is_cckratesonly_included(u8 *rate)
+{
+	u32 i = 0;
+
+
+	while (rate[i] != 0) {
+			if  ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
+				(((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22))
+
+			return false;
+
+			i++;
+	}
+
+	return true;
+
+}
+
+int rtw_check_network_type(unsigned char *rate, int ratelen, int channel)
+{
+	if (channel > 14) {
+		if ((rtw_is_cckrates_included(rate)) == true)
+			return WIRELESS_INVALID;
+		else
+			return WIRELESS_11A;
+	} else{ /*  could be pure B, pure G, or B/G */
+		if ((rtw_is_cckratesonly_included(rate)) == true)
+			return WIRELESS_11B;
+		else if ((rtw_is_cckrates_included(rate)) == true)
+			return	WIRELESS_11BG;
+		else
+			return WIRELESS_11G;
+	}
+
+}
+
+u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source,
+				unsigned int *frlen)
+{
+	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);
+}
+
+/*----------------------------------------------------------------------------
+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 buf[MAX_IE_SZ] = {0};
+			u8 *remain_ies = target_ie + target_ielen;
+			uint remain_len = search_len - (remain_ies - start);
+
+			memcpy(buf, remain_ies, remain_len);
+			memcpy(target_ie, buf, remain_len);
+			*ies_len = *ies_len - target_ielen;
+			ret = _SUCCESS;
+
+			start = target_ie;
+			search_len = remain_len;
+		} else {
+			break;
+		}
+	}
+exit:
+	return ret;
+}
+
+void rtw_set_supported_rate(u8 *SupportedRates, uint mode)
+{
+	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;
+	struct 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 */
+	*(u16 *)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{
+		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 == true)) {
+		/* todo: */
+	}
+
+	/* pdev_network->IELength =  sz; update IELength */
+
+	/* return _SUCCESS; */
+
+	return sz;
+}
+
+unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
+{
+	int len;
+	u16 val16;
+	unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
+	u8 *pbuf = pie;
+	int limit_new = limit;
+	__le16 le_tmp;
+
+	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 *)&le_tmp, (pbuf + 6), sizeof(val16));
+
+			val16 = le16_to_cpu(le_tmp);
+			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) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie length mismatch, %u too much", __func__, left));
+
+		return _FAIL;
+	}
+
+
+	/* pairwise_cipher */
+	if (left >= 2) {
+		/* count = le16_to_cpu(*(u16*)pos); */
+		count = RTW_GET_LE16(pos);
+		pos += 2;
+		left -= 2;
+
+		if (count == 0 || left < count * WPA_SELECTOR_LEN) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie count botch (pairwise), "
+						"count %u left %u", __func__, count, left));
+			return _FAIL;
+		}
+
+		for (i = 0; i < count; i++) {
+			*pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
+
+			pos += WPA_SELECTOR_LEN;
+			left -= WPA_SELECTOR_LEN;
+		}
+
+	} else if (left == 1) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie too short (for key mgmt)",   __func__));
+		return _FAIL;
+	}
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (!memcmp(pos, SUITE_1X, 4)) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s : there has 802.1x auth\n", __func__));
+				*is_8021x = 1;
+			}
+		}
+	}
+
+	return ret;
+
+}
+
+int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+{
+	int i, ret = _SUCCESS;
+	int left, count;
+	u8 *pos;
+	u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01};
+
+	if (rsn_ie_len <= 0) {
+		/* No RSN IE - fail silently */
+		return _FAIL;
+	}
+
+
+	if ((*rsn_ie != _WPA2_IE_ID_) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2))) {
+		return _FAIL;
+	}
+
+	pos = rsn_ie;
+	pos += 4;
+	left = rsn_ie_len - 4;
+
+	/* group_cipher */
+	if (left >= RSN_SELECTOR_LEN) {
+
+		*group_cipher = rtw_get_wpa2_cipher_suite(pos);
+
+		pos += RSN_SELECTOR_LEN;
+		left -= RSN_SELECTOR_LEN;
+
+	} else if (left > 0) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie length mismatch, %u too much", __func__, left));
+		return _FAIL;
+	}
+
+	/* pairwise_cipher */
+	if (left >= 2) {
+	  /* count = le16_to_cpu(*(u16*)pos); */
+		count = RTW_GET_LE16(pos);
+		pos += 2;
+		left -= 2;
+
+		if (count == 0 || left < count * RSN_SELECTOR_LEN) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie count botch (pairwise), "
+						 "count %u left %u", __func__, count, left));
+			return _FAIL;
+		}
+
+		for (i = 0; i < count; i++) {
+			*pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
+
+			pos += RSN_SELECTOR_LEN;
+			left -= RSN_SELECTOR_LEN;
+		}
+
+	} else if (left == 1) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie too short (for key mgmt)",  __func__));
+
+		return _FAIL;
+	}
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (!memcmp(pos, SUITE_1X, 4)) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s (): there has 802.1x auth\n", __func__));
+				*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);
+
+				for (i = 0; i < (in_ie[cnt+1]+2); i = i+8) {
+					RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
+								wapi_ie[i], wapi_ie[i+1], wapi_ie[i+2], wapi_ie[i+3], wapi_ie[i+4],
+								wapi_ie[i+5], wapi_ie[i+6], wapi_ie[i+7]));
+				}
+			}
+
+			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))) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n rtw_get_wpa_ie: sec_idx =%d in_ie[cnt+1]+2 =%d\n", sec_idx, in_ie[cnt+1]+2));
+
+				if (wpa_ie) {
+				memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+				for (i = 0; i < (in_ie[cnt+1]+2); i = i+8) {
+						RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
+									wpa_ie[i], wpa_ie[i+1], wpa_ie[i+2], wpa_ie[i+3], wpa_ie[i+4],
+									wpa_ie[i+5], wpa_ie[i+6], wpa_ie[i+7]));
+					}
+				}
+
+				*wpa_len = in_ie[cnt+1]+2;
+				cnt += in_ie[cnt+1]+2;  /* get next */
+		} else{
+			if (authmode == _WPA2_IE_ID_) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n get_rsn_ie: sec_idx =%d in_ie[cnt+1]+2 =%d\n", sec_idx, in_ie[cnt+1]+2));
+
+				if (rsn_ie) {
+				memcpy(rsn_ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+				for (i = 0; i < (in_ie[cnt+1]+2); i = i+8) {
+						RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
+									rsn_ie[i], rsn_ie[i+1], rsn_ie[i+2], rsn_ie[i+3], rsn_ie[i+4],
+									rsn_ie[i+5], rsn_ie[i+6], rsn_ie[i+7]));
+					}
+				}
+
+				*rsn_len = in_ie[cnt+1]+2;
+				cnt += in_ie[cnt+1]+2;  /* get next */
+			} else{
+				cnt += in_ie[cnt+1]+2;   /* get next */
+			}
+		}
+
+	}
+
+	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))) {
+		/* DBG_8192C("==> found WPS_IE.....\n"); */
+		*wps_ielen = ie_ptr[1]+2;
+		match = true;
+	}
+	return match;
+}
+
+/**
+ * rtw_get_wps_ie - Search WPS IE from a series of IEs
+ * @in_ie: Address of IEs to search
+ * @in_len: Length limit from in_ie
+ * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie
+ * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE
+ *
+ * Returns: The address of the WPS IE found, or NULL
+ */
+u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
+{
+	uint cnt;
+	u8 *wpsie_ptr = NULL;
+	u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
+
+	if (wps_ielen)
+		*wps_ielen = 0;
+
+	if (!in_ie || in_len <= 0)
+		return wpsie_ptr;
+
+	cnt = 0;
+
+	while (cnt < in_len) {
+		eid = in_ie[cnt];
+
+		if ((eid == _WPA_IE_ID_) && (!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;
+
+			cnt += in_ie[cnt+1]+2;
+
+			break;
+		} else{
+			cnt += in_ie[cnt+1]+2; /* goto next */
+		}
+
+	}
+
+	return wpsie_ptr;
+}
+
+/**
+ * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
+ * @wps_ie: Address of WPS IE to search
+ * @wps_ielen: Length limit from wps_ie
+ * @target_attr_id: The attribute ID of WPS attribute to search
+ * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
+ * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
+ *
+ * Returns: the address of the specific WPS attribute found, or NULL
+ */
+u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_attr, u32 *len_attr)
+{
+	u8 *attr_ptr = NULL;
+	u8 *target_attr_ptr = NULL;
+	u8 wps_oui[4] = {0x00, 0x50, 0xF2, 0x04};
+
+	if (len_attr)
+		*len_attr = 0;
+
+	if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) ||
+		(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;
+
+		/* DBG_871X("%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) {
+			DBG_871X("short vendor specific "
+				   "information element ignored (len =%lu)\n",
+				   (unsigned long) elen);
+		}
+		return -1;
+	}
+
+	oui = RTW_GET_BE24(pos);
+	switch (oui) {
+	case OUI_MICROSOFT:
+		/* Microsoft/Wi-Fi information elements are further typed and
+		 * subtyped */
+		switch (pos[3]) {
+		case 1:
+			/* Microsoft OUI (00:50:F2) with OUI Type 1:
+			 * real WPA information element */
+			elems->wpa_ie = pos;
+			elems->wpa_ie_len = elen;
+			break;
+		case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
+			if (elen < 5) {
+				DBG_871X("short WME "
+					   "information element ignored "
+					   "(len =%lu)\n",
+					   (unsigned long) elen);
+				return -1;
+			}
+			switch (pos[4]) {
+			case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
+			case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
+				elems->wme = pos;
+				elems->wme_len = elen;
+				break;
+			case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
+				elems->wme_tspec = pos;
+				elems->wme_tspec_len = elen;
+				break;
+			default:
+				DBG_871X("unknown WME "
+					   "information element ignored "
+					   "(subtype =%d len =%lu)\n",
+					   pos[4], (unsigned long) elen);
+				return -1;
+			}
+			break;
+		case 4:
+			/* Wi-Fi Protected Setup (WPS) IE */
+			elems->wps_ie = pos;
+			elems->wps_ie_len = elen;
+			break;
+		default:
+			DBG_871X("Unknown Microsoft "
+				   "information element ignored "
+				   "(type =%d len =%lu)\n",
+				   pos[3], (unsigned long) elen);
+			return -1;
+		}
+		break;
+
+	case OUI_BROADCOM:
+		switch (pos[3]) {
+		case VENDOR_HT_CAPAB_OUI_TYPE:
+			elems->vendor_ht_cap = pos;
+			elems->vendor_ht_cap_len = elen;
+			break;
+		default:
+			DBG_871X("Unknown Broadcom "
+				   "information element ignored "
+				   "(type =%d len =%lu)\n",
+				   pos[3], (unsigned long) elen);
+			return -1;
+		}
+		break;
+
+	default:
+		DBG_871X("unknown vendor specific information "
+			   "element ignored (vendor OUI %02x:%02x:%02x "
+			   "len =%lu)\n",
+			   pos[0], pos[1], pos[2], (unsigned long) elen);
+		return -1;
+	}
+
+	return 0;
+
+}
+
+/**
+ * ieee802_11_parse_elems - Parse information elements in management frames
+ * @start: Pointer to the start of IEs
+ * @len: Length of IE buffer in octets
+ * @elems: Data structure for parsed elements
+ * @show_errors: Whether to show parsing errors in debug log
+ * Returns: Parsing result
+ */
+ParseRes rtw_ieee802_11_parse_elems(u8 *start, uint len,
+				struct rtw_ieee802_11_elems *elems,
+				int show_errors)
+{
+	uint left = len;
+	u8 *pos = start;
+	int unknown = 0;
+
+	memset(elems, 0, sizeof(*elems));
+
+	while (left >= 2) {
+		u8 id, elen;
+
+		id = *pos++;
+		elen = *pos++;
+		left -= 2;
+
+		if (elen > left) {
+			if (show_errors) {
+				DBG_871X("IEEE 802.11 element "
+					   "parse failed (id =%d elen =%d "
+					   "left =%lu)\n",
+					   id, elen, (unsigned long) left);
+			}
+			return ParseFailed;
+		}
+
+		switch (id) {
+		case WLAN_EID_SSID:
+			elems->ssid = pos;
+			elems->ssid_len = elen;
+			break;
+		case WLAN_EID_SUPP_RATES:
+			elems->supp_rates = pos;
+			elems->supp_rates_len = elen;
+			break;
+		case WLAN_EID_FH_PARAMS:
+			elems->fh_params = pos;
+			elems->fh_params_len = elen;
+			break;
+		case WLAN_EID_DS_PARAMS:
+			elems->ds_params = pos;
+			elems->ds_params_len = elen;
+			break;
+		case WLAN_EID_CF_PARAMS:
+			elems->cf_params = pos;
+			elems->cf_params_len = elen;
+			break;
+		case WLAN_EID_TIM:
+			elems->tim = pos;
+			elems->tim_len = elen;
+			break;
+		case WLAN_EID_IBSS_PARAMS:
+			elems->ibss_params = pos;
+			elems->ibss_params_len = elen;
+			break;
+		case WLAN_EID_CHALLENGE:
+			elems->challenge = pos;
+			elems->challenge_len = elen;
+			break;
+		case WLAN_EID_ERP_INFO:
+			elems->erp_info = pos;
+			elems->erp_info_len = elen;
+			break;
+		case WLAN_EID_EXT_SUPP_RATES:
+			elems->ext_supp_rates = pos;
+			elems->ext_supp_rates_len = elen;
+			break;
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (rtw_ieee802_11_parse_vendor_specific(pos, elen,
+							     elems,
+							     show_errors))
+				unknown++;
+			break;
+		case WLAN_EID_RSN:
+			elems->rsn_ie = pos;
+			elems->rsn_ie_len = elen;
+			break;
+		case WLAN_EID_PWR_CAPABILITY:
+			elems->power_cap = pos;
+			elems->power_cap_len = elen;
+			break;
+		case WLAN_EID_SUPPORTED_CHANNELS:
+			elems->supp_channels = pos;
+			elems->supp_channels_len = elen;
+			break;
+		case WLAN_EID_MOBILITY_DOMAIN:
+			elems->mdie = pos;
+			elems->mdie_len = elen;
+			break;
+		case WLAN_EID_FAST_BSS_TRANSITION:
+			elems->ftie = pos;
+			elems->ftie_len = elen;
+			break;
+		case WLAN_EID_TIMEOUT_INTERVAL:
+			elems->timeout_int = pos;
+			elems->timeout_int_len = elen;
+			break;
+		case WLAN_EID_HT_CAP:
+			elems->ht_capabilities = pos;
+			elems->ht_capabilities_len = elen;
+			break;
+		case WLAN_EID_HT_OPERATION:
+			elems->ht_operation = pos;
+			elems->ht_operation_len = elen;
+			break;
+		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;
+			DBG_871X("IEEE 802.11 element parse "
+				   "ignored unknown element (id =%d elen =%d)\n",
+				   id, elen);
+			break;
+		}
+
+		left -= elen;
+		pos += elen;
+	}
+
+	if (left)
+		return ParseFailed;
+
+	return unknown ? ParseUnknown : ParseOK;
+
+}
+
+static u8 key_char2num(u8 ch);
+static u8 key_char2num(u8 ch)
+{
+		if ((ch >= '0') && (ch <= '9'))
+			return ch - '0';
+		else if ((ch >= 'a') && (ch <= 'f'))
+			return ch - 'a' + 10;
+		else if ((ch >= 'A') && (ch <= 'F'))
+			return ch - 'A' + 10;
+		else
+			return 0xff;
+}
+
+u8 key_2char2num(u8 hch, u8 lch);
+u8 key_2char2num(u8 hch, u8 lch)
+{
+		return ((key_char2num(hch) << 4) | key_char2num(lch));
+}
+
+void rtw_macaddr_cfg(struct device *dev, u8 *mac_addr)
+{
+	u8 mac[ETH_ALEN];
+	struct device_node *np = dev->of_node;
+	const unsigned char *addr;
+	int len;
+
+	if (mac_addr == NULL)
+		return;
+
+	if (rtw_initmac) {	/* 	Users specify the mac address */
+		int jj, kk;
+
+		for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3) {
+			mac[jj] = key_2char2num(rtw_initmac[kk], rtw_initmac[kk + 1]);
+		}
+		memcpy(mac_addr, mac, ETH_ALEN);
+	} else{	/* 	Use the mac address stored in the Efuse */
+		memcpy(mac, mac_addr, ETH_ALEN);
+	}
+
+	if (((mac[0] == 0xff) && (mac[1] == 0xff) && (mac[2] == 0xff) &&
+	     (mac[3] == 0xff) && (mac[4] == 0xff) && (mac[5] == 0xff)) ||
+	    ((mac[0] == 0x00) && (mac[1] == 0x00) && (mac[2] == 0x00) &&
+	     (mac[3] == 0x00) && (mac[4] == 0x00) && (mac[5] == 0x00))) {
+	        if (np &&
+	            (addr = of_get_property(np, "local-mac-address", &len)) &&
+	            len == ETH_ALEN) {
+			memcpy(mac_addr, addr, ETH_ALEN);
+		} else {
+			mac[0] = 0x00;
+			mac[1] = 0xe0;
+			mac[2] = 0x4c;
+			mac[3] = 0x87;
+			mac[4] = 0x00;
+			mac[5] = 0x00;
+			/*  use default mac addresss */
+			memcpy(mac_addr, mac, ETH_ALEN);
+			DBG_871X("MAC Address from efuse error, assign default one !!!\n");
+		}
+	}
+
+	DBG_871X("rtw_macaddr_cfg MAC Address  = "MAC_FMT"\n", MAC_ARG(mac_addr));
+}
+
+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)) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_cipher_info: wpa_ielen: %d", wpa_ielen));
+		if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
+
+			pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+			pnetwork->BcnInfo.group_cipher = group_cipher;
+			pnetwork->BcnInfo.is_8021x = is8021x;
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s: pnetwork->pairwise_cipher: %d, is_8021x is %d",
+						__func__, pnetwork->BcnInfo.pairwise_cipher, pnetwork->BcnInfo.is_8021x));
+			ret = _SUCCESS;
+		}
+	} else {
+
+		pbuf = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12);
+
+		if (pbuf && (wpa_ielen > 0)) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("get RSN IE\n"));
+			if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("get RSN IE  OK!!!\n"));
+				pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+				pnetwork->BcnInfo.group_cipher = group_cipher;
+				pnetwork->BcnInfo.is_8021x = is8021x;
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s: pnetwork->pairwise_cipher: %d,"
+							"pnetwork->group_cipher is %d, is_8021x is %d",	__func__, pnetwork->BcnInfo.pairwise_cipher,
+							pnetwork->BcnInfo.group_cipher, pnetwork->BcnInfo.is_8021x));
+				ret = _SUCCESS;
+			}
+		}
+	}
+
+	return ret;
+}
+
+void rtw_get_bcn_info(struct wlan_network *pnetwork)
+{
+	unsigned short cap = 0;
+	u8 bencrypt = 0;
+	/* u8 wpa_ie[255], rsn_ie[255]; */
+	u16 wpa_len = 0, rsn_len = 0;
+	struct HT_info_element *pht_info = NULL;
+	struct ieee80211_ht_cap *pht_cap = NULL;
+	unsigned int		len;
+	unsigned char 	*p;
+	__le16 le_cap;
+
+	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);
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_len));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_len));
+
+	if (rsn_len > 0) {
+		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+	} else if (wpa_len > 0) {
+		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA;
+	} else {
+		if (bencrypt)
+			pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
+	}
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
+				pnetwork->BcnInfo.encryp_protocol));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
+				pnetwork->BcnInfo.encryp_protocol));
+	rtw_get_cipher_info(pnetwork);
+
+	/* get bwmode and ch_offset */
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+			pht_cap = (struct 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;
+	}
+}
+
+/* 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 (rf_type == RF_1T1R) {
+		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);
+	} 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 ieee80211_hdr_3addr);
+	u16 fc;
+	u8 c;
+	u8 a = ACT_PUBLIC_MAX;
+
+	fc = le16_to_cpu(((struct ieee80211_hdr_3addr *)frame)->frame_control);
+
+	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 -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_io.c linux/3rdparty/rtl8723bs/core/rtw_io.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_io.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_io.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,203 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+/*
+
+The purpose of rtw_io.c
+
+a. provides the API
+
+b. provides the protocol engine
+
+c. provides the software interface between caller and the hardware interface
+
+
+Compiler Flag Option:
+
+1. CONFIG_SDIO_HCI:
+    a. USE_SYNC_IRP:  Only sync operations are provided.
+    b. USE_ASYNC_IRP:Both sync/async operations are provided.
+
+jackson@realtek.com.tw
+
+*/
+
+#define _RTW_IO_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+#define rtw_le16_to_cpu(val)		val
+#define rtw_le32_to_cpu(val)		val
+#define rtw_cpu_to_le16(val)		val
+#define rtw_cpu_to_le32(val)		val
+
+u8 _rtw_read8(struct 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(struct adapter *adapter, u32 addr)
+{
+	u16 r_val;
+	/* struct	io_queue	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u16 (*_read16)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_read16 = pintfhdl->io_ops._read16;
+
+	r_val = _read16(pintfhdl, addr);
+	return rtw_le16_to_cpu(r_val);
+}
+
+u32 _rtw_read32(struct adapter *adapter, u32 addr)
+{
+	u32 r_val;
+	/* struct	io_queue	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u32 (*_read32)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_read32 = pintfhdl->io_ops._read32;
+
+	r_val = _read32(pintfhdl, addr);
+	return rtw_le32_to_cpu(r_val);
+
+}
+
+int _rtw_write8(struct 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(struct adapter *adapter, u32 addr, u16 val)
+{
+	/* struct	io_queue	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+	int ret;
+
+	_write16 = pintfhdl->io_ops._write16;
+
+	ret = _write16(pintfhdl, addr, val);
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write32(struct adapter *adapter, u32 addr, u32 val)
+{
+	/* struct	io_queue	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+	int ret;
+
+	_write32 = pintfhdl->io_ops._write32;
+
+	ret = _write32(pintfhdl, addr, val);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+u8 _rtw_sd_f0_read8(struct adapter *adapter, u32 addr)
+{
+	u8 r_val = 0x00;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+	u8 (*_sd_f0_read8)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_sd_f0_read8 = pintfhdl->io_ops._sd_f0_read8;
+
+	if (_sd_f0_read8)
+		r_val = _sd_f0_read8(pintfhdl, addr);
+	else
+		DBG_871X_LEVEL(_drv_warning_, FUNC_ADPT_FMT" _sd_f0_read8 callback is NULL\n", FUNC_ADPT_ARG(adapter));
+
+	return r_val;
+}
+
+u32 _rtw_write_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	u32 (*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	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;
+}
+
+int rtw_init_io_priv(struct adapter *padapter, void (*set_intf_ops)(struct 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 = atomic_inc_return(&dvobj->continual_io_error);
+	if (value > MAX_CONTINUAL_IO_ERR) {
+		DBG_871X("[dvobj:%p][ERROR] continual_io_error:%d > %d\n", dvobj, value, MAX_CONTINUAL_IO_ERR);
+		ret = true;
+	} else {
+		/* DBG_871X("[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);
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_ioctl_set.c linux/3rdparty/rtl8723bs/core/rtw_ioctl_set.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_ioctl_set.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_ioctl_set.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,696 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#define _RTW_IOCTL_SET_C_
+
+#include <drv_types.h>
+#include <rtw_debug.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(struct ndis_802_11_ssid *ssid)
+{
+	u8 ret = true;
+
+	if (ssid->SsidLength > 32) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("ssid length >32\n"));
+		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))) {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("ssid has nonprintabl ascii\n"));
+			ret = false;
+			break;
+		}
+	}
+#endif /* CONFIG_VALIDATE_SSID */
+
+exit:
+	return ret;
+}
+
+u8 rtw_do_join(struct adapter *padapter);
+u8 rtw_do_join(struct adapter *padapter)
+{
+	struct list_head	*plist, *phead;
+	u8 *pibss = NULL;
+	struct	mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct __queue	*queue	= &(pmlmepriv->scanned_queue);
+	u8 ret = _SUCCESS;
+
+	spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+	phead = get_list_head(queue);
+	plist = get_next(phead);
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("\n rtw_do_join: phead = %p; plist = %p\n\n\n", phead, plist));
+
+	pmlmepriv->cur_network.join_res = -2;
+
+	set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+	pmlmepriv->pscanned = plist;
+
+	pmlmepriv->to_join = true;
+
+	if (list_empty(&queue->queue)) {
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+		_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
+		) {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("rtw_do_join(): site survey if scanned_queue is empty\n."));
+			/*  submit site_survey_cmd */
+			ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0);
+			if (_SUCCESS != ret) {
+				pmlmepriv->to_join = false;
+				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("rtw_do_join(): site survey return error\n."));
+			}
+		} else{
+			pmlmepriv->to_join = false;
+			ret = _FAIL;
+		}
+
+		goto exit;
+	} else{
+		int select_ret;
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+		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) == true) {
+				/*  submit createbss_cmd to change to a ADHOC_MASTER */
+
+				/* pmlmepriv->lock has been acquired by caller... */
+				struct wlan_bssid_ex    *pdev_network = &(padapter->registrypriv.dev_network);
+
+				pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+
+				pibss = padapter->registrypriv.dev_network.MacAddress;
+
+				memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
+				memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+				rtw_update_registrypriv_dev_network(padapter);
+
+				rtw_generate_random_ibss(pibss);
+
+				if (rtw_createbss_cmd(padapter) != _SUCCESS) {
+					RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("***Error =>do_goin: rtw_createbss_cmd status FAIL***\n "));
+					ret =  false;
+					goto exit;
+				}
+
+				pmlmepriv->to_join = false;
+
+				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("***Error => rtw_select_and_join_from_scanned_queue FAIL under STA_Mode***\n "));
+
+			} else{
+				/*  can't associate ; reset under-linking */
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+				/* 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
+				) {
+					/* DBG_871X("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;
+						RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("do_join(): site survey return error\n."));
+					}
+				} else{
+					ret = _FAIL;
+					pmlmepriv->to_join = false;
+				}
+			}
+
+		}
+
+	}
+
+exit:
+	return ret;
+}
+
+u8 rtw_set_802_11_bssid(struct adapter *padapter, u8 *bssid)
+{
+	u8 status = _SUCCESS;
+
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	DBG_871X_LEVEL(_drv_always_, "set bssid:%pM\n", bssid);
+
+	if ((bssid[0] == 0x00 && bssid[1] == 0x00 && bssid[2] == 0x00 && bssid[3] == 0x00 && bssid[4] == 0x00 && bssid[5] == 0x00) ||
+	    (bssid[0] == 0xFF && bssid[1] == 0xFF && bssid[2] == 0xFF && bssid[3] == 0xFF && bssid[4] == 0xFF && bssid[5] == 0xFF)) {
+		status = _FAIL;
+		goto exit;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+
+	DBG_871X("Set BSSID under fw_state = 0x%08x\n", get_fwstate(pmlmepriv));
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+		goto handle_tkip_countermeasure;
+	} else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
+		goto release_mlme_lock;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE) == true) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_bssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n"));
+
+		if (!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 {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("Set BSSID not the same bssid\n"));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_bssid ="MAC_FMT"\n", MAC_ARG(bssid)));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("cur_bssid ="MAC_FMT"\n", MAC_ARG(pmlmepriv->cur_network.network.MacAddress)));
+
+			rtw_disassoc_cmd(padapter, 0, true);
+
+			if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+				rtw_indicate_disconnect(padapter);
+
+			rtw_free_assoc_resources(padapter, 1);
+
+			if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+		}
+	}
+
+handle_tkip_countermeasure:
+	if (rtw_handle_tkip_countermeasure(padapter, __func__) == _FAIL) {
+		status = _FAIL;
+		goto release_mlme_lock;
+	}
+
+	memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid));
+	memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN);
+	pmlmepriv->assoc_by_bssid = true;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+		pmlmepriv->to_join = true;
+	} else {
+		status = rtw_do_join(padapter);
+	}
+
+release_mlme_lock:
+	spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+		("rtw_set_802_11_bssid: status =%d\n", status));
+
+	return status;
+}
+
+u8 rtw_set_802_11_ssid(struct adapter *padapter, struct ndis_802_11_ssid *ssid)
+{
+	u8 status = _SUCCESS;
+
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *pnetwork = &pmlmepriv->cur_network;
+
+	DBG_871X_LEVEL(_drv_always_, "set ssid [%s] fw_state = 0x%08x\n",
+			ssid->Ssid, get_fwstate(pmlmepriv));
+
+	if (padapter->hw_init_completed == false) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+			 ("set_ssid: hw_init_completed ==false =>exit!!!\n"));
+		status = _FAIL;
+		goto exit;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	DBG_871X("Set SSID under fw_state = 0x%08x\n", get_fwstate(pmlmepriv));
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+		goto handle_tkip_countermeasure;
+	} else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
+		goto release_mlme_lock;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE) == true) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+			 ("set_ssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n"));
+
+		if ((pmlmepriv->assoc_ssid.SsidLength == ssid->SsidLength) &&
+		    (!memcmp(&pmlmepriv->assoc_ssid.Ssid, ssid->Ssid, ssid->SsidLength))) {
+			if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false)) {
+				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+					 ("Set SSID is the same ssid, fw_state = 0x%08x\n",
+					  get_fwstate(pmlmepriv)));
+
+				if (rtw_is_same_ibss(padapter, pnetwork) == false) {
+					/* if in WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE, create bss or rejoin again */
+					rtw_disassoc_cmd(padapter, 0, true);
+
+					if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+						rtw_indicate_disconnect(padapter);
+
+					rtw_free_assoc_resources(padapter, 1);
+
+					if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+						_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+						set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+					}
+				} else{
+					goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */
+				}
+			} else {
+				rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_JOINBSS, 1);
+			}
+		} else{
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("Set SSID not the same ssid\n"));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_ssid =[%s] len = 0x%x\n", ssid->Ssid, (unsigned int)ssid->SsidLength));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("assoc_ssid =[%s] len = 0x%x\n", pmlmepriv->assoc_ssid.Ssid, (unsigned int)pmlmepriv->assoc_ssid.SsidLength));
+
+			rtw_disassoc_cmd(padapter, 0, true);
+
+			if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+				rtw_indicate_disconnect(padapter);
+
+			rtw_free_assoc_resources(padapter, 1);
+
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+		}
+	}
+
+handle_tkip_countermeasure:
+	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(struct ndis_802_11_ssid));
+	pmlmepriv->assoc_by_bssid = false;
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+		pmlmepriv->to_join = true;
+	} else {
+		status = rtw_do_join(padapter);
+	}
+
+release_mlme_lock:
+	spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+		("-rtw_set_802_11_ssid: status =%d\n", status));
+
+	return status;
+}
+
+u8 rtw_set_802_11_connect(struct adapter *padapter, u8 *bssid, struct ndis_802_11_ssid *ssid)
+{
+	u8 status = _SUCCESS;
+	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) {
+		DBG_871X(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 (padapter->hw_init_completed == false) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+			 ("set_ssid: hw_init_completed ==false =>exit!!!\n"));
+		status = _FAIL;
+		goto exit;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT"  fw_state = 0x%08x\n",
+		FUNC_ADPT_ARG(padapter), get_fwstate(pmlmepriv));
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+		goto handle_tkip_countermeasure;
+	} else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
+		goto release_mlme_lock;
+	}
+
+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(struct ndis_802_11_ssid));
+	else
+		memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct 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) == true) {
+		pmlmepriv->to_join = true;
+	} else {
+		status = rtw_do_join(padapter);
+	}
+
+release_mlme_lock:
+	spin_unlock_bh(&pmlmepriv->lock);
+
+exit:
+	return status;
+}
+
+u8 rtw_set_802_11_infrastructure_mode(struct adapter *padapter,
+	enum NDIS_802_11_NETWORK_INFRASTRUCTURE networktype)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct	wlan_network	*cur_network = &pmlmepriv->cur_network;
+	enum NDIS_802_11_NETWORK_INFRASTRUCTURE *pold_state = &(cur_network->network.InfrastructureMode);
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_notice_,
+		 ("+rtw_set_802_11_infrastructure_mode: old =%d new =%d fw_state = 0x%08x\n",
+		  *pold_state, networktype, get_fwstate(pmlmepriv)));
+
+	if (*pold_state != networktype) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, (" change mode!"));
+		/* DBG_871X("change mode, old_mode =%d, new_mode =%d, fw_state = 0x%x\n", *pold_state, networktype, get_fwstate(pmlmepriv)); */
+
+		if (*pold_state == Ndis802_11APMode) {
+			/* change to other mode from Ndis802_11APMode */
+			cur_network->join_res = -1;
+
+			stop_ap_mode(padapter);
+		}
+
+		spin_lock_bh(&pmlmepriv->lock);
+
+		if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) || (*pold_state == Ndis802_11IBSS))
+			rtw_disassoc_cmd(padapter, 0, true);
+
+		if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) ||
+			(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true))
+			rtw_free_assoc_resources(padapter, 1);
+
+		if ((*pold_state == Ndis802_11Infrastructure) || (*pold_state == Ndis802_11IBSS)) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+				rtw_indicate_disconnect(padapter); /* will clr Linked_state; before this function, we must have chked whether  issue dis-assoc_cmd or not */
+			}
+	       }
+
+		*pold_state = networktype;
+
+		_clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE);
+
+		switch (networktype) {
+		case Ndis802_11IBSS:
+			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			break;
+
+		case Ndis802_11Infrastructure:
+			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+			break;
+
+		case Ndis802_11APMode:
+			set_fwstate(pmlmepriv, WIFI_AP_STATE);
+			start_ap_mode(padapter);
+			/* rtw_indicate_connect(padapter); */
+
+			break;
+
+		case Ndis802_11AutoUnknown:
+		case Ndis802_11InfrastructureMax:
+			break;
+		}
+
+		/* SecClearAllKeys(adapter); */
+
+		/* RT_TRACE(COMP_OID_SET, DBG_LOUD, ("set_infrastructure: fw_state:%x after changing mode\n", */
+		/* 									get_fwstate(pmlmepriv))); */
+
+		spin_unlock_bh(&pmlmepriv->lock);
+	}
+	return true;
+}
+
+
+u8 rtw_set_802_11_disassociate(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_disassociate: rtw_indicate_disconnect\n"));
+
+		rtw_disassoc_cmd(padapter, 0, true);
+		rtw_indicate_disconnect(padapter);
+		/* modify for CONFIG_IEEE80211W, none 11w can use it */
+		rtw_free_assoc_resources_cmd(padapter);
+		if (_FAIL == rtw_pwr_wakeup(padapter))
+			DBG_871X("%s(): rtw_pwr_wakeup fail !!!\n", __func__);
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	return true;
+}
+
+u8 rtw_set_802_11_bssid_list_scan(struct adapter *padapter, struct ndis_802_11_ssid *pssid, int ssid_max_num)
+{
+	struct	mlme_priv 	*pmlmepriv = &padapter->mlmepriv;
+	u8 res = true;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("+rtw_set_802_11_bssid_list_scan(), fw_state =%x\n", get_fwstate(pmlmepriv)));
+
+	if (padapter == NULL) {
+		res = false;
+		goto exit;
+	}
+	if (padapter->hw_init_completed == false) {
+		res = false;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n ===rtw_set_802_11_bssid_list_scan:hw_init_completed ==false ===\n"));
+		goto exit;
+	}
+
+	if ((check_fwstate(pmlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == true) ||
+		(pmlmepriv->LinkDetectInfo.bBusyTraffic == true)) {
+		/*  Scan or linking is in progress, do nothing. */
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("rtw_set_802_11_bssid_list_scan fail since fw_state = %x\n", get_fwstate(pmlmepriv)));
+		res = true;
+
+		if (check_fwstate(pmlmepriv, (_FW_UNDER_SURVEY|_FW_UNDER_LINKING)) == true) {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n###_FW_UNDER_SURVEY|_FW_UNDER_LINKING\n\n"));
+		} else {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n###pmlmepriv->sitesurveyctrl.traffic_busy ==true\n\n"));
+		}
+	} else {
+		if (rtw_is_scan_deny(padapter)) {
+			DBG_871X(FUNC_ADPT_FMT": scan deny\n", FUNC_ADPT_ARG(padapter));
+			indicate_wx_scan_complete_event(padapter);
+			return _SUCCESS;
+		}
+
+		spin_lock_bh(&pmlmepriv->lock);
+
+		res = rtw_sitesurvey_cmd(padapter, pssid, ssid_max_num, NULL, 0);
+
+		spin_unlock_bh(&pmlmepriv->lock);
+	}
+exit:
+
+	return res;
+}
+
+u8 rtw_set_802_11_authentication_mode(struct adapter *padapter, enum NDIS_802_11_AUTHENTICATION_MODE authmode)
+{
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	int res;
+	u8 ret;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_802_11_auth.mode(): mode =%x\n", authmode));
+
+	psecuritypriv->ndisauthtype = authmode;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("rtw_set_802_11_authentication_mode:psecuritypriv->ndisauthtype =%d", psecuritypriv->ndisauthtype));
+
+	if (psecuritypriv->ndisauthtype > 3)
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
+
+	res = rtw_set_auth(padapter, psecuritypriv);
+
+	if (res == _SUCCESS)
+		ret = true;
+	else
+		ret = false;
+
+	return ret;
+}
+
+u8 rtw_set_802_11_add_wep(struct adapter *padapter, struct 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) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("MgntActrtw_set_802_11_add_wep:keyid>4 =>fail\n"));
+		ret = false;
+		goto exit;
+	}
+
+	switch (wep->KeyLength) {
+	case 5:
+		psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength =5\n"));
+		break;
+	case 13:
+		psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength = 13\n"));
+		break;
+	default:
+		psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength!=5 or 13\n"));
+		break;
+	}
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("rtw_set_802_11_add_wep:befor memcpy, wep->KeyLength = 0x%x wep->KeyIndex = 0x%x  keyid =%x\n", wep->KeyLength, wep->KeyIndex, keyid));
+
+	memcpy(&(psecuritypriv->dot11DefKey[keyid].skey[0]), &(wep->KeyMaterial), wep->KeyLength);
+
+	psecuritypriv->dot11DefKeylen[keyid] = wep->KeyLength;
+
+	psecuritypriv->dot11PrivacyKeyIndex = keyid;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("rtw_set_802_11_add_wep:security key material : %x %x %x %x %x %x %x %x %x %x %x %x %x\n",
+		psecuritypriv->dot11DefKey[keyid].skey[0], psecuritypriv->dot11DefKey[keyid].skey[1], psecuritypriv->dot11DefKey[keyid].skey[2],
+		psecuritypriv->dot11DefKey[keyid].skey[3], psecuritypriv->dot11DefKey[keyid].skey[4], psecuritypriv->dot11DefKey[keyid].skey[5],
+		psecuritypriv->dot11DefKey[keyid].skey[6], psecuritypriv->dot11DefKey[keyid].skey[7], psecuritypriv->dot11DefKey[keyid].skey[8],
+		psecuritypriv->dot11DefKey[keyid].skey[9], psecuritypriv->dot11DefKey[keyid].skey[10], psecuritypriv->dot11DefKey[keyid].skey[11],
+		psecuritypriv->dot11DefKey[keyid].skey[12]));
+
+	res = rtw_set_key(padapter, psecuritypriv, keyid, 1, true);
+
+	if (res == _FAIL)
+		ret = false;
+exit:
+
+	return ret;
+}
+
+/*
+* rtw_get_cur_max_rate -
+* @adapter: pointer to struct adapter structure
+*
+* Return 0 or 100Kbps
+*/
+u16 rtw_get_cur_max_rate(struct adapter *adapter)
+{
+	int	i = 0;
+	u16 rate = 0, max_rate = 0;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct wlan_bssid_ex	*pcur_bss = &pmlmepriv->cur_network.network;
+	struct sta_info *psta = NULL;
+	u8 short_GI = 0;
+	u8 rf_type = 0;
+
+	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);
+
+	if (IsSupportedHT(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;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_mlme.c linux/3rdparty/rtl8723bs/core/rtw_mlme.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_mlme.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_mlme.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,3155 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#define _RTW_MLME_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <linux/jiffies.h>
+
+extern u8 rtw_do_join(struct adapter *padapter);
+
+sint	_rtw_init_mlme_priv(struct adapter *padapter)
+{
+	sint	i;
+	u8 *pbuf;
+	struct wlan_network	*pnetwork;
+	struct mlme_priv 	*pmlmepriv = &padapter->mlmepriv;
+	sint	res = _SUCCESS;
+
+	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */
+	/* memset((u8 *)pmlmepriv, 0, sizeof(struct mlme_priv)); */
+
+	pmlmepriv->nic_hdl = (u8 *)padapter;
+
+	pmlmepriv->pscanned = NULL;
+	pmlmepriv->fw_state = WIFI_STATION_STATE; /*  Must sync with rtw_wdev_alloc() */
+	/*  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(struct ndis_802_11_ssid));
+
+	pbuf = vzalloc(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);
+
+	#define RTW_ROAM_SCAN_RESULT_EXP_MS 5000
+	#define RTW_ROAM_RSSI_DIFF_TH 10
+	#define RTW_ROAM_SCAN_INTERVAL_MS 10000
+
+	pmlmepriv->roam_flags = 0
+		| RTW_ROAM_ON_EXPIRED
+		| RTW_ROAM_ON_RESUME
+		#ifdef CONFIG_LAYER2_ROAMING_ACTIVE /* FIXME */
+		| 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;
+
+	rtw_init_mlme_timer(padapter);
+
+exit:
+
+	return res;
+}
+
+static void rtw_free_mlme_ie_data(u8 **ppie, u32 *plen)
+{
+	if (*ppie) {
+		kfree(*ppie);
+		*plen = 0;
+		*ppie = NULL;
+	}
+}
+
+void rtw_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
+{
+	rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
+	rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_beacon_ie, &pmlmepriv->wps_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_req_ie, &pmlmepriv->wps_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_resp_ie, &pmlmepriv->wps_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_assoc_resp_ie, &pmlmepriv->wps_assoc_resp_ie_len);
+
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_beacon_ie, &pmlmepriv->p2p_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_req_ie, &pmlmepriv->p2p_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_resp_ie, &pmlmepriv->p2p_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_go_probe_resp_ie, &pmlmepriv->p2p_go_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_req_ie, &pmlmepriv->p2p_assoc_req_ie_len);
+}
+
+void _rtw_free_mlme_priv(struct mlme_priv *pmlmepriv)
+{
+	rtw_free_mlme_priv_ie_data(pmlmepriv);
+
+	if (pmlmepriv) {
+		if (pmlmepriv->free_bss_buf) {
+			vfree(pmlmepriv->free_bss_buf);
+		}
+	}
+}
+
+/*
+struct	wlan_network *_rtw_dequeue_network(struct __queue *queue)
+{
+	_irqL irqL;
+
+	struct wlan_network *pnetwork;
+
+	spin_lock_bh(&queue->lock);
+
+	if (list_empty(&queue->queue))
+
+		pnetwork = NULL;
+
+	else
+	{
+		pnetwork = LIST_CONTAINOR(get_next(&queue->queue), struct wlan_network, list);
+
+		list_del_init(&(pnetwork->list));
+	}
+
+	spin_unlock_bh(&queue->lock);
+
+	return pnetwork;
+}
+*/
+
+struct	wlan_network *_rtw_alloc_network(struct	mlme_priv *pmlmepriv)/* _queue *free_queue) */
+{
+	struct	wlan_network	*pnetwork;
+	struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+	struct list_head *plist = NULL;
+
+	spin_lock_bh(&free_queue->lock);
+
+	if (list_empty(&free_queue->queue)) {
+		pnetwork = NULL;
+		goto exit;
+	}
+	plist = get_next(&(free_queue->queue));
+
+	pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+	list_del_init(&pnetwork->list);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("_rtw_alloc_network: ptr =%p\n", plist));
+	pnetwork->network_type = 0;
+	pnetwork->fixed = false;
+	pnetwork->last_scanned = jiffies;
+	pnetwork->aid = 0;
+	pnetwork->join_res = 0;
+
+	pmlmepriv->num_of_scanned++;
+
+exit:
+	spin_unlock_bh(&free_queue->lock);
+
+	return pnetwork;
+}
+
+void _rtw_free_network(struct	mlme_priv *pmlmepriv, struct wlan_network *pnetwork, u8 isfreeall)
+{
+	unsigned int delta_time;
+	u32 lifetime = SCANQUEUE_LIFETIME;
+/* 	_irqL irqL; */
+	struct __queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+	if (pnetwork == NULL)
+		return;
+
+	if (pnetwork->fixed == true)
+		return;
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
+		lifetime = 1;
+
+	if (!isfreeall) {
+		delta_time = jiffies_to_msecs(jiffies - pnetwork->last_scanned);
+		if (delta_time < lifetime)/*  unit:msec */
+			return;
+	}
+
+	spin_lock_bh(&free_queue->lock);
+
+	list_del_init(&(pnetwork->list));
+
+	list_add_tail(&(pnetwork->list), &(free_queue->queue));
+
+	pmlmepriv->num_of_scanned--;
+
+
+	/* DBG_871X("_rtw_free_network:SSID =%s\n", pnetwork->network.Ssid.Ssid); */
+
+	spin_unlock_bh(&free_queue->lock);
+}
+
+void _rtw_free_network_nolock(struct	mlme_priv *pmlmepriv, struct wlan_network *pnetwork)
+{
+
+	struct __queue *free_queue = &(pmlmepriv->free_bss_pool);
+
+	if (pnetwork == NULL)
+		return;
+
+	if (pnetwork->fixed == true)
+		return;
+
+	/* spin_lock_irqsave(&free_queue->lock, irqL); */
+
+	list_del_init(&(pnetwork->list));
+
+	list_add_tail(&(pnetwork->list), get_list_head(free_queue));
+
+	pmlmepriv->num_of_scanned--;
+
+	/* spin_unlock_irqrestore(&free_queue->lock, irqL); */
+}
+
+/*
+	return the wlan_network with the matching addr
+
+	Shall be calle under atomic context... to avoid possible racing condition...
+*/
+struct wlan_network *_rtw_find_network(struct __queue *scanned_queue, u8 *addr)
+{
+	struct list_head	*phead, *plist;
+	struct	wlan_network *pnetwork = NULL;
+	u8 zero_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
+
+	if (!memcmp(zero_addr, addr, ETH_ALEN)) {
+		pnetwork = NULL;
+		goto exit;
+	}
+
+	/* spin_lock_bh(&scanned_queue->lock); */
+
+	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;
+
+	/* spin_unlock_bh(&scanned_queue->lock); */
+
+exit:
+	return pnetwork;
+}
+
+void _rtw_free_network_queue(struct adapter *padapter, u8 isfreeall)
+{
+	struct list_head *phead, *plist;
+	struct wlan_network *pnetwork;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct __queue *scanned_queue = &pmlmepriv->scanned_queue;
+
+	spin_lock_bh(&scanned_queue->lock);
+
+	phead = get_list_head(scanned_queue);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+
+		pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		plist = get_next(plist);
+
+		_rtw_free_network(pmlmepriv, pnetwork, isfreeall);
+
+	}
+
+	spin_unlock_bh(&scanned_queue->lock);
+}
+
+
+
+
+sint rtw_if_up(struct adapter *padapter)
+{
+
+	sint res;
+
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+		(check_fwstate(&padapter->mlmepriv, _FW_LINKED) == false)) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_if_up:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved));
+		res = false;
+	} else
+		res =  true;
+	return res;
+}
+
+
+void rtw_generate_random_ibss(u8 *pibss)
+{
+	unsigned long curtime = jiffies;
+
+	pibss[0] = 0x02;  /* in ad-hoc mode bit1 must set to 1 */
+	pibss[1] = 0x11;
+	pibss[2] = 0x87;
+	pibss[3] = (u8)(curtime & 0xff) ;/* p[0]; */
+	pibss[4] = (u8)((curtime>>8) & 0xff) ;/* p[1]; */
+	pibss[5] = (u8)((curtime>>16) & 0xff) ;/* p[2]; */
+	return;
+}
+
+u8 *rtw_get_capability_from_ie(u8 *ie)
+{
+	return (ie + 8 + 2);
+}
+
+
+u16 rtw_get_capability(struct 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_beacon_interval_from_ie(u8 *ie)
+{
+	return (ie + 8);
+}
+
+
+int	rtw_init_mlme_priv(struct 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)
+{
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_mlme_priv\n"));
+	_rtw_free_mlme_priv(pmlmepriv);
+}
+
+/*
+static struct	wlan_network *rtw_dequeue_network(struct __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_nolock(struct adapter *padapter, struct wlan_network *pnetwork);
+void rtw_free_network_nolock(struct adapter *padapter, struct wlan_network *pnetwork)
+{
+	/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_network ==> ssid = %s\n\n" , pnetwork->network.Ssid.Ssid)); */
+	_rtw_free_network_nolock(&(padapter->mlmepriv), pnetwork);
+	rtw_cfg80211_unlink_bss(padapter, pnetwork);
+}
+
+
+void rtw_free_network_queue(struct 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(struct __queue *scanned_queue, u8 *addr)
+{
+	struct	wlan_network *pnetwork = _rtw_find_network(scanned_queue, addr);
+
+	return pnetwork;
+}
+
+int rtw_is_same_ibss(struct adapter *adapter, struct wlan_network *pnetwork)
+{
+	int ret = true;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+
+	if ((psecuritypriv->dot11PrivacyAlgrthm != _NO_PRIVACY_) &&
+		    (pnetwork->network.Privacy == 0))
+		ret = false;
+	else if ((psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_) &&
+		 (pnetwork->network.Privacy == 1))
+		ret = false;
+	else
+		ret = true;
+
+	return ret;
+
+}
+
+inline int is_same_ess(struct wlan_bssid_ex *a, struct wlan_bssid_ex *b)
+{
+	/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("(%s,%d)(%s,%d)\n", */
+	/* 		a->Ssid.Ssid, a->Ssid.SsidLength, b->Ssid.Ssid, b->Ssid.SsidLength)); */
+	return (a->Ssid.SsidLength == b->Ssid.SsidLength)
+		&&  !memcmp(a->Ssid.Ssid, b->Ssid.Ssid, a->Ssid.SsidLength);
+}
+
+int is_same_network(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst, u8 feature)
+{
+	u16 s_cap, d_cap;
+	__le16 tmps, tmpd;
+
+	if (rtw_bug_check(dst, src, &s_cap, &d_cap) == false)
+			return false;
+
+	memcpy((u8 *)&tmps, rtw_get_capability_from_ie(src->IEs), 2);
+	memcpy((u8 *)&tmpd, rtw_get_capability_from_ie(dst->IEs), 2);
+
+
+	s_cap = le16_to_cpu(tmps);
+	d_cap = le16_to_cpu(tmpd);
+
+	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(struct __queue *scanned_queue, struct wlan_network *network)
+{
+	struct list_head *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;
+
+	return found;
+}
+
+struct	wlan_network	*rtw_get_oldest_wlan_network(struct __queue *scanned_queue)
+{
+	struct list_head	*plist, *phead;
+
+
+	struct	wlan_network	*pwlan = NULL;
+	struct	wlan_network	*oldest = NULL;
+
+	phead = get_list_head(scanned_queue);
+
+	plist = get_next(phead);
+
+	while (1) {
+
+		if (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(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
+	struct adapter *padapter, bool update_ie)
+{
+	long rssi_ori = dst->Rssi;
+
+	u8 sq_smp = src->PhyInfo.SignalQuality;
+
+	u8 ss_final;
+	u8 sq_final;
+	long rssi_final;
+
+	#if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) && 1
+	if (strcmp(dst->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
+		DBG_871X(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) {
+		DBG_871X(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(struct adapter *adapter, struct 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) == true) && (is_same_network(&(pmlmepriv->cur_network.network), pnetwork, 0))) {
+		/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,"Same Network\n"); */
+
+		/* if (pmlmepriv->cur_network.network.IELength<= pnetwork->IELength) */
+		{
+			update_network(&(pmlmepriv->cur_network.network), pnetwork, adapter, true);
+			rtw_update_protection(adapter, (pmlmepriv->cur_network.network.IEs) + sizeof(struct ndis_802_11_fix_ie),
+									pmlmepriv->cur_network.network.IELength);
+		}
+	}
+}
+
+
+/*
+
+Caller must hold pmlmepriv->lock first.
+
+
+*/
+void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *target)
+{
+	struct list_head	*plist, *phead;
+	u32 bssid_ex_sz;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct __queue	*queue	= &(pmlmepriv->scanned_queue);
+	struct wlan_network	*pnetwork = NULL;
+	struct wlan_network	*oldest = NULL;
+	int target_find = 0;
+	u8 feature = 0;
+
+	spin_lock_bh(&queue->lock);
+	phead = get_list_head(queue);
+	plist = get_next(phead);
+
+	while (1) {
+		if (phead == plist)
+			break;
+
+		pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		rtw_bug_check(pnetwork, pnetwork, pnetwork, pnetwork);
+
+		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 (phead == plist) { */
+	if (!target_find) {
+		if (list_empty(&pmlmepriv->free_bss_pool.queue)) {
+			/* If there are no more slots, expire the oldest */
+			/* list_del_init(&oldest->list); */
+			pnetwork = oldest;
+			if (pnetwork == NULL) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n\nsomething wrong here\n\n\n"));
+				goto exit;
+			}
+			memcpy(&(pnetwork->network), target,  get_wlan_bssid_ex_sz(target));
+			/*  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) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n\nsomething wrong here\n\n\n"));
+				goto exit;
+			}
+
+			bssid_ex_sz = get_wlan_bssid_ex_sz(target);
+			target->Length = bssid_ex_sz;
+			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:
+	spin_unlock_bh(&queue->lock);
+}
+
+void rtw_add_network(struct adapter *adapter, struct wlan_bssid_ex *pnetwork);
+void rtw_add_network(struct adapter *adapter, struct wlan_bssid_ex *pnetwork)
+{
+	/* struct __queue	*queue	= &(pmlmepriv->scanned_queue); */
+
+	/* spin_lock_bh(&queue->lock); */
+
+	update_current_network(adapter, pnetwork);
+
+	rtw_update_scanned_network(adapter, pnetwork);
+
+	/* spin_unlock_bh(&queue->lock); */
+}
+
+/* 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(struct adapter *adapter, struct wlan_network *pnetwork);
+int rtw_is_desired_network(struct adapter *adapter, struct wlan_network *pnetwork)
+{
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u32 desired_encmode;
+	u32 privacy;
+
+	/* u8 wps_ie[512]; */
+	uint wps_ielen;
+
+	int bselected = true;
+
+	desired_encmode = psecuritypriv->ndisencryptstatus;
+	privacy = pnetwork->network.Privacy;
+
+	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+		if (rtw_get_wps_ie(pnetwork->network.IEs+_FIXED_IE_LENGTH_, pnetwork->network.IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen) != NULL)
+			return true;
+		else
+			return false;
+
+	}
+	if (adapter->registrypriv.wifi_spec == 1) { /* for  correct flow of 8021X  to do.... */
+		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)) {
+		DBG_871X("desired_encmode: %d, privacy: %d\n", desired_encmode, privacy);
+		bselected = false;
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) {
+		if (pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode)
+			bselected = false;
+	}
+
+
+	return bselected;
+}
+
+/* TODO: Perry : For Power Management */
+void rtw_atimdone_event_callback(struct adapter	*adapter, u8 *pbuf)
+{
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("receive atimdone_evet\n"));
+}
+
+
+void rtw_survey_event_callback(struct adapter	*adapter, u8 *pbuf)
+{
+	u32 len;
+	struct wlan_bssid_ex *pnetwork;
+	struct	mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+	pnetwork = (struct wlan_bssid_ex *)pbuf;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_survey_event_callback, ssid =%s\n",  pnetwork->Ssid.Ssid));
+
+	len = get_wlan_bssid_ex_sz(pnetwork);
+	if (len > (sizeof(struct wlan_bssid_ex))) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n ****rtw_survey_event_callback: return a wrong bss ***\n"));
+		return;
+	}
+
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	/*  update IBSS_network 's timestamp */
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) == true) {
+		/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,"rtw_survey_event_callback : WIFI_ADHOC_MASTER_STATE\n\n"); */
+		if (!memcmp(&(pmlmepriv->cur_network.network.MacAddress), pnetwork->MacAddress, ETH_ALEN)) {
+			struct wlan_network *ibss_wlan = NULL;
+
+			memcpy(pmlmepriv->cur_network.network.IEs, pnetwork->IEs, 8);
+			spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+			ibss_wlan = rtw_find_network(&pmlmepriv->scanned_queue,  pnetwork->MacAddress);
+			if (ibss_wlan) {
+				memcpy(ibss_wlan->network.IEs, pnetwork->IEs, 8);
+				spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+				goto exit;
+			}
+			spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+		}
+	}
+
+	/*  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:
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	return;
+}
+
+
+
+void rtw_surveydone_event_callback(struct adapter	*adapter, u8 *pbuf)
+{
+	u8 timer_cancelled = false;
+	struct	mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+	spin_lock_bh(&pmlmepriv->lock);
+	if (pmlmepriv->wps_probe_req_ie) {
+		pmlmepriv->wps_probe_req_ie_len = 0;
+		kfree(pmlmepriv->wps_probe_req_ie);
+		pmlmepriv->wps_probe_req_ie = NULL;
+	}
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_surveydone_event_callback: fw_state:%x\n\n", get_fwstate(pmlmepriv)));
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+		/* u8 timer_cancelled; */
+
+		timer_cancelled = true;
+		/* _cancel_timer(&pmlmepriv->scan_to_timer, &timer_cancelled); */
+
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	} else {
+
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("nic status =%x, survey done event comes too late!\n", get_fwstate(pmlmepriv)));
+	}
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	if (timer_cancelled)
+		_cancel_timer(&pmlmepriv->scan_to_timer, &timer_cancelled);
+
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	rtw_set_signal_stat_timer(&adapter->recvpriv);
+
+	if (pmlmepriv->to_join == true) {
+		if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED) == false) {
+				set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+
+				if (rtw_select_and_join_from_scanned_queue(pmlmepriv) == _SUCCESS) {
+					_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+				} else{
+					struct wlan_bssid_ex    *pdev_network = &(adapter->registrypriv.dev_network);
+					u8 *pibss = adapter->registrypriv.dev_network.MacAddress;
+
+					/* pmlmepriv->fw_state ^= _FW_UNDER_SURVEY;because don't set assoc_timer */
+					_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("switching to adhoc master\n"));
+
+					memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
+					memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+					rtw_update_registrypriv_dev_network(adapter);
+					rtw_generate_random_ibss(pibss);
+
+					pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+
+					if (rtw_createbss_cmd(adapter) != _SUCCESS) {
+					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error =>rtw_createbss_cmd status FAIL\n"));
+					}
+
+					pmlmepriv->to_join = false;
+				}
+			}
+		} else{
+			int s_ret;
+			set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+			pmlmepriv->to_join = false;
+			s_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+			if (_SUCCESS == s_ret) {
+			     _set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+			} else if (s_ret == 2) {/* there is no need to wait for join */
+				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+				rtw_indicate_connect(adapter);
+			} else{
+				DBG_871X("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);
+							DBG_871X("change to widi listen\n");
+						}
+#endif /*  CONFIG_INTEL_WIDI */
+						rtw_free_assoc_resources(adapter, 1);
+						rtw_indicate_disconnect(adapter);
+					} else {
+						pmlmepriv->to_join = true;
+					}
+				} else
+					rtw_indicate_disconnect(adapter);
+
+				_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) {
+					receive_disconnect(adapter, pmlmepriv->cur_network.network.MacAddress
+						, WLAN_REASON_ACTIVE_ROAM);
+				}
+			}
+		}
+	}
+
+	/* DBG_871X("scan complete in %dms\n", jiffies_to_msecs(jiffies - pmlmepriv->scan_start_time)); */
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	rtw_os_xmit_schedule(adapter);
+
+	rtw_cfg80211_surveydone_event_callback(adapter);
+
+	rtw_indicate_scan_done(adapter, false);
+}
+
+void rtw_dummy_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+}
+
+void rtw_fwdbg_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+}
+
+static void free_scanqueue(struct	mlme_priv *pmlmepriv)
+{
+	struct __queue *free_queue = &pmlmepriv->free_bss_pool;
+	struct __queue *scan_queue = &pmlmepriv->scanned_queue;
+	struct list_head	*plist, *phead, *ptemp;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+free_scanqueue\n"));
+	spin_lock_bh(&scan_queue->lock);
+	spin_lock_bh(&free_queue->lock);
+
+	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--;
+	}
+
+	spin_unlock_bh(&free_queue->lock);
+	spin_unlock_bh(&scan_queue->lock);
+}
+
+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;
+}
+
+static void find_network(struct adapter *adapter)
+{
+	struct wlan_network *pwlan = NULL;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+
+	pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+	if (pwlan)
+		pwlan->fixed = false;
+	else
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_assoc_resources : pwlan == NULL\n\n"));
+
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) &&
+	    (adapter->stapriv.asoc_sta_count == 1))
+		rtw_free_network_nolock(adapter, pwlan);
+}
+
+/*
+*rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock
+*/
+void rtw_free_assoc_resources(struct adapter *adapter, int lock_scanned_queue)
+{
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct dvobj_priv *psdpriv = adapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+rtw_free_assoc_resources\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("tgt_network->network.MacAddress ="MAC_FMT" ssid =%s\n",
+		MAC_ARG(tgt_network->network.MacAddress), tgt_network->network.Ssid.Ssid));
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_AP_STATE)) {
+		struct sta_info *psta;
+
+		psta = rtw_get_stainfo(&adapter->stapriv, tgt_network->network.MacAddress);
+		spin_lock_bh(&(pstapriv->sta_hash_lock));
+		rtw_free_stainfo(adapter,  psta);
+
+		spin_unlock_bh(&(pstapriv->sta_hash_lock));
+
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE)) {
+		struct sta_info *psta;
+
+		rtw_free_all_stainfo(adapter);
+
+		psta = rtw_get_bcmc_stainfo(adapter);
+		rtw_free_stainfo(adapter, psta);
+
+		rtw_init_bcmc_stainfo(adapter);
+	}
+
+	if (lock_scanned_queue) {
+		find_network(adapter);
+	} else {
+		find_network(adapter);
+	}
+
+	if (lock_scanned_queue)
+
+	adapter->securitypriv.key_mask = 0;
+
+	rtw_reset_rx_info(pdbgpriv);
+}
+
+/*
+*rtw_indicate_connect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_connect(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_connect\n"));
+
+	pmlmepriv->to_join = false;
+
+	if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+
+		set_fwstate(pmlmepriv, _FW_LINKED);
+
+		rtw_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);
+		DBG_871X("change to widi listen\n");
+	}
+#endif /*  CONFIG_INTEL_WIDI */
+
+	rtw_set_scan_deny(padapter, 3000);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("-rtw_indicate_connect: fw_state = 0x%08x\n", get_fwstate(pmlmepriv)));
+}
+
+/*
+*rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_disconnect(struct adapter *padapter)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_disconnect\n"));
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING|WIFI_UNDER_WPS);
+
+	/* DBG_871X("clear wps when %s\n", __func__); */
+
+	if (rtw_to_roam(padapter) > 0)
+		_clr_fwstate_(pmlmepriv, _FW_LINKED);
+
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED)
+		|| (rtw_to_roam(padapter) <= 0)
+	) {
+		rtw_os_indicate_disconnect(padapter);
+
+		/* set ips_deny_time to avoid enter IPS before LPS leave */
+		rtw_set_ips_deny(padapter, 3000);
+
+	      _clr_fwstate_(pmlmepriv, _FW_LINKED);
+
+		rtw_clear_scan_deny(padapter);
+	}
+
+	rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 1);
+}
+
+inline void rtw_indicate_scan_done(struct adapter *padapter, bool aborted)
+{
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	rtw_os_indicate_scan_done(padapter, aborted);
+
+	if (is_primary_adapter(padapter) &&
+	    (!adapter_to_pwrctl(padapter)->bInSuspend) &&
+	    (!check_fwstate(&padapter->mlmepriv,
+			    WIFI_ASOC_STATE|WIFI_UNDER_LINKING))) {
+		struct pwrctrl_priv *pwrpriv;
+
+		pwrpriv = adapter_to_pwrctl(padapter);
+		rtw_set_ips_deny(padapter, 0);
+		_set_timer(&padapter->mlmepriv.dynamic_chk_timer, 1);
+	}
+}
+
+void rtw_scan_abort(struct adapter *adapter)
+{
+	unsigned long start;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(adapter->mlmeextpriv);
+
+	start = jiffies;
+	pmlmeext->scan_abort = true;
+	while (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)
+		&& jiffies_to_msecs(start) <= 200) {
+
+		if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+			break;
+
+		DBG_871X(FUNC_NDEV_FMT"fw_state = _FW_UNDER_SURVEY!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+		msleep(20);
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+		if (!adapter->bDriverStopped && !adapter->bSurpriseRemoved)
+			DBG_871X(FUNC_NDEV_FMT"waiting for scan_abort time out!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+		rtw_indicate_scan_done(adapter, true);
+	}
+	pmlmeext->scan_abort = false;
+}
+
+static struct sta_info *rtw_joinbss_update_stainfo(struct adapter *padapter, struct wlan_network *pnetwork)
+{
+	int i;
+	struct sta_info *bmc_sta, *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	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 */
+
+		DBG_871X("%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 */
+		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;
+
+			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;
+			memset((u8 *)&psta->dot11wtxpn, 0, sizeof(union pn48));
+			memset((u8 *)&psta->dot11rxpn, 0, sizeof(union pn48));
+		}
+
+		/* 	Commented by Albert 2012/07/21 */
+		/* 	When doing the WPS, the wps_ie_len won't equal to 0 */
+		/* 	And the Wi-Fi driver shouldn't allow the data packet to be tramsmitted. */
+		if (padapter->securitypriv.wps_ie_len != 0) {
+			psta->ieee8021x_blocked = true;
+			padapter->securitypriv.wps_ie_len = 0;
+		}
+
+
+		/* for A-MPDU Rx reordering buffer control for bmc_sta & sta_info */
+		/* if A-MPDU Rx is enabled, reseting  rx_ordering_ctrl wstart_b(indicate_seq) to default value = 0xffff */
+		/* todo: check if AP can send A-MPDU packets */
+		for (i = 0; i < 16 ; i++) {
+			/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+			preorder_ctrl->enable = false;
+			preorder_ctrl->indicate_seq = 0xffff;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __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 */
+		}
+
+
+		bmc_sta = rtw_get_bcmc_stainfo(padapter);
+		if (bmc_sta) {
+			for (i = 0; i < 16 ; i++) {
+				/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+				preorder_ctrl = &bmc_sta->recvreorder_ctrl[i];
+				preorder_ctrl->enable = false;
+				preorder_ctrl->indicate_seq = 0xffff;
+				#ifdef DBG_RX_SEQ
+				DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __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 */
+			}
+		}
+	}
+
+	return psta;
+
+}
+
+/* pnetwork : returns from rtw_joinbss_event_callback */
+/* ptarget_wlan: found from scanned_queue */
+static void rtw_joinbss_update_network(struct adapter *padapter, struct wlan_network *ptarget_wlan, struct wlan_network  *pnetwork)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct wlan_network  *cur_network = &(pmlmepriv->cur_network);
+
+	DBG_871X("%s\n", __func__);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\nfw_state:%x, BSSID:"MAC_FMT"\n"
+		, get_fwstate(pmlmepriv), MAC_ARG(pnetwork->network.MacAddress)));
+
+
+	/*  why not use ptarget_wlan?? */
+	memcpy(&cur_network->network, &pnetwork->network, pnetwork->network.Length);
+	/*  some IEs in pnetwork is wrong, so we should use ptarget_wlan IEs */
+	cur_network->network.IELength = ptarget_wlan->network.IELength;
+	memcpy(&cur_network->network.IEs[0], &ptarget_wlan->network.IEs[0], MAX_IE_SZ);
+
+	cur_network->aid = pnetwork->join_res;
+
+
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+
+	padapter->recvpriv.signal_strength = ptarget_wlan->network.PhyInfo.SignalStrength;
+	padapter->recvpriv.signal_qual = ptarget_wlan->network.PhyInfo.SignalQuality;
+	/* the ptarget_wlan->network.Rssi is raw data, we use ptarget_wlan->network.PhyInfo.SignalStrength instead (has scaled) */
+	padapter->recvpriv.rssi = translate_percentage_to_dbm(ptarget_wlan->network.PhyInfo.SignalStrength);
+	#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1
+		DBG_871X(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
+
+	rtw_set_signal_stat_timer(&padapter->recvpriv);
+
+	/* update fw_state will clr _FW_UNDER_LINKING here indirectly */
+	switch (pnetwork->network.InfrastructureMode) {
+	case Ndis802_11Infrastructure:
+
+			if (pmlmepriv->fw_state&WIFI_UNDER_WPS)
+				pmlmepriv->fw_state = WIFI_STATION_STATE|WIFI_UNDER_WPS;
+			else
+				pmlmepriv->fw_state = WIFI_STATION_STATE;
+
+			break;
+	case Ndis802_11IBSS:
+			pmlmepriv->fw_state = WIFI_ADHOC_STATE;
+			break;
+	default:
+			pmlmepriv->fw_state = WIFI_NULL_STATE;
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Invalid network_mode\n"));
+			break;
+	}
+
+	rtw_update_protection(padapter, (cur_network->network.IEs) + sizeof(struct ndis_802_11_fix_ie),
+									(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(struct adapter *adapter, u8 *pbuf)
+{
+	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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("joinbss event call back received with res =%d\n", pnetwork->join_res));
+
+	rtw_get_encrypt_decrypt_from_registrypriv(adapter);
+
+
+	if (pmlmepriv->assoc_ssid.SsidLength == 0) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("@@@@@   joinbss event call back  for Any SSid\n"));
+	} else{
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("@@@@@   rtw_joinbss_event_callback for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
+	}
+
+	the_same_macaddr = !memcmp(pnetwork->network.MacAddress, cur_network->network.MacAddress, ETH_ALEN);
+
+	pnetwork->network.Length = get_wlan_bssid_ex_sz(&pnetwork->network);
+	if (pnetwork->network.Length > sizeof(struct wlan_bssid_ex)) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n ***joinbss_evt_callback return a wrong bss ***\n\n"));
+		return;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
+	pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n rtw_joinbss_event_callback !! spin_lock_irqsave\n"));
+
+	if (pnetwork->join_res > 0) {
+		spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+		retry = 0;
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+			/* s1. find ptarget_wlan */
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				if (the_same_macaddr == true) {
+					ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
+				} else{
+					pcur_wlan = rtw_find_network(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
+					if (pcur_wlan)
+						pcur_wlan->fixed = false;
+
+					pcur_sta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress);
+					if (pcur_sta)
+						rtw_free_stainfo(adapter,  pcur_sta);
+
+					ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
+					if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+						if (ptarget_wlan)
+							ptarget_wlan->fixed = true;
+					}
+				}
+
+			} else{
+				ptarget_wlan = _rtw_find_same_network(&pmlmepriv->scanned_queue, pnetwork);
+				if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+					if (ptarget_wlan)
+						ptarget_wlan->fixed = true;
+				}
+			}
+
+			/* s2. update cur_network */
+			if (ptarget_wlan) {
+				rtw_joinbss_update_network(adapter, ptarget_wlan, pnetwork);
+			} else{
+				DBG_871X_LEVEL(_drv_always_, "Can't find ptarget_wlan when joinbss_event callback\n");
+				spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+				goto ignore_joinbss_callback;
+			}
+
+
+			/* s3. find ptarget_sta & update ptarget_sta after update cur_network only for station mode */
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+				ptarget_sta = rtw_joinbss_update_stainfo(adapter, pnetwork);
+				if (ptarget_sta == NULL) {
+					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Can't update stainfo when joinbss_event callback\n"));
+					spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+					goto ignore_joinbss_callback;
+				}
+			}
+
+			/* s4. indicate connect */
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+				pmlmepriv->cur_network_scanned = ptarget_wlan;
+				rtw_indicate_connect(adapter);
+			} else{
+				/* adhoc mode will rtw_indicate_connect when rtw_stassoc_event_callback */
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("adhoc mode, fw_state:%x", get_fwstate(pmlmepriv)));
+			}
+
+
+			/* s5. Cancle assoc_timer */
+			_cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
+
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("Cancle assoc_timer\n"));
+
+		} else{
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_joinbss_event_callback err: fw_state:%x", get_fwstate(pmlmepriv)));
+			spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+			goto ignore_joinbss_callback;
+		}
+
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+
+	} else if (pnetwork->join_res == -4) {
+		rtw_reset_securitypriv(adapter);
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+
+		/* rtw_free_assoc_resources(adapter, 1); */
+
+		if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == true) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("fail! clear _FW_UNDER_LINKING ^^^fw_state =%x\n", get_fwstate(pmlmepriv)));
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+		}
+
+	} else{/* if join_res < 0 (join fails), then try again */
+
+		#ifdef REJOIN
+		res = _FAIL;
+		if (retry < 2) {
+			res = rtw_select_and_join_from_scanned_queue(pmlmepriv);
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_select_and_join_from_scanned_queue again! res:%d\n", res));
+		}
+
+		if (res == _SUCCESS) {
+			/* extend time of assoc_timer */
+			_set_timer(&pmlmepriv->assoc_timer, MAX_JOIN_TIMEOUT);
+			retry++;
+		} else if (res == 2) {/* there is no need to wait for join */
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+			rtw_indicate_connect(adapter);
+		} else{
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Set Assoc_Timer = 1; can't find match ssid in scanned_q\n"));
+		#endif
+
+			_set_timer(&pmlmepriv->assoc_timer, 1);
+			/* rtw_free_assoc_resources(adapter, 1); */
+			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+
+		#ifdef REJOIN
+			retry = 0;
+		}
+		#endif
+	}
+
+ignore_joinbss_callback:
+
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+void rtw_joinbss_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+	struct wlan_network	*pnetwork	= (struct wlan_network *)pbuf;
+
+	mlmeext_joinbss_event_callback(adapter, pnetwork->join_res);
+
+	rtw_os_xmit_schedule(adapter);
+}
+
+/* FOR STA, AP , AD-HOC mode */
+void rtw_sta_media_status_rpt(struct adapter *adapter, struct sta_info *psta, u32 mstatus)
+{
+	u16 media_status_rpt;
+
+	if (psta == NULL)
+		return;
+
+	media_status_rpt = (u16)((psta->mac_id<<8)|mstatus); /*   MACID|OPMODE:1 connect */
+	rtw_hal_set_hwreg(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status_rpt);
+}
+
+void rtw_stassoc_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+	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 (rtw_access_ctrl(adapter, pstassoc->macaddr) == false)
+		return;
+
+	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 */
+			DBG_871X("indicate_sta_assoc_event to upper layer - hostapd\n");
+			spin_lock_bh(&psta->lock);
+			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);
+
+					kfree(psta->passoc_req);
+					psta->passoc_req = NULL;
+					psta->assoc_req_len = 0;
+				}
+			}
+			spin_unlock_bh(&psta->lock);
+
+			if (passoc_req && assoc_req_len > 0) {
+				rtw_cfg80211_indicate_sta_assoc(adapter, passoc_req, assoc_req_len);
+
+				kfree(passoc_req);
+			}
+#endif /* CONFIG_AUTO_AP_MODE */
+		}
+		return;
+	}
+
+	/* for AD-HOC mode */
+	psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+	if (psta != NULL) {
+		/* the sta have been in sta_info_queue => do nothing */
+
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error: rtw_stassoc_event_callback: sta has been in sta_hash_queue\n"));
+
+		return; /* between drv has received this event before and  fw have not yet to set key to CAM_ENTRY) */
+	}
+
+	psta = rtw_alloc_stainfo(&adapter->stapriv, pstassoc->macaddr);
+	if (psta == NULL) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Can't alloc sta_info when rtw_stassoc_event_callback\n"));
+		return;
+	}
+
+	/* to do : init sta_info variable */
+	psta->qos_option = 0;
+	psta->mac_id = (uint)pstassoc->cam_id;
+	/* psta->aid = (uint)pstassoc->cam_id; */
+	DBG_871X("%s\n", __func__);
+	/* for ad-hoc mode */
+	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;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) {
+		if (adapter->stapriv.asoc_sta_count == 2) {
+			spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+			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;
+			spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+			/*  a sta + bc/mc_stainfo (not Ibss_stainfo) */
+			rtw_indicate_connect(adapter);
+		}
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+
+	mlmeext_sta_add_event_callback(adapter, psta);
+}
+
+void rtw_stadel_event_callback(struct adapter *adapter, u8 *pbuf)
+{
+	int mac_id = (-1);
+	struct sta_info *psta;
+	struct wlan_network *pwlan = NULL;
+	struct wlan_bssid_ex    *pdev_network = NULL;
+	u8 *pibss = NULL;
+	struct	mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct	stadel_event *pstadel	= (struct stadel_event *)pbuf;
+	struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	psta = rtw_get_stainfo(&adapter->stapriv, pstadel->macaddr);
+	if (psta)
+		mac_id = psta->mac_id;
+	else
+		mac_id = pstadel->mac_id;
+
+	DBG_871X("%s(mac_id =%d) =" MAC_FMT "\n", __func__, mac_id, MAC_ARG(pstadel->macaddr));
+
+	if (mac_id >= 0) {
+		u16 media_status;
+		media_status = (mac_id<<8)|0; /*   MACID|OPMODE:0 means disconnect */
+		/* for STA, AP, ADHOC mode, report disconnect stauts to FW */
+		rtw_hal_set_hwreg(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
+	}
+
+	/* if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) */
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+		return;
+
+
+	mlmeext_sta_del_event_callback(adapter);
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		u16 reason = *((unsigned short *)(pstadel->rsvd));
+		bool roam = false;
+		struct wlan_network *roam_target = NULL;
+
+		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 == true) {
+			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);
+		}
+
+		rtw_free_uc_swdec_pending_queue(adapter);
+
+		rtw_free_assoc_resources(adapter, 1);
+		rtw_indicate_disconnect(adapter);
+
+		spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+		/*  remove the network entry in scanned_queue */
+		pwlan = rtw_find_network(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+		if (pwlan) {
+			pwlan->fixed = false;
+			rtw_free_network_nolock(adapter, pwlan);
+		}
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+
+#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)) {
+
+		rtw_free_stainfo(adapter,  psta);
+
+		if (adapter->stapriv.asoc_sta_count == 1) {/* a sta + bc/mc_stainfo (not Ibss_stainfo) */
+			/* rtw_indicate_disconnect(adapter);removed@20091105 */
+			spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+			/* 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);
+			}
+			spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+			/* 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(struct ndis_802_11_ssid));
+			memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct ndis_802_11_ssid));
+
+			rtw_update_registrypriv_dev_network(adapter);
+
+			rtw_generate_random_ibss(pibss);
+
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+				set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
+				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE);
+			}
+
+			if (rtw_createbss_cmd(adapter) != _SUCCESS) {
+
+				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("***Error =>stadel_event_callback: rtw_createbss_cmd status FAIL***\n "));
+
+			}
+
+
+		}
+
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+void rtw_cpwm_event_callback(struct adapter *padapter, u8 *pbuf)
+{
+	struct reportpwrstate_parm *preportpwrstate;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_cpwm_event_callback !!!\n"));
+	preportpwrstate = (struct reportpwrstate_parm *)pbuf;
+	preportpwrstate->state |= (u8)(adapter_to_pwrctl(padapter)->cpwm_tog + 0x80);
+	cpwm_int_hdl(padapter, preportpwrstate);
+}
+
+
+void rtw_wmm_event_callback(struct adapter *padapter, u8 *pbuf)
+{
+	WMMOnAssocRsp(padapter);
+}
+
+/*
+* _rtw_join_timeout_handler - Timeout/faliure handler for CMD JoinBss
+* @adapter: pointer to struct adapter structure
+*/
+void _rtw_join_timeout_handler (struct adapter *adapter)
+{
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	DBG_871X("%s, fw_state =%x\n", __func__, get_fwstate(pmlmepriv));
+
+	if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
+		return;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	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;
+				DBG_871X("%s try another roaming\n", __func__);
+				do_join_r = rtw_do_join(adapter);
+				if (_SUCCESS != do_join_r) {
+					DBG_871X("%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);
+					DBG_871X("change to widi listen\n");
+				}
+#endif /*  CONFIG_INTEL_WIDI */
+				DBG_871X("%s We've try roaming but fail\n", __func__);
+				rtw_indicate_disconnect(adapter);
+				break;
+			}
+		}
+
+	} else{
+		rtw_indicate_disconnect(adapter);
+		free_scanqueue(pmlmepriv);/*  */
+
+		/* indicate disconnect for the case that join_timeout and check_fwstate != FW_LINKED */
+		rtw_cfg80211_indicate_disconnect(adapter);
+
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+/*
+* rtw_scan_timeout_handler - Timeout/Faliure handler for CMD SiteSurvey
+* @adapter: pointer to struct adapter structure
+*/
+void rtw_scan_timeout_handler (struct adapter *adapter)
+{
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	DBG_871X(FUNC_ADPT_FMT" fw_state =%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv));
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	rtw_indicate_scan_done(adapter, true);
+}
+
+void rtw_mlme_reset_auto_scan_int(struct adapter *adapter)
+{
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pmlmeinfo->VHT_enable) /* disable auto scan when connect to 11AC AP */
+		mlme->auto_scan_int_ms = 0;
+	else if (adapter->registrypriv.wifi_spec && is_client_associated_to_ap(adapter) == true)
+		mlme->auto_scan_int_ms = 60*1000;
+	else if (rtw_chk_roam_flags(adapter, RTW_ROAM_ACTIVE)) {
+		if (check_fwstate(mlme, WIFI_STATION_STATE) && check_fwstate(mlme, _FW_LINKED))
+			mlme->auto_scan_int_ms = mlme->roam_scan_int_ms;
+	} else
+		mlme->auto_scan_int_ms = 0; /* disabled */
+
+	return;
+}
+
+static void rtw_auto_scan_handler(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	rtw_mlme_reset_auto_scan_int(padapter);
+
+	if (pmlmepriv->auto_scan_int_ms != 0
+		&& jiffies_to_msecs(jiffies - pmlmepriv->scan_start_time) > pmlmepriv->auto_scan_int_ms) {
+
+		if (!padapter->registrypriv.wifi_spec) {
+			if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == true) {
+				DBG_871X(FUNC_ADPT_FMT" _FW_UNDER_SURVEY|_FW_UNDER_LINKING\n", FUNC_ADPT_ARG(padapter));
+				goto exit;
+			}
+
+			if (pmlmepriv->LinkDetectInfo.bBusyTraffic == true) {
+				DBG_871X(FUNC_ADPT_FMT" exit BusyTraffic\n", FUNC_ADPT_ARG(padapter));
+				goto exit;
+			}
+		}
+
+		DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+		rtw_set_802_11_bssid_list_scan(padapter, NULL, 0);
+	}
+
+exit:
+	return;
+}
+
+void rtw_dynamic_check_timer_handlder(struct adapter *adapter)
+{
+	if (!adapter)
+		return;
+
+	if (adapter->hw_init_completed == false)
+		return;
+
+	if ((adapter->bDriverStopped == true) || (adapter->bSurpriseRemoved == true))
+		return;
+
+	if (adapter->net_closed == true)
+		return;
+
+	if (is_primary_adapter(adapter))
+		DBG_871X("IsBtDisabled =%d, IsBtControlLps =%d\n", rtw_btcoex_IsBtDisabled(adapter), rtw_btcoex_IsBtControlLps(adapter));
+
+	if ((adapter_to_pwrctl(adapter)->bFwCurrentInPSMode == true)
+		&& (rtw_btcoex_IsBtControlLps(adapter) == false)
+		) {
+		u8 bEnterPS;
+
+		linked_status_chk(adapter);
+
+		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() */
+		}
+
+	} else{
+		if (is_primary_adapter(adapter)) {
+			rtw_dynamic_chk_wk_cmd(adapter);
+		}
+	}
+
+	/* auto site survey */
+	rtw_auto_scan_handler(adapter);
+}
+
+
+inline bool rtw_is_scan_deny(struct adapter *adapter)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+	return (atomic_read(&mlmepriv->set_scan_deny) != 0) ? true : false;
+}
+
+inline void rtw_clear_scan_deny(struct adapter *adapter)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+	atomic_set(&mlmepriv->set_scan_deny, 0);
+
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
+}
+
+void rtw_set_scan_deny_timer_hdl(struct adapter *adapter)
+{
+	rtw_clear_scan_deny(adapter);
+}
+
+void rtw_set_scan_deny(struct adapter *adapter, u32 ms)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
+	atomic_set(&mlmepriv->set_scan_deny, 1);
+	_set_timer(&mlmepriv->set_scan_deny_timer, ms);
+}
+
+/*
+* 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;
+	struct adapter *adapter = container_of(mlme, struct adapter, mlmepriv);
+
+	if (is_same_ess(&competitor->network, &mlme->cur_network.network) == false)
+		goto exit;
+
+	if (rtw_is_desired_network(adapter, competitor) == false)
+		goto exit;
+
+	DBG_871X("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,
+		jiffies_to_msecs(jiffies - 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 (jiffies_to_msecs(jiffies - 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)
+{
+	int ret = _FAIL;
+	struct list_head	*phead;
+	struct adapter *adapter;
+	struct __queue	*queue	= &(mlme->scanned_queue);
+	struct	wlan_network	*pnetwork = NULL;
+	struct	wlan_network	*candidate = NULL;
+
+	if (mlme->cur_network_scanned == NULL) {
+		rtw_warn_on(1);
+		return ret;
+	}
+
+	spin_lock_bh(&(mlme->scanned_queue.lock));
+	phead = get_list_head(queue);
+	adapter = (struct adapter *)mlme->nic_hdl;
+
+	mlme->pscanned = get_next(phead);
+
+	while (phead != mlme->pscanned) {
+
+		pnetwork = LIST_CONTAINOR(mlme->pscanned, struct wlan_network, list);
+		if (pnetwork == NULL) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s return _FAIL:(pnetwork == NULL)\n", __func__));
+			ret = _FAIL;
+			goto exit;
+		}
+
+		mlme->pscanned = get_next(mlme->pscanned);
+
+		DBG_871X("%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) {
+		DBG_871X("%s: return _FAIL(candidate == NULL)\n", __func__);
+		ret = _FAIL;
+		goto exit;
+	} else {
+		DBG_871X("%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:
+	spin_unlock_bh(&(mlme->scanned_queue.lock));
+
+	return ret;
+}
+
+/*
+* 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;
+	struct adapter *adapter = container_of(mlme, struct adapter, mlmepriv);
+
+
+	/* check bssid, if needed */
+	if (mlme->assoc_by_bssid == true) {
+		if (memcmp(competitor->network.MacAddress, mlme->assoc_bssid, ETH_ALEN))
+			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)
+		)
+			goto exit;
+	}
+
+	if (rtw_is_desired_network(adapter, competitor)  == false)
+		goto exit;
+
+	if (rtw_to_roam(adapter) > 0) {
+		if (jiffies_to_msecs(jiffies - competitor->last_scanned) >= mlme->roam_scanr_exp_ms
+			|| is_same_ess(&competitor->network, &mlme->cur_network.network) == false
+		)
+			goto exit;
+	}
+
+	if (*candidate == NULL || (*candidate)->network.Rssi < competitor->network.Rssi) {
+		*candidate = competitor;
+		updated = true;
+	}
+
+	if (updated) {
+		DBG_871X("[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)
+{
+	int ret;
+	struct list_head	*phead;
+	struct adapter *adapter;
+	struct __queue	*queue	= &(pmlmepriv->scanned_queue);
+	struct	wlan_network	*pnetwork = NULL;
+	struct	wlan_network	*candidate = NULL;
+
+	adapter = (struct adapter *)pmlmepriv->nic_hdl;
+
+	spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+
+	if (pmlmepriv->roam_network) {
+		candidate = pmlmepriv->roam_network;
+		pmlmepriv->roam_network = NULL;
+		goto candidate_exist;
+	}
+
+	phead = get_list_head(queue);
+	pmlmepriv->pscanned = get_next(phead);
+
+	while (phead != pmlmepriv->pscanned) {
+
+		pnetwork = LIST_CONTAINOR(pmlmepriv->pscanned, struct wlan_network, list);
+		if (pnetwork == NULL) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s return _FAIL:(pnetwork == NULL)\n", __func__));
+			ret = _FAIL;
+			goto exit;
+		}
+
+		pmlmepriv->pscanned = get_next(pmlmepriv->pscanned);
+
+		DBG_871X("%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) {
+		DBG_871X("%s: return _FAIL(candidate == NULL)\n", __func__);
+#ifdef CONFIG_WOWLAN
+		_clr_fwstate_(pmlmepriv, _FW_LINKED|_FW_UNDER_LINKING);
+#endif
+		ret = _FAIL;
+		goto exit;
+	} else {
+		DBG_871X("%s: candidate: %s("MAC_FMT", ch:%u)\n", __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) == true) {
+		DBG_871X("%s: _FW_LINKED while ask_for_joinbss!!!\n", __func__);
+
+		rtw_disassoc_cmd(adapter, 0, true);
+		rtw_indicate_disconnect(adapter);
+		rtw_free_assoc_resources(adapter, 0);
+	}
+
+	set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
+	ret = rtw_joinbss_cmd(adapter, candidate);
+
+exit:
+	spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+	return ret;
+}
+
+sint rtw_set_auth(struct 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) {
+		kfree((unsigned char *)pcmd);
+		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);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("after enqueue set_auth_cmd, auth_mode =%x\n", psecuritypriv->dot11AuthAlgrthm));
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+	return res;
+}
+
+sint rtw_set_key(struct 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);
+	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;
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key: psetkeyparm->algorithm =(unsigned char)psecuritypriv->dot118021XGrpPrivacy =%d\n", psetkeyparm->algorithm));
+	} else {
+		psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm;
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key: psetkeyparm->algorithm =(u8)psecuritypriv->dot11PrivacyAlgrthm =%d\n", psetkeyparm->algorithm));
+
+	}
+	psetkeyparm->keyid = (u8)keyid;/* 0~3 */
+	psetkeyparm->set_tx = set_tx;
+	if (is_wep_enc(psetkeyparm->algorithm))
+		adapter->securitypriv.key_mask |= BIT(psetkeyparm->keyid);
+
+	DBG_871X("==> rtw_set_key algorithm(%x), keyid(%x), key_mask(%x)\n", psetkeyparm->algorithm, psetkeyparm->keyid, adapter->securitypriv.key_mask);
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key: psetkeyparm->algorithm =%d psetkeyparm->keyid =(u8)keyid =%d\n", psetkeyparm->algorithm, keyid));
+
+	switch (psetkeyparm->algorithm) {
+
+	case _WEP40_:
+		keylen = 5;
+		memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen);
+		break;
+	case _WEP104_:
+		keylen = 13;
+		memcpy(&(psetkeyparm->key[0]), &(psecuritypriv->dot11DefKey[keyid].skey[0]), keylen);
+		break;
+	case _TKIP_:
+		keylen = 16;
+		memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen);
+		psetkeyparm->grpkey = 1;
+		break;
+	case _AES_:
+		keylen = 16;
+		memcpy(&psetkeyparm->key, &psecuritypriv->dot118021XGrpKey[keyid], keylen);
+		psetkeyparm->grpkey = 1;
+		break;
+	default:
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key:psecuritypriv->dot11PrivacyAlgrthm = %x (must be 1 or 2 or 4 or 5)\n", psecuritypriv->dot11PrivacyAlgrthm));
+		res = _FAIL;
+		kfree((unsigned char *)psetkeyparm);
+		goto exit;
+	}
+
+
+	if (enqueue) {
+		pcmd = (struct	cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
+		if (pcmd == NULL) {
+			kfree((unsigned char *)psetkeyparm);
+			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);
+		kfree((u8 *) psetkeyparm);
+	}
+exit:
+	return res;
+}
+
+/* adjust IEs for rtw_joinbss_cmd in WMM */
+int rtw_restruct_wmm_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len, uint initial_out_len)
+{
+	unsigned	int ielength = 0;
+	unsigned int i, j;
+
+	i = 12; /* after the fixed IE */
+	while (i < in_len) {
+		ielength = initial_out_len;
+
+		if (in_ie[i] == 0xDD && in_ie[i+2] == 0x00 && in_ie[i+3] == 0x50  && in_ie[i+4] == 0xF2 && in_ie[i+5] == 0x02 && i+5 < in_len) { /* WMM element ID and OUI */
+			for (j = i; j < i + 9; j++) {
+					out_ie[ielength] = in_ie[j];
+					ielength++;
+			}
+			out_ie[initial_out_len + 1] = 0x07;
+			out_ie[initial_out_len + 6] = 0x00;
+			out_ie[initial_out_len + 8] = 0x00;
+
+			break;
+		}
+
+		i += (in_ie[i+1]+2); /*  to the next IE element */
+	}
+
+	return ielength;
+
+}
+
+
+/*  */
+/*  Ported from 8185: IsInPreAuthKeyList(). (Renamed from SecIsInPreAuthKeyList(), 2006-10-13.) */
+/*  Added by Annie, 2006-05-07. */
+/*  */
+/*  Search by BSSID, */
+/*  Return Value: */
+/* 		-1		:if there is no pre-auth key in the  table */
+/* 		>= 0		:if there is pre-auth key, and   return the entry id */
+/*  */
+/*  */
+
+static int SecIsInPMKIDList(struct adapter *Adapter, u8 *bssid)
+{
+	struct security_priv *psecuritypriv = &Adapter->securitypriv;
+	int i = 0;
+
+	do {
+		if ((psecuritypriv->PMKIDList[i].bUsed) &&
+				(!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(struct adapter *Adapter, int iEntry, u8 *ie, uint ie_len)
+{
+	struct security_priv *psecuritypriv = &Adapter->securitypriv;
+
+	if (ie[13] <= 20) {
+		/*  The RSN IE didn't include the PMK ID, append the PMK information */
+			ie[ie_len] = 1;
+			ie_len++;
+			ie[ie_len] = 0;	/* PMKID count = 0x0100 */
+			ie_len++;
+			memcpy(&ie[ie_len], &psecuritypriv->PMKIDList[iEntry].PMKID, 16);
+
+			ie_len += 16;
+			ie[13] += 18;/* PMKID length = 2+16 */
+
+	}
+	return ie_len;
+}
+
+sint rtw_restruct_sec_ie(struct adapter *adapter, u8 *in_ie, u8 *out_ie, uint in_len)
+{
+	u8 authmode = 0x0;
+	uint	ielength;
+	int iEntry;
+
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct security_priv *psecuritypriv = &adapter->securitypriv;
+	uint	ndisauthmode = psecuritypriv->ndisauthtype;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+		 ("+rtw_restruct_sec_ie: ndisauthmode =%d ndissecuritytype =%d\n",
+		  ndisauthmode, ndissecuritytype));
+
+	/* copy fixed ie only */
+	memcpy(out_ie, in_ie, 12);
+	ielength = 12;
+	if ((ndisauthmode == Ndis802_11AuthModeWPA) || (ndisauthmode == Ndis802_11AuthModeWPAPSK))
+			authmode = _WPA_IE_ID_;
+	if ((ndisauthmode == Ndis802_11AuthModeWPA2) || (ndisauthmode == Ndis802_11AuthModeWPA2PSK))
+			authmode = _WPA2_IE_ID_;
+
+	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+		memcpy(out_ie+ielength, psecuritypriv->wps_ie, psecuritypriv->wps_ie_len);
+
+		ielength += psecuritypriv->wps_ie_len;
+	} else if ((authmode == _WPA_IE_ID_) || (authmode == _WPA2_IE_ID_)) {
+		/* copy RSN or SSN */
+		memcpy(&out_ie[ielength], &psecuritypriv->supplicant_ie[0], psecuritypriv->supplicant_ie[1]+2);
+		/* debug for CONFIG_IEEE80211W
+		{
+			int jj;
+			printk("supplicant_ie_length =%d &&&&&&&&&&&&&&&&&&&\n", psecuritypriv->supplicant_ie[1]+2);
+			for (jj = 0; jj < psecuritypriv->supplicant_ie[1]+2; jj++)
+				printk(" %02x ", psecuritypriv->supplicant_ie[jj]);
+			printk("\n");
+		}*/
+		ielength += psecuritypriv->supplicant_ie[1]+2;
+		rtw_report_sec_ie(adapter, authmode, psecuritypriv->supplicant_ie);
+	}
+
+	iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid);
+	if (iEntry < 0) {
+		return ielength;
+	} else{
+		if (authmode == _WPA2_IE_ID_)
+			ielength = rtw_append_pmkid(adapter, iEntry, out_ie, ielength);
+	}
+	return ielength;
+}
+
+void rtw_init_registrypriv_dev_network(struct adapter *adapter)
+{
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct eeprom_priv *peepriv = &adapter->eeprompriv;
+	struct wlan_bssid_ex    *pdev_network = &pregistrypriv->dev_network;
+	u8 *myhwaddr = myid(peepriv);
+
+	memcpy(pdev_network->MacAddress, myhwaddr, ETH_ALEN);
+
+	memcpy(&pdev_network->Ssid, &pregistrypriv->ssid, sizeof(struct ndis_802_11_ssid));
+
+	pdev_network->Configuration.Length = sizeof(struct ndis_802_11_conf);
+	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(struct adapter *adapter)
+{
+	int sz = 0;
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct wlan_bssid_ex    *pdev_network = &pregistrypriv->dev_network;
+	struct	security_priv *psecuritypriv = &adapter->securitypriv;
+	struct	wlan_network	*cur_network = &adapter->mlmepriv.cur_network;
+	/* struct	xmit_priv *pxmitpriv = &adapter->xmitpriv; */
+
+	pdev_network->Privacy = (psecuritypriv->dot11PrivacyAlgrthm > 0 ? 1 : 0) ; /*  adhoc no 802.1x */
+
+	pdev_network->Rssi = 0;
+
+	switch (pregistrypriv->wireless_mode) {
+	case WIRELESS_11B:
+		pdev_network->NetworkTypeInUse = (Ndis802_11DS);
+		break;
+	case WIRELESS_11G:
+	case WIRELESS_11BG:
+	case WIRELESS_11_24N:
+	case WIRELESS_11G_24N:
+	case WIRELESS_11BG_24N:
+		pdev_network->NetworkTypeInUse = (Ndis802_11OFDM24);
+		break;
+	case WIRELESS_11A:
+	case WIRELESS_11A_5N:
+		pdev_network->NetworkTypeInUse = (Ndis802_11OFDM5);
+		break;
+	case WIRELESS_11ABGN:
+		if (pregistrypriv->channel > 14)
+			pdev_network->NetworkTypeInUse = (Ndis802_11OFDM5);
+		else
+			pdev_network->NetworkTypeInUse = (Ndis802_11OFDM24);
+		break;
+	default:
+		/*  TODO */
+		break;
+	}
+
+	pdev_network->Configuration.DSConfig = (pregistrypriv->channel);
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("pregistrypriv->channel =%d, pdev_network->Configuration.DSConfig = 0x%x\n", pregistrypriv->channel, pdev_network->Configuration.DSConfig));
+
+	if (cur_network->network.InfrastructureMode == Ndis802_11IBSS)
+		pdev_network->Configuration.ATIMWindow = (0);
+
+	pdev_network->InfrastructureMode = (cur_network->network.InfrastructureMode);
+
+	/*  1. Supported rates */
+	/*  2. IE */
+
+	/* rtw_set_supported_rate(pdev_network->SupportedRates, pregistrypriv->wireless_mode) ;  will be called in rtw_generate_ie */
+	sz = rtw_generate_ie(pregistrypriv);
+
+	pdev_network->IELength = sz;
+
+	pdev_network->Length = get_wlan_bssid_ex_sz((struct wlan_bssid_ex  *)pdev_network);
+
+	/* notes: translate IELength & Length after assign the Length to cmdsz in createbss_cmd(); */
+	/* pdev_network->IELength = cpu_to_le32(sz); */
+}
+
+void rtw_get_encrypt_decrypt_from_registrypriv(struct adapter *adapter)
+{
+}
+
+/* the fucntion is at passive_level */
+void rtw_joinbss_reset(struct adapter *padapter)
+{
+	u8 threshold;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	struct ht_priv 	*phtpriv = &pmlmepriv->htpriv;
+
+	/* todo: if you want to do something io/reg/hw setting before join_bss, please add code here */
+
+	pmlmepriv->num_FortyMHzIntolerant = 0;
+
+	pmlmepriv->num_sta_no_ht = 0;
+
+	phtpriv->ampdu_enable = false;/* reset to disabled */
+
+	/*  TH = 1 => means that invalidate usb rx aggregation */
+	/*  TH = 0 => means that validate usb rx aggregation, use init value. */
+	if (phtpriv->ht_option) {
+		if (padapter->registrypriv.wifi_spec == 1)
+			threshold = 1;
+		else
+			threshold = 0;
+		rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+	} else{
+		threshold = 1;
+		rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+	}
+}
+
+void rtw_ht_use_default_setting(struct 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;
+	bool		bHwSupportBeamformer = false, bHwSupportBeamformee = false;
+
+	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)
+		DBG_871X("[HT] 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)
+		DBG_871X("[HT] Support STBC = 0x%02X\n", phtpriv->stbc_cap);
+
+	/*  Beamforming setting */
+	rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMER, (u8 *)&bHwSupportBeamformer);
+	rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMEE, (u8 *)&bHwSupportBeamformee);
+	CLEAR_FLAGS(phtpriv->beamform_cap);
+	if (TEST_FLAG(pregistrypriv->beamform_cap, BIT4) && bHwSupportBeamformer) {
+		SET_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE);
+		DBG_871X("[HT] Support Beamformer\n");
+	}
+	if (TEST_FLAG(pregistrypriv->beamform_cap, BIT5) && bHwSupportBeamformee) {
+		SET_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE);
+		DBG_871X("[HT] Support Beamformee\n");
+	}
+}
+
+void rtw_build_wmm_ie_ht(struct 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(struct adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len, u8 channel)
+{
+	u32 ielen, out_len;
+	enum HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor;
+	unsigned char *p, *pframe;
+	struct rtw_ieee80211_ht_cap ht_capie;
+	u8 cbw40_enable = 0, stbc_rx_enable = 0, rf_type = 0, operation_bw = 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;
+
+	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 (channel > 14) {
+		if ((pregistrypriv->bw_mode & 0xf0) > 0)
+			cbw40_enable = 1;
+	} else {
+		if ((pregistrypriv->bw_mode & 0x0f) > 0)
+			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);
+	}
+
+	if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX))
+		ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_TX_STBC);
+
+	/* todo: disable SM power save mode */
+	ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_SM_PS);
+
+	if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_RX)) {
+		if ((channel <= 14 && pregistrypriv->rx_stbc == 0x1) ||	/* enable for 2.4GHz */
+			(pregistrypriv->wifi_spec == 1)) {
+			stbc_rx_enable = 1;
+			DBG_871X("declare supporting RX STBC\n");
+		}
+	}
+
+	/* 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));
+
+	switch (rf_type) {
+	case RF_1T1R:
+		if (stbc_rx_enable)
+			ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_RX_STBC_1R);/* RX STBC One spatial stream */
+
+	    set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_1R);
+			break;
+
+	case RF_2T2R:
+	case RF_1T2R:
+	default:
+		if (stbc_rx_enable)
+			ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_RX_STBC_2R);/* RX STBC two spatial stream */
+
+		#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
+				set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R);
+		#else /* CONFIG_DISABLE_MCS13TO15 */
+			set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R);
+		#endif /* CONFIG_DISABLE_MCS13TO15 */
+		break;
+	}
+
+	{
+		u32 rx_packet_offset, max_recvbuf_sz;
+		rtw_hal_get_def_var(padapter, HAL_DEF_RX_PACKET_OFFSET, &rx_packet_offset);
+		rtw_hal_get_def_var(padapter, HAL_DEF_MAX_RECVBUF_SZ, &max_recvbuf_sz);
+	}
+
+	if (padapter->driver_rx_ampdu_factor != 0xFF)
+		max_rx_ampdu_factor =
+		  (enum 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->securitypriv.dot11PrivacyAlgrthm == _AES_)
+		ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&(0x07<<2));
+	else
+		ht_capie.ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_DENSITY&0x00);
+
+	pframe = rtw_set_ie(out_ie+out_len, _HT_CAPABILITY_IE_,
+						sizeof(struct rtw_ieee80211_ht_cap), (unsigned char *)&ht_capie, pout_len);
+
+	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(struct 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;
+
+	DBG_871X("+rtw_update_ht_cap()\n");
+
+	/* maybe needs check if ap supports rx ampdu. */
+	if ((phtpriv->ampdu_enable == false) && (pregistrypriv->ampdu_enable == 1)) {
+		if (pregistrypriv->wifi_spec == 1) {
+			/* remove this part because testbed AP should disable RX AMPDU */
+			/* phtpriv->ampdu_enable = false; */
+			phtpriv->ampdu_enable = true;
+		} else {
+			phtpriv->ampdu_enable = true;
+		}
+	} else if (pregistrypriv->ampdu_enable == 2) {
+		/* remove this part because testbed AP should disable RX AMPDU */
+		/* phtpriv->ampdu_enable = true; */
+	}
+
+
+	/* check Max Rx A-MPDU Size */
+	len = 0;
+	p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fix_ie), _HT_CAPABILITY_IE_, &len, ie_len-sizeof(struct ndis_802_11_fix_ie));
+	if (p && len > 0) {
+		pht_capie = (struct rtw_ieee80211_ht_cap *)(p+2);
+		max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_CAP_AMPDU_FACTOR);
+		max_ampdu_sz = 1 << (max_ampdu_sz+3); /*  max_ampdu_sz (kbytes); */
+
+		/* DBG_871X("rtw_update_ht_cap(): max_ampdu_sz =%d\n", max_ampdu_sz); */
+		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
+
+	}
+
+
+	len = 0;
+	p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fix_ie), _HT_ADD_INFO_IE_, &len, ie_len-sizeof(struct ndis_802_11_fix_ie));
+	if (p && len > 0) {
+		pht_addtinfo = (struct ieee80211_ht_addt_info *)(p+2);
+		/* todo: */
+	}
+
+	if (channel > 14) {
+		if ((pregistrypriv->bw_mode & 0xf0) > 0)
+			cbw40_enable = 1;
+	} else {
+		if ((pregistrypriv->bw_mode & 0x0f) > 0)
+			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))) {
+		int i;
+		u8 rf_type;
+
+		rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+		/* 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 (rf_type) {
+		case RF_1T1R:
+		case RF_1T2R:
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R);
+			break;
+		case RF_2T2R:
+		default:
+#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
+				set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R);
+#else /* CONFIG_DISABLE_MCS13TO15 */
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R);
+#endif /* CONFIG_DISABLE_MCS13TO15 */
+	}
+
+	/* 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)
+		DBG_871X("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__);
+
+	/*  */
+	/*  Config current HT Protection mode. */
+	/*  */
+	pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3;
+}
+
+void rtw_issue_addbareq_cmd(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	u8 issued;
+	int priority;
+	struct sta_info *psta = NULL;
+	struct ht_priv *phtpriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	s32 bmcst = IS_MCAST(pattrib->ra);
+
+	/* if (bmcst || (padapter->mlmepriv.LinkDetectInfo.bTxBusyTraffic == false)) */
+	if (bmcst || (padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod < 100))
+		return;
+
+	priority = pattrib->priority;
+
+	psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	if (pattrib->psta != psta) {
+		DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+		return;
+	}
+
+	if (psta == NULL) {
+		DBG_871X("%s, psta ==NUL\n", __func__);
+		return;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return;
+	}
+
+
+	phtpriv = &psta->htpriv;
+
+	if ((phtpriv->ht_option == true) && (phtpriv->ampdu_enable == true)) {
+		issued = (phtpriv->agg_enable_bitmap>>priority)&0x1;
+		issued |= (phtpriv->candidate_tid_bitmap>>priority)&0x1;
+
+		if (0 == issued) {
+			DBG_871X("rtw_issue_addbareq_cmd, p =%d\n", priority);
+			psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority);
+			rtw_addbareq_cmd(padapter, (u8) priority, pattrib->ra);
+		}
+	}
+
+}
+
+void rtw_append_exented_cap(struct 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;
+
+
+	if (phtpriv->bss_coexist) {
+		SET_EXT_CAPABILITY_ELE_BSS_COEXIST(cap_content, 1);
+	}
+
+	pframe = rtw_set_ie(out_ie + *pout_len, EID_EXTCapability, 8, cap_content, pout_len);
+}
+
+inline void rtw_set_to_roam(struct 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(struct adapter *adapter)
+{
+	adapter->mlmepriv.to_roam--;
+	return adapter->mlmepriv.to_roam;
+}
+
+inline u8 rtw_to_roam(struct adapter *adapter)
+{
+	return adapter->mlmepriv.to_roam;
+}
+
+void rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	spin_lock_bh(&pmlmepriv->lock);
+	_rtw_roaming(padapter, tgt_network);
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+void _rtw_roaming(struct 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)) {
+		DBG_871X("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(struct ndis_802_11_ssid));
+
+		pmlmepriv->assoc_by_bssid = false;
+
+		while (1) {
+			do_join_r = rtw_do_join(padapter);
+			if (_SUCCESS == do_join_r) {
+				break;
+			} else {
+				DBG_871X("roaming do_join return %d\n", do_join_r);
+				rtw_dec_to_roam(padapter);
+
+				if (rtw_to_roam(padapter) > 0) {
+					continue;
+				} else {
+					DBG_871X("%s(%d) -to roaming fail, indicate_disconnect\n", __func__, __LINE__);
+					rtw_indicate_disconnect(padapter);
+					break;
+				}
+			}
+		}
+	}
+
+}
+
+sint rtw_linked_check(struct adapter *padapter)
+{
+	if ((check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == true) ||
+			(check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE) == true)) {
+		if (padapter->stapriv.asoc_sta_count > 2)
+			return true;
+	} else{	/* Station mode */
+		if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) == true)
+			return true;
+	}
+	return false;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_mlme_ext.c linux/3rdparty/rtl8723bs/core/rtw_mlme_ext.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_mlme_ext.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_mlme_ext.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,6940 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#define _RTW_MLME_EXT_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtw_wifi_regd.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},
+};
+
+static struct action_handler OnAction_tbl[] = {
+	{RTW_WLAN_CATEGORY_SPECTRUM_MGMT,	 "ACTION_SPECTRUM_MGMT", on_action_spct},
+	{RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &DoReserved},
+	{RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &DoReserved},
+	{RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
+	{RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
+	{RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
+	{RTW_WLAN_CATEGORY_FT, "ACTION_FT",	&DoReserved},
+	{RTW_WLAN_CATEGORY_HT,	"ACTION_HT",	&OnAction_ht},
+	{RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &OnAction_sa_query},
+	{RTW_WLAN_CATEGORY_UNPROTECTED_WNM, "ACTION_UNPROTECTED_WNM", &DoReserved},
+	{RTW_WLAN_CATEGORY_SELF_PROTECTED, "ACTION_SELF_PROTECTED", &DoReserved},
+	{RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &DoReserved},
+	{RTW_WLAN_CATEGORY_VHT, "ACTION_VHT", &DoReserved},
+	{RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &DoReserved},
+};
+
+
+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};
+
+static unsigned char REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
+
+/********************************************************
+ChannelPlan definitions
+*********************************************************/
+static RT_CHANNEL_PLAN_2G	RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},		/*  0x00, RT_CHANNEL_DOMAIN_2G_WORLD , Passive scan CH 12, 13 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},		/*  0x01, RT_CHANNEL_DOMAIN_2G_ETSI1 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11},			/*  0x02, RT_CHANNEL_DOMAIN_2G_FCC1 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14},	/*  0x03, RT_CHANNEL_DOMAIN_2G_MIKK1 */
+	{{10, 11, 12, 13}, 4},						/*  0x04, RT_CHANNEL_DOMAIN_2G_ETSI2 */
+	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14},	/*  0x05, RT_CHANNEL_DOMAIN_2G_GLOBAL , Passive scan CH 12, 13, 14 */
+	{{}, 0},								/*  0x06, RT_CHANNEL_DOMAIN_2G_NULL */
+};
+
+static RT_CHANNEL_PLAN_5G	RTW_ChannelPlan5G[RT_CHANNEL_DOMAIN_5G_MAX] = {
+	{{}, 0},																					/*  0x00, RT_CHANNEL_DOMAIN_5G_NULL */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 19},						/*  0x01, RT_CHANNEL_DOMAIN_5G_ETSI1 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165}, 24},	/*  0x02, RT_CHANNEL_DOMAIN_5G_ETSI2 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 149, 153, 157, 161, 165}, 22},			/*  0x03, RT_CHANNEL_DOMAIN_5G_ETSI3 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165}, 24},	/*  0x04, RT_CHANNEL_DOMAIN_5G_FCC1 */
+	{{36, 40, 44, 48, 149, 153, 157, 161, 165}, 9},														/*  0x05, RT_CHANNEL_DOMAIN_5G_FCC2 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165}, 13},											/*  0x06, RT_CHANNEL_DOMAIN_5G_FCC3 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161}, 12},												/*  0x07, RT_CHANNEL_DOMAIN_5G_FCC4 */
+	{{149, 153, 157, 161, 165}, 5},																	/*  0x08, RT_CHANNEL_DOMAIN_5G_FCC5 */
+	{{36, 40, 44, 48, 52, 56, 60, 64}, 8},																/*  0x09, RT_CHANNEL_DOMAIN_5G_FCC6 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165}, 20},					/*  0x0A, RT_CHANNEL_DOMAIN_5G_FCC7_IC1 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 149, 153, 157, 161, 165}, 20},					/*  0x0B, RT_CHANNEL_DOMAIN_5G_KCC1 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 19},						/*  0x0C, RT_CHANNEL_DOMAIN_5G_MKK1 */
+	{{36, 40, 44, 48, 52, 56, 60, 64}, 8},																/*  0x0D, RT_CHANNEL_DOMAIN_5G_MKK2 */
+	{{100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 11},											/*  0x0E, RT_CHANNEL_DOMAIN_5G_MKK3 */
+	{{56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165}, 15},								/*  0x0F, RT_CHANNEL_DOMAIN_5G_NCC1 */
+	{{56, 60, 64, 149, 153, 157, 161, 165}, 8},															/*  0x10, RT_CHANNEL_DOMAIN_5G_NCC2 */
+	{{149, 153, 157, 161, 165}, 5},																	/*  0x11, RT_CHANNEL_DOMAIN_5G_NCC3 */
+	{{36, 40, 44, 48}, 4},																			/*  0x12, RT_CHANNEL_DOMAIN_5G_ETSI4 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165}, 20},					/*  0x13, RT_CHANNEL_DOMAIN_5G_ETSI5 */
+	{{149, 153, 157, 161}, 4},																		/*  0x14, RT_CHANNEL_DOMAIN_5G_FCC8 */
+	{{36, 40, 44, 48, 52, 56, 60, 64}, 8},																/*  0x15, RT_CHANNEL_DOMAIN_5G_ETSI6 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165}, 13},											/*  0x16, RT_CHANNEL_DOMAIN_5G_ETSI7 */
+	{{36, 40, 44, 48, 149, 153, 157, 161, 165}, 9},														/*  0x17, RT_CHANNEL_DOMAIN_5G_ETSI8 */
+	{{100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 11},											/*  0x18, RT_CHANNEL_DOMAIN_5G_ETSI9 */
+	{{149, 153, 157, 161, 165}, 5},																	/*  0x19, RT_CHANNEL_DOMAIN_5G_ETSI10 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 132, 136, 140, 149, 153, 157, 161, 165}, 16},									/*  0x1A, RT_CHANNEL_DOMAIN_5G_ETSI11 */
+	{{52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165}, 17},							/*  0x1B, RT_CHANNEL_DOMAIN_5G_NCC4 */
+	{{149, 153, 157, 161}, 4},																		/*  0x1C, RT_CHANNEL_DOMAIN_5G_ETSI12 */
+	{{36, 40, 44, 48, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165}, 17},							/*  0x1D, RT_CHANNEL_DOMAIN_5G_FCC9 */
+	{{36, 40, 44, 48, 100, 104, 108, 112, 116, 132, 136, 140}, 12},											/*  0x1E, RT_CHANNEL_DOMAIN_5G_ETSI13 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161}, 20},					/*  0x1F, RT_CHANNEL_DOMAIN_5G_FCC10 */
+
+	/*  Driver self defined for old channel plan Compatible , Remember to modify if have new channel plan definition ===== */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165}, 21},				/*  0x20, RT_CHANNEL_DOMAIN_5G_FCC */
+	{{36, 40, 44, 48}, 4},																			/*  0x21, RT_CHANNEL_DOMAIN_5G_JAPAN_NO_DFS */
+	{{36, 40, 44, 48, 149, 153, 157, 161}, 8},															/*  0x22, RT_CHANNEL_DOMAIN_5G_FCC4_NO_DFS */
+};
+
+static RT_CHANNEL_PLAN_MAP	RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
+	/*  0x00 ~ 0x1F , Old Define ===== */
+	{0x02, 0x20},	/* 0x00, RT_CHANNEL_DOMAIN_FCC */
+	{0x02, 0x0A},	/* 0x01, RT_CHANNEL_DOMAIN_IC */
+	{0x01, 0x01},	/* 0x02, RT_CHANNEL_DOMAIN_ETSI */
+	{0x01, 0x00},	/* 0x03, RT_CHANNEL_DOMAIN_SPAIN */
+	{0x01, 0x00},	/* 0x04, RT_CHANNEL_DOMAIN_FRANCE */
+	{0x03, 0x00},	/* 0x05, RT_CHANNEL_DOMAIN_MKK */
+	{0x03, 0x00},	/* 0x06, RT_CHANNEL_DOMAIN_MKK1 */
+	{0x01, 0x09},	/* 0x07, RT_CHANNEL_DOMAIN_ISRAEL */
+	{0x03, 0x09},	/* 0x08, RT_CHANNEL_DOMAIN_TELEC */
+	{0x03, 0x00},	/* 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN */
+	{0x00, 0x00},	/* 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13 */
+	{0x02, 0x0F},	/* 0x0B, RT_CHANNEL_DOMAIN_TAIWAN */
+	{0x01, 0x08},	/* 0x0C, RT_CHANNEL_DOMAIN_CHINA */
+	{0x02, 0x06},	/* 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO */
+	{0x02, 0x0B},	/* 0x0E, RT_CHANNEL_DOMAIN_KOREA */
+	{0x02, 0x09},	/* 0x0F, RT_CHANNEL_DOMAIN_TURKEY */
+	{0x01, 0x01},	/* 0x10, RT_CHANNEL_DOMAIN_JAPAN */
+	{0x02, 0x05},	/* 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS */
+	{0x01, 0x21},	/* 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+	{0x00, 0x04},	/* 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G */
+	{0x02, 0x10},	/* 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS */
+	{0x00, 0x21},	/* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */
+	{0x00, 0x22},	/* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */
+	{0x03, 0x21},	/* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+	{0x06, 0x08},	/* 0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS */
+	{0x02, 0x08},	/* 0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS */
+	{0x00, 0x00},	/* 0x1A, */
+	{0x00, 0x00},	/* 0x1B, */
+	{0x00, 0x00},	/* 0x1C, */
+	{0x00, 0x00},	/* 0x1D, */
+	{0x00, 0x00},	/* 0x1E, */
+	{0x06, 0x04},	/* 0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G */
+	/*  0x20 ~ 0x7F , New Define ===== */
+	{0x00, 0x00},	/* 0x20, RT_CHANNEL_DOMAIN_WORLD_NULL */
+	{0x01, 0x00},	/* 0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL */
+	{0x02, 0x00},	/* 0x22, RT_CHANNEL_DOMAIN_FCC1_NULL */
+	{0x03, 0x00},	/* 0x23, RT_CHANNEL_DOMAIN_MKK1_NULL */
+	{0x04, 0x00},	/* 0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL */
+	{0x02, 0x04},	/* 0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1 */
+	{0x00, 0x01},	/* 0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1 */
+	{0x03, 0x0C},	/* 0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1 */
+	{0x00, 0x0B},	/* 0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1 */
+	{0x00, 0x05},	/* 0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2 */
+	{0x00, 0x00},	/* 0x2A, */
+	{0x00, 0x00},	/* 0x2B, */
+	{0x00, 0x00},	/* 0x2C, */
+	{0x00, 0x00},	/* 0x2D, */
+	{0x00, 0x00},	/* 0x2E, */
+	{0x00, 0x00},	/* 0x2F, */
+	{0x00, 0x06},	/* 0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3 */
+	{0x00, 0x07},	/* 0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4 */
+	{0x00, 0x08},	/* 0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5 */
+	{0x00, 0x09},	/* 0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6 */
+	{0x02, 0x0A},	/* 0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7 */
+	{0x00, 0x02},	/* 0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2 */
+	{0x00, 0x03},	/* 0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3 */
+	{0x03, 0x0D},	/* 0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2 */
+	{0x03, 0x0E},	/* 0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3 */
+	{0x02, 0x0F},	/* 0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1 */
+	{0x00, 0x00},	/* 0x3A, */
+	{0x00, 0x00},	/* 0x3B, */
+	{0x00, 0x00},	/* 0x3C, */
+	{0x00, 0x00},	/* 0x3D, */
+	{0x00, 0x00},	/* 0x3E, */
+	{0x00, 0x00},	/* 0x3F, */
+	{0x02, 0x10},	/* 0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2 */
+	{0x05, 0x00},	/* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_NULL */
+	{0x01, 0x12},	/* 0x42, RT_CHANNEL_DOMAIN_ETSI1_ETSI4 */
+	{0x02, 0x05},	/* 0x43, RT_CHANNEL_DOMAIN_FCC1_FCC2 */
+	{0x02, 0x11},	/* 0x44, RT_CHANNEL_DOMAIN_FCC1_NCC3 */
+	{0x00, 0x13},	/* 0x45, RT_CHANNEL_DOMAIN_WORLD_ETSI5 */
+	{0x02, 0x14},	/* 0x46, RT_CHANNEL_DOMAIN_FCC1_FCC8 */
+	{0x00, 0x15},	/* 0x47, RT_CHANNEL_DOMAIN_WORLD_ETSI6 */
+	{0x00, 0x16},	/* 0x48, RT_CHANNEL_DOMAIN_WORLD_ETSI7 */
+	{0x00, 0x17},	/* 0x49, RT_CHANNEL_DOMAIN_WORLD_ETSI8 */
+	{0x00, 0x18},	/* 0x50, RT_CHANNEL_DOMAIN_WORLD_ETSI9 */
+	{0x00, 0x19},	/* 0x51, RT_CHANNEL_DOMAIN_WORLD_ETSI10 */
+	{0x00, 0x1A},	/* 0x52, RT_CHANNEL_DOMAIN_WORLD_ETSI11 */
+	{0x02, 0x1B},	/* 0x53, RT_CHANNEL_DOMAIN_FCC1_NCC4 */
+	{0x00, 0x1C},	/* 0x54, RT_CHANNEL_DOMAIN_WORLD_ETSI12 */
+	{0x02, 0x1D},	/* 0x55, RT_CHANNEL_DOMAIN_FCC1_FCC9 */
+	{0x00, 0x1E},	/* 0x56, RT_CHANNEL_DOMAIN_WORLD_ETSI13 */
+	{0x02, 0x1F},	/* 0x57, RT_CHANNEL_DOMAIN_FCC1_FCC10 */
+};
+
+static RT_CHANNEL_PLAN_MAP	RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03, 0x02}; /* use the conbination for max channel numbers */
+
+/*
+ * 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 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(struct adapter *adapter, const u32 ch)
+{
+	if (adapter->setband == GHZ24_50 /* 2.4G and 5G */
+		|| (adapter->setband == GHZ_24 && ch < 35) /* 2.4G only */
+		|| (adapter->setband == GHZ_50 && ch > 35) /* 5G only */
+	) {
+		return true;
+	}
+	return false;
+}
+
+/****************************************************************************
+
+Following are the initialization functions for WiFi MLME
+
+*****************************************************************************/
+
+int init_hw_mlme_ext(struct adapter *padapter)
+{
+	struct	mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	return _SUCCESS;
+}
+
+void init_mlme_default_rate_set(struct 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, 0x00, 0x00, 0x01, 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(struct 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 */
+	pmlmeext->sa_query_seq = 0;
+	pmlmeext->mgnt_80211w_IPN = 0;
+	pmlmeext->mgnt_80211w_IPN_rx = 0;
+	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;
+
+	pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+	pmlmeext->sitesurvey_res.channel_idx = 0;
+	pmlmeext->sitesurvey_res.bss_cnt = 0;
+	pmlmeext->scan_abort = false;
+
+	pmlmeinfo->state = WIFI_FW_NULL_STATE;
+	pmlmeinfo->reauth_count = 0;
+	pmlmeinfo->reassoc_count = 0;
+	pmlmeinfo->link_count = 0;
+	pmlmeinfo->auth_seq = 0;
+	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+	pmlmeinfo->key_index = 0;
+	pmlmeinfo->iv = 0;
+
+	pmlmeinfo->enc_algo = _NO_PRIVACY_;
+	pmlmeinfo->authModeToggle = 0;
+
+	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(struct adapter *padapter, RT_CHANNEL_INFO *channel_set,
+							  u8 chanset_size,
+							  struct p2p_channels *channel_list) {
+
+	struct p2p_oper_class_map op_class[] = {
+		{ IEEE80211G,  81,   1,  13,  1, BW20 },
+		{ IEEE80211G,  82,  14,  14,  1, BW20 },
+		{ IEEE80211A, 115,  36,  48,  4, BW20 },
+		{ IEEE80211A, 116,  36,  44,  8, BW40PLUS },
+		{ IEEE80211A, 117,  40,  48,  8, BW40MINUS },
+		{ IEEE80211A, 124, 149, 161,  4, BW20 },
+		{ IEEE80211A, 125, 149, 169,  4, BW20 },
+		{ IEEE80211A, 126, 149, 157,  8, BW40PLUS },
+		{ IEEE80211A, 127, 153, 161,  8, BW40MINUS },
+		{ -1, 0, 0, 0, 0, BW20 }
+	};
+
+	int cla, op;
+
+	cla = 0;
+
+	for (op = 0; op_class[op].op_class; op++) {
+		u8 ch;
+		struct p2p_oper_class_map *o = &op_class[op];
+		struct p2p_reg_class *reg = NULL;
+
+		for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
+			if (!has_channel(channel_set, chanset_size, ch)) {
+				continue;
+			}
+
+			if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
+				continue;
+
+			if ((0 < (padapter->registrypriv.bw_mode & 0xf0)) &&
+				((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
+				continue;
+
+			if (reg == NULL) {
+				reg = &channel_list->reg_class[cla];
+				cla++;
+				reg->reg_class = o->op_class;
+				reg->channels = 0;
+			}
+			reg->channel[reg->channels] = ch;
+			reg->channels++;
+		}
+	}
+	channel_list->reg_classes = cla;
+
+}
+
+static u8 init_channel_set(struct adapter *padapter, u8 ChannelPlan, RT_CHANNEL_INFO *channel_set)
+{
+	u8 index, chanset_size = 0;
+	u8 b5GBand = false, b2_4GBand = false;
+	u8 Index2G = 0, Index5G = 0;
+
+	memset(channel_set, 0, sizeof(RT_CHANNEL_INFO)*MAX_CHANNEL_NUM);
+
+	if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
+		DBG_871X("ChannelPlan ID %x error !!!!!\n", ChannelPlan);
+		return chanset_size;
+	}
+
+	if (IsSupported24G(padapter->registrypriv.wireless_mode)) {
+		b2_4GBand = true;
+		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
+			Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
+		else
+			Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
+	}
+
+	if (b2_4GBand) {
+		for (index = 0; index < RTW_ChannelPlan2G[Index2G].Len; index++) {
+			channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
+
+			if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||/* Channel 1~11 is active, and 12~14 is passive */
+				(RT_CHANNEL_DOMAIN_GLOBAL_NULL == ChannelPlan)) {
+				if (channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else if ((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
+					channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
+			} else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
+				RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan ||
+				RT_CHANNEL_DOMAIN_2G_WORLD == Index2G) { /*  channel 12~13, passive scan */
+				if (channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+			} else
+				channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+
+			chanset_size++;
+		}
+	}
+
+	if (b5GBand) {
+		for (index = 0; index < RTW_ChannelPlan5G[Index5G].Len; index++) {
+			if (RTW_ChannelPlan5G[Index5G].Channel[index] <= 48
+				|| RTW_ChannelPlan5G[Index5G].Channel[index] >= 149) {
+				channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
+				if (RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)/* passive scan for all 5G channels */
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				DBG_871X("%s(): channel_set[%d].ChannelNum = %d\n", __func__, chanset_size, channel_set[chanset_size].ChannelNum);
+				chanset_size++;
+			}
+		}
+	}
+
+	DBG_871X("%s ChannelPlan ID %x Chan num:%d \n", __func__, ChannelPlan, chanset_size);
+	return chanset_size;
+}
+
+int	init_mlme_ext_priv(struct adapter *padapter)
+{
+	int	res = _SUCCESS;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */
+	/* 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);
+
+	init_mlme_ap_info(padapter);
+
+	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->chan_scan_time = SURVEY_TO;
+	pmlmeext->mlmeext_init = true;
+	pmlmeext->active_keep_alive_check = true;
+
+#ifdef DBG_FIXED_CHAN
+	pmlmeext->fixed_chan = 0xFF;
+#endif
+
+	return res;
+
+}
+
+void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
+{
+	struct adapter *padapter = pmlmeext->padapter;
+
+	if (!padapter)
+		return;
+
+	if (padapter->bDriverStopped == true) {
+		del_timer_sync(&pmlmeext->survey_timer);
+		del_timer_sync(&pmlmeext->link_timer);
+		/* del_timer_sync(&pmlmeext->ADDBA_timer); */
+	}
+}
+
+static void _mgt_dispatcher(struct adapter *padapter, struct mlme_handler *ptable, union recv_frame *precv_frame)
+{
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+
+	  if (ptable->func) {
+		 /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
+			if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
+				memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
+				return;
+
+			ptable->func(padapter, precv_frame);
+		}
+
+}
+
+void mgt_dispatcher(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	int index;
+	struct mlme_handler *ptable;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+		 ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
+		  GetFrameType(pframe), GetFrameSubType(pframe)));
+
+	if (GetFrameType(pframe) != WIFI_MGT_TYPE) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
+		return;
+	}
+
+	/* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
+	if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
+		memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN)) {
+		return;
+	}
+
+	ptable = mlme_sta_tbl;
+
+	index = GetFrameSubType(pframe) >> 4;
+
+	if (index >= (sizeof(mlme_sta_tbl) / sizeof(struct mlme_handler))) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Currently we do not support reserved sub-fr-type =%d\n", index));
+		return;
+	}
+	ptable += index;
+
+	if (psta != NULL) {
+		if (GetRetry(pframe)) {
+			if (precv_frame->u.hdr.attrib.seq_num == psta->RxMgmtFrameSeqNum) {
+				/* drop the duplicate management frame */
+				pdbgpriv->dbg_rx_dup_mgt_frame_drop_count++;
+				DBG_871X("Drop duplicate management frame with seq_num = %d.\n", precv_frame->u.hdr.attrib.seq_num);
+				return;
+			}
+		}
+		psta->RxMgmtFrameSeqNum = precv_frame->u.hdr.attrib.seq_num;
+	}
+
+	switch (GetFrameSubType(pframe)) {
+	case WIFI_AUTH:
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+			ptable->func = &OnAuth;
+		else
+			ptable->func = &OnAuthClient;
+		/* pass through */
+	case WIFI_ASSOCREQ:
+	case WIFI_REASSOCREQ:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	case WIFI_PROBEREQ:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	case WIFI_BEACON:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	case WIFI_ACTION:
+		/* if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) */
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	default:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	}
+}
+
+/****************************************************************************
+
+Following are the callback functions for each subtype of the management frames
+
+*****************************************************************************/
+
+unsigned int OnProbeReq(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int	ielen;
+	unsigned char *p;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex	*cur = &(pmlmeinfo->network);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	u8 is_valid_p2p_probereq = false;
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		return _SUCCESS;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == false &&
+		check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE) == false) {
+		return _SUCCESS;
+	}
+
+
+	/* DBG_871X("+OnProbeReq\n"); */
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true &&
+			pmlmepriv->cur_network.join_res == true) {
+		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)) {
+			DBG_871X("%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;
+		}
+
+		DBG_871X("%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 */
+			DBG_871X("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: */
+				DBG_871X(" Exceed the upper limit of supported clients...\n");
+				return _SUCCESS;
+			}
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			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++;
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+			/* generate pairing ID */
+			mac_addr = myid(&(padapter->eeprompriv));
+			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;
+			/* psta->aid = 0; */
+			/* psta->mac_id = 2; */
+
+			/* get a unique AID */
+			if (psta->aid > 0) {
+				DBG_871X("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;
+					DBG_871X("no room for more AIDs\n");
+					return _SUCCESS;
+				} else {
+					pstapriv->sta_aid[psta->aid - 1] = psta;
+					DBG_871X("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));
+
+			spin_lock_bh(&psta->lock);
+			psta->state |= _FW_LINKED;
+			spin_unlock_bh(&psta->lock);
+
+			report_add_sta_event(padapter, psta->hwaddr, psta->aid);
+
+		}
+
+		issue_probersp(padapter, get_sa(pframe), false);
+
+		return _SUCCESS;
+
+	}
+
+_non_rc_device:
+
+	return _SUCCESS;
+
+#endif /* CONFIG_AUTO_AP_MODE */
+
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
+			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+
+	/* check (wildcard) SSID */
+	if (p != NULL) {
+		if (is_valid_p2p_probereq == true)
+			goto _issue_probersp;
+
+		if ((ielen != 0 && false == !memcmp((void *)(p+2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength))
+			|| (ielen == 0 && pmlmeinfo->hidden_ssid_mode)
+		)
+			return _SUCCESS;
+
+_issue_probersp:
+		if (((check_fwstate(pmlmepriv, _FW_LINKED) == true &&
+			pmlmepriv->cur_network.join_res == true)) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+			/* DBG_871X("+issue_probersp during ap mode\n"); */
+			issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
+		}
+
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnProbeRsp(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		report_survey_event(padapter, precv_frame);
+		return _SUCCESS;
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnBeacon(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	int cam_idx;
+	struct sta_info *psta;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint len = precv_frame->u.hdr.len;
+	struct wlan_bssid_ex *pbss;
+	int ret = _SUCCESS;
+	u8 *p = NULL;
+	u32 ielen = 0;
+
+	p = rtw_get_ie(pframe + sizeof(struct ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ielen, precv_frame->u.hdr.len - sizeof(struct ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_);
+	if ((p != NULL) && (ielen > 0)) {
+		if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D)) {
+			/* Invalid value 0x2D is detected in Extended Supported Rates (ESR) IE. Try to fix the IE length to avoid failed Beacon parsing. */
+			DBG_871X("[WIFIDBG] Error in ESR IE is detected in Beacon of BSSID:"MAC_FMT". Fix the length of ESR IE to avoid failed Beacon parsing.\n", MAC_ARG(GetAddr3Ptr(pframe)));
+			*(p + 1) = ielen - 1;
+		}
+	}
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		report_survey_event(padapter, precv_frame);
+		return _SUCCESS;
+	}
+
+	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 = (struct wlan_bssid_ex *)rtw_malloc(sizeof(struct wlan_bssid_ex));
+			if (pbss) {
+				if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
+					update_network(&(pmlmepriv->cur_network.network), pbss, padapter, true);
+					rtw_get_bcn_info(&(pmlmepriv->cur_network));
+				}
+				kfree((u8 *)pbss);
+			}
+
+			/* check the vendor of the assoc AP */
+			pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct ieee80211_hdr_3addr), len-sizeof(struct 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));
+
+			/* start auth */
+			start_clnt_auth(padapter);
+
+			return _SUCCESS;
+		}
+
+		if (((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+			if (psta != NULL) {
+				ret = rtw_check_bcn_info(padapter, pframe, len);
+				if (!ret) {
+						DBG_871X_LEVEL(_drv_always_, "ap has changed, disconnect now\n ");
+						receive_disconnect(padapter, pmlmeinfo->network.MacAddress, 0);
+						return _SUCCESS;
+				}
+				/* update WMM, ERP in the beacon */
+				/* todo: the timer is used instead of the number of the beacon received */
+				if ((sta_rx_pkts(psta) & 0xf) == 0)
+					/* DBG_871X("update_bcn_info\n"); */
+					update_beacon_info(padapter, pframe, len, psta);
+
+				adaptive_early_32k(pmlmeext, pframe, len);
+			}
+		} else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+			if (psta != NULL) {
+				/* update WMM, ERP in the beacon */
+				/* todo: the timer is used instead of the number of the beacon received */
+				if ((sta_rx_pkts(psta) & 0xf) == 0) {
+					/* DBG_871X("update_bcn_info\n"); */
+					update_beacon_info(padapter, pframe, len, psta);
+				}
+			} else{
+				/* allocate a new CAM entry for IBSS station */
+				cam_idx = allocate_fw_sta_entry(padapter);
+				if (cam_idx == NUM_STA)
+					goto _END_ONBEACON_;
+
+				/* get supported rate */
+				if (update_sta_support_rate(padapter, (pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_), (len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_), cam_idx) == _FAIL) {
+					pmlmeinfo->FW_sta_info[cam_idx].status = 0;
+					goto _END_ONBEACON_;
+				}
+
+				/* update TSF Value */
+				update_TSF(pmlmeext, pframe, len);
+
+				/* report sta add event */
+				report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
+			}
+		}
+	}
+
+_END_ONBEACON_:
+
+	return _SUCCESS;
+
+}
+
+unsigned int OnAuth(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	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;
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	DBG_871X("+OnAuth\n");
+
+	sa = GetAddr2Ptr(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));
+
+	DBG_871X("auth alg =%x, seq =%X\n", algorithm, seq);
+
+	if (auth_mode == 2 &&
+			psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
+			psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
+		auth_mode = 0;
+
+	if ((algorithm > 0 && auth_mode == 0) ||	/*  rx a shared-key auth but shared not enabled */
+		(algorithm == 0 && auth_mode == 1)) {	/*  rx a open-system auth but shared-key is enabled */
+		DBG_871X("auth rejected due to bad alg [alg =%d, auth_mib =%d] %02X%02X%02X%02X%02X%02X\n",
+			algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
+
+		status = _STATS_NO_SUPP_ALG_;
+
+		goto auth_fail;
+	}
+
+	if (rtw_access_ctrl(padapter, sa) == false) {
+		status = _STATS_UNABLE_HANDLE_STA_;
+		goto auth_fail;
+	}
+
+	pstat = rtw_get_stainfo(pstapriv, sa);
+	if (pstat == NULL) {
+
+		/*  allocate a new one */
+		DBG_871X("going to alloc stainfo for sa ="MAC_FMT"\n",  MAC_ARG(sa));
+		pstat = rtw_alloc_stainfo(pstapriv, sa);
+		if (pstat == NULL) {
+			DBG_871X(" Exceed the upper limit of supported clients...\n");
+			status = _STATS_UNABLE_HANDLE_STA_;
+			goto auth_fail;
+		}
+
+		pstat->state = WIFI_FW_AUTH_NULL;
+		pstat->auth_seq = 0;
+
+		/* pstat->flags = 0; */
+		/* pstat->capability = 0; */
+	} else{
+
+		spin_lock_bh(&pstapriv->asoc_list_lock);
+		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 */
+			}
+		}
+		spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+		if (seq == 1) {
+			/* TODO: STA re_auth and auth timeout */
+		}
+	}
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+	if (list_empty(&pstat->auth_list)) {
+
+		list_add_tail(&pstat->auth_list, &pstapriv->auth_list);
+		pstapriv->auth_list_cnt++;
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	if (pstat->auth_seq == 0)
+		pstat->expire_to = pstapriv->auth_to;
+
+
+	if ((pstat->auth_seq + 1) != seq) {
+		DBG_871X("(1)auth rejected because out of seq [rx_seq =%d, exp_seq =%d]!\n",
+			seq, pstat->auth_seq+1);
+		status = _STATS_OUT_OF_AUTH_SEQ_;
+		goto auth_fail;
+	}
+
+	if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2 || auth_mode == 3)) {
+		if (seq == 1) {
+			pstat->state &= ~WIFI_FW_AUTH_NULL;
+			pstat->state |= WIFI_FW_AUTH_SUCCESS;
+			pstat->expire_to = pstapriv->assoc_to;
+			pstat->authalg = algorithm;
+		} else{
+			DBG_871X("(2)auth rejected because out of seq [rx_seq =%d, exp_seq =%d]!\n",
+				seq, pstat->auth_seq+1);
+			status = _STATS_OUT_OF_AUTH_SEQ_;
+			goto auth_fail;
+		}
+	} else{ /*  shared system or auto authentication */
+		if (seq == 1) {
+			/* prepare for the challenging txt... */
+			memset((void *)pstat->chg_txt, 78, 128);
+
+			pstat->state &= ~WIFI_FW_AUTH_NULL;
+			pstat->state |= WIFI_FW_AUTH_STATE;
+			pstat->authalg = algorithm;
+			pstat->auth_seq = 2;
+		} else if (seq == 3) {
+			/* checking for challenging txt... */
+			DBG_871X("checking for challenging txt...\n");
+
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&ie_len,
+					len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
+
+			if ((p == NULL) || (ie_len <= 0)) {
+				DBG_871X("auth rejected because challenge failure!(1)\n");
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+
+			if (!memcmp((void *)(p + 2), pstat->chg_txt, 128)) {
+				pstat->state &= (~WIFI_FW_AUTH_STATE);
+				pstat->state |= WIFI_FW_AUTH_SUCCESS;
+				/*  challenging txt is correct... */
+				pstat->expire_to =  pstapriv->assoc_to;
+			} else{
+				DBG_871X("auth rejected because challenge failure!\n");
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+		} else{
+			DBG_871X("(3)auth rejected because out of seq [rx_seq =%d, exp_seq =%d]!\n",
+				seq, pstat->auth_seq+1);
+			status = _STATS_OUT_OF_AUTH_SEQ_;
+			goto auth_fail;
+		}
+	}
+
+
+	/*  Now, we are going to issue_auth... */
+	pstat->auth_seq = seq + 1;
+
+	issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
+
+	if (pstat->state & WIFI_FW_AUTH_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);
+
+	issue_auth(padapter, pstat, (unsigned short)status);
+
+	return _FAIL;
+
+}
+
+unsigned int OnAuthClient(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int	seq, len, status, algthm, offset;
+	unsigned char *p;
+	unsigned int	go2asoc = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+
+	DBG_871X("%s\n", __func__);
+
+	/* check A1 matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
+		return _SUCCESS;
+
+	offset = (GetPrivacy(pframe)) ? 4 : 0;
+
+	algthm	= le16_to_cpu(*(__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) {
+		DBG_871X("clnt auth fail, status: %d\n", status);
+		if (status == 13) { /*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
+			if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+			else
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
+			/* pmlmeinfo->reauth_count = 0; */
+		}
+
+		set_link_timer(pmlmeext, 1);
+		goto authclnt_fail;
+	}
+
+	if (seq == 2) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
+			 /*  legendary shared system */
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
+				pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
+
+			if (p == NULL) {
+				/* DBG_871X("marc: no challenge text?\n"); */
+				goto authclnt_fail;
+			}
+
+			memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
+			pmlmeinfo->auth_seq = 3;
+			issue_auth(padapter, NULL, 0);
+			set_link_timer(pmlmeext, REAUTH_TO);
+
+			return _SUCCESS;
+		} else{
+			/*  open system */
+			go2asoc = 1;
+		}
+	} else if (seq == 4) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
+			go2asoc = 1;
+		} else{
+			goto authclnt_fail;
+		}
+	} else{
+		/*  this is also illegal */
+		/* DBG_871X("marc: clnt auth failed due to illegal seq =%x\n", seq); */
+		goto authclnt_fail;
+	}
+
+	if (go2asoc) {
+		DBG_871X_LEVEL(_drv_always_, "auth success, start assoc\n");
+		start_clnt_assoc(padapter);
+		return _SUCCESS;
+	}
+
+authclnt_fail:
+
+	/* pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE); */
+
+	return _FAIL;
+
+}
+
+unsigned int OnAssocReq(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	u16 capab_info, listen_interval;
+	struct rtw_ieee802_11_elems elems;
+	struct sta_info *pstat;
+	unsigned char 	reassoc, *p, *pos, *wpa_ie;
+	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+	int		i, ie_len, wpa_ie_len, left;
+	unsigned char 	supportRate[16];
+	int					supportRateNum;
+	unsigned short		status = _STATS_SUCCESSFUL_;
+	unsigned short		frame_type, ie_offset = 0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex	*cur = &(pmlmeinfo->network);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	frame_type = GetFrameSubType(pframe);
+	if (frame_type == WIFI_ASSOCREQ) {
+		reassoc = 0;
+		ie_offset = _ASOCREQ_IE_OFFSET_;
+	} else{ /*  WIFI_REASSOCREQ */
+		reassoc = 1;
+		ie_offset = _REASOCREQ_IE_OFFSET_;
+	}
+
+
+	if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
+		DBG_871X("handle_assoc(reassoc =%d) - too short payload (len =%lu)"
+		       "\n", reassoc, (unsigned long)pkt_len);
+		return _FAIL;
+	}
+
+	pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+	if (pstat == (struct sta_info *)NULL) {
+		status = _RSON_CLS2_;
+		goto asoc_class2_error;
+	}
+
+	capab_info = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN);
+	/* capab_info = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN)); */
+	/* listen_interval = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN+2)); */
+	listen_interval = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN+2);
+
+	left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
+	pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
+
+
+	DBG_871X("%s\n", __func__);
+
+	/*  check if this stat has been successfully authenticated/assocated */
+	if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) {
+		if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) {
+			status = _RSON_CLS2_;
+			goto asoc_class2_error;
+		} else{
+			pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
+			pstat->state |= WIFI_FW_ASSOC_STATE;
+		}
+	} else{
+		pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
+		pstat->state |= WIFI_FW_ASSOC_STATE;
+	}
+
+
+	pstat->capability = capab_info;
+
+	/* now parse all ieee802_11 ie to point to elems */
+	if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
+	    !elems.ssid) {
+		DBG_871X("STA " MAC_FMT " sent invalid association request\n",
+		       MAC_ARG(pstat->hwaddr));
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+
+
+	/*  now we should check all the fields... */
+	/*  checking SSID */
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
+		pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+	if (p == NULL) {
+		status = _STATS_FAILURE_;
+	}
+
+	if (ie_len == 0) /*  broadcast ssid, however it is not allowed in assocreq */
+		status = _STATS_FAILURE_;
+	else {
+		/*  check if ssid match */
+		if (memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
+			status = _STATS_FAILURE_;
+
+		if (ie_len != cur->Ssid.SsidLength)
+			status = _STATS_FAILURE_;
+	}
+
+	if (_STATS_SUCCESSFUL_ != status)
+		goto OnAssocReqFail;
+
+	/*  check if the supported rate is ok */
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+	if (p == NULL) {
+		DBG_871X("Rx a sta assoc-req which supported rate is empty!\n");
+		/*  use our own rate set as statoin used */
+		/* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
+		/* supportRateNum = AP_BSSRATE_LEN; */
+
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	} else {
+		memcpy(supportRate, p+2, ie_len);
+		supportRateNum = ie_len;
+
+		p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_, &ie_len,
+				pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+		if (p !=  NULL) {
+
+			if (supportRateNum <= sizeof(supportRate)) {
+				memcpy(supportRate+supportRateNum, p+2, ie_len);
+				supportRateNum += ie_len;
+			}
+		}
+	}
+
+	/* todo: mask supportRate between AP & STA -> move to update raid */
+	/* get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0); */
+
+	/* update station supportRate */
+	pstat->bssratelen = supportRateNum;
+	memcpy(pstat->bssrateset, supportRate, supportRateNum);
+	UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
+
+	/* check RSN/WPA/WPS */
+	pstat->dot8021xalg = 0;
+	pstat->wpa_psk = 0;
+	pstat->wpa_group_cipher = 0;
+	pstat->wpa2_group_cipher = 0;
+	pstat->wpa_pairwise_cipher = 0;
+	pstat->wpa2_pairwise_cipher = 0;
+	memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
+	if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
+
+		int group_cipher = 0, pairwise_cipher = 0;
+
+		wpa_ie = elems.rsn_ie;
+		wpa_ie_len = elems.rsn_ie_len;
+
+		if (rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+			pstat->wpa_psk |= BIT(1);
+
+			pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
+			pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
+
+			if (!pstat->wpa2_group_cipher)
+				status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
+
+			if (!pstat->wpa2_pairwise_cipher)
+				status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
+		} else{
+			status = WLAN_STATUS_INVALID_IE;
+		}
+
+	} else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
+
+		int group_cipher = 0, pairwise_cipher = 0;
+
+		wpa_ie = elems.wpa_ie;
+		wpa_ie_len = elems.wpa_ie_len;
+
+		if (rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+			pstat->wpa_psk |= BIT(0);
+
+			pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
+			pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
+
+			if (!pstat->wpa_group_cipher)
+				status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
+
+			if (!pstat->wpa_pairwise_cipher)
+				status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
+
+		} else{
+			status = WLAN_STATUS_INVALID_IE;
+		}
+
+	} else {
+		wpa_ie = NULL;
+		wpa_ie_len = 0;
+	}
+
+	if (_STATS_SUCCESSFUL_ != status)
+		goto OnAssocReqFail;
+
+	pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
+	if (wpa_ie == NULL) {
+		if (elems.wps_ie) {
+			DBG_871X("STA included WPS IE in "
+				   "(Re)Association Request - assume WPS is "
+				   "used\n");
+			pstat->flags |= WLAN_STA_WPS;
+			/* wpabuf_free(sta->wps_ie); */
+			/* sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4, */
+			/* 				elems.wps_ie_len - 4); */
+		} else {
+			DBG_871X("STA did not include WPA/RSN IE "
+				   "in (Re)Association Request - possible WPS "
+				   "use\n");
+			pstat->flags |= WLAN_STA_MAYBE_WPS;
+		}
+
+
+		/*  AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
+		/*  that the selected registrar of AP is _FLASE */
+		if ((psecuritypriv->wpa_psk > 0)
+			&& (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS))) {
+			if (pmlmepriv->wps_beacon_ie) {
+				u8 selected_registrar = 0;
+
+				rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR, &selected_registrar, NULL);
+
+				if (!selected_registrar) {
+					DBG_871X("selected_registrar is false , or AP is not ready to do WPS\n");
+
+					status = _STATS_UNABLE_HANDLE_STA_;
+
+					goto OnAssocReqFail;
+				}
+			}
+		}
+
+	} else{
+		int copy_len;
+
+		if (psecuritypriv->wpa_psk == 0) {
+			DBG_871X("STA " MAC_FMT ": WPA/RSN IE in association "
+			"request, but AP don't support WPA/RSN\n", MAC_ARG(pstat->hwaddr));
+
+			status = WLAN_STATUS_INVALID_IE;
+
+			goto OnAssocReqFail;
+
+		}
+
+		if (elems.wps_ie) {
+			DBG_871X("STA included WPS IE in "
+				   "(Re)Association Request - WPS is "
+				   "used\n");
+			pstat->flags |= WLAN_STA_WPS;
+			copy_len = 0;
+		} else{
+			copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)):(wpa_ie_len+2);
+		}
+
+
+		if (copy_len > 0)
+			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;
+		}
+	}
+
+	/* 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;
+
+
+	if ((pmlmepriv->htpriv.ht_option == false) && (pstat->flags&WLAN_STA_HT)) {
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+
+
+	if ((pstat->flags & WLAN_STA_HT) &&
+		    ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
+		      (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP))) {
+		DBG_871X("HT: " MAC_FMT " tried to "
+				   "use TKIP with HT association\n", MAC_ARG(pstat->hwaddr));
+
+		/* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
+		/* goto OnAssocReqFail; */
+	}
+	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;
+
+	/* TODO: identify_proprietary_vendor_ie(); */
+	/*  Realtek proprietary IE */
+	/*  identify if this is Broadcom sta */
+	/*  identify if this is ralink sta */
+	/*  Customer proprietary IE */
+
+
+
+	/* get a unique AID */
+	if (pstat->aid > 0) {
+		DBG_871X("  old AID %d\n", pstat->aid);
+	} else {
+		for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
+			if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
+				break;
+
+		/* if (pstat->aid > NUM_STA) { */
+		if (pstat->aid > pstapriv->max_num_sta) {
+
+			pstat->aid = 0;
+
+			DBG_871X("  no room for more AIDs\n");
+
+			status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
+
+			goto OnAssocReqFail;
+
+
+		} else {
+			pstapriv->sta_aid[pstat->aid - 1] = pstat;
+			DBG_871X("allocate new AID = (%d)\n", pstat->aid);
+		}
+	}
+
+
+	pstat->state &= (~WIFI_FW_ASSOC_STATE);
+	pstat->state |= WIFI_FW_ASSOC_SUCCESS;
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+	if (!list_empty(&pstat->auth_list)) {
+		list_del_init(&pstat->auth_list);
+		pstapriv->auth_list_cnt--;
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	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++;
+	}
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	/*  now the station is qualified to join our BSS... */
+	if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_ == status)) {
+		/* 1 bss_cap_update & sta_info_update */
+		bss_cap_update_on_sta_join(padapter, pstat);
+		sta_info_update(padapter, pstat);
+
+		/* 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);
+
+		spin_lock_bh(&pstat->lock);
+		if (pstat->passoc_req) {
+			kfree(pstat->passoc_req);
+			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;
+		}
+		spin_unlock_bh(&pstat->lock);
+
+		/* 3-(1) report sta add event */
+		report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
+	}
+
+	return _SUCCESS;
+
+asoc_class2_error:
+
+	issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
+
+	return _FAIL;
+
+OnAssocReqFail:
+
+	pstat->aid = 0;
+	if (frame_type == WIFI_ASSOCREQ)
+		issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+	else
+		issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+
+	return _FAIL;
+}
+
+unsigned int OnAssocRsp(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	uint i;
+	int res;
+	unsigned short	status;
+	struct ndis_80211_var_ie *pIE;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	/* struct wlan_bssid_ex			*cur_network = &(pmlmeinfo->network); */
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint pkt_len = precv_frame->u.hdr.len;
+
+	DBG_871X("%s\n", __func__);
+
+	/* check A1 matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
+		return _SUCCESS;
+
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+		return _SUCCESS;
+
+	del_timer_sync(&pmlmeext->link_timer);
+
+	/* status */
+	status = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 2));
+	if (status > 0) {
+		DBG_871X("assoc reject, status code: %d\n", status);
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		res = -4;
+		goto report_assoc_result;
+	}
+
+	/* get capabilities */
+	pmlmeinfo->capability = le16_to_cpu(*(__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 = (struct ndis_80211_var_ie *)(pframe + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			if (!memcmp(pIE->data, WMM_PARA_OUI, 6))	/* WMM */
+				WMM_param_handler(padapter, pIE);
+			break;
+
+		case _HT_CAPABILITY_IE_:	/* HT caps */
+			HT_caps_handler(padapter, pIE);
+			break;
+
+		case _HT_EXTRA_INFO_IE_:	/* HT info */
+			HT_info_handler(padapter, pIE);
+			break;
+
+		case _ERPINFO_IE_:
+			ERP_IE_handler(padapter, pIE);
+
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+	pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
+	pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+
+	/* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
+	UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);
+
+report_assoc_result:
+	if (res > 0) {
+		rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
+	} else {
+		rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
+	}
+
+	report_join_res(padapter, res);
+
+	return _SUCCESS;
+}
+
+unsigned int OnDeAuth(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned short	reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+
+	/* check A3 */
+	if (memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
+		return _SUCCESS;
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	DBG_871X("%s Reason code(%d)\n", __func__, reason);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		/* spin_lock_bh(&(pstapriv->sta_hash_lock)); */
+		/* rtw_free_stainfo(padapter, psta); */
+		/* spin_unlock_bh(&(pstapriv->sta_hash_lock)); */
+
+		DBG_871X_LEVEL(_drv_always_, "ap recv deauth reason code(%d) sta:%pM\n",
+				reason, GetAddr2Ptr(pframe));
+
+		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+		if (psta) {
+			u8 updated = false;
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			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);
+
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+			associated_clients_update(padapter, updated);
+		}
+
+
+		return _SUCCESS;
+	} else{
+		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;
+			}
+		}
+
+		DBG_871X_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM, ignore = %d\n",
+				reason, GetAddr3Ptr(pframe), ignore_received_deauth);
+
+		if (0 == ignore_received_deauth) {
+			receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
+		}
+	}
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+	return _SUCCESS;
+
+}
+
+unsigned int OnDisassoc(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned short	reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+
+	/* check A3 */
+	if (memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
+		return _SUCCESS;
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	DBG_871X("%s Reason code(%d)\n", __func__, reason);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		/* spin_lock_bh(&(pstapriv->sta_hash_lock)); */
+		/* rtw_free_stainfo(padapter, psta); */
+		/* spin_unlock_bh(&(pstapriv->sta_hash_lock)); */
+
+		DBG_871X_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
+				reason, GetAddr2Ptr(pframe));
+
+		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+		if (psta) {
+			u8 updated = false;
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			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);
+
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+			associated_clients_update(padapter, updated);
+		}
+
+		return _SUCCESS;
+	} else{
+		DBG_871X_LEVEL(_drv_always_, "sta recv disassoc reason code(%d) sta:%pM\n",
+				reason, GetAddr3Ptr(pframe));
+
+		receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
+	}
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+	return _SUCCESS;
+
+}
+
+unsigned int OnAtim(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	DBG_871X("%s\n", __func__);
+	return _SUCCESS;
+}
+
+unsigned int on_action_spct(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u8 *frame_body = (u8 *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+	u8 category;
+	u8 action;
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+
+	psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+
+	if (!psta)
+		goto exit;
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case RTW_WLAN_ACTION_SPCT_MSR_REQ:
+	case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
+	case RTW_WLAN_ACTION_SPCT_TPC_REQ:
+	case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
+	case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
+		break;
+	default:
+		break;
+	}
+
+exit:
+	return ret;
+}
+
+unsigned int OnAction_back(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	u8 *addr;
+	struct sta_info *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	unsigned char 	*frame_body;
+	unsigned char 	category, action;
+	unsigned short	tid, status, reason_code = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	DBG_871X("%s\n", __func__);
+
+	/* check RA matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))/* for if1, sta/ap mode */
+		return _SUCCESS;
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+			return _SUCCESS;
+
+	addr = GetAddr2Ptr(pframe);
+	psta = rtw_get_stainfo(pstapriv, addr);
+
+	if (psta == NULL)
+		return _SUCCESS;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+	if (category == RTW_WLAN_CATEGORY_BACK) {/*  representing Block Ack */
+		if (!pmlmeinfo->HT_enable) {
+			return _SUCCESS;
+		}
+
+		action = frame_body[1];
+		DBG_871X("%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);
+
+			if (pmlmeinfo->bAcceptAddbaReq == true) {
+				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0);
+			} else{
+				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);/* reject ADDBA Req */
+			}
+
+			break;
+
+		case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
+			status = RTW_GET_LE16(&frame_body[3]);
+			tid = ((frame_body[5] >> 2) & 0x7);
+
+			if (status == 0) {
+				/* successful */
+				DBG_871X("agg_enable for TID =%d\n", tid);
+				psta->htpriv.agg_enable_bitmap |= 1 << tid;
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+			} else{
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+			}
+
+			if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+				DBG_871X("%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;
+			}
+
+			/* DBG_871X("marc: ADDBA RSP: %x\n", pmlmeinfo->agg_enable_bitmap); */
+			break;
+
+		case RTW_WLAN_ACTION_DELBA: /* DELBA */
+			if ((frame_body[3] & BIT(3)) == 0) {
+				psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+				psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+
+				/* reason_code = frame_body[4] | (frame_body[5] << 8); */
+				reason_code = RTW_GET_LE16(&frame_body[4]);
+			} else if ((frame_body[3] & BIT(3)) == BIT(3)) {
+				tid = (frame_body[3] >> 4) & 0x0F;
+
+				preorder_ctrl =  &psta->recvreorder_ctrl[tid];
+				preorder_ctrl->enable = false;
+				preorder_ctrl->indicate_seq = 0xffff;
+				#ifdef DBG_RX_SEQ
+				DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__,
+					preorder_ctrl->indicate_seq);
+				#endif
+			}
+
+			DBG_871X("%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;
+}
+
+static s32 rtw_action_public_decache(union recv_frame *recv_frame, s32 token)
+{
+	struct adapter *adapter = recv_frame->u.hdr.adapter;
+	struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
+	u8 *frame = recv_frame->u.hdr.rx_data;
+	u16 seq_ctrl = ((recv_frame->u.hdr.attrib.seq_num&0xffff) << 4) |
+		(recv_frame->u.hdr.attrib.frag_num & 0xf);
+
+	if (GetRetry(frame)) {
+		if (token >= 0) {
+			if ((seq_ctrl == mlmeext->action_public_rxseq)
+				&& (token == mlmeext->action_public_dialog_token)) {
+				DBG_871X(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x, token:%d\n",
+					FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
+				return _FAIL;
+			}
+		} else {
+			if (seq_ctrl == mlmeext->action_public_rxseq) {
+				DBG_871X(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x\n",
+					FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq);
+				return _FAIL;
+			}
+		}
+	}
+
+	mlmeext->action_public_rxseq = seq_ctrl;
+
+	if (token >= 0)
+		mlmeext->action_public_dialog_token = token;
+
+	return _SUCCESS;
+}
+
+static unsigned int on_action_public_p2p(union recv_frame *precv_frame)
+{
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u8 *frame_body;
+	u8 dialogToken = 0;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	dialogToken = frame_body[7];
+
+	if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
+		return _FAIL;
+
+	return _SUCCESS;
+}
+
+static unsigned int on_action_public_vendor(union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+
+	if (!memcmp(frame_body + 2, P2P_OUI, 4)) {
+		ret = on_action_public_p2p(precv_frame);
+	}
+
+	return ret;
+}
+
+static unsigned int on_action_public_default(union recv_frame *precv_frame, u8 action)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	uint frame_len = precv_frame->u.hdr.len;
+	u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+	u8 token;
+	struct adapter *adapter = precv_frame->u.hdr.adapter;
+	int cnt = 0;
+	char msg[64];
+
+	token = frame_body[2];
+
+	if (rtw_action_public_decache(precv_frame, token) == _FAIL)
+		goto exit;
+
+	cnt += sprintf((msg+cnt), "%s(token:%u)", action_public_str(action), token);
+	rtw_cfg80211_rx_action(adapter, pframe, frame_len, msg);
+
+	ret = _SUCCESS;
+
+exit:
+	return ret;
+}
+
+unsigned int on_action_public(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+	u8 category, action;
+
+	/* check RA matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
+		goto exit;
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_PUBLIC)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case ACT_PUBLIC_VENDOR:
+		ret = on_action_public_vendor(precv_frame);
+		break;
+	default:
+		ret = on_action_public_default(precv_frame, action);
+		break;
+	}
+
+exit:
+	return ret;
+}
+
+unsigned int OnAction_ht(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u8 *frame_body = pframe + sizeof(struct ieee80211_hdr_3addr);
+	u8 category, action;
+
+	/* check RA matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), 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_COMPRESS_BEAMFORMING:
+		break;
+	default:
+		break;
+	}
+
+exit:
+
+	return _SUCCESS;
+}
+
+unsigned int OnAction_sa_query(struct 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);
+	unsigned short tid;
+	/* Baron */
+
+	DBG_871X("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(unsigned short));
+		DBG_871X("OnAction_sa_query request, action =%d, tid =%04x\n", pframe[WLAN_HDR_A3_LEN+1], tid);
+		issue_action_SA_Query(padapter, GetAddr2Ptr(pframe), 1, tid);
+		break;
+
+	case 1: /* SA Query rsp */
+		del_timer_sync(&pmlmeext->sa_query_timer);
+		DBG_871X("OnAction_sa_query response, action =%d, tid =%04x, cancel timer\n", pframe[WLAN_HDR_A3_LEN+1], pframe[WLAN_HDR_A3_LEN+2]);
+		break;
+	default:
+		break;
+	}
+	if (0) {
+		int pp;
+		printk("pattrib->pktlen = %d =>", pattrib->pkt_len);
+		for (pp = 0; pp < pattrib->pkt_len; pp++)
+			printk(" %02x ", pframe[pp]);
+		printk("\n");
+	}
+
+	return _SUCCESS;
+}
+
+unsigned int OnAction(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	int i;
+	unsigned char category;
+	struct action_handler *ptable;
+	unsigned char *frame_body;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+
+	for (i = 0; i < sizeof(OnAction_tbl)/sizeof(struct action_handler); i++) {
+		ptable = &OnAction_tbl[i];
+
+		if (category == ptable->num)
+			ptable->func(padapter, precv_frame);
+
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int DoReserved(struct adapter *padapter, union recv_frame *precv_frame)
+{
+
+	/* DBG_871X("rcvd mgt frame(%x, %x)\n", (GetFrameSubType(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) {
+		DBG_871X(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) {
+		DBG_871X(FUNC_ADPT_FMT" alloc xmitbuf fail\n", FUNC_ADPT_ARG(pxmitpriv->adapter));
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		pmgntframe = NULL;
+		goto exit;
+	}
+
+	pmgntframe->frame_tag = MGNT_FRAMETAG;
+	pmgntframe->pxmitbuf = pxmitbuf;
+	pmgntframe->buf_addr = pxmitbuf->pbuf;
+	pxmitbuf->priv_data = pmgntframe;
+
+exit:
+	return pmgntframe;
+
+}
+
+inline struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
+{
+	return _alloc_mgtxmitframe(pxmitpriv, false);
+}
+
+/****************************************************************************
+
+Following are some TX fuctions for WiFi MLME
+
+*****************************************************************************/
+
+void update_mgnt_tx_rate(struct adapter *padapter, u8 rate)
+{
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+	pmlmeext->tx_rate = rate;
+	/* DBG_871X("%s(): rate = %x\n", __func__, rate); */
+}
+
+void update_mgntframe_attrib(struct adapter *padapter, struct pkt_attrib *pattrib)
+{
+	u8 wireless_mode;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+
+	/* memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib)); */
+
+	pattrib->hdrlen = 24;
+	pattrib->nr_frags = 1;
+	pattrib->priority = 7;
+	pattrib->mac_id = 0;
+	pattrib->qsel = 0x12;
+
+	pattrib->pktlen = 0;
+
+	if (pmlmeext->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 = 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;
+
+}
+
+void update_mgntframe_attrib_addr(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	u8 *pframe;
+	struct pkt_attrib	*pattrib = &pmgntframe->attrib;
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+
+	memcpy(pattrib->ra, GetAddr1Ptr(pframe), ETH_ALEN);
+	memcpy(pattrib->ta, GetAddr2Ptr(pframe), ETH_ALEN);
+}
+
+void dump_mgntframe(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	if (padapter->bSurpriseRemoved == true ||
+		padapter->bDriverStopped == true) {
+		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(struct adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
+{
+	s32 ret = _FAIL;
+	_irqL irqL;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
+	struct submit_ctx sctx;
+
+	if (padapter->bSurpriseRemoved == true ||
+		padapter->bDriverStopped == true) {
+		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__);
+
+	spin_lock_irqsave(&pxmitpriv->lock_sctx, irqL);
+	pxmitbuf->sctx = NULL;
+	spin_unlock_irqrestore(&pxmitpriv->lock_sctx, irqL);
+
+	 return ret;
+}
+
+s32 dump_mgntframe_and_wait_ack(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	static u8 seq_no = 0;
+	s32 ret = _FAIL;
+	u32 timeout_ms = 500;/*   500ms */
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	if (padapter->bSurpriseRemoved == true ||
+		padapter->bDriverStopped == true) {
+		rtw_free_xmitbuf(&padapter->xmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(&padapter->xmitpriv, pmgntframe);
+		return -1;
+	}
+
+	if (mutex_lock_interruptible(&pxmitpriv->ack_tx_mutex) == 0) {
+		pxmitpriv->ack_tx = true;
+		pxmitpriv->seq_no = seq_no++;
+		pmgntframe->ack_report = 1;
+		if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS) {
+			ret = rtw_ack_tx_wait(pxmitpriv, timeout_ms);
+		}
+
+		pxmitpriv->ack_tx = false;
+		mutex_unlock(&pxmitpriv->ack_tx_mutex);
+	}
+
+	 return ret;
+}
+
+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);
+
+	/* DBG_871X("%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;
+}
+
+void issue_beacon(struct adapter *padapter, int timeout_ms)
+{
+	struct xmit_frame	*pmgntframe;
+	struct pkt_attrib	*pattrib;
+	unsigned char *pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	unsigned int	rate_len;
+	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*cur_network = &(pmlmeinfo->network);
+	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	/* DBG_871X("%s\n", __func__); */
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		DBG_871X("%s, alloc mgnt frame fail\n", __func__);
+		return;
+	}
+
+	spin_lock_bh(&pmlmepriv->bcn_update_lock);
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->qsel = 0x10;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
+	/* pmlmeext->mgnt_seq++; */
+	SetFrameSubType(pframe, WIFI_BEACON);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
+		/* DBG_871X("ie len =%d\n", cur_network->IELength); */
+		{
+			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 ieee80211_hdr_3addr)+_BEACON_IE_OFFSET_,
+				pattrib->pktlen-sizeof(struct 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);
+		}
+
+		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:
+
+	pmlmepriv->update_bcn = false;
+
+	spin_unlock_bh(&pmlmepriv->bcn_update_lock);
+
+	if ((pattrib->pktlen + TXDESC_SIZE) > 512) {
+		DBG_871X("beacon frame too large\n");
+		return;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	/* DBG_871X("issue bcn_sz =%d\n", pattrib->last_txcmdsz); */
+	if (timeout_ms > 0)
+		dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
+	else
+		dump_mgntframe(padapter, pmgntframe);
+
+}
+
+void issue_probersp(struct adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 				*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	unsigned char 				*mac, *bssid;
+	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
+
+	u8 *pwps_ie;
+	uint wps_ielen;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*cur_network = &(pmlmeinfo->network);
+	unsigned int	rate_len;
+
+	/* DBG_871X("%s\n", __func__); */
+
+	if (da == NULL)
+		return;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		DBG_871X("%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 ieee80211_hdr *)pframe;
+
+	mac = myid(&(padapter->eeprompriv));
+	bssid = cur_network->MacAddress;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(fctrl, WIFI_PROBERSP);
+
+	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+
+	if (cur_network->IELength > MAX_IE_SZ)
+		return;
+
+	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 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) {
+					DBG_871X_LEVEL(_drv_warning_, 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;
+			}
+		}
+	} else{
+		/* 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_AUTO_AP_MODE
+{
+	struct sta_info *psta;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	DBG_871X("(%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;
+
+		DBG_871X("%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(struct adapter *padapter, struct 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 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);
+	int	bssrate_len = 0;
+	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+issue_probereq\n"));
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	mac = myid(&(padapter->eeprompriv));
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	if (da) {
+		/* 	unicast probe request frame */
+		memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, da, ETH_ALEN);
+	} else{
+		/* 	broadcast probe request frame */
+		memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
+	}
+
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_PROBEREQ);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct 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;
+		}
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("issuing probe_req, tx_len =%d\n", pattrib->last_txcmdsz));
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else {
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+inline void issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da)
+{
+	_issue_probereq(padapter, pssid, da, 0, 1, false);
+}
+
+int issue_probereq_ex(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da, u8 ch, bool append_wps,
+	int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+
+	do {
+		ret = _issue_probereq(padapter, pssid, da, ch, append_wps, wait_ms > 0?true:false);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			msleep(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+	}
+exit:
+	return ret;
+}
+
+/*  if psta == NULL, indiate we are station(client) now... */
+void issue_auth(struct adapter *padapter, struct sta_info *psta, unsigned short status)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 				*pframe;
+	struct 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_tmp;
+
+	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 ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_AUTH);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+
+	if (psta) { /*  for AP mode */
+		memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+		/*  setting auth algo number */
+		val16 = (u16)psta->authalg;
+
+		if (status != _STATS_SUCCESSFUL_)
+			val16 = 0;
+
+		if (val16)
+			use_shared_key = 1;
+
+		le_tmp = cpu_to_le16(val16);
+
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+		/*  setting auth seq number */
+		val16 = (u16)psta->auth_seq;
+		le_tmp = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+		/*  setting status code... */
+		val16 = status;
+		le_tmp = cpu_to_le16(val16);
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_tmp, &(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));
+
+	} else{
+		memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
+
+		/*  setting auth algo number */
+		val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) ? 1 : 0;/*  0:OPEN System, 1:Shared key */
+		if (val16) {
+			use_shared_key = 1;
+		}
+		le_tmp = cpu_to_le16(val16);
+		/* DBG_871X("%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_tmp32;
+
+			/* DBG_871X("==> iv(%d), key_index(%d)\n", pmlmeinfo->iv, pmlmeinfo->key_index); */
+			val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
+			le_tmp32 = cpu_to_le32(val32);
+			pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&le_tmp32, &(pattrib->pktlen));
+
+			pattrib->iv_len = 4;
+		}
+
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+		/*  setting auth seq number */
+		le_tmp = cpu_to_le16(pmlmeinfo->auth_seq);
+		pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+
+		/*  setting status code... */
+		le_tmp = cpu_to_le16(status);
+		pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+		/*  then checking to see if sending challenging text... */
+		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
+			pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen));
+
+			SetPrivacy(fctrl);
+
+			pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
+
+			pattrib->encrypt = _WEP40_;
+
+			pattrib->icv_len = 4;
+
+			pattrib->pktlen += pattrib->icv_len;
+
+		}
+
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	rtw_wep_encrypt(padapter, (u8 *)pmgntframe);
+	DBG_871X("%s\n", __func__);
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+
+void issue_asocrsp(struct adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
+{
+	struct xmit_frame	*pmgntframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	struct pkt_attrib *pattrib;
+	unsigned char *pbuf, *pframe;
+	unsigned short val;
+	__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);
+	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+	u8 *ie = pnetwork->IEs;
+	__le16 lestatus, le_tmp;
+
+	DBG_871X("%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 ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
+	memcpy((void *)GetAddr2Ptr(pwlanhdr), myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
+		SetFrameSubType(pwlanhdr, pkt_type);
+	else
+		return;
+
+	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen += pattrib->hdrlen;
+	pframe += pattrib->hdrlen;
+
+	/* capability */
+	val = *(unsigned short *)rtw_get_capability_from_ie(ie);
+
+	pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_, (unsigned char *)&val, &(pattrib->pktlen));
+
+	lestatus = cpu_to_le16(status);
+	pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&lestatus, &(pattrib->pktlen));
+
+	le_tmp = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
+	pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+	if (pstat->bssratelen <= 8) {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen));
+	} else{
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &(pattrib->pktlen));
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (pstat->bssratelen-8), pstat->bssrateset+8, &(pattrib->pktlen));
+	}
+
+	if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) {
+		uint ie_len = 0;
+
+		/* FILL HT CAP INFO IE */
+		/* 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);
+		}
+
+	}
+
+	/* 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;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_assocreq(struct adapter *padapter)
+{
+	int ret = _FAIL;
+	struct xmit_frame				*pmgntframe;
+	struct pkt_attrib				*pattrib;
+	unsigned char 				*pframe;
+	struct ieee80211_hdr			*pwlanhdr;
+	__le16 *fctrl;
+	__le16 val16;
+	unsigned int					i, j, index = 0;
+	unsigned char bssrate[NumRates], sta_bssrate[NumRates];
+	struct ndis_80211_var_ie *pIE;
+	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;
+
+	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 ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ASSOCREQ);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	/* caps */
+	memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
+
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/* listen interval */
+	/* todo: listen interval for power saving */
+	val16 = cpu_to_le16(3);
+	memcpy(pframe, (unsigned char *)&val16, 2);
+	pframe += 2;
+	pattrib->pktlen += 2;
+
+	/* SSID */
+	pframe = rtw_set_ie(pframe, _SSID_IE_,  pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &(pattrib->pktlen));
+
+	/* supported rate & extended supported rate */
+
+	/*  Check if the AP's supported rates are also supported by STA. */
+	get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);
+	/* DBG_871X("sta_bssrate_len =%d\n", sta_bssrate_len); */
+
+	if (pmlmeext->cur_channel == 14) /*  for JAPAN, channel 14 can only uses B Mode(CCK) */
+		sta_bssrate_len = 4;
+
+
+	/* for (i = 0; i < sta_bssrate_len; i++) { */
+	/* 	DBG_871X("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]); */
+	/*  */
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		if (pmlmeinfo->network.SupportedRates[i] == 0)
+			break;
+		DBG_871X("network.SupportedRates[%d]=%02X\n", i, pmlmeinfo->network.SupportedRates[i]);
+	}
+
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		if (pmlmeinfo->network.SupportedRates[i] == 0)
+			break;
+
+
+		/*  Check if the AP's supported rates are also supported by STA. */
+		for (j = 0; j < sta_bssrate_len; j++) {
+			 /*  Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP */
+			if ((pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK)
+					== (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)) {
+				/* DBG_871X("match i = %d, j =%d\n", i, j); */
+				break;
+			} else {
+				/* DBG_871X("not match: %02X != %02X\n", (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK), (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)); */
+			}
+		}
+
+		if (j == sta_bssrate_len) {
+			/*  the rate is not supported by STA */
+			DBG_871X("%s(): the rate[%d]=%02X is not supported by STA!\n", __func__, i, pmlmeinfo->network.SupportedRates[i]);
+		} else {
+			/*  the rate is supported by STA */
+			bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
+		}
+	}
+
+	bssrate_len = index;
+	DBG_871X("bssrate_len = %d\n", bssrate_len);
+
+	if (bssrate_len == 0) {
+		rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
+		rtw_free_xmitframe(pxmitpriv, pmgntframe);
+		goto exit; /* don't connect to AP if no joint supported rate */
+	}
+
+
+	if (bssrate_len > 8) {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, bssrate, &(pattrib->pktlen));
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
+	} else
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, bssrate_len, bssrate, &(pattrib->pktlen));
+
+	/* vendor specific IE, such as WPA, WMM, WPS */
+	for (i = sizeof(struct ndis_802_11_fix_ie); i < pmlmeinfo->network.IELength;) {
+		pIE = (struct ndis_80211_var_ie *)(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:
+			pframe = rtw_set_ie(pframe, EID_WPA2, pIE->Length, pIE->data, &(pattrib->pktlen));
+			break;
+		case EID_HTCapability:
+			if (padapter->mlmepriv.htpriv.ht_option == true) {
+				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 == true)
+				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));
+
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+	dump_mgntframe(padapter, pmgntframe);
+
+	ret = _SUCCESS;
+
+exit:
+	if (ret == _SUCCESS)
+		rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen);
+	else
+		rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
+
+	return;
+}
+
+/* when wait_ack is ture, this function shoule be called at process context */
+static int _issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 				*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct xmit_priv *pxmitpriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+
+	/* DBG_871X("%s:%d\n", __func__, power_mode); */
+
+	if (!padapter)
+		goto exit;
+
+	pxmitpriv = &(padapter->xmitpriv);
+	pmlmeext = &(padapter->mlmeextpriv);
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		goto exit;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->retry_ctrl = false;
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+		SetFrDs(fctrl);
+	else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+		SetToDs(fctrl);
+
+	if (power_mode)
+		SetPwrMgt(fctrl);
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_DATA_NULL);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct 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 shoule be called at process context
+ * da == NULL for station mode
+ */
+int issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_info *psta;
+
+
+	/* 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 (power_mode)
+			rtw_hal_macid_sleep(padapter, psta->mac_id);
+		else
+			rtw_hal_macid_wakeup(padapter, psta->mac_id);
+	} else {
+		DBG_871X(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 (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			msleep(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+	}
+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(struct adapter *padapter, u8 *da)
+{
+	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, 0, false);
+
+	return ret;
+}
+
+/* when wait_ack is ture, this function shoule be called at process context */
+static int _issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int wait_ack)
+{
+	int ret = _FAIL;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 				*pframe;
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	u16 *qc;
+	struct xmit_priv 		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	DBG_871X("%s\n", __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 ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
+		SetFrDs(fctrl);
+	else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
+		SetToDs(fctrl);
+
+	if (pattrib->mdata)
+		SetMData(fctrl);
+
+	qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
+
+	SetPriority(qc, tid);
+
+	SetEOSP(qc, pattrib->eosp);
+
+	SetAckpolicy(qc, pattrib->ack_policy);
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+
+	pframe += sizeof(struct ieee80211_qos_hdr);
+	pattrib->pktlen = sizeof(struct ieee80211_qos_hdr);
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else{
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+/* when wait_ms >0 , this function shoule be called at process context */
+/* da == NULL for station mode */
+int issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms)
+{
+	int ret;
+	int i = 0;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* da == NULL, assum it's null data for sta to ap*/
+	if (da == NULL)
+		da = get_my_bssid(&(pmlmeinfo->network));
+
+	do {
+		ret = _issue_qos_nulldata(padapter, da, tid, wait_ms > 0?true:false);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			msleep(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+	}
+exit:
+	return ret;
+}
+
+static int _issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack)
+{
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 				*pframe;
+	struct 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);
+	int ret = _FAIL;
+	__le16 le_tmp;
+
+	/* DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); */
+
+	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 ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_DEAUTH);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	le_tmp = cpu_to_le16(reason);
+	pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_, (unsigned char *)&le_tmp, &(pattrib->pktlen));
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+
+	if (wait_ack) {
+		ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
+	} else{
+		dump_mgntframe(padapter, pmgntframe);
+		ret = _SUCCESS;
+	}
+
+exit:
+	return ret;
+}
+
+int issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short reason)
+{
+	DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
+	return _issue_deauth(padapter, da, reason, false);
+}
+
+int issue_deauth_ex(struct adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
+	int wait_ms)
+{
+	int ret;
+	int i = 0;
+
+	do {
+		ret = _issue_deauth(padapter, da, reason, wait_ms > 0?true:false);
+
+		i++;
+
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+			break;
+
+		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
+			msleep(wait_ms);
+
+	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
+
+	if (ret != _FAIL) {
+		ret = _SUCCESS;
+		#ifndef DBG_XMIT_ACK
+		goto exit;
+		#endif
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+	}
+exit:
+	return ret;
+}
+
+void issue_action_SA_Query(struct adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short tid)
+{
+	u8 category = RTW_WLAN_CATEGORY_SA_QUERY;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	u8 			*pframe;
+	struct 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);
+	__le16 le_tmp;
+
+	DBG_871X("%s\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		DBG_871X("%s: alloc_mgtxmitframe 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 ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	if (raddr)
+		memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	else
+		memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct 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 */
+		set_sa_query_timer(pmlmeext, 1000);
+		break;
+
+	case 1: /* SA Query rsp */
+		le_tmp = cpu_to_le16(tid);
+		pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)&le_tmp, &pattrib->pktlen);
+		break;
+	default:
+		break;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+void issue_action_BA(struct adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short status)
+{
+	u8 category = RTW_WLAN_CATEGORY_BACK;
+	u16 start_seq;
+	u16 BA_para_set;
+	u16 reason_code;
+	u16 BA_timeout_value;
+	u16 BA_starting_seqctrl = 0;
+	enum HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor;
+	struct xmit_frame		*pmgntframe;
+	struct pkt_attrib		*pattrib;
+	u8 			*pframe;
+	struct 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;
+	__le16 le_tmp;
+
+	DBG_871X("%s, category =%d, action =%d, status =%d\n", __func__, category, action, status);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		return;
+
+	/* update attribute */
+	pattrib = &pmgntframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+
+	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
+
+	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	/* memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); */
+	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
+
+	pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
+	pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
+
+	if (category == 3) {
+		switch (action) {
+		case 0: /* ADDBA req */
+			do {
+				pmlmeinfo->dialogToken++;
+			} while (pmlmeinfo->dialogToken == 0);
+			pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen));
+
+			if (rtw_btcoex_IsBTCoexCtrlAMPDUSize(padapter)) {
+				/*  A-MSDU NOT Supported */
+				BA_para_set = 0;
+				/*  immediate Block Ack */
+				BA_para_set |= (1 << 1) & IEEE80211_ADDBA_PARAM_POLICY_MASK;
+				/*  TID */
+				BA_para_set |= (status << 2) & IEEE80211_ADDBA_PARAM_TID_MASK;
+				/*  max buffer size is 8 MSDU */
+				BA_para_set |= (8 << 6) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+			} else {
+				BA_para_set = (0x1002 | ((status & 0xf) << 2)); /* immediate ack & 64 buffer size */
+			}
+			le_tmp = cpu_to_le16(BA_para_set);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+			BA_timeout_value = 5000;/*  5ms */
+			le_tmp = cpu_to_le16(BA_timeout_value);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+			/* 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[status & 0x07]&0xfff) + 1;
+
+				DBG_871X("BA_starting_seqctrl = %d for TID =%d\n", start_seq, status & 0x07);
+
+				psta->BA_starting_seqctrl[status & 0x07] = start_seq;
+
+				BA_starting_seqctrl = start_seq << 4;
+			}
+
+			le_tmp = cpu_to_le16(BA_starting_seqctrl);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+			break;
+
+		case 1: /* ADDBA rsp */
+			pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen));
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&status), &(pattrib->pktlen));
+			if (padapter->driver_rx_ampdu_factor != 0xFF)
+				max_rx_ampdu_factor =
+				  (enum 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);
+
+			if (MAX_AMPDU_FACTOR_64K == max_rx_ampdu_factor)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
+			else if (MAX_AMPDU_FACTOR_32K == max_rx_ampdu_factor)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0800); /* 32 buffer size */
+			else if (MAX_AMPDU_FACTOR_16K == max_rx_ampdu_factor)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0400); /* 16 buffer size */
+			else if (MAX_AMPDU_FACTOR_8K == max_rx_ampdu_factor)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0200); /* 8 buffer size */
+			else
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
+
+			if (rtw_btcoex_IsBTCoexCtrlAMPDUSize(padapter) &&
+			    padapter->driver_rx_ampdu_factor == 0xFF) {
+				/*  max buffer size is 8 MSDU */
+				BA_para_set &= ~RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+				BA_para_set |= (8 << 6) & RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+			}
+
+			if (pregpriv->ampdu_amsdu == 0)/* disabled */
+				le_tmp = cpu_to_le16(BA_para_set & ~BIT(0));
+			else if (pregpriv->ampdu_amsdu == 1)/* enabled */
+				le_tmp = cpu_to_le16(BA_para_set | BIT(0));
+			else /* auto */
+				le_tmp = cpu_to_le16(BA_para_set);
+
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen));
+			break;
+		case 2:/* DELBA */
+			BA_para_set = (status & 0x1F) << 3;
+			le_tmp = cpu_to_le16(BA_para_set);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+
+			reason_code = 37;
+			le_tmp = cpu_to_le16(reason_code);
+			pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(le_tmp)), &(pattrib->pktlen));
+			break;
+		default:
+			break;
+		}
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+static void issue_action_BSSCoexistPacket(struct adapter *padapter)
+{
+	struct list_head		*plist, *phead;
+	unsigned char category, action;
+	struct xmit_frame			*pmgntframe;
+	struct pkt_attrib			*pattrib;
+	unsigned char 			*pframe;
+	struct 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);
+	struct __queue		*queue	= &(pmlmepriv->scanned_queue);
+	u8 InfoContent[16] = {0};
+	u8 ICS[8][15];
+
+	if ((pmlmepriv->num_FortyMHzIntolerant == 0) || (pmlmepriv->num_sta_no_ht == 0))
+		return;
+
+	if (true == pmlmeinfo->bwmode_updated)
+		return;
+
+
+	DBG_871X("%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 ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pmlmeext->mgnt_seq++;
+	SetFrameSubType(pframe, WIFI_ACTION);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pattrib->pktlen = sizeof(struct 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;
+
+		spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+
+		phead = get_list_head(queue);
+		plist = get_next(phead);
+
+		while (1) {
+			int len;
+			u8 *p;
+			struct wlan_bssid_ex *pbss_network;
+
+			if (phead == plist)
+				break;
+
+			pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+			plist = get_next(plist);
+
+			pbss_network = (struct wlan_bssid_ex *)&pnetwork->network;
+
+			p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
+			if ((p == NULL) || (len == 0)) {/* non-HT */
+
+				if ((pbss_network->Configuration.DSConfig <= 0) || (pbss_network->Configuration.DSConfig > 14))
+					continue;
+
+				ICS[0][pbss_network->Configuration.DSConfig] = 1;
+
+				if (ICS[0][0] == 0)
+					ICS[0][0] = 1;
+			}
+
+		}
+
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+
+
+		for (i = 0; i < 8; i++) {
+			if (ICS[i][0] == 1) {
+				int j, k = 0;
+
+				InfoContent[k] = i;
+				/* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent, i); */
+				k++;
+
+				for (j = 1; j <= 14; j++) {
+					if (ICS[i][j] == 1) {
+						if (k < 16) {
+							InfoContent[k] = j; /* channel number */
+							/* SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j); */
+							k++;
+						}
+					}
+				}
+
+				pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &(pattrib->pktlen));
+
+			}
+
+		}
+
+
+	}
+
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	dump_mgntframe(padapter, pmgntframe);
+}
+
+unsigned int send_delba(struct adapter *padapter, u8 initiator, u8 *addr)
+{
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+	/* struct recv_reorder_ctrl *preorder_ctrl; */
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u16 tid;
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+			return _SUCCESS;
+
+	psta = rtw_get_stainfo(pstapriv, addr);
+	if (psta == NULL)
+		return _SUCCESS;
+
+	/* DBG_871X("%s:%s\n", __func__, (initiator == 0)?"RX_DIR":"TX_DIR"); */
+
+	if (initiator == 0) {/*  recipient */
+		for (tid = 0; tid < MAXTID; tid++) {
+			if (psta->recvreorder_ctrl[tid].enable == true) {
+				DBG_871X("rx agg disable tid(%d)\n", tid);
+				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid << 1) | initiator)&0x1F));
+				psta->recvreorder_ctrl[tid].enable = false;
+				psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
+				#ifdef DBG_RX_SEQ
+				DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__,
+					psta->recvreorder_ctrl[tid].indicate_seq);
+				#endif
+			}
+		}
+	} else if (initiator == 1) {/*  originator */
+		/* DBG_871X("tx agg_enable_bitmap(0x%08x)\n", psta->htpriv.agg_enable_bitmap); */
+		for (tid = 0; tid < MAXTID; tid++) {
+			if (psta->htpriv.agg_enable_bitmap & BIT(tid)) {
+				DBG_871X("tx agg disable tid(%d)\n", tid);
+				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid << 1) | initiator)&0x1F));
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+
+			}
+		}
+	}
+
+	return _SUCCESS;
+
+}
+
+unsigned int send_beacon(struct adapter *padapter)
+{
+	u8 bxmitok = false;
+	int	issue = 0;
+	int poll = 0;
+	unsigned long 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 {
+			yield();
+			rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
+			poll++;
+		} while ((poll%10) != 0 && false == bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+	} while (false == bxmitok && issue < 100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+	if (padapter->bSurpriseRemoved || padapter->bDriverStopped) {
+		return _FAIL;
+	}
+
+
+	if (false == bxmitok) {
+		DBG_871X("%s fail! %u ms\n", __func__, jiffies_to_msecs(jiffies - start));
+		return _FAIL;
+	} else{
+		unsigned long passing_time = jiffies_to_msecs(jiffies - start);
+
+		if (passing_time > 100 || issue > 3)
+			DBG_871X("%s success, issue:%d, poll:%d, %lu ms\n", __func__, issue, poll, passing_time);
+		/* else */
+		/* 	DBG_871X("%s success, issue:%d, poll:%d, %u ms\n", __func__, issue, poll, passing_time); */
+
+		return _SUCCESS;
+	}
+}
+
+/****************************************************************************
+
+Following are some utitity fuctions for WiFi MLME
+
+*****************************************************************************/
+
+void site_survey(struct adapter *padapter)
+{
+	unsigned char 	survey_channel = 0, val8;
+	RT_SCAN_TYPE	ScanType = SCAN_PASSIVE;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u32 initialgain = 0;
+	u32 channel_scan_time_ms = 0;
+
+	{
+		struct rtw_ieee80211_channel *ch;
+		if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
+			ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
+			survey_channel = ch->hw_value;
+			ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
+		}
+	}
+
+	DBG_871X(FUNC_ADPT_FMT" ch:%u (cnt:%u) at %dms, %c%c%c\n"
+		 , FUNC_ADPT_ARG(padapter)
+		 , survey_channel
+		 , pmlmeext->sitesurvey_res.channel_idx
+		 , jiffies_to_msecs(jiffies - padapter->mlmepriv.scan_start_time)
+		 , ScanType?'A':'P', pmlmeext->sitesurvey_res.scan_mode?'A':'P'
+		 , pmlmeext->sitesurvey_res.ssid[0].SsidLength?'S':' '
+		);
+#ifdef DBG_FIXED_CHAN
+	DBG_871X(FUNC_ADPT_FMT" fixed_chan:%u\n", pmlmeext->fixed_chan);
+#endif
+
+	if (survey_channel != 0) {
+		/* PAUSE 4-AC Queue when site_survey */
+		/* rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+		/* val8 |= 0x0f; */
+		/* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+		if (pmlmeext->sitesurvey_res.channel_idx == 0) {
+#ifdef DBG_FIXED_CHAN
+			if (pmlmeext->fixed_chan != 0xff)
+				set_channel_bwmode(padapter, pmlmeext->fixed_chan, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+			else
+#endif
+				set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+		} else{
+#ifdef DBG_FIXED_CHAN
+			if (pmlmeext->fixed_chan != 0xff)
+				SelectChannel(padapter, pmlmeext->fixed_chan);
+			else
+#endif
+				SelectChannel(padapter, survey_channel);
+		}
+
+		if (ScanType == SCAN_ACTIVE) { /* obey the channel plan setting... */
+			{
+				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);
+				}
+			}
+		}
+
+		channel_scan_time_ms = pmlmeext->chan_scan_time;
+
+		set_survey_timer(pmlmeext, channel_scan_time_ms);
+#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+		{
+			struct noise_info info;
+			info.bPauseDIG = false;
+			info.IGIValue = 0;
+			info.max_time = channel_scan_time_ms/2;/* ms */
+			info.chan = survey_channel;
+			rtw_hal_set_odm_var(padapter, HAL_ODM_NOISE_MONITOR, &info, false);
+		}
+#endif
+
+	} else{
+
+		/* 	channel number is 0 or this channel is not valid. */
+
+		{
+			pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
+
+			/* switch back to the original channel */
+			/* SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset); */
+
+			set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+			/* flush 4-AC Queue after site_survey */
+			/* val8 = 0; */
+			/* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+
+			/* config MSR */
+			Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+			initialgain = 0xff; /* restore RX GAIN */
+			rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
+			/* turn on dynamic functions */
+			Restore_DM_Func_Flag(padapter);
+			/* Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); */
+
+			if (is_client_associated_to_ap(padapter) == true)
+				issue_nulldata(padapter, NULL, 0, 3, 500);
+
+			val8 = 0; /* survey done */
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+			report_surveydone_event(padapter);
+
+			pmlmeext->chan_scan_time = SURVEY_TO;
+			pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+
+			issue_action_BSSCoexistPacket(padapter);
+			issue_action_BSSCoexistPacket(padapter);
+			issue_action_BSSCoexistPacket(padapter);
+		}
+	}
+
+	return;
+
+}
+
+/* collect bss info from Beacon and Probe request/response frames. */
+u8 collect_bss_info(struct adapter *padapter, union recv_frame *precv_frame, struct wlan_bssid_ex *bssid)
+{
+	int	i;
+	u32 len;
+	u8 *p;
+	u16 val16, subtype;
+	u8 *pframe = precv_frame->u.hdr.rx_data;
+	u32 packet_len = precv_frame->u.hdr.len;
+	u8 ie_offset;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	__le32 le32_tmp;
+
+	len = packet_len - sizeof(struct ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ) {
+		/* DBG_871X("IE too long for survey event\n"); */
+		return _FAIL;
+	}
+
+	memset(bssid, 0, sizeof(struct wlan_bssid_ex));
+
+	subtype = GetFrameSubType(pframe);
+
+	if (subtype == WIFI_BEACON) {
+		bssid->Reserved[0] = 1;
+		ie_offset = _BEACON_IE_OFFSET_;
+	} else {
+		/*  FIXME : more type */
+		if (subtype == WIFI_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(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+
+	/* below is to copy the information element */
+	bssid->IELength = len;
+	memcpy(bssid->IEs, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->IELength);
+
+	/* get the signal strength */
+	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 */
+
+	/*  checking SSID */
+	p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset);
+	if (p == NULL) {
+		DBG_871X("marc: cannot find SSID for survey event\n");
+		return _FAIL;
+	}
+
+	if (*(p + 1)) {
+		if (len > NDIS_802_11_LENGTH_SSID) {
+			DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __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) {
+			DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+			return _FAIL;
+		}
+		memcpy(bssid->SupportedRates, (p + 2), len);
+		i = len;
+	}
+
+	p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
+	if (p != NULL) {
+		if (len > (NDIS_802_11_LENGTH_RATES_EX-i)) {
+			DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+			return _FAIL;
+		}
+		memcpy(bssid->SupportedRates + i, (p + 2), len);
+	}
+
+	bssid->NetworkTypeInUse = Ndis802_11OFDM24;
+
+	if (bssid->IELength < 12)
+		return _FAIL;
+
+	/*  Checking for DSConfig */
+	p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
+
+	bssid->Configuration.DSConfig = 0;
+	bssid->Configuration.Length = 0;
+
+	if (p) {
+		bssid->Configuration.DSConfig = *(p + 2);
+	} else {
+		/*  In 5G, some ap do not have DSSET IE */
+		/*  checking HT info for channel */
+		p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
+		if (p) {
+			struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
+			bssid->Configuration.DSConfig = HT_info->primary_channel;
+		} else { /*  use current channel */
+			bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter);
+		}
+	}
+
+	memcpy(&le32_tmp, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
+	bssid->Configuration.BeaconPeriod = le32_to_cpu(le32_tmp);
+
+	val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
+
+	if (val16 & BIT(0)) {
+		bssid->InfrastructureMode = Ndis802_11Infrastructure;
+		memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
+	} else {
+		bssid->InfrastructureMode = Ndis802_11IBSS;
+		memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN);
+	}
+
+	if (val16 & BIT(4))
+		bssid->Privacy = 1;
+	else
+		bssid->Privacy = 0;
+
+	bssid->Configuration.ATIMWindow = 0;
+
+	/* 20/40 BSS Coexistence check */
+	if ((pregistrypriv->wifi_spec == 1) && (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 (le16_to_cpu(pHT_caps->u.HT_cap_element.HT_caps_info) & 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) {
+		DBG_871X("Receiving %s("MAC_FMT", DSConfig:%u) from ch%u with ss:%3u, sq:%3u, RawRSSI:%3ld\n"
+			, bssid->Ssid.Ssid, MAC_ARG(bssid->MacAddress), bssid->Configuration.DSConfig
+			, rtw_get_oper_ch(padapter)
+			, bssid->PhyInfo.SignalStrength, bssid->PhyInfo.SignalQuality, bssid->Rssi
+		);
+	}
+	#endif
+
+	/*  mark bss info receving from nearby channel as SignalQuality 101 */
+	if (bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
+		bssid->PhyInfo.SignalQuality = 101;
+
+	return _SUCCESS;
+}
+
+void start_create_ibss(struct adapter *padapter)
+{
+	unsigned short	caps;
+	u8 val8;
+	u8 join_type;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
+
+	/* update wireless mode */
+	update_wireless_mode(padapter);
+
+	/* udpate capability */
+	caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork);
+	update_capinfo(padapter, caps);
+	if (caps&cap_IBSS) {/* adhoc master */
+		val8 = 0xcf;
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
+
+		/* switch channel */
+		/* SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */
+		set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20);
+
+		beacon_timing_control(padapter);
+
+		/* set msr to WIFI_FW_ADHOC_STATE */
+		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+		Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+		/* issue beacon */
+		if (send_beacon(padapter) == _FAIL) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("issuing beacon frame fail....\n"));
+
+			report_join_res(padapter, -1);
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		} else{
+			rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
+			join_type = 0;
+			rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+
+			report_join_res(padapter, 1);
+			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+			rtw_indicate_connect(padapter);
+		}
+	} else{
+		DBG_871X("start_create_ibss, invalid cap:%x\n", caps);
+		return;
+	}
+	/* update bc/mc sta_info */
+	update_bmc_sta(padapter);
+
+}
+
+void start_clnt_join(struct adapter *padapter)
+{
+	unsigned short	caps;
+	u8 val8;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	int beacon_timeout;
+
+	/* update wireless mode */
+	update_wireless_mode(padapter);
+
+	/* udpate capability */
+	caps = rtw_get_capability((struct wlan_bssid_ex *)pnetwork);
+	update_capinfo(padapter, caps);
+	if (caps&cap_ESS) {
+		Set_MSR(padapter, WIFI_FW_STATION_STATE);
+
+		val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf;
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+		/*  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. */
+		{
+				/* 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);
+		}
+
+		/* here wait for receiving the beacon to start auth */
+		/* and enable a timer */
+		beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
+		set_link_timer(pmlmeext, beacon_timeout);
+		_set_timer(&padapter->mlmepriv.assoc_timer,
+			(REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) + beacon_timeout);
+
+		pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
+	} else if (caps&cap_IBSS) { /* adhoc client */
+		Set_MSR(padapter, WIFI_FW_ADHOC_STATE);
+
+		val8 = 0xcf;
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+
+		beacon_timing_control(padapter);
+
+		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
+
+		report_join_res(padapter, 1);
+	} else{
+		/* DBG_871X("marc: invalid cap:%x\n", caps); */
+		return;
+	}
+
+}
+
+void start_clnt_auth(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	del_timer_sync(&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;
+
+
+	DBG_871X_LEVEL(_drv_always_, "start auth\n");
+	issue_auth(padapter, NULL, 0);
+
+	set_link_timer(pmlmeext, REAUTH_TO);
+
+}
+
+
+void start_clnt_assoc(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	del_timer_sync(&pmlmeext->link_timer);
+
+	pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
+	pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
+
+	issue_assocreq(padapter);
+
+	set_link_timer(pmlmeext, REASSOC_TO);
+}
+
+unsigned int receive_disconnect(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* check A3 */
+	if (!(!memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
+		return _SUCCESS;
+
+	DBG_871X("%s\n", __func__);
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) {
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_del_sta_event(padapter, MacAddr, reason);
+
+		} else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) {
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_join_res(padapter, -2);
+		}
+	}
+
+	return _SUCCESS;
+}
+
+static void process_80211d(struct adapter *padapter, struct wlan_bssid_ex *bssid)
+{
+	struct registry_priv *pregistrypriv;
+	struct mlme_ext_priv *pmlmeext;
+	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;
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+				("%s: 802.11d country =%s\n", __func__, country));
+
+		i = 0;
+		while ((ie - p) >= 3) {
+			fcn = *(p++);
+			noc = *(p++);
+			p++;
+
+			for (j = 0; j < noc; j++) {
+				if (fcn <= 14)
+					channel = fcn + j; /*  2.4 GHz */
+				else
+					channel = fcn + j*4; /*  5 GHz */
+
+				chplan_ap.Channel[i++] = channel;
+			}
+		}
+		chplan_ap.Len = i;
+
+#ifdef CONFIG_DEBUG_RTL871X
+		i = 0;
+		DBG_871X("%s: AP[%s] channel plan {", __func__, bssid->Ssid.Ssid);
+		while ((i < chplan_ap.Len) && (chplan_ap.Channel[i] != 0)) {
+			DBG_8192C("%02d,", chplan_ap.Channel[i]);
+			i++;
+		}
+		DBG_871X("}\n");
+#endif
+
+		memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
+#ifdef CONFIG_DEBUG_RTL871X
+		i = 0;
+		DBG_871X("%s: STA channel plan {", __func__);
+		while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
+			DBG_871X("%02d(%c),", chplan_sta[i].ChannelNum, chplan_sta[i].ScanType == SCAN_PASSIVE?'p':'a');
+			i++;
+		}
+		DBG_871X("}\n");
+#endif
+
+		memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
+		chplan_new = pmlmeext->channel_set;
+
+		i = j = k = 0;
+		if (pregistrypriv->wireless_mode & WIRELESS_11G) {
+			do {
+				if ((i == MAX_CHANNEL_NUM) ||
+					(chplan_sta[i].ChannelNum == 0) ||
+					(chplan_sta[i].ChannelNum > 14))
+					break;
+
+				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
+					break;
+
+				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					i++;
+					j++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+/* 					chplan_new[k].ScanType = chplan_sta[i].ScanType; */
+					chplan_new[k].ScanType = SCAN_PASSIVE;
+					i++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					j++;
+					k++;
+				}
+			} while (1);
+
+			/*  change AP not support channel to Passive scan */
+			while ((i < MAX_CHANNEL_NUM) &&
+				(chplan_sta[i].ChannelNum != 0) &&
+				(chplan_sta[i].ChannelNum <= 14)) {
+
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+/* 				chplan_new[k].ScanType = chplan_sta[i].ScanType; */
+				chplan_new[k].ScanType = SCAN_PASSIVE;
+				i++;
+				k++;
+			}
+
+			/*  add channel AP supported */
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
+				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				chplan_new[k].ScanType = SCAN_ACTIVE;
+				j++;
+				k++;
+			}
+		} else{
+			/*  keep original STA 2.4G channel plan */
+			while ((i < MAX_CHANNEL_NUM) &&
+				(chplan_sta[i].ChannelNum != 0) &&
+				(chplan_sta[i].ChannelNum <= 14)) {
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+				chplan_new[k].ScanType = chplan_sta[i].ScanType;
+				i++;
+				k++;
+			}
+
+			/*  skip AP 2.4G channel plan */
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
+				j++;
+			}
+		}
+
+		if (pregistrypriv->wireless_mode & WIRELESS_11A) {
+			do {
+				if ((i == MAX_CHANNEL_NUM) ||
+					(chplan_sta[i].ChannelNum == 0))
+					break;
+
+				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0))
+					break;
+
+				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					i++;
+					j++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+/* 					chplan_new[k].ScanType = chplan_sta[i].ScanType; */
+					chplan_new[k].ScanType = SCAN_PASSIVE;
+					i++;
+					k++;
+				} else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+					chplan_new[k].ScanType = SCAN_ACTIVE;
+					j++;
+					k++;
+				}
+			} while (1);
+
+			/*  change AP not support channel to Passive scan */
+			while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+/* 				chplan_new[k].ScanType = chplan_sta[i].ScanType; */
+				chplan_new[k].ScanType = SCAN_PASSIVE;
+				i++;
+				k++;
+			}
+
+			/*  add channel AP supported */
+			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] != 0)) {
+				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				chplan_new[k].ScanType = SCAN_ACTIVE;
+				j++;
+				k++;
+			}
+		} else{
+			/*  keep original STA 5G channel plan */
+			while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
+				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+				chplan_new[k].ScanType = chplan_sta[i].ScanType;
+				i++;
+				k++;
+			}
+		}
+
+		pmlmeext->update_channel_plan_by_ap_done = 1;
+
+#ifdef CONFIG_DEBUG_RTL871X
+		k = 0;
+		DBG_871X("%s: new STA channel plan {", __func__);
+		while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0)) {
+			DBG_871X("%02d(%c),", chplan_new[k].ChannelNum, chplan_new[k].ScanType == SCAN_PASSIVE?'p':'c');
+			k++;
+		}
+		DBG_871X("}\n");
+#endif
+	}
+
+	/*  If channel is used by AP, set channel scan type to active */
+	channel = bssid->Configuration.DSConfig;
+	chplan_new = pmlmeext->channel_set;
+	i = 0;
+	while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0)) {
+		if (chplan_new[i].ChannelNum == channel) {
+			if (chplan_new[i].ScanType == SCAN_PASSIVE) {
+				/* 5G Bnad 2, 3 (DFS) doesn't change to active scan */
+				if (channel >= 52 && channel <= 144)
+					break;
+
+				chplan_new[i].ScanType = SCAN_ACTIVE;
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+						 ("%s: change channel %d scan type from passive to active\n",
+						  __func__, channel));
+			}
+			break;
+		}
+		i++;
+	}
+}
+
+/****************************************************************************
+
+Following are the functions to report events
+
+*****************************************************************************/
+
+void report_survey_event(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct survey_event	*psurvey_evt;
+	struct C2HEvent_Header *pc2h_evt_hdr;
+	struct mlme_ext_priv *pmlmeext;
+	struct cmd_priv *pcmdpriv;
+	/* u8 *pframe = precv_frame->u.hdr.rx_data; */
+	/* uint len = precv_frame->u.hdr.len; */
+
+	if (!padapter)
+		return;
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree((u8 *)pcmd_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, (struct wlan_bssid_ex *)&psurvey_evt->bss) == _FAIL) {
+		kfree((u8 *)pcmd_obj);
+		kfree((u8 *)pevtcmd);
+		return;
+	}
+
+	process_80211d(padapter, &psurvey_evt->bss);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	pmlmeext->sitesurvey_res.bss_cnt++;
+
+	return;
+
+}
+
+void report_surveydone_event(struct adapter *padapter)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct surveydone_event *psurveydone_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree((u8 *)pcmd_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;
+
+	DBG_871X("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;
+
+}
+
+void report_join_res(struct adapter *padapter, int res)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct joinbss_event		*pjoinbss_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree((u8 *)pcmd_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 joinbss_event);
+	pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	pjoinbss_evt = (struct joinbss_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(struct wlan_bssid_ex));
+	pjoinbss_evt->network.join_res	= pjoinbss_evt->network.aid = res;
+
+	DBG_871X("report_join_res(%d)\n", res);
+
+
+	rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network);
+
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+
+}
+
+void report_wmm_edca_update(struct 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 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) {
+		kfree((u8 *)pcmd_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;
+
+}
+
+void report_del_sta_event(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct sta_info *psta;
+	int	mac_id;
+	struct stadel_event			*pdel_sta_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL) {
+		return;
+	}
+
+	cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree((u8 *)pcmd_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(_DelSTA);
+	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
+
+	pdel_sta_evt = (struct stadel_event *)(pevtcmd + sizeof(struct C2HEvent_Header));
+	memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
+	memcpy((unsigned char *)(pdel_sta_evt->rsvd), (unsigned char *)(&reason), 2);
+
+
+	psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
+	if (psta)
+		mac_id = (int)psta->mac_id;
+	else
+		mac_id = (-1);
+
+	pdel_sta_evt->mac_id = mac_id;
+
+	DBG_871X("report_del_sta_event: delete STA, mac_id =%d\n", mac_id);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+}
+
+void report_add_sta_event(struct adapter *padapter, unsigned char *MacAddr, int cam_idx)
+{
+	struct cmd_obj *pcmd_obj;
+	u8 *pevtcmd;
+	u32 cmdsz;
+	struct stassoc_event		*padd_sta_evt;
+	struct C2HEvent_Header	*pc2h_evt_hdr;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+
+	pcmd_obj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree((u8 *)pcmd_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);
+	padd_sta_evt->cam_id = cam_idx;
+
+	DBG_871X("report_add_sta_event: add STA\n");
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+}
+
+
+bool rtw_port_switch_chk(struct adapter *adapter)
+{
+	bool switch_needed = false;
+	return switch_needed;
+}
+
+/****************************************************************************
+
+Following are the event callback functions
+
+*****************************************************************************/
+
+/* for sta/adhoc mode */
+void update_sta_info(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* ERP */
+	VCS_update(padapter, psta);
+
+	/* HT */
+	if (pmlmepriv->htpriv.ht_option) {
+		psta->htpriv.ht_option = true;
+
+		psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
+
+		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.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);
+
+	spin_lock_bh(&psta->lock);
+	psta->state = _FW_LINKED;
+	spin_unlock_bh(&psta->lock);
+
+}
+
+static void rtw_mlmeext_disconnect(struct adapter *padapter)
+{
+	struct mlme_priv 	*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	u8 state_backup = (pmlmeinfo->state&0x03);
+
+	/* set_opmode_cmd(padapter, infra_client_with_mlme); */
+
+	/*
+	 * 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. */
+	{
+		struct sta_info *psta;
+		psta = rtw_get_stainfo(&padapter->stapriv, get_my_bssid(pnetwork));
+		if (psta)
+			rtw_hal_macid_wakeup(padapter, psta->mac_id);
+	}
+
+	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_);
+
+	pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+	if (state_backup == WIFI_FW_STATION_STATE) {
+		if (rtw_port_switch_chk(padapter) == true) {
+			rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
+			{
+				struct 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);
+			}
+		}
+	}
+
+	/* switch to the 20M Hz mode after disconnect */
+	pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
+	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	flush_all_cam_entry(padapter);
+
+	del_timer_sync(&pmlmeext->link_timer);
+
+	/* pmlmepriv->LinkDetectInfo.TrafficBusyState = false; */
+	pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
+	pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
+
+}
+
+void mlmeext_joinbss_event_callback(struct adapter *padapter, int join_res)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*cur_network = &(pmlmeinfo->network);
+	struct sta_priv 	*pstapriv = &padapter->stapriv;
+	u8 join_type;
+	struct sta_info *psta;
+	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;
+	}
+
+	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);
+
+	/* Set cur_channel&cur_bwmode&cur_ch_offset */
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
+	if (psta) { /* only for infra. mode */
+
+		pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+		/* DBG_871X("set_sta_rate\n"); */
+
+		psta->wireless_mode = pmlmeext->cur_wireless_mode;
+
+		/* set per sta rate after updating HT cap. */
+		set_sta_rate(padapter, psta);
+
+		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);
+	}
+
+	if (rtw_port_switch_chk(padapter) == true)
+		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); */
+	}
+
+	if (get_iface_type(padapter) == IFACE_PORT0)
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
+
+exit_mlmeext_joinbss_event_callback:
+
+	DBG_871X("=>%s\n", __func__);
+
+}
+
+/* currently only adhoc mode will go here */
+void mlmeext_sta_add_event_callback(struct adapter *padapter, struct sta_info *psta)
+{
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 join_type;
+
+	DBG_871X("%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) {
+				pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
+
+				pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
+
+				return;
+			}
+
+			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+
+		}
+
+		join_type = 2;
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	}
+
+	pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+	psta->bssratelen = rtw_get_rateset_len(pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates);
+	memcpy(psta->bssrateset, pmlmeinfo->FW_sta_info[psta->mac_id].SupportedRates, psta->bssratelen);
+
+	/* 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(struct adapter *padapter)
+{
+	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(struct adapter *padapter)
+{
+	int i;
+	struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
+	int UndecoratedSmoothedPWDB;
+	struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter);
+
+	if (padapter->bLinkInfoDump) {
+
+		DBG_871X("\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, &UndecoratedSmoothedPWDB);
+
+			DBG_871X("AP[" MAC_FMT "] - UndecoratedSmoothedPWDB:%d\n",
+				MAC_ARG(padapter->mlmepriv.cur_network.network.MacAddress), UndecoratedSmoothedPWDB);
+		} else if ((pmlmeinfo->state&0x03) == _HW_STATE_AP_) {
+			struct list_head	*phead, *plist;
+
+			struct sta_info *psta = NULL;
+			struct sta_priv *pstapriv = &padapter->stapriv;
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			phead = &pstapriv->asoc_list;
+			plist = get_next(phead);
+			while (phead != plist) {
+				psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+				plist = get_next(plist);
+
+				DBG_871X("STA[" MAC_FMT "]:UndecoratedSmoothedPWDB:%d\n",
+					MAC_ARG(psta->hwaddr), psta->rssi_stat.UndecoratedSmoothedPWDB);
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+		}
+		for (i = 0; i < NUM_STA; i++) {
+			if (pdvobj->macid[i] == true) {
+				if (i != 1) /* skip bc/mc sta */
+					/*   tx info ============ */
+					rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP, &i);
+			}
+		}
+		rtw_hal_set_def_var(padapter, HAL_DEF_DBG_RX_INFO_DUMP, NULL);
+
+
+	}
+
+
+}
+
+static u8 chk_ap_is_alive(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 ret = false;
+
+	#ifdef DBG_EXPIRATION_CHK
+	DBG_871X(FUNC_ADPT_FMT" rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu"
+				/*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/
+				", retry:%u\n"
+		, FUNC_ADPT_ARG(padapter)
+		, STA_RX_PKTS_DIFF_ARG(psta)
+		, psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts
+		, psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts
+		/*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts
+		, psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts
+		, psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts
+		, pmlmeinfo->bcn_interval*/
+		, pmlmeext->retry
+	);
+
+	DBG_871X(FUNC_ADPT_FMT" tx_pkts:%llu, link_count:%u\n", FUNC_ADPT_ARG(padapter)
+		, padapter->xmitpriv.tx_pkts
+		, pmlmeinfo->link_count
+	);
+	#endif
+
+	if ((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta))
+		&& sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta)
+		&& sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta)
+	) {
+		ret = false;
+	} else{
+		ret = true;
+	}
+
+	sta_update_last_rx_pkts(psta);
+
+	return ret;
+}
+
+void linked_status_chk(struct adapter *padapter)
+{
+	u32 i;
+	struct sta_info 	*psta;
+	struct xmit_priv 	*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_priv 	*pstapriv = &padapter->stapriv;
+
+
+	if (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;
+
+		#if defined(DBG_ROAMING_TEST)
+		rx_chk_limit = 1;
+		#else
+		rx_chk_limit = 8;
+		#endif
+			link_count_limit = 7; /*  16 sec */
+
+		/*  Marked by Kurt 20130715 */
+		/*  For WiDi 3.5 and latered on, they don't ask WiDi sink to do roaming, so we could not check rx limit that strictly. */
+		/*  todo: To check why we under miracast session, rx_chk would be false */
+		/* ifdef CONFIG_INTEL_WIDI */
+		/* if (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_NONE) */
+		/* 	rx_chk_limit = 1; */
+		/* endif */
+
+		psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
+		if (psta != NULL) {
+			if (chk_ap_is_alive(padapter, psta) == false)
+				rx_chk = _FAIL;
+
+			if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
+				tx_chk = _FAIL;
+
+			{
+				if (rx_chk != _SUCCESS) {
+					if (pmlmeext->retry == 0) {
+						#ifdef DBG_EXPIRATION_CHK
+						DBG_871X("issue_probereq to trigger probersp, retry =%d\n", pmlmeext->retry);
+						#endif
+						issue_probereq_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 DBG_EXPIRATION_CHK
+					DBG_871X("%s issue_nulldata 0\n", __func__);
+					#endif
+					tx_chk = issue_nulldata_in_interrupt(padapter, NULL);
+				}
+			}
+
+			if (rx_chk == _FAIL) {
+				pmlmeext->retry++;
+				if (pmlmeext->retry > rx_chk_limit) {
+					DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n",
+						FUNC_ADPT_ARG(padapter));
+					receive_disconnect(padapter, pmlmeinfo->network.MacAddress
+						, WLAN_REASON_EXPIRATION_CHK);
+					return;
+				}
+			} else {
+				pmlmeext->retry = 0;
+			}
+
+			if (tx_chk == _FAIL) {
+				pmlmeinfo->link_count %= (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)) {
+		/* linked IBSS mode */
+		/* for each assoc list entry to check the rx pkt counter */
+		for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
+			if (pmlmeinfo->FW_sta_info[i].status == 1) {
+				psta = pmlmeinfo->FW_sta_info[i].psta;
+
+				if (NULL == psta)
+					continue;
+
+				if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta)) {
+
+					if (pmlmeinfo->FW_sta_info[i].retry < 3) {
+						pmlmeinfo->FW_sta_info[i].retry++;
+					} else{
+						pmlmeinfo->FW_sta_info[i].retry = 0;
+						pmlmeinfo->FW_sta_info[i].status = 0;
+						report_del_sta_event(padapter, psta->hwaddr
+							, 65535/*  indicate disconnect caused by no rx */
+						);
+					}
+				} else{
+					pmlmeinfo->FW_sta_info[i].retry = 0;
+					pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
+				}
+			}
+		}
+
+		/* set_link_timer(pmlmeext, DISCONNECT_TO); */
+
+	}
+
+}
+
+void survey_timer_hdl(struct adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct sitesurvey_parm	*psurveyPara;
+	struct cmd_priv 				*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+
+	/* DBG_871X("marc: survey timer\n"); */
+
+	/* issue rtw_sitesurvey_cmd */
+	if (pmlmeext->sitesurvey_res.state > SCAN_START) {
+		if (pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS) {
+			pmlmeext->sitesurvey_res.channel_idx++;
+		}
+
+		if (pmlmeext->scan_abort == true) {
+			{
+				pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
+				DBG_871X("%s idx:%d\n", __func__
+					, pmlmeext->sitesurvey_res.channel_idx
+				);
+			}
+
+			pmlmeext->scan_abort = false;/* reset */
+		}
+
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL) {
+			goto exit_survey_timer_hdl;
+		}
+
+		psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm));
+		if (psurveyPara == NULL) {
+			kfree((unsigned char *)ph2c);
+			goto exit_survey_timer_hdl;
+		}
+
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+		rtw_enqueue_cmd(pcmdpriv, ph2c);
+	}
+
+
+exit_survey_timer_hdl:
+
+	return;
+}
+
+void link_timer_hdl(struct adapter *padapter)
+{
+	/* 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; */
+
+
+	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+		DBG_871X("link_timer_hdl:no beacon while connecting\n");
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		report_join_res(padapter, -3);
+	} else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) {
+		/* re-auth timer */
+		if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) {
+			/* if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto) */
+			/*  */
+				pmlmeinfo->state = 0;
+				report_join_res(padapter, -1);
+				return;
+			/*  */
+			/* else */
+			/*  */
+			/* 	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */
+			/* 	pmlmeinfo->reauth_count = 0; */
+			/*  */
+		}
+
+		DBG_871X("link_timer_hdl: auth timeout and try again\n");
+		pmlmeinfo->auth_seq = 1;
+		issue_auth(padapter, NULL, 0);
+		set_link_timer(pmlmeext, REAUTH_TO);
+	} else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) {
+		/* re-assoc timer */
+		if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) {
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+			report_join_res(padapter, -2);
+			return;
+		}
+
+		DBG_871X("link_timer_hdl: assoc timeout and try again\n");
+		issue_assocreq(padapter);
+		set_link_timer(pmlmeext, REASSOC_TO);
+	}
+
+	return;
+}
+
+void addba_timer_hdl(struct sta_info *psta)
+{
+	struct ht_priv *phtpriv;
+
+	if (!psta)
+		return;
+
+	phtpriv = &psta->htpriv;
+
+	if ((phtpriv->ht_option == true) && (phtpriv->ampdu_enable == true)) {
+		if (phtpriv->candidate_tid_bitmap)
+			phtpriv->candidate_tid_bitmap = 0x0;
+
+	}
+}
+
+void sa_query_timer_hdl(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	/* disconnect */
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+		rtw_disassoc_cmd(padapter, 0, true);
+		rtw_indicate_disconnect(padapter);
+		rtw_free_assoc_resources(padapter, 1);
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+	DBG_871X("SA query timeout disconnect\n");
+}
+
+u8 NULL_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	return H2C_SUCCESS;
+}
+
+#ifdef CONFIG_AUTO_AP_MODE
+static int rtw_auto_ap_start_beacon(struct 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 */
+	*(u16 *)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 (check_buddy_fwstate(adapter, _FW_LINKED) &&
+		check_buddy_fwstate(adapter, WIFI_STATION_STATE)) {
+		struct adapter *pbuddystruct adapter = adapter->pbuddystruct adapter;
+		struct mlme_ext_priv *pbuddy_mlmeext  = &pbuddystruct adapter->mlmeextpriv;
+
+		oper_channel = pbuddy_mlmeext->cur_channel;
+	} 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);
+	}
+
+	DBG_871X("%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;
+	}
+
+
+	kfree(pbuf);
+
+	return ret;
+
+}
+#endif/* CONFIG_AUTO_AP_MODE */
+
+u8 setopmode_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	u8 type;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
+
+	if (psetop->mode == Ndis802_11APMode) {
+		pmlmeinfo->state = WIFI_FW_AP_STATE;
+		type = _HW_STATE_AP_;
+		/* start_ap_mode(padapter); */
+	} else if (psetop->mode == Ndis802_11Infrastructure) {
+		pmlmeinfo->state &= ~(BIT(0)|BIT(1));/*  clear state */
+		pmlmeinfo->state |= WIFI_FW_STATION_STATE;/* set to	STATION_STATE */
+		type = _HW_STATE_STATION_;
+	} else if (psetop->mode == Ndis802_11IBSS) {
+		type = _HW_STATE_ADHOC_;
+	} else{
+		type = _HW_STATE_NOLINK_;
+	}
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
+	/* Set_NETYPE0_MSR(padapter, type); */
+
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (psetop->mode == Ndis802_11APMode)
+		rtw_auto_ap_start_beacon(padapter);
+#endif
+
+	if (rtw_port_switch_chk(padapter) == true) {
+		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) {
+			struct 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);
+		}
+	}
+
+	if (psetop->mode == Ndis802_11APMode) {
+		/*  Do this after port switch to */
+		/*  prevent from downloading rsvd page to wrong port */
+		rtw_btcoex_MediaStatusNotify(padapter, 1); /* connect */
+	}
+
+	return H2C_SUCCESS;
+
+}
+
+u8 createbss_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex	*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
+	/* u32 initialgain; */
+
+	if (pmlmeinfo->state == WIFI_FW_AP_STATE) {
+		struct wlan_bssid_ex *network = &padapter->mlmepriv.cur_network.network;
+		start_bss_network(padapter, (u8 *)network);
+		return H2C_SUCCESS;
+	}
+
+	/* below is for ad-hoc master */
+	if (pparm->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;
+
+		/* disable dynamic functions, such as high power, DIG */
+		Save_DM_Func_Flag(padapter);
+		Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false);
+
+		/* config the initial gain under linking, need to write the BB registers */
+		/* initialgain = 0x1E; */
+		/* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
+
+		/* cancel link timer */
+		del_timer_sync(&pmlmeext->link_timer);
+
+		/* clear CAM */
+		flush_all_cam_entry(padapter);
+
+		memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, IELength));
+		pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength;
+
+		if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
+			return H2C_PARAMETERS_ERROR;
+
+		memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength);
+
+		start_create_ibss(padapter);
+
+	}
+
+	return H2C_SUCCESS;
+
+}
+
+u8 join_cmd_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	u8 join_type;
+	struct ndis_80211_var_ie *pIE;
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	u32 i;
+	u8 cbw40_enable = 0;
+	/* u32 initialgain; */
+	/* u32 acparm; */
+	u8 ch, bw, offset;
+
+	/* 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);
+
+		del_timer_sync(&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);
+	}
+
+	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;
+	pmlmeinfo->bwmode_updated = false;
+	/* pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX; */
+	pmlmeinfo->VHT_enable = 0;
+
+	memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, IELength));
+	pnetwork->IELength = ((struct wlan_bssid_ex *)pbuf)->IELength;
+
+	if (pnetwork->IELength > MAX_IE_SZ)/* Check pbuf->IELength */
+		return H2C_PARAMETERS_ERROR;
+
+	memcpy(pnetwork->IEs, ((struct wlan_bssid_ex *)pbuf)->IEs, pnetwork->IELength);
+
+	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	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(struct ndis_802_11_fix_ie) */
+	for (i = _FIXED_IE_LENGTH_; i < pnetwork->IELength;) {
+		pIE = (struct ndis_80211_var_ie *)(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;
+
+			/* spec case only for cisco's ap because cisco's ap issue assoc rsp using mcs rate @40MHz or @20MHz */
+			{
+				struct HT_info_element *pht_info = (struct HT_info_element *)(pIE->data);
+
+				if (pnetwork->Configuration.DSConfig > 14) {
+					if ((pregpriv->bw_mode >> 4) > CHANNEL_WIDTH_20)
+						cbw40_enable = 1;
+				} else {
+					if ((pregpriv->bw_mode & 0x0f) > CHANNEL_WIDTH_20)
+						cbw40_enable = 1;
+				}
+
+				if ((cbw40_enable) && (pht_info->infos[0] & BIT(2))) {
+					/* switch to the 40M Hz mode according to the AP */
+					pmlmeext->cur_bwmode = CHANNEL_WIDTH_40;
+					switch (pht_info->infos[0] & 0x3) {
+					case 1:
+						pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+						break;
+
+					case 3:
+						pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+						break;
+
+					default:
+						pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+						pmlmeext->cur_bwmode = CHANNEL_WIDTH_20;
+						break;
+					}
+
+					DBG_871X("set HT ch/bw before connected\n");
+				}
+			}
+			break;
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+	/* check channel, bandwidth, offset and switch */
+	if (rtw_chk_start_clnt_join(padapter, &ch, &bw, &offset) == _FAIL) {
+		report_join_res(padapter, (-4));
+		return H2C_SUCCESS;
+	}
+
+	/* disable dynamic functions, such as high power, DIG */
+	/* Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false); */
+
+	/* config the initial gain under linking, need to write the BB registers */
+	/* initialgain = 0x1E; */
+	/* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
+	join_type = 0;
+	rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK, NULL);
+
+	set_channel_bwmode(padapter, ch, offset, bw);
+
+	/* cancel link timer */
+	del_timer_sync(&pmlmeext->link_timer);
+
+	start_clnt_join(padapter);
+
+	return H2C_SUCCESS;
+
+}
+
+u8 disconnect_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*pnetwork = (struct wlan_bssid_ex *)(&(pmlmeinfo->network));
+	u8 val8;
+
+	if (is_client_associated_to_ap(padapter)) {
+			issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100);
+	}
+
+	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 int rtw_scan_ch_decision(struct adapter *padapter, struct rtw_ieee80211_channel *out,
+	u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
+{
+	int i, j;
+	int set_idx;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	/* clear first */
+	memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num);
+
+	/* acquire channels from in */
+	j = 0;
+	for (i = 0; i < in_num; i++) {
+
+		DBG_871X(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[i]));
+
+		set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value);
+		if (in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED)
+			&& set_idx >= 0
+			&& rtw_mlme_band_check(padapter, in[i].hw_value) == true
+		) {
+			if (j >= out_num) {
+				DBG_871X_LEVEL(_drv_always_, 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++) {
+
+			DBG_871X(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter), pmlmeext->channel_set[i].ChannelNum);
+
+			if (rtw_mlme_band_check(padapter, pmlmeext->channel_set[i].ChannelNum) == true) {
+
+				if (j >= out_num) {
+					DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" out_num:%u not enough\n",
+						FUNC_ADPT_ARG(padapter), out_num);
+					break;
+				}
+
+				out[j].hw_value = pmlmeext->channel_set[i].ChannelNum;
+
+				if (pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
+					out[j].flags |= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
+
+				j++;
+			}
+		}
+	}
+
+	return j;
+}
+
+u8 sitesurvey_cmd_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct sitesurvey_parm	*pparm = (struct sitesurvey_parm *)pbuf;
+	u8 bdelayscan = false;
+	u8 val8;
+	u32 initialgain;
+	u32 i;
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE) {
+		pmlmeext->sitesurvey_res.state = SCAN_START;
+		pmlmeext->sitesurvey_res.bss_cnt = 0;
+		pmlmeext->sitesurvey_res.channel_idx = 0;
+
+		for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
+			if (pparm->ssid[i].SsidLength) {
+				memcpy(pmlmeext->sitesurvey_res.ssid[i].Ssid, pparm->ssid[i].Ssid, IW_ESSID_MAX_SIZE);
+				pmlmeext->sitesurvey_res.ssid[i].SsidLength = pparm->ssid[i].SsidLength;
+			} else {
+				pmlmeext->sitesurvey_res.ssid[i].SsidLength = 0;
+			}
+		}
+
+		pmlmeext->sitesurvey_res.ch_num = rtw_scan_ch_decision(padapter
+			, pmlmeext->sitesurvey_res.ch, RTW_CHANNEL_SCAN_AMOUNT
+			, pparm->ch, pparm->ch_num
+		);
+
+		pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
+
+		/* issue null data if associating to the AP */
+		if (is_client_associated_to_ap(padapter) == true) {
+			pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
+
+			issue_nulldata(padapter, NULL, 1, 3, 500);
+
+			bdelayscan = true;
+		}
+		if (bdelayscan) {
+			/* delay 50ms to protect nulldata(1). */
+			set_survey_timer(pmlmeext, 50);
+			return H2C_SUCCESS;
+		}
+	}
+
+	if ((pmlmeext->sitesurvey_res.state == SCAN_START) || (pmlmeext->sitesurvey_res.state == SCAN_TXNULL)) {
+		/* disable dynamic functions, such as high power, DIG */
+		Save_DM_Func_Flag(padapter);
+		Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, false);
+
+		/* config the initial gain under scaning, need to write the BB registers */
+			initialgain = 0x1e;
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
+
+		/* set MSR to no link state */
+		Set_MSR(padapter, _HW_STATE_NOLINK_);
+
+		val8 = 1; /* under site survey */
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+
+		pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
+	}
+
+	site_survey(padapter);
+
+	return H2C_SUCCESS;
+
+}
+
+u8 setauth_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct setauth_parm		*pparm = (struct setauth_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pparm->mode < 4) {
+		pmlmeinfo->auth_algo = pparm->mode;
+	}
+
+	return	H2C_SUCCESS;
+}
+
+u8 setkey_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	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;
+
+	/* main tx key for wep. */
+	if (pparm->set_tx)
+		pmlmeinfo->key_index = pparm->keyid;
+
+	cam_id = rtw_camid_alloc(padapter, NULL, pparm->keyid);
+
+	if (cam_id < 0) {
+	} else {
+		if (cam_id > 3) /* not default key, searched by A2 */
+			addr = get_bssid(&padapter->mlmepriv);
+		else
+			addr = null_addr;
+
+		ctrl = BIT(15) | BIT6 | ((pparm->algorithm) << 2) | pparm->keyid;
+		write_cam(padapter, cam_id, ctrl, addr, pparm->key);
+		DBG_871X_LEVEL(_drv_always_, "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));
+	}
+
+	if (cam_id >= 0 && cam_id <= 3)
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8 *)true);
+
+	/* allow multicast packets to driver */
+	padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_ON_RCR_AM, null_addr);
+
+	return H2C_SUCCESS;
+}
+
+u8 set_stakey_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	u16 ctrl = 0;
+	s16 cam_id = 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) {
+		DBG_871X_LEVEL(_drv_always_, "%s sta:"MAC_FMT" not found\n", __func__, MAC_ARG(pparm->addr));
+		ret = H2C_REJECTED;
+		goto exit;
+	}
+
+	pmlmeinfo->enc_algo = pparm->algorithm;
+	cam_id = rtw_camid_alloc(padapter, psta, 0);
+	if (cam_id < 0)
+		goto exit;
+
+write_to_cam:
+	if (pparm->algorithm == _NO_PRIVACY_) {
+		while ((cam_id = rtw_camid_search(padapter, pparm->addr, -1)) >= 0) {
+			DBG_871X_LEVEL(_drv_always_, "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 {
+		DBG_871X_LEVEL(_drv_always_, "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(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct addBaReq_parm	*pparm = (struct addBaReq_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr);
+
+	if (!psta)
+		return	H2C_SUCCESS;
+
+	if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
+		((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)) {
+		/* pmlmeinfo->ADDBA_retry_count = 0; */
+		/* pmlmeinfo->candidate_tid_bitmap |= (0x1 << pparm->tid); */
+		/* psta->htpriv.candidate_tid_bitmap |= BIT(pparm->tid); */
+		issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
+		/* _set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO); */
+		_set_timer(&psta->addba_retry_timer, ADDBA_TO);
+	} else{
+		psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
+	}
+	return	H2C_SUCCESS;
+}
+
+
+u8 chk_bmc_sleepq_cmd(struct 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(struct adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct Tx_Beacon_param	*ptxBeacon_parm;
+	struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 res = _SUCCESS;
+	int len_diff = 0;
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param));
+	if (ptxBeacon_parm == NULL) {
+		kfree((unsigned char *)ph2c);
+		res = _FAIL;
+		goto exit;
+	}
+
+	memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(struct wlan_bssid_ex));
+
+	len_diff = update_hidden_ssid(
+		ptxBeacon_parm->network.IEs+_BEACON_IE_OFFSET_
+		, ptxBeacon_parm->network.IELength-_BEACON_IE_OFFSET_
+		, pmlmeinfo->hidden_ssid_mode
+	);
+	ptxBeacon_parm->network.IELength += len_diff;
+
+	init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
+
+	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
+
+exit:
+	return res;
+}
+
+
+u8 mlme_evt_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	u8 evt_code, evt_seq;
+	u16 evt_sz;
+	uint	*peventbuf;
+	void (*event_callback)(struct 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)) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("Evetn Seq Error! %d vs %d\n", (evt_seq & 0x7f), (atomic_read(&pevt_priv->event_seq) & 0x7f)));
+
+		pevt_priv->event_seq = (evt_seq+1)&0x7f;
+
+		goto _abort_event_;
+	}
+	#endif
+
+	/*  checking if event code is valid */
+	if (evt_code >= MAX_C2HEVT) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent Code(%d) mismatch!\n", evt_code));
+		goto _abort_event_;
+	}
+
+	/*  checking if event size match the event parm size */
+	if ((wlanevents[evt_code].parmsize != 0) &&
+			(wlanevents[evt_code].parmsize != evt_sz)) {
+
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
+			evt_code, wlanevents[evt_code].parmsize, evt_sz));
+		goto _abort_event_;
+
+	}
+
+	atomic_inc(&pevt_priv->event_seq);
+
+	peventbuf += 2;
+
+	if (peventbuf) {
+		event_callback = wlanevents[evt_code].event_callback;
+		event_callback(padapter, (u8 *)peventbuf);
+
+		pevt_priv->evt_done_cnt++;
+	}
+
+
+_abort_event_:
+
+
+	return H2C_SUCCESS;
+
+}
+
+u8 h2c_msg_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	return H2C_SUCCESS;
+}
+
+u8 chk_bmc_sleepq_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct sta_info *psta_bmc;
+	struct list_head	*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)) {
+		msleep(10);/*  10ms, ATIM(HIQ) Windows */
+
+		/* spin_lock_bh(&psta_bmc->sleep_q.lock); */
+		spin_lock_bh(&pxmitpriv->lock);
+
+		xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
+		xmitframe_plist = get_next(xmitframe_phead);
+
+		while (xmitframe_phead != xmitframe_plist) {
+			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) == true)
+				pxmitframe->attrib.qsel = 0x11;/* HIQ */
+
+			rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+		}
+
+		/* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
+		spin_unlock_bh(&pxmitpriv->lock);
+
+		/* check hi queue and bmc_sleepq */
+		rtw_chk_hi_queue_cmd(padapter);
+	}
+
+	return H2C_SUCCESS;
+}
+
+u8 tx_beacon_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	if (send_beacon(padapter) == _FAIL) {
+		DBG_871X("issue_beacon, fail!\n");
+		return H2C_PARAMETERS_ERROR;
+	}
+
+	/* tx bc/mc frames after update TIM */
+	chk_bmc_sleepq_hdl(padapter, NULL);
+
+	return H2C_SUCCESS;
+}
+
+int rtw_chk_start_clnt_join(struct adapter *padapter, u8 *ch, u8 *bw, u8 *offset)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	unsigned char cur_ch = pmlmeext->cur_channel;
+	unsigned char cur_bw = pmlmeext->cur_bwmode;
+	unsigned char cur_ch_offset = pmlmeext->cur_ch_offset;
+	bool connect_allow = true;
+
+	if (!ch || !bw || !offset) {
+		rtw_warn_on(1);
+		connect_allow = false;
+	}
+
+	if (connect_allow == true) {
+		DBG_871X("start_join_set_ch_bw: ch =%d, bwmode =%d, ch_offset =%d\n", cur_ch, cur_bw, cur_ch_offset);
+		*ch = cur_ch;
+		*bw = cur_bw;
+		*offset = cur_ch_offset;
+	}
+
+	return connect_allow == true ? _SUCCESS : _FAIL;
+}
+
+/* Find union about ch, bw, ch_offset of all linked/linking interfaces */
+int rtw_get_ch_setting_union(struct adapter *adapter, u8 *ch, u8 *bw, u8 *offset)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct adapter *iface;
+	struct mlme_ext_priv *mlmeext;
+	u8 ch_ret = 0;
+	u8 bw_ret = CHANNEL_WIDTH_20;
+	u8 offset_ret = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	if (ch)
+		*ch = 0;
+	if (bw)
+		*bw = CHANNEL_WIDTH_20;
+	if (offset)
+		*offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	iface = dvobj->padapters;
+	mlmeext = &iface->mlmeextpriv;
+
+	if (!check_fwstate(&iface->mlmepriv, _FW_LINKED|_FW_UNDER_LINKING))
+		return 0;
+
+	ch_ret = mlmeext->cur_channel;
+	bw_ret = mlmeext->cur_bwmode;
+	offset_ret = mlmeext->cur_ch_offset;
+
+	return 1;
+}
+
+u8 set_ch_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	struct set_ch_parm *set_ch_parm;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	set_ch_parm = (struct set_ch_parm *)pbuf;
+
+	DBG_871X(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
+		FUNC_NDEV_ARG(padapter->pnetdev),
+		set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
+
+	pmlmeext->cur_channel = set_ch_parm->ch;
+	pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
+	pmlmeext->cur_bwmode = set_ch_parm->bw;
+
+	set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
+
+	return	H2C_SUCCESS;
+}
+
+u8 set_chplan_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct SetChannelPlan_param *setChannelPlan_param;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
+
+	pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+
+	if ((padapter->rtw_wdev != NULL) && (padapter->rtw_wdev->wiphy)) {
+		struct regulatory_request request;
+		request.initiator = NL80211_REGDOM_SET_BY_DRIVER;
+		rtw_reg_notifier(padapter->rtw_wdev->wiphy, &request);
+	}
+
+	return	H2C_SUCCESS;
+}
+
+u8 led_blink_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct LedBlink_param *ledBlink_param;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	ledBlink_param = (struct LedBlink_param *)pbuf;
+	return	H2C_SUCCESS;
+}
+
+u8 set_csa_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	return	H2C_REJECTED;
+}
+
+/*  TDLS_ESTABLISHED	: write RCR DATA BIT */
+/*  TDLS_CS_OFF		: go back to the channel linked with AP, terminating channel switch procedure */
+/*  TDLS_INIT_CH_SEN	: init channel sensing, receive all data and mgnt frame */
+/*  TDLS_DONE_CH_SEN: channel sensing and report candidate channel */
+/*  TDLS_OFF_CH		: first time set channel to off channel */
+/*  TDLS_BASE_CH		: go back tp the channel linked with AP when set base channel as target channel */
+/*  TDLS_P_OFF_CH	: periodically go to off channel */
+/*  TDLS_P_BASE_CH	: periodically go back to base channel */
+/*  TDLS_RS_RCR		: restore RCR */
+/*  TDLS_TEAR_STA	: free tdls sta */
+u8 tdls_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	return H2C_REJECTED;
+}
+
+u8 run_in_thread_hdl(struct 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;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_odm.c linux/3rdparty/rtl8723bs/core/rtw_odm.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_odm.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_odm.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,195 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 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.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtw_odm.h>
+#include <hal_data.h>
+
+static const char *odm_comp_str[] = {
+	/* BIT0 */"ODM_COMP_DIG",
+	/* BIT1 */"ODM_COMP_RA_MASK",
+	/* BIT2 */"ODM_COMP_DYNAMIC_TXPWR",
+	/* BIT3 */"ODM_COMP_FA_CNT",
+	/* BIT4 */"ODM_COMP_RSSI_MONITOR",
+	/* BIT5 */"ODM_COMP_CCK_PD",
+	/* BIT6 */"ODM_COMP_ANT_DIV",
+	/* BIT7 */"ODM_COMP_PWR_SAVE",
+	/* BIT8 */"ODM_COMP_PWR_TRAIN",
+	/* BIT9 */"ODM_COMP_RATE_ADAPTIVE",
+	/* BIT10 */"ODM_COMP_PATH_DIV",
+	/* BIT11 */"ODM_COMP_PSD",
+	/* BIT12 */"ODM_COMP_DYNAMIC_PRICCA",
+	/* BIT13 */"ODM_COMP_RXHP",
+	/* BIT14 */"ODM_COMP_MP",
+	/* BIT15 */"ODM_COMP_DYNAMIC_ATC",
+	/* BIT16 */"ODM_COMP_EDCA_TURBO",
+	/* BIT17 */"ODM_COMP_EARLY_MODE",
+	/* BIT18 */NULL,
+	/* BIT19 */NULL,
+	/* BIT20 */NULL,
+	/* BIT21 */NULL,
+	/* BIT22 */NULL,
+	/* BIT23 */NULL,
+	/* BIT24 */"ODM_COMP_TX_PWR_TRACK",
+	/* BIT25 */"ODM_COMP_RX_GAIN_TRACK",
+	/* BIT26 */"ODM_COMP_CALIBRATION",
+	/* BIT27 */NULL,
+	/* BIT28 */NULL,
+	/* BIT29 */NULL,
+	/* BIT30 */"ODM_COMP_COMMON",
+	/* BIT31 */"ODM_COMP_INIT",
+};
+
+#define RTW_ODM_COMP_MAX 32
+
+static const char *odm_ability_str[] = {
+	/* BIT0 */"ODM_BB_DIG",
+	/* BIT1 */"ODM_BB_RA_MASK",
+	/* BIT2 */"ODM_BB_DYNAMIC_TXPWR",
+	/* BIT3 */"ODM_BB_FA_CNT",
+	/* BIT4 */"ODM_BB_RSSI_MONITOR",
+	/* BIT5 */"ODM_BB_CCK_PD",
+	/* BIT6 */"ODM_BB_ANT_DIV",
+	/* BIT7 */"ODM_BB_PWR_SAVE",
+	/* BIT8 */"ODM_BB_PWR_TRAIN",
+	/* BIT9 */"ODM_BB_RATE_ADAPTIVE",
+	/* BIT10 */"ODM_BB_PATH_DIV",
+	/* BIT11 */"ODM_BB_PSD",
+	/* BIT12 */"ODM_BB_RXHP",
+	/* BIT13 */"ODM_BB_ADAPTIVITY",
+	/* BIT14 */"ODM_BB_DYNAMIC_ATC",
+	/* BIT15 */NULL,
+	/* BIT16 */"ODM_MAC_EDCA_TURBO",
+	/* BIT17 */"ODM_MAC_EARLY_MODE",
+	/* BIT18 */NULL,
+	/* BIT19 */NULL,
+	/* BIT20 */NULL,
+	/* BIT21 */NULL,
+	/* BIT22 */NULL,
+	/* BIT23 */NULL,
+	/* BIT24 */"ODM_RF_TX_PWR_TRACK",
+	/* BIT25 */"ODM_RF_RX_GAIN_TRACK",
+	/* BIT26 */"ODM_RF_CALIBRATION",
+};
+
+#define RTW_ODM_ABILITY_MAX 27
+
+static const char *odm_dbg_level_str[] = {
+	NULL,
+	"ODM_DBG_OFF",
+	"ODM_DBG_SERIOUS",
+	"ODM_DBG_WARNING",
+	"ODM_DBG_LOUD",
+	"ODM_DBG_TRACE",
+};
+
+#define RTW_ODM_DBG_LEVEL_NUM 6
+
+void rtw_odm_dbg_comp_msg(void *sel, struct adapter *adapter)
+{
+	u64 dbg_comp;
+	int i;
+
+	rtw_hal_get_def_var(adapter, HW_DEF_ODM_DBG_FLAG, &dbg_comp);
+	DBG_871X_SEL_NL(sel, "odm.DebugComponents = 0x%016llx\n", dbg_comp);
+	for (i = 0; i < RTW_ODM_COMP_MAX; i++) {
+		if (odm_comp_str[i])
+		DBG_871X_SEL_NL(sel, "%cBIT%-2d %s\n",
+			(BIT0 << i) & dbg_comp ? '+' : ' ', i, odm_comp_str[i]);
+	}
+}
+
+inline void rtw_odm_dbg_comp_set(struct adapter *adapter, u64 comps)
+{
+	rtw_hal_set_def_var(adapter, HW_DEF_ODM_DBG_FLAG, &comps);
+}
+
+void rtw_odm_dbg_level_msg(void *sel, struct adapter *adapter)
+{
+	u32 dbg_level;
+	int i;
+
+	rtw_hal_get_def_var(adapter, HW_DEF_ODM_DBG_LEVEL, &dbg_level);
+	DBG_871X_SEL_NL(sel, "odm.DebugLevel = %u\n", dbg_level);
+	for (i = 0; i < RTW_ODM_DBG_LEVEL_NUM; i++) {
+		if (odm_dbg_level_str[i])
+			DBG_871X_SEL_NL(sel, "%u %s\n", i, odm_dbg_level_str[i]);
+	}
+}
+
+inline void rtw_odm_dbg_level_set(struct adapter *adapter, u32 level)
+{
+	rtw_hal_set_def_var(adapter, HW_DEF_ODM_DBG_LEVEL, &level);
+}
+
+void rtw_odm_ability_msg(void *sel, struct adapter *adapter)
+{
+	u32 ability = 0;
+	int i;
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_DM_FLAG, (u8 *)&ability);
+	DBG_871X_SEL_NL(sel, "odm.SupportAbility = 0x%08x\n", ability);
+	for (i = 0; i < RTW_ODM_ABILITY_MAX; i++) {
+		if (odm_ability_str[i])
+		DBG_871X_SEL_NL(sel, "%cBIT%-2d %s\n",
+			(BIT0 << i) & ability ? '+' : ' ', i, odm_ability_str[i]);
+	}
+}
+
+inline void rtw_odm_ability_set(struct adapter *adapter, u32 ability)
+{
+	rtw_hal_set_hwreg(adapter, HW_VAR_DM_FLAG, (u8 *)&ability);
+}
+
+void rtw_odm_adaptivity_parm_msg(void *sel, struct adapter *adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &pHalData->odmpriv;
+
+	DBG_871X_SEL_NL(sel, "%10s %16s %8s %10s %11s %14s\n"
+		, "TH_L2H_ini", "TH_EDCCA_HL_diff", "IGI_Base", "ForceEDCCA", "AdapEn_RSSI", "IGI_LowerBound");
+	DBG_871X_SEL_NL(sel, "0x%-8x %-16d 0x%-6x %-10d %-11u %-14u\n"
+		, (u8)odm->TH_L2H_ini
+		, odm->TH_EDCCA_HL_diff
+		, odm->IGI_Base
+		, odm->ForceEDCCA
+		, odm->AdapEn_RSSI
+		, odm->IGI_LowerBound
+	);
+}
+
+void rtw_odm_adaptivity_parm_set(struct adapter *adapter, s8 TH_L2H_ini, s8 TH_EDCCA_HL_diff,
+	s8 IGI_Base, bool ForceEDCCA, u8 AdapEn_RSSI, u8 IGI_LowerBound)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &pHalData->odmpriv;
+
+	odm->TH_L2H_ini = TH_L2H_ini;
+	odm->TH_EDCCA_HL_diff = TH_EDCCA_HL_diff;
+	odm->IGI_Base = IGI_Base;
+	odm->ForceEDCCA = ForceEDCCA;
+	odm->AdapEn_RSSI = AdapEn_RSSI;
+	odm->IGI_LowerBound = IGI_LowerBound;
+}
+
+void rtw_odm_get_perpkt_rssi(void *sel, struct adapter *adapter)
+{
+	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &(hal_data->odmpriv);
+
+	DBG_871X_SEL_NL(sel, "RxRate = %s, RSSI_A = %d(%%), RSSI_B = %d(%%)\n",
+	HDATA_RATE(odm->RxRate), odm->RSSI_A, odm->RSSI_B);
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_pwrctrl.c linux/3rdparty/rtl8723bs/core/rtw_pwrctrl.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_pwrctrl.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_pwrctrl.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,1421 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#define _RTW_PWRCTRL_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+#include <linux/jiffies.h>
+
+
+void _ips_enter(struct 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++;
+	DBG_871X("==>ips_enter cnts:%d\n", pwrpriv->ips_enter_cnts);
+
+	if (rf_off == pwrpriv->change_rfpwrstate) {
+		pwrpriv->bpower_saving = true;
+		DBG_871X_LEVEL(_drv_always_, "nolinked power save enter\n");
+
+		if (pwrpriv->ips_mode == IPS_LEVEL_2)
+			pwrpriv->bkeepfwalive = true;
+
+		rtw_ips_pwr_down(padapter);
+		pwrpriv->rf_pwrstate = rf_off;
+	}
+	pwrpriv->bips_processing = false;
+
+}
+
+void ips_enter(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+
+	rtw_btcoex_IpsNotify(padapter, pwrpriv->ips_mode_req);
+
+	down(&pwrpriv->lock);
+	_ips_enter(padapter);
+	up(&pwrpriv->lock);
+}
+
+int _ips_leave(struct 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++;
+		DBG_871X("==>ips_leave cnts:%d\n", pwrpriv->ips_leave_cnts);
+
+		result = rtw_ips_pwr_up(padapter);
+		if (result == _SUCCESS) {
+			pwrpriv->rf_pwrstate = rf_on;
+		}
+		DBG_871X_LEVEL(_drv_always_, "nolinked power save leave\n");
+
+		DBG_871X("==> 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(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	int ret;
+
+	if (!is_primary_adapter(padapter))
+		return _SUCCESS;
+
+	down(&pwrpriv->lock);
+	ret = _ips_leave(padapter);
+	up(&pwrpriv->lock);
+
+	if (_SUCCESS == ret)
+		rtw_btcoex_IpsNotify(padapter, IPS_NONE);
+
+	return ret;
+}
+
+static bool rtw_pwr_unassociated_idle(struct adapter *adapter)
+{
+	struct adapter *buddy = adapter->pbuddy_adapter;
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+	struct xmit_priv *pxmit_priv = &adapter->xmitpriv;
+
+	bool ret = false;
+
+	if (adapter_to_pwrctl(adapter)->bpower_saving == true) {
+		/* DBG_871X("%s: already in LPS or IPS mode\n", __func__); */
+		goto exit;
+	}
+
+	if (time_before(jiffies, adapter_to_pwrctl(adapter)->ips_deny_time)) {
+		/* DBG_871X("%s ips_deny_time\n", __func__); */
+		goto exit;
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
+		|| check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
+		|| check_fwstate(pmlmepriv, WIFI_AP_STATE)
+		|| check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
+	)
+		goto exit;
+
+	/* consider buddy, if exist */
+	if (buddy) {
+		struct mlme_priv *b_pmlmepriv = &(buddy->mlmepriv);
+
+		if (check_fwstate(b_pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
+			|| check_fwstate(b_pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
+			|| check_fwstate(b_pmlmepriv, WIFI_AP_STATE)
+			|| check_fwstate(b_pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
+		)
+			goto exit;
+	}
+
+	if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF ||
+		pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) {
+		DBG_871X_LEVEL(_drv_always_, "There are some pkts to transmit\n");
+		DBG_871X_LEVEL(_drv_always_, "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(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	u32 ps_deny = 0;
+
+	down(&adapter_to_pwrctl(padapter)->lock);
+	ps_deny = rtw_ps_deny_get(padapter);
+	up(&adapter_to_pwrctl(padapter)->lock);
+	if (ps_deny != 0) {
+		DBG_871X(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++;
+		DBG_871X("%s, pwrpriv->bInSuspend == true ignore this process\n", __func__);
+		return;
+	}
+
+	pwrpriv->ps_processing = true;
+
+	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)) {
+		DBG_871X("==>%s\n", __func__);
+		pwrpriv->change_rfpwrstate = rf_off;
+		{
+			ips_enter(padapter);
+		}
+	}
+exit:
+	pwrpriv->ps_processing = false;
+	return;
+}
+
+void pwr_state_check_handler(RTW_TIMER_HDL_ARGS);
+void pwr_state_check_handler(RTW_TIMER_HDL_ARGS)
+{
+	struct adapter *padapter = (struct adapter *)FunctionContext;
+	rtw_ps_cmd(padapter);
+}
+
+void traffic_check_for_leave_lps(struct adapter *padapter, u8 tx, u32 tx_packets)
+{
+	static unsigned long 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 (jiffies_to_msecs(jiffies - 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)
+					&& (rtw_btcoex_IsBtControlLps(padapter) == false)
+					) {
+					DBG_871X("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)
+				&& (rtw_btcoex_IsBtControlLps(padapter) == false)
+				) {
+				DBG_871X("leave lps via Rx = %d\n", pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod);
+				bLeaveLPS = true;
+			}
+		}
+	}
+
+	if (bLeaveLPS)
+		/* DBG_871X("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, LPS_CTRL_LEAVE, tx?0:1);
+}
+
+/*
+ * 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(struct adapter *padapter, u8 pslv)
+{
+	u8 rpwm;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	u8 cpwm_orig;
+
+	pslv = PS_STATE(pslv);
+
+	if (pwrpriv->brpwmtimeout == true) {
+		DBG_871X("%s: RPWM timeout, force to set RPWM(0x%02X) again!\n", __func__, pslv);
+	} else{
+		if ((pwrpriv->rpwm == pslv)
+			|| ((pwrpriv->rpwm >= PS_STATE_S2) && (pslv >= PS_STATE_S2))) {
+			RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+				("%s: Already set rpwm[0x%02X], new = 0x%02X!\n", __func__, pwrpriv->rpwm, pslv));
+			return;
+		}
+	}
+
+	if ((padapter->bSurpriseRemoved == true) ||
+		(padapter->hw_init_completed == false)) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+				 ("%s: SurpriseRemoved(%d) hw_init_completed(%d)\n",
+				  __func__, padapter->bSurpriseRemoved, padapter->hw_init_completed));
+
+		pwrpriv->cpwm = PS_STATE_S4;
+
+		return;
+	}
+
+	if (padapter->bDriverStopped == true) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+				 ("%s: change power state(0x%02X) when DriverStopped\n", __func__, pslv));
+
+		if (pslv < PS_STATE_S2) {
+			RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+					 ("%s: Reject to enter PS_STATE(0x%02X) lower than S2 when DriverStopped!!\n", __func__, pslv));
+			return;
+		}
+	}
+
+	rpwm = pslv | pwrpriv->tog;
+	/*  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;
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("rtw_set_rpwm: rpwm = 0x%02x cpwm = 0x%02x\n", rpwm, pwrpriv->cpwm));
+
+	pwrpriv->rpwm = pslv;
+
+	cpwm_orig = 0;
+	if (rpwm & PS_ACK)
+		rtw_hal_get_hwreg(padapter, HW_VAR_CPWM, &cpwm_orig);
+
+	if (rpwm & PS_ACK)
+		_set_timer(&pwrpriv->pwr_rpwm_timer, LPS_RPWM_WAIT_MS);
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm));
+
+	pwrpriv->tog += 0x80;
+
+	/*  No LPS 32K, No Ack */
+	if (rpwm & PS_ACK) {
+		unsigned long start_time;
+		u8 cpwm_now;
+		u8 poll_cnt = 0;
+
+		start_time = jiffies;
+
+		/*  polling cpwm */
+		do {
+			mdelay(1);
+			poll_cnt++;
+			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;
+				break;
+			}
+
+			if (jiffies_to_msecs(jiffies - start_time) > LPS_RPWM_WAIT_MS) {
+				DBG_871X("%s: polling cpwm timeout! poll_cnt =%d, cpwm_orig =%02x, cpwm_now =%02x\n", __func__, poll_cnt, cpwm_orig, cpwm_now);
+				_set_timer(&pwrpriv->pwr_rpwm_timer, 1);
+				break;
+			}
+		} while (1);
+	} else
+		pwrpriv->cpwm = pslv;
+}
+
+static u8 PS_RDY_CHECK(struct adapter *padapter)
+{
+	unsigned long curr_time, delta_time;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+	if (true == pwrpriv->bInSuspend && pwrpriv->wowlan_mode)
+		return true;
+	else if (true == pwrpriv->bInSuspend && pwrpriv->wowlan_ap_mode)
+		return true;
+	else if (true == pwrpriv->bInSuspend)
+		return false;
+#else
+	if (true == 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)
+		|| rtw_is_scan_deny(padapter)
+	)
+		return false;
+
+	if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && (padapter->securitypriv.binstallGrpkey == false)) {
+		DBG_871X("Group handshake still in progress !!!\n");
+		return false;
+	}
+
+	if (!rtw_cfg80211_pwr_mgmt(padapter))
+		return false;
+
+	return true;
+}
+
+void rtw_set_ps_mode(struct adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode, const char *msg)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+	struct debug_priv *pdbgpriv = &padapter->dvobj->drv_dbg;
+#endif
+
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("%s: PowerMode =%d Smart_PS =%d\n",
+			  __func__, ps_mode, smart_ps));
+
+	if (ps_mode > PM_Card_Disable) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_, ("ps_mode:%d error\n", ps_mode));
+		return;
+	}
+
+	if (pwrpriv->pwr_mode == ps_mode)
+		if (PS_MODE_ACTIVE == ps_mode)
+			return;
+
+
+	down(&pwrpriv->lock);
+
+	/* if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) */
+	if (ps_mode == PS_MODE_ACTIVE) {
+		if (1
+			&& (((rtw_btcoex_IsBtControlLps(padapter) == false)
+					)
+				|| ((rtw_btcoex_IsBtControlLps(padapter) == true)
+					&& (rtw_btcoex_IsLpsOn(padapter) == false))
+				)
+			) {
+			DBG_871X(FUNC_ADPT_FMT" Leave 802.11 power save - %s\n",
+				FUNC_ADPT_ARG(padapter), msg);
+
+			pwrpriv->pwr_mode = ps_mode;
+			rtw_set_rpwm(padapter, PS_STATE_S4);
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+			if (pwrpriv->wowlan_mode == true ||
+					pwrpriv->wowlan_ap_mode == true) {
+				unsigned long start_time;
+				u32 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 (jiffies_to_msecs(jiffies - start_time) > delay_ms) {
+						DBG_871X("%s: Wait for FW 32K leave more than %u ms!!!\n",
+								__func__, delay_ms);
+						pdbgpriv->dbg_wow_leave_ps_fail_cnt++;
+						break;
+					}
+					msleep(1);
+				} while (1);
+			}
+#endif
+			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+			pwrpriv->bFwCurrentInPSMode = false;
+
+			rtw_btcoex_LpsNotify(padapter, ps_mode);
+		}
+	} else{
+		if ((PS_RDY_CHECK(padapter) && check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE))
+			|| ((rtw_btcoex_IsBtControlLps(padapter) == true)
+				&& (rtw_btcoex_IsLpsOn(padapter) == true))
+			) {
+			u8 pslv;
+
+			DBG_871X(FUNC_ADPT_FMT" Enter 802.11 power save - %s\n",
+				FUNC_ADPT_ARG(padapter), msg);
+
+			rtw_btcoex_LpsNotify(padapter, ps_mode);
+
+			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));
+
+			pslv = PS_STATE_S2;
+			if (pwrpriv->alives == 0)
+				pslv = PS_STATE_S0;
+
+			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;
+			}
+
+			rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	up(&pwrpriv->lock);
+}
+
+/*
+ * Return:
+ *0:	Leave OK
+ *-1:	Timeout
+ *-2:	Other error
+ */
+s32 LPS_RF_ON_check(struct adapter *padapter, u32 delay_ms)
+{
+	unsigned long 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 (true == bAwake)
+			break;
+
+		if (true == padapter->bSurpriseRemoved) {
+			err = -2;
+			DBG_871X("%s: device surprise removed!!\n", __func__);
+			break;
+		}
+
+		if (jiffies_to_msecs(jiffies - start_time) > delay_ms) {
+			err = -1;
+			DBG_871X("%s: Wait for FW LPS leave more than %u ms!!!\n", __func__, delay_ms);
+			break;
+		}
+		msleep(1);
+	}
+
+	return err;
+}
+
+/*  */
+/* 	Description: */
+/* 		Enter the leisure power save mode. */
+/*  */
+void LPS_Enter(struct adapter *padapter, const char *msg)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(dvobj);
+	int n_assoc_iface = 0;
+	char buf[32] = {0};
+
+	if (rtw_btcoex_IsBtControlLps(padapter) == true)
+		return;
+
+	/* Skip lps enter request if number of assocated adapters is not 1 */
+	if (check_fwstate(&(dvobj->padapters->mlmepriv), WIFI_ASOC_STATE))
+		n_assoc_iface++;
+	if (n_assoc_iface != 1)
+		return;
+
+	/* Skip lps enter request for adapter not port0 */
+	if (get_iface_type(padapter) != IFACE_PORT0)
+		return;
+
+	if (PS_RDY_CHECK(dvobj->padapters) == false)
+			return;
+
+	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++;
+	}
+
+/* 	DBG_871X("-LeisurePSEnter\n"); */
+}
+
+/*  */
+/* 	Description: */
+/* 		Leave the leisure power save mode. */
+/*  */
+void LPS_Leave(struct adapter *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);
+	char buf[32] = {0};
+
+/* 	DBG_871X("+LeisurePSLeave\n"); */
+
+	if (rtw_btcoex_IsBtControlLps(padapter) == true)
+		return;
+
+	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;
+/* 	DBG_871X("-LeisurePSLeave\n"); */
+
+}
+
+void LeaveAllPowerSaveModeDirect(struct adapter *Adapter)
+{
+	struct adapter *pri_padapter = GET_PRIMARY_ADAPTER(Adapter);
+	struct mlme_priv *pmlmepriv = &(Adapter->mlmepriv);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(Adapter);
+
+	DBG_871X("%s.....\n", __func__);
+
+	if (true == Adapter->bSurpriseRemoved) {
+		DBG_871X(FUNC_ADPT_FMT ": bSurpriseRemoved =%d Skip!\n",
+			FUNC_ADPT_ARG(Adapter), Adapter->bSurpriseRemoved);
+		return;
+	}
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED) == true)) { /* connect */
+
+		if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
+			DBG_871X("%s: Driver Already Leave LPS\n", __func__);
+			return;
+		}
+
+		down(&pwrpriv->lock);
+
+		rtw_set_rpwm(Adapter, PS_STATE_S4);
+
+	up(&pwrpriv->lock);
+
+		rtw_lps_ctrl_wk_cmd(pri_padapter, LPS_CTRL_LEAVE, 0);
+	} else{
+		if (pwrpriv->rf_pwrstate == rf_off)
+			if (false == ips_leave(pri_padapter))
+				DBG_871X("======> ips_leave fail.............\n");
+	}
+}
+
+/*  */
+/*  Description: Leave all power save mode: LPS, FwLPS, IPS if needed. */
+/*  Move code to function by tynli. 2010.03.26. */
+/*  */
+void LeaveAllPowerSaveMode(struct adapter *Adapter)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(Adapter);
+	u8 enqueue = 0;
+	int n_assoc_iface = 0;
+
+	if (!Adapter->bup) {
+		DBG_871X(FUNC_ADPT_FMT ": bup =%d Skip!\n",
+			FUNC_ADPT_ARG(Adapter), Adapter->bup);
+		return;
+	}
+
+	if (Adapter->bSurpriseRemoved) {
+		DBG_871X(FUNC_ADPT_FMT ": bSurpriseRemoved =%d Skip!\n",
+			FUNC_ADPT_ARG(Adapter), Adapter->bSurpriseRemoved);
+		return;
+	}
+
+	if (check_fwstate(&(dvobj->padapters->mlmepriv), WIFI_ASOC_STATE))
+		n_assoc_iface++;
+
+	if (n_assoc_iface) { /* connect */
+		enqueue = 1;
+
+		rtw_lps_ctrl_wk_cmd(Adapter, LPS_CTRL_LEAVE, enqueue);
+
+		LPS_Leave_check(Adapter);
+	} else {
+		if (adapter_to_pwrctl(Adapter)->rf_pwrstate == rf_off) {
+			if (false == ips_leave(Adapter))
+				DBG_871X("======> ips_leave fail.............\n");
+		}
+	}
+}
+
+void LPS_Leave_check(
+	struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrpriv;
+	unsigned long	start_time;
+	u8 bReady;
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	bReady = false;
+	start_time = jiffies;
+
+	yield();
+
+	while (1) {
+		down(&pwrpriv->lock);
+
+		if ((padapter->bSurpriseRemoved == true)
+			|| (padapter->hw_init_completed == false)
+			|| (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+			)
+			bReady = true;
+
+		up(&pwrpriv->lock);
+
+		if (true == bReady)
+			break;
+
+		if (jiffies_to_msecs(jiffies - start_time) > 100) {
+			DBG_871X("Wait for cpwm event  than 100 ms!!!\n");
+			break;
+		}
+		msleep(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(
+	struct adapter *padapter,
+	struct reportpwrstate_parm *preportpwrstate)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	down(&pwrpriv->lock);
+
+	if (pwrpriv->rpwm < PS_STATE_S2) {
+		DBG_871X("%s: Redundant CPWM Int. RPWM = 0x%02X CPWM = 0x%02x\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+		up(&pwrpriv->lock);
+		goto exit;
+	}
+
+	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);
+	}
+
+	up(&pwrpriv->lock);
+
+exit:
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("cpwm_int_hdl: cpwm = 0x%02x\n", pwrpriv->cpwm));
+}
+
+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);
+	struct adapter *adapter = dvobj->if1;
+	struct reportpwrstate_parm report;
+
+	/* DBG_871X("%s\n", __func__); */
+
+	report.state = PS_STATE_S2;
+	cpwm_int_hdl(adapter, &report);
+}
+
+static void rpwmtimeout_workitem_callback(struct work_struct *work)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *dvobj;
+	struct pwrctrl_priv *pwrpriv;
+
+
+	pwrpriv = container_of(work, struct pwrctrl_priv, rpwmtimeoutwi);
+	dvobj = pwrctl_to_dvobj(pwrpriv);
+	padapter = dvobj->if1;
+/* 	DBG_871X("+%s: rpwm = 0x%02X cpwm = 0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); */
+
+	down(&pwrpriv->lock);
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
+		DBG_871X("%s: rpwm = 0x%02X cpwm = 0x%02X CPWM done!\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+		goto exit;
+	}
+	up(&pwrpriv->lock);
+
+	if (rtw_read8(padapter, 0x100) != 0xEA) {
+		struct reportpwrstate_parm report;
+
+		report.state = PS_STATE_S2;
+		DBG_871X("\n%s: FW already leave 32K!\n\n", __func__);
+		cpwm_int_hdl(padapter, &report);
+
+		return;
+	}
+
+	down(&pwrpriv->lock);
+
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
+		DBG_871X("%s: cpwm =%d, nothing to do!\n", __func__, pwrpriv->cpwm);
+		goto exit;
+	}
+	pwrpriv->brpwmtimeout = true;
+	rtw_set_rpwm(padapter, pwrpriv->rpwm);
+	pwrpriv->brpwmtimeout = false;
+
+exit:
+	up(&pwrpriv->lock);
+}
+
+/*
+ * This function is a timer handler, can't do any IO in it.
+ */
+static void pwr_rpwm_timeout_handler(void *FunctionContext)
+{
+	struct adapter *padapter;
+	struct pwrctrl_priv *pwrpriv;
+
+
+	padapter = (struct adapter *)FunctionContext;
+	pwrpriv = adapter_to_pwrctl(padapter);
+	DBG_871X("+%s: rpwm = 0x%02X cpwm = 0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
+		DBG_871X("+%s: cpwm =%d, nothing to do!\n", __func__, pwrpriv->cpwm);
+		return;
+	}
+
+	_set_workitem(&pwrpriv->rpwmtimeoutwi);
+}
+
+static __inline void register_task_alive(struct pwrctrl_priv *pwrctrl, u32 tag)
+{
+	pwrctrl->alives |= tag;
+}
+
+static __inline 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(struct adapter *padapter, u32 task)
+{
+	s32 res;
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	res = _SUCCESS;
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S2;
+
+	down(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, task);
+
+	if (pwrctrl->bFwCurrentInPSMode == true) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+				 ("%s: task = 0x%x cpwm = 0x%02x alives = 0x%08x\n",
+				  __func__, task, pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	up(&pwrctrl->lock);
+
+	if (_FAIL == res)
+		if (pwrctrl->cpwm >= PS_STATE_S2)
+			res = _SUCCESS;
+
+	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(struct adapter *padapter, u32 task)
+{
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S0;
+
+	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;
+	}
+
+	down(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, task);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
+		&& (pwrctrl->bFwCurrentInPSMode == true)) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+				 ("%s: cpwm = 0x%02x alives = 0x%08x\n",
+				  __func__, pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm > pslv)
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+
+	}
+
+	up(&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(struct adapter *padapter)
+{
+	s32 res;
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	res = _SUCCESS;
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S2;
+
+	down(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, XMIT_ALIVE);
+
+	if (pwrctrl->bFwCurrentInPSMode == true) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+				 ("rtw_register_tx_alive: cpwm = 0x%02x alives = 0x%08x\n",
+				  pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	up(&pwrctrl->lock);
+
+	if (_FAIL == res)
+		if (pwrctrl->cpwm >= PS_STATE_S2)
+			res = _SUCCESS;
+
+	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(struct adapter *padapter)
+{
+	s32 res;
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	res = _SUCCESS;
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S2;
+
+	down(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, CMD_ALIVE);
+
+	if (pwrctrl->bFwCurrentInPSMode == true) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_info_,
+				 ("rtw_register_cmd_alive: cpwm = 0x%02x alives = 0x%08x\n",
+				  pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	up(&pwrctrl->lock);
+
+	if (_FAIL == res)
+		if (pwrctrl->cpwm >= PS_STATE_S2)
+			res = _SUCCESS;
+
+	return res;
+}
+
+/*
+ * 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(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S0;
+
+	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;
+	}
+
+	down(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, XMIT_ALIVE);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
+		&& (pwrctrl->bFwCurrentInPSMode == true)) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+				 ("%s: cpwm = 0x%02x alives = 0x%08x\n",
+				  __func__, pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm > pslv)
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+	}
+
+	up(&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(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctrl;
+	u8 pslv;
+
+	pwrctrl = adapter_to_pwrctl(padapter);
+	pslv = PS_STATE_S0;
+
+	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;
+	}
+
+	down(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, CMD_ALIVE);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
+		&& (pwrctrl->bFwCurrentInPSMode == true)) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_info_,
+				 ("%s: cpwm = 0x%02x alives = 0x%08x\n",
+				  __func__, pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm > pslv) {
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	up(&pwrctrl->lock);
+}
+
+void rtw_init_pwrctrl_priv(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	sema_init(&pwrctrlpriv->lock, 1);
+	sema_init(&pwrctrlpriv->check_32k_lock, 1);
+	pwrctrlpriv->rf_pwrstate = rf_on;
+	pwrctrlpriv->ips_enter_cnts = 0;
+	pwrctrlpriv->ips_leave_cnts = 0;
+	pwrctrlpriv->bips_processing = false;
+
+	pwrctrlpriv->ips_mode = padapter->registrypriv.ips_mode;
+	pwrctrlpriv->ips_mode_req = padapter->registrypriv.ips_mode;
+
+	pwrctrlpriv->pwr_state_check_interval = RTW_PWR_STATE_CHK_INTERVAL;
+	pwrctrlpriv->pwr_state_check_cnts = 0;
+	pwrctrlpriv->bInternalAutoSuspend = false;
+	pwrctrlpriv->bInSuspend = false;
+	pwrctrlpriv->bkeepfwalive = false;
+
+	pwrctrlpriv->LpsIdleCount = 0;
+	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;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_SET_RPWM, (u8 *)(&pwrctrlpriv->rpwm));
+
+	_init_workitem(&pwrctrlpriv->cpwm_event, cpwm_event_callback, NULL);
+
+	pwrctrlpriv->brpwmtimeout = false;
+	_init_workitem(&pwrctrlpriv->rpwmtimeoutwi, rpwmtimeout_workitem_callback, NULL);
+	_init_timer(&pwrctrlpriv->pwr_rpwm_timer, padapter->pnetdev, pwr_rpwm_timeout_handler, padapter);
+
+	rtw_init_timer(&pwrctrlpriv->pwr_state_check_timer, padapter, pwr_state_check_handler);
+
+	pwrctrlpriv->wowlan_mode = false;
+	pwrctrlpriv->wowlan_ap_mode = false;
+
+#ifdef CONFIG_PNO_SUPPORT
+	pwrctrlpriv->pno_inited = false;
+	pwrctrlpriv->pnlo_info = NULL;
+	pwrctrlpriv->pscan_info = NULL;
+	pwrctrlpriv->pno_ssid_list = NULL;
+	pwrctrlpriv->pno_in_resume = true;
+#endif
+}
+
+
+void rtw_free_pwrctrl_priv(struct adapter *adapter)
+{
+	/* memset((unsigned char *)pwrctrlpriv, 0, sizeof(struct pwrctrl_priv)); */
+
+#ifdef CONFIG_PNO_SUPPORT
+	if (pwrctrlpriv->pnlo_info != NULL)
+		printk("****** pnlo_info memory leak********\n");
+
+	if (pwrctrlpriv->pscan_info != NULL)
+		printk("****** pscan_info memory leak********\n");
+
+	if (pwrctrlpriv->pno_ssid_list != NULL)
+		printk("****** pno_ssid_list memory leak********\n");
+#endif
+}
+
+inline void rtw_set_ips_deny(struct adapter *padapter, u32 ms)
+{
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	pwrpriv->ips_deny_time = jiffies + msecs_to_jiffies(ms);
+}
+
+/*
+* rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend
+* @adapter: pointer to struct adapter structure
+* @ips_deffer_ms: the ms wiil prevent from falling into IPS after wakeup
+* Return _SUCCESS or _FAIL
+*/
+
+int _rtw_pwr_wakeup(struct 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;
+	unsigned long start = jiffies;
+	unsigned long deny_time = jiffies + msecs_to_jiffies(ips_deffer_ms);
+
+	/* for LPS */
+	LeaveAllPowerSaveMode(padapter);
+
+	/* IPS still bound with primary adapter */
+	padapter = GET_PRIMARY_ADAPTER(padapter);
+	pmlmepriv = &padapter->mlmepriv;
+
+	if (time_before(pwrpriv->ips_deny_time, deny_time))
+		pwrpriv->ips_deny_time = deny_time;
+
+
+	if (pwrpriv->ps_processing) {
+		DBG_871X("%s wait ps_processing...\n", __func__);
+		while (pwrpriv->ps_processing && jiffies_to_msecs(jiffies - start) <= 3000)
+			msleep(10);
+		if (pwrpriv->ps_processing)
+			DBG_871X("%s wait ps_processing timeout\n", __func__);
+		else
+			DBG_871X("%s wait ps_processing done\n", __func__);
+	}
+
+	if (pwrpriv->bInternalAutoSuspend == false && pwrpriv->bInSuspend) {
+		DBG_871X("%s wait bInSuspend...\n", __func__);
+		while (pwrpriv->bInSuspend
+			&& jiffies_to_msecs(jiffies - start) <= 3000
+		) {
+			msleep(10);
+		}
+		if (pwrpriv->bInSuspend)
+			DBG_871X("%s wait bInSuspend timeout\n", __func__);
+		else
+			DBG_871X("%s wait bInSuspend done\n", __func__);
+	}
+
+	/* System suspend is not allowed to wakeup */
+	if ((pwrpriv->bInternalAutoSuspend == false) && (true == pwrpriv->bInSuspend)) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* block??? */
+	if ((pwrpriv->bInternalAutoSuspend == true)  && (padapter->net_closed == true)) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* I think this should be check in IPS, LPS, autosuspend functions... */
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+		ret = _SUCCESS;
+		goto exit;
+	}
+
+	if (rf_off == pwrpriv->rf_pwrstate) {
+		{
+			DBG_8192C("%s call ips_leave....\n", __func__);
+			if (_FAIL ==  ips_leave(padapter)) {
+				DBG_8192C("======> ips_leave fail.............\n");
+				ret = _FAIL;
+				goto exit;
+			}
+		}
+	}
+
+	/* TODO: the following checking need to be merged... */
+	if (padapter->bDriverStopped
+		|| !padapter->bup
+		|| !padapter->hw_init_completed
+	) {
+		DBG_8192C("%s: bDriverStopped =%d, bup =%d, hw_init_completed =%u\n"
+			, caller
+			, padapter->bDriverStopped
+			, padapter->bup
+			, padapter->hw_init_completed);
+		ret = false;
+		goto exit;
+	}
+
+exit:
+	deny_time = jiffies + msecs_to_jiffies(ips_deffer_ms);
+	if (time_before(pwrpriv->ips_deny_time, deny_time))
+		pwrpriv->ips_deny_time = deny_time;
+	return ret;
+
+}
+
+int rtw_pm_set_lps(struct 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(struct 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);
+		DBG_871X("%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);
+		DBG_871X("%s %s\n", __func__, "IPS_NONE");
+		if ((padapter->bSurpriseRemoved == 0) && (_FAIL == rtw_pwr_wakeup(padapter)))
+			return -EFAULT;
+	} else
+		return -EINVAL;
+
+	return 0;
+}
+
+/*
+ * ATTENTION:
+ *This function will request pwrctrl LOCK!
+ */
+void rtw_ps_deny(struct adapter *padapter, enum PS_DENY_REASON reason)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+/* 	DBG_871X("+" FUNC_ADPT_FMT ": Request PS deny for %d (0x%08X)\n", */
+/* 		FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	down(&pwrpriv->lock);
+	if (pwrpriv->ps_deny & BIT(reason)) {
+		DBG_871X(FUNC_ADPT_FMT ": [WARNING] Reason %d had been set before!!\n",
+			FUNC_ADPT_ARG(padapter), reason);
+	}
+	pwrpriv->ps_deny |= BIT(reason);
+	up(&pwrpriv->lock);
+
+/* 	DBG_871X("-" 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(struct adapter *padapter, enum PS_DENY_REASON reason)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+
+/* 	DBG_871X("+" FUNC_ADPT_FMT ": Cancel PS deny for %d(0x%08X)\n", */
+/* 		FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	down(&pwrpriv->lock);
+	if ((pwrpriv->ps_deny & BIT(reason)) == 0) {
+		DBG_871X(FUNC_ADPT_FMT ": [ERROR] Reason %d had been canceled before!!\n",
+			FUNC_ADPT_ARG(padapter), reason);
+	}
+	pwrpriv->ps_deny &= ~BIT(reason);
+	up(&pwrpriv->lock);
+
+/* 	DBG_871X("-" 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(struct adapter *padapter)
+{
+	u32 deny;
+
+
+	deny = adapter_to_pwrctl(padapter)->ps_deny;
+
+	return deny;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_recv.c linux/3rdparty/rtl8723bs/core/rtw_recv.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_recv.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_recv.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,2693 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#define _RTW_RECV_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <linux/jiffies.h>
+#include <rtw_recv.h>
+
+static u8 SNAP_ETH_TYPE_IPX[2] = {0x81, 0x37};
+static u8 SNAP_ETH_TYPE_APPLETALK_AARP[2] = {0x80, 0xf3};
+
+u8 rtw_rfc1042_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
+/* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
+u8 rtw_bridge_tunnel_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 };
+
+void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS);
+
+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, struct 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 vzalloc(). */
+	/* memset((unsigned char *)precvpriv, 0, sizeof (struct  recv_priv)); */
+
+	spin_lock_init(&precvpriv->lock);
+
+	_rtw_init_queue(&precvpriv->free_recv_queue);
+	_rtw_init_queue(&precvpriv->recv_pending_queue);
+	_rtw_init_queue(&precvpriv->uc_swdec_pending_queue);
+
+	precvpriv->adapter = padapter;
+
+	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
+
+	precvpriv->pallocated_frame_buf = vzalloc(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++;
+
+	}
+
+	res = rtw_hal_init_recv_priv(padapter);
+
+	rtw_init_timer(&precvpriv->signal_stat_timer, padapter, rtw_signal_stat_timer_hdl);
+
+	precvpriv->signal_stat_sampling_interval = 2000; /* ms */
+
+	rtw_set_signal_stat_timer(precvpriv);
+
+exit:
+	return res;
+}
+
+void _rtw_free_recv_priv(struct recv_priv *precvpriv)
+{
+	struct adapter	*padapter = precvpriv->adapter;
+
+	rtw_free_uc_swdec_pending_queue(padapter);
+
+	rtw_os_recv_resource_free(precvpriv);
+
+	if (precvpriv->pallocated_frame_buf)
+		vfree(precvpriv->pallocated_frame_buf);
+
+	rtw_hal_free_recv_priv(padapter);
+}
+
+union recv_frame *_rtw_alloc_recvframe(struct __queue *pfree_recv_queue)
+{
+
+	union recv_frame  *precvframe;
+	struct list_head	*plist, *phead;
+	struct adapter *padapter;
+	struct recv_priv *precvpriv;
+
+	if (list_empty(&pfree_recv_queue->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(struct __queue *pfree_recv_queue)
+{
+	union recv_frame  *precvframe;
+
+	spin_lock_bh(&pfree_recv_queue->lock);
+
+	precvframe = _rtw_alloc_recvframe(pfree_recv_queue);
+
+	spin_unlock_bh(&pfree_recv_queue->lock);
+
+	return precvframe;
+}
+
+int rtw_free_recvframe(union recv_frame *precvframe, struct __queue *pfree_recv_queue)
+{
+	struct adapter *padapter = precvframe->u.hdr.adapter;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+
+	rtw_os_free_recvframe(precvframe);
+
+
+	spin_lock_bh(&pfree_recv_queue->lock);
+
+	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++;
+	}
+	spin_unlock_bh(&pfree_recv_queue->lock);
+	return _SUCCESS;
+}
+
+
+
+
+sint _rtw_enqueue_recvframe(union recv_frame *precvframe, struct __queue *queue)
+{
+
+	struct 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, struct __queue *queue)
+{
+	sint ret;
+
+	/* _spinlock(&pfree_recv_queue->lock); */
+	spin_lock_bh(&queue->lock);
+	ret = _rtw_enqueue_recvframe(precvframe, queue);
+	/* spin_unlock(&pfree_recv_queue->lock); */
+	spin_unlock_bh(&queue->lock);
+
+	return ret;
+}
+
+/*
+sint	rtw_enqueue_recvframe(union recv_frame *precvframe, struct __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(struct __queue *pframequeue,  struct __queue *pfree_recv_queue)
+{
+	union	recv_frame	*precvframe;
+	struct list_head	*plist, *phead;
+
+	spin_lock(&pframequeue->lock);
+
+	phead = get_list_head(pframequeue);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+		precvframe = LIST_CONTAINOR(plist, union recv_frame, u);
+
+		plist = get_next(plist);
+
+		rtw_free_recvframe(precvframe, pfree_recv_queue);
+	}
+
+	spin_unlock(&pframequeue->lock);
+}
+
+u32 rtw_free_uc_swdec_pending_queue(struct 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)
+		DBG_871X(FUNC_ADPT_FMT" dequeue %d\n", FUNC_ADPT_ARG(adapter), cnt);
+
+	return cnt;
+}
+
+
+sint rtw_enqueue_recvbuf_to_head(struct recv_buf *precvbuf, struct __queue *queue)
+{
+	spin_lock_bh(&queue->lock);
+
+	list_del_init(&precvbuf->list);
+	list_add(&precvbuf->list, get_list_head(queue));
+
+	spin_unlock_bh(&queue->lock);
+
+	return _SUCCESS;
+}
+
+sint rtw_enqueue_recvbuf(struct recv_buf *precvbuf, struct __queue *queue)
+{
+	spin_lock_bh(&queue->lock);
+
+	list_del_init(&precvbuf->list);
+
+	list_add_tail(&precvbuf->list, get_list_head(queue));
+	spin_unlock_bh(&queue->lock);
+	return _SUCCESS;
+
+}
+
+struct recv_buf *rtw_dequeue_recvbuf(struct __queue *queue)
+{
+	struct recv_buf *precvbuf;
+	struct list_head	*plist, *phead;
+
+	spin_lock_bh(&queue->lock);
+
+	if (list_empty(&queue->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);
+
+	}
+
+	spin_unlock_bh(&queue->lock);
+
+	return precvbuf;
+
+}
+
+sint recvframe_chkmic(struct adapter *adapter,  union recv_frame *precvframe);
+sint recvframe_chkmic(struct 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_) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic:prxattrib->encrypt == _TKIP_\n"));
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic:da = 0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+			prxattrib->ra[0], prxattrib->ra[1], prxattrib->ra[2], prxattrib->ra[3], prxattrib->ra[4], prxattrib->ra[5]));
+
+		/* calculate mic code */
+		if (stainfo != NULL) {
+			if (IS_MCAST(prxattrib->ra)) {
+				/* mickey =&psecuritypriv->dot118021XGrprxmickey.skey[0]; */
+				/* iv = precvframe->u.hdr.rx_data+prxattrib->hdrlen; */
+				/* rxdata_key_idx =(((iv[3])>>6)&0x3) ; */
+				mickey = &psecuritypriv->dot118021XGrprxmickey[prxattrib->key_index].skey[0];
+
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic: bcmc key\n"));
+				/* DBG_871X("\n recvframe_chkmic: bcmc key psecuritypriv->dot118021XGrpKeyid(%d), pmlmeinfo->key_index(%d) , recv key_id(%d)\n", */
+				/* 								psecuritypriv->dot118021XGrpKeyid, pmlmeinfo->key_index, rxdata_key_idx); */
+
+				if (psecuritypriv->binstallGrpkey == false) {
+					res = _FAIL;
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n"));
+					DBG_871X("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n");
+					goto exit;
+				}
+			} else {
+				mickey = &stainfo->dot11tkiprxmickey.skey[0];
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n recvframe_chkmic: unicast key\n"));
+			}
+
+			datalen = precvframe->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len-prxattrib->icv_len-8;/* icv_len included the mic code */
+			pframe = precvframe->u.hdr.rx_data;
+			payload = pframe+prxattrib->hdrlen+prxattrib->iv_len;
+
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n prxattrib->iv_len =%d prxattrib->icv_len =%d\n", prxattrib->iv_len, prxattrib->icv_len));
+
+
+			rtw_seccalctkipmic(mickey, pframe, payload, datalen, &miccode[0], (unsigned char)prxattrib->priority); /* care the length of the data */
+
+			pframemic = payload+datalen;
+
+			bmic_err = false;
+
+			for (i = 0; i < 8; i++) {
+				if (miccode[i] != *(pframemic+i)) {
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chkmic:miccode[%d](%02x) != *(pframemic+%d)(%02x) ", i, miccode[i], i, *(pframemic+i)));
+					bmic_err = true;
+				}
+			}
+
+
+			if (bmic_err == true) {
+
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n *(pframemic-8)-*(pframemic-1) = 0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+					*(pframemic-8), *(pframemic-7), *(pframemic-6), *(pframemic-5), *(pframemic-4), *(pframemic-3), *(pframemic-2), *(pframemic-1)));
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n *(pframemic-16)-*(pframemic-9) = 0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+					*(pframemic-16), *(pframemic-15), *(pframemic-14), *(pframemic-13), *(pframemic-12), *(pframemic-11), *(pframemic-10), *(pframemic-9)));
+
+				{
+					uint i;
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n ======demp packet (len =%d) ======\n", precvframe->u.hdr.len));
+					for (i = 0; i < precvframe->u.hdr.len; i = i+8) {
+						RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x",
+							*(precvframe->u.hdr.rx_data+i), *(precvframe->u.hdr.rx_data+i+1),
+							*(precvframe->u.hdr.rx_data+i+2), *(precvframe->u.hdr.rx_data+i+3),
+							*(precvframe->u.hdr.rx_data+i+4), *(precvframe->u.hdr.rx_data+i+5),
+							*(precvframe->u.hdr.rx_data+i+6), *(precvframe->u.hdr.rx_data+i+7)));
+					}
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n ======demp packet end [len =%d]======\n", precvframe->u.hdr.len));
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n hrdlen =%d,\n", prxattrib->hdrlen));
+				}
+
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("ra = 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x psecuritypriv->binstallGrpkey =%d ",
+					prxattrib->ra[0], prxattrib->ra[1], prxattrib->ra[2],
+					prxattrib->ra[3], prxattrib->ra[4], prxattrib->ra[5], psecuritypriv->binstallGrpkey));
+
+				/*  double check key_index for some timing issue , */
+				/*  cannot compare with psecuritypriv->dot118021XGrpKeyid also cause timing issue */
+				if ((IS_MCAST(prxattrib->ra) == true)  && (prxattrib->key_index != pmlmeinfo->key_index))
+					brpt_micerror = false;
+
+				if ((prxattrib->bdecrypted == true) && (brpt_micerror == true)) {
+					rtw_handle_tkip_mic_err(adapter, (u8)IS_MCAST(prxattrib->ra));
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" mic error :prxattrib->bdecrypted =%d ", prxattrib->bdecrypted));
+					DBG_871X(" mic error :prxattrib->bdecrypted =%d\n", prxattrib->bdecrypted);
+				} else{
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" mic error :prxattrib->bdecrypted =%d ", prxattrib->bdecrypted));
+					DBG_871X(" mic error :prxattrib->bdecrypted =%d\n", prxattrib->bdecrypted);
+				}
+
+				res = _FAIL;
+
+			} else {
+				/* mic checked ok */
+				if ((psecuritypriv->bcheck_grpkey == false) && (IS_MCAST(prxattrib->ra) == true)) {
+					psecuritypriv->bcheck_grpkey = true;
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("psecuritypriv->bcheck_grpkey =true"));
+				}
+			}
+
+		} else
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chkmic: rtw_get_stainfo == NULL!!!\n"));
+
+		recvframe_pull_tail(precvframe, 8);
+
+	}
+
+exit:
+	return res;
+
+}
+
+/* decrypt and set the ivlen, icvlen of the recv_frame */
+union recv_frame *decryptor(struct adapter *padapter, union recv_frame *precv_frame);
+union recv_frame *decryptor(struct 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);
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("prxstat->decrypted =%x prxattrib->encrypt = 0x%03x\n", prxattrib->bdecrypted, prxattrib->encrypt));
+
+	if (prxattrib->encrypt > 0) {
+		u8 *iv = precv_frame->u.hdr.rx_data+prxattrib->hdrlen;
+		prxattrib->key_index = (((iv[3])>>6)&0x3);
+
+		if (prxattrib->key_index > WEP_KEYS) {
+			DBG_871X("prxattrib->key_index(%d) > WEP_KEYS\n", prxattrib->key_index);
+
+			switch (prxattrib->encrypt) {
+			case _WEP40_:
+			case _WEP104_:
+				prxattrib->key_index = psecuritypriv->dot11PrivacyKeyIndex;
+				break;
+			case _TKIP_:
+			case _AES_:
+			default:
+				prxattrib->key_index = psecuritypriv->dot118021XGrpKeyid;
+				break;
+			}
+		}
+	}
+
+	if ((prxattrib->encrypt > 0) && ((prxattrib->bdecrypted == 0) || (psecuritypriv->sw_decrypt == true))) {
+		psecuritypriv->hw_decrypted = false;
+
+		#ifdef DBG_RX_DECRYPTOR
+		DBG_871X("[%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;
+		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
+		DBG_871X("[%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
+		DBG_871X("[%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 */
+union recv_frame *portctrl(struct adapter *adapter, union recv_frame *precv_frame);
+union recv_frame *portctrl(struct 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;
+
+	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);
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:adapter->securitypriv.dot11AuthAlgrthm =%d\n", adapter->securitypriv.dot11AuthAlgrthm));
+
+	if (auth_alg == 2) {
+		if ((psta != NULL) && (psta->ieee8021x_blocked)) {
+			__be16 be_tmp;
+
+			/* blocked */
+			/* only accept EAPOL frame */
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:psta->ieee8021x_blocked == 1\n"));
+
+			prtnframe = precv_frame;
+
+			/* get ether_type */
+			ptr = ptr+pfhdr->attrib.hdrlen+pfhdr->attrib.iv_len+LLC_HEADER_SIZE;
+			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 */
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:psta->ieee8021x_blocked == 0\n"));
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("portctrl:precv_frame->hdr.attrib.privacy =%x\n", precv_frame->u.hdr.attrib.privacy));
+
+			if (pattrib->bdecrypted == 0)
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("portctrl:prxstat->decrypted =%x\n", pattrib->bdecrypted));
+
+			prtnframe = precv_frame;
+			/* check is the EAPOL frame or not (Rekey) */
+			/* if (ether_type == eapol_type) { */
+			/* 	RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("########portctrl:ether_type == 0x888e\n")); */
+				/* check Rekey */
+
+			/* 	prtnframe =precv_frame; */
+			/*  */
+			/* else { */
+			/* 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:ether_type = 0x%04x\n", ether_type)); */
+			/*  */
+		}
+	} else
+		prtnframe = precv_frame;
+
+	return prtnframe;
+}
+
+sint recv_decache(union recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache);
+sint recv_decache(union recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache)
+{
+	sint tid = precv_frame->u.hdr.attrib.priority;
+
+	u16 seq_ctrl = ((precv_frame->u.hdr.attrib.seq_num&0xffff) << 4) |
+		(precv_frame->u.hdr.attrib.frag_num & 0xf);
+
+	if (tid > 15) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_decache, (tid>15)! seq_ctrl = 0x%x, tid = 0x%x\n", seq_ctrl, tid));
+
+		return _FAIL;
+	}
+
+	if (1) { /* if (bretry) */
+		if (seq_ctrl == prxcache->tid_rxseq[tid]) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_decache, seq_ctrl = 0x%x, tid = 0x%x, tid_rxseq = 0x%x\n", seq_ctrl, tid, prxcache->tid_rxseq[tid]));
+
+			return _FAIL;
+		}
+	}
+
+	prxcache->tid_rxseq[tid] = seq_ctrl;
+
+	return _SUCCESS;
+
+}
+
+void process_pwrbit_data(struct adapter *padapter, union recv_frame *precv_frame);
+void process_pwrbit_data(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned char pwrbit;
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, pattrib->src);
+
+	pwrbit = GetPwrMgt(ptr);
+
+	if (psta) {
+		if (pwrbit) {
+			if (!(psta->state & WIFI_SLEEP_STATE)) {
+				/* psta->state |= WIFI_SLEEP_STATE; */
+				/* pstapriv->sta_dz_bitmap |= BIT(psta->aid); */
+
+				stop_sta_xmit(padapter, psta);
+
+				/* DBG_871X("to sleep, sta_dz_bitmap =%x\n", pstapriv->sta_dz_bitmap); */
+			}
+		} else{
+			if (psta->state & WIFI_SLEEP_STATE) {
+				/* psta->state ^= WIFI_SLEEP_STATE; */
+				/* pstapriv->sta_dz_bitmap &= ~BIT(psta->aid); */
+
+				wakeup_sta_to_xmit(padapter, psta);
+
+				/* DBG_871X("to wakeup, sta_dz_bitmap =%x\n", pstapriv->sta_dz_bitmap); */
+			}
+		}
+
+	}
+}
+
+void process_wmmps_data(struct adapter *padapter, union recv_frame *precv_frame);
+void process_wmmps_data(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	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;
+
+	if (!psta->qos_option)
+		return;
+
+	if (!(psta->qos_info&0xf))
+		return;
+
+	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);
+		}
+	}
+}
+
+void count_rx_stats(struct adapter *padapter, union recv_frame *prframe, struct sta_info *sta);
+void count_rx_stats(struct adapter *padapter, union recv_frame *prframe, struct sta_info *sta)
+{
+	int	sz;
+	struct sta_info 	*psta = NULL;
+	struct stainfo_stats	*pstats = NULL;
+	struct rx_pkt_attrib	*pattrib = &prframe->u.hdr.attrib;
+	struct recv_priv 	*precvpriv = &padapter->recvpriv;
+
+	sz = get_recvframe_len(prframe);
+	precvpriv->rx_bytes += sz;
+
+	padapter->mlmepriv.LinkDetectInfo.NumRxOkInPeriod++;
+
+	if ((!MacAddr_isBcst(pattrib->dst)) && (!IS_MCAST(pattrib->dst))) {
+		padapter->mlmepriv.LinkDetectInfo.NumRxUnicastOkInPeriod++;
+	}
+
+	if (sta)
+		psta = sta;
+	else
+		psta = prframe->u.hdr.psta;
+
+	if (psta) {
+		pstats = &psta->sta_stats;
+
+		pstats->rx_data_pkts++;
+		pstats->rx_bytes += sz;
+	}
+
+	traffic_check_for_leave_lps(padapter, false, 0);
+}
+
+sint sta2sta_data_frame(
+	struct adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta
+);
+sint sta2sta_data_frame(
+	struct adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta
+)
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	sint ret = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct	sta_priv 	*pstapriv = &adapter->stapriv;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = myid(&adapter->eeprompriv);
+	u8 *sta_addr = NULL;
+	sint bmcast = IS_MCAST(pattrib->dst);
+
+	/* DBG_871X("[%s] %d, seqnum:%d\n", __func__, __LINE__, pattrib->seq_num); */
+
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
+
+		/*  filter packets that SA is myself or multicast or broadcast */
+		if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" SA ==myself\n"));
+			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) == true) {
+		{
+			/*  For Station mode, sa and bssid should always be BSSID, and DA is my mac-address */
+			if (memcmp(pattrib->bssid, pattrib->src, ETH_ALEN)) {
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("bssid != TA under STATION_MODE; drop pkt\n"));
+				ret = _FAIL;
+				goto exit;
+			}
+
+		sta_addr = pattrib->bssid;
+		}
+
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		if (bmcast) {
+			/*  For AP mode, if DA == MCAST, then BSSID should be also MCAST */
+			if (!IS_MCAST(pattrib->bssid)) {
+					ret = _FAIL;
+					goto exit;
+			}
+		} else{ /*  not mc-frame */
+			/*  For AP mode, if DA is non-MCAST, then it must be BSSID, and bssid == BSSID */
+			if (memcmp(pattrib->bssid, pattrib->dst, ETH_ALEN)) {
+				ret = _FAIL;
+				goto exit;
+			}
+
+			sta_addr = pattrib->src;
+		}
+
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) {
+		memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->src, GetAddr2Ptr(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 */
+
+	if (*psta == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under sta2sta_data_frame ; drop pkt\n"));
+		ret = _FAIL;
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+sint ap2sta_data_frame(
+	struct adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta);
+sint ap2sta_data_frame(
+	struct adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta)
+{
+	u8 *ptr = precv_frame->u.hdr.rx_data;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	sint ret = _SUCCESS;
+	struct	sta_priv 	*pstapriv = &adapter->stapriv;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = myid(&adapter->eeprompriv);
+	sint bmcast = IS_MCAST(pattrib->dst);
+
+	if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
+		&& (check_fwstate(pmlmepriv, _FW_LINKED) == true
+			|| check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true)
+		) {
+
+		/*  filter packets that SA is myself or multicast or broadcast */
+		if (!memcmp(myhwaddr, pattrib->src, ETH_ALEN)) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" SA ==myself\n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s SA ="MAC_FMT", myhwaddr ="MAC_FMT"\n",
+				__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)) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
+				(" ap2sta_data_frame:  compare DA fail; DA ="MAC_FMT"\n", MAC_ARG(pattrib->dst)));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s DA ="MAC_FMT"\n", __func__, MAC_ARG(pattrib->dst));
+			#endif
+			ret = _FAIL;
+			goto exit;
+		}
+
+
+		/*  check BSSID */
+		if (!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))) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
+				(" ap2sta_data_frame:  compare BSSID fail ; BSSID ="MAC_FMT"\n", MAC_ARG(pattrib->bssid)));
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("mybssid ="MAC_FMT"\n", MAC_ARG(mybssid)));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s BSSID ="MAC_FMT", mybssid ="MAC_FMT"\n",
+				__func__, MAC_ARG(pattrib->bssid), MAC_ARG(mybssid));
+			DBG_871X("this adapter = %d, buddy adapter = %d\n", adapter->adapter_type, adapter->pbuddystruct adapter->adapter_type);
+			#endif
+
+			if (!bmcast) {
+				DBG_871X("issue_deauth to the nonassociated ap =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->bssid));
+				issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+			}
+
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if (bmcast)
+			*psta = rtw_get_bcmc_stainfo(adapter);
+		else
+			*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /*  get ap_info */
+
+		if (*psta == NULL) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("ap2sta: can't get psta under STATION_MODE ; drop pkt\n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s can't get psta under STATION_MODE ; drop pkt\n", __func__);
+			#endif
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if ((GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) {
+		}
+
+		if (GetFrameSubType(ptr) & BIT(6)) {
+			/* No data, will not indicate to upper layer, temporily count it here */
+			count_rx_stats(adapter, precv_frame, *psta);
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+
+	} else if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) &&
+		     (check_fwstate(pmlmepriv, _FW_LINKED) == true)) {
+		memcpy(pattrib->dst, GetAddr1Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->src, GetAddr2Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->bssid, GetAddr3Ptr(ptr), ETH_ALEN);
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+
+		/*  */
+		memcpy(pattrib->bssid,  mybssid, ETH_ALEN);
+
+
+		*psta = rtw_get_stainfo(pstapriv, pattrib->bssid); /*  get sta_info */
+		if (*psta == NULL) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under MP_MODE ; drop pkt\n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s can't get psta under WIFI_MP_STATE ; drop pkt\n", __func__);
+			#endif
+			ret = _FAIL;
+			goto exit;
+		}
+
+
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		/* 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 unsigned long send_issue_deauth_time = 0;
+
+				/* DBG_871X("After send deauth , %u ms has elapsed.\n", jiffies_to_msecs(jiffies - send_issue_deauth_time)); */
+
+				if (jiffies_to_msecs(jiffies - send_issue_deauth_time) > 10000 || send_issue_deauth_time == 0) {
+					send_issue_deauth_time = jiffies;
+
+					DBG_871X("issue_deauth to the ap =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->bssid));
+
+					issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+				}
+			}
+		}
+
+		ret = _FAIL;
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s fw_state:0x%x\n", __func__, get_fwstate(pmlmepriv));
+		#endif
+	}
+
+exit:
+	return ret;
+}
+
+sint sta2ap_data_frame(
+	struct adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta);
+sint sta2ap_data_frame(
+	struct 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) == true) {
+		/* 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) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under AP_MODE; drop pkt\n"));
+			DBG_871X("issue_deauth to sta =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->src));
+
+			issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+
+		process_pwrbit_data(adapter, precv_frame);
+
+		if ((GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) {
+			process_wmmps_data(adapter, precv_frame);
+		}
+
+		if (GetFrameSubType(ptr) & BIT(6)) {
+			/* No data, will not indicate to upper layer, temporily count it here */
+			count_rx_stats(adapter, precv_frame, *psta);
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+	} else {
+		u8 *myhwaddr = myid(&adapter->eeprompriv);
+		if (memcmp(pattrib->ra, myhwaddr, ETH_ALEN)) {
+			ret = RTW_RX_HANDLED;
+			goto exit;
+		}
+		DBG_871X("issue_deauth to sta =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->src));
+		issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+		ret = RTW_RX_HANDLED;
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+sint validate_recv_ctrl_frame(struct adapter *padapter, union recv_frame *precv_frame);
+sint validate_recv_ctrl_frame(struct 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; */
+
+	/* DBG_871X("+validate_recv_ctrl_frame\n"); */
+
+	if (GetFrameType(pframe) != WIFI_CTRL_TYPE)
+		return _FAIL;
+
+	/* receive the frames that ra(a1) is my address */
+	if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN))
+		return _FAIL;
+
+	psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+	if (psta == NULL)
+		return _FAIL;
+
+	/* for rx pkt statistics */
+	psta->sta_stats.rx_ctrl_pkts++;
+
+	/* only handle ps-poll */
+	if (GetFrameSubType(pframe) == WIFI_PSPOLL) {
+		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) {
+			DBG_871X("%s alive check-rx ps-poll\n", __func__);
+			psta->expire_to = pstapriv->expire_to;
+			psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+		}
+
+		if ((psta->state&WIFI_SLEEP_STATE) && (pstapriv->sta_dz_bitmap&BIT(psta->aid))) {
+			struct list_head	*xmitframe_plist, *xmitframe_phead;
+			struct xmit_frame *pxmitframe = NULL;
+			struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+			/* spin_lock_bh(&psta->sleep_q.lock); */
+			spin_lock_bh(&pxmitpriv->lock);
+
+			xmitframe_phead = get_list_head(&psta->sleep_q);
+			xmitframe_plist = get_next(xmitframe_phead);
+
+			if (xmitframe_phead != xmitframe_plist) {
+				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;
+
+				/* DBG_871X("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);
+
+					/* DBG_871X("after handling ps-poll, tim =%x\n", pstapriv->tim_bitmap); */
+
+					/* upate BCN for TIM IE */
+					/* update_BCNTIM(padapter); */
+					update_beacon(padapter, _TIM_IE_, NULL, true);
+				}
+
+				/* spin_unlock_bh(&psta->sleep_q.lock); */
+				spin_unlock_bh(&pxmitpriv->lock);
+
+			} else{
+				/* spin_unlock_bh(&psta->sleep_q.lock); */
+				spin_unlock_bh(&pxmitpriv->lock);
+
+				/* DBG_871X("no buffered packets to xmit\n"); */
+				if (pstapriv->tim_bitmap&BIT(psta->aid)) {
+					if (psta->sleepq_len == 0) {
+						DBG_871X("no buffered packets to xmit\n");
+
+						/* issue nulldata with More data bit = 0 to indicate we have no buffered packets */
+						issue_nulldata_in_interrupt(padapter, psta->hwaddr);
+					} else{
+						DBG_871X("error!psta->sleepq_len =%d\n", psta->sleepq_len);
+						psta->sleepq_len = 0;
+					}
+
+					pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+					/* upate BCN for TIM IE */
+					/* update_BCNTIM(padapter); */
+					update_beacon(padapter, _TIM_IE_, NULL, true);
+				}
+			}
+		}
+	}
+
+	return _FAIL;
+
+}
+
+union recv_frame *recvframe_chk_defrag(struct adapter *padapter, union recv_frame *precv_frame);
+sint validate_recv_mgnt_frame(struct adapter *padapter, union recv_frame *precv_frame);
+sint validate_recv_mgnt_frame(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	/* struct mlme_priv *pmlmepriv = &adapter->mlmepriv; */
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("+validate_recv_mgnt_frame\n"));
+
+	precv_frame = recvframe_chk_defrag(padapter, precv_frame);
+	if (precv_frame == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("%s: fragment packet\n", __func__));
+		return _SUCCESS;
+	}
+
+	{
+		/* for rx pkt statistics */
+		struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(precv_frame->u.hdr.rx_data));
+		if (psta) {
+			psta->sta_stats.rx_mgnt_pkts++;
+			if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_BEACON)
+				psta->sta_stats.rx_beacon_pkts++;
+			else if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_PROBEREQ)
+				psta->sta_stats.rx_probereq_pkts++;
+			else if (GetFrameSubType(precv_frame->u.hdr.rx_data) == WIFI_PROBERSP) {
+				if (!memcmp(padapter->eeprompriv.mac_addr, 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++;
+			}
+		}
+	}
+
+	mgt_dispatcher(padapter, precv_frame);
+
+	return _SUCCESS;
+
+}
+
+sint validate_recv_data_frame(struct adapter *adapter, union recv_frame *precv_frame);
+sint validate_recv_data_frame(struct 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 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
+		DBG_871X("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, GetAddr2Ptr(ptr), ETH_ALEN);
+		ret = _FAIL;
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" case 3\n"));
+		break;
+
+	default:
+		ret = _FAIL;
+		break;
+
+	}
+
+	if (ret == _FAIL) {
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s case:%d, res:%d\n", __func__, pattrib->to_fr_ds, ret);
+		#endif
+		goto exit;
+	} else if (ret == RTW_RX_HANDLED) {
+		goto exit;
+	}
+
+
+	if (psta == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" after to_fr_ds_chk; psta == NULL\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s psta == NULL\n", __func__);
+		#endif
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* psta->rssi = prxcmd->rssi; */
+	/* psta->signal_quality = prxcmd->sq; */
+	precv_frame->u.hdr.psta = psta;
+
+
+	pattrib->amsdu = 0;
+	pattrib->ack_policy = 0;
+	/* parsing QC field */
+	if (pattrib->qos == 1) {
+		pattrib->priority = GetPriority((ptr + 24));
+		pattrib->ack_policy = GetAckpolicy((ptr + 24));
+		pattrib->amsdu = GetAMsdu((ptr + 24));
+		pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 32 : 26;
+
+		if (pattrib->priority != 0 && pattrib->priority != 3)
+			adapter->recvpriv.bIsAnyNonBEPkts = true;
+
+	} else{
+		pattrib->priority = 0;
+		pattrib->hdrlen = pattrib->to_fr_ds == 3 ? 30 : 24;
+	}
+
+
+	if (pattrib->order)/* HT-CTRL 11n */
+		pattrib->hdrlen += 4;
+
+	precv_frame->u.hdr.preorder_ctrl = &psta->recvreorder_ctrl[pattrib->priority];
+
+	/*  decache, drop duplicate recv packets */
+	if (recv_decache(precv_frame, bretry, &psta->sta_recvpriv.rxcache) == _FAIL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("decache : drop pkt\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s recv_decache return _FAIL\n", __func__);
+		#endif
+		ret = _FAIL;
+		goto exit;
+	}
+
+	if (pattrib->privacy) {
+
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("validate_recv_data_frame:pattrib->privacy =%x\n", pattrib->privacy));
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n ^^^^^^^^^^^IS_MCAST(pattrib->ra(0x%02x)) =%d^^^^^^^^^^^^^^^6\n", pattrib->ra[0], IS_MCAST(pattrib->ra)));
+
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, IS_MCAST(pattrib->ra));
+
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n pattrib->encrypt =%d\n", pattrib->encrypt));
+
+		SET_ICE_IV_LEN(pattrib->iv_len, pattrib->icv_len, pattrib->encrypt);
+	} else{
+		pattrib->encrypt = 0;
+		pattrib->iv_len = pattrib->icv_len = 0;
+	}
+
+exit:
+	return ret;
+}
+
+static sint validate_80211w_mgmt(struct 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;
+	u8 type;
+	u8 subtype;
+
+	type =  GetFrameType(ptr);
+	subtype = GetFrameSubType(ptr); /* bit(7)~bit(2) */
+
+	/* only support station mode */
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) && check_fwstate(pmlmepriv, _FW_LINKED)
+		&& adapter->securitypriv.binstallBIPkey == true) {
+		/* 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 = GetAddr2Ptr(ptr);
+
+			pattrib->bdecrypted = 0;
+			pattrib->encrypt = _AES_;
+			pattrib->hdrlen = sizeof(struct 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, GetAddr2Ptr(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) {
+				DBG_871X("%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;
+			kfree(mgmt_DATA);
+			if (!precv_frame) {
+				DBG_871X("%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) {
+				/* DBG_871X("802.11w BIP verify fail\n"); */
+				goto validate_80211w_fail;
+			} else if (BIP_ret == RTW_RX_HANDLED) {
+				/* DBG_871X("802.11w recv none protected packet\n"); */
+				/* issue sa query request */
+				issue_action_SA_Query(adapter, NULL, 0, 0);
+				goto validate_80211w_fail;
+			}
+		} else { /* 802.11w protect */
+			if (subtype == WIFI_ACTION) {
+				/* 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) {
+					DBG_871X("action frame category =%d should robust\n", ptr[WLAN_HDR_A3_LEN]);
+					goto validate_80211w_fail;
+				}
+			} else if (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC) {
+				DBG_871X("802.11w recv none protected packet\n");
+				/* issue sa query request */
+				issue_action_SA_Query(adapter, NULL, 0, 0);
+				goto validate_80211w_fail;
+			}
+		}
+	}
+	return _SUCCESS;
+
+validate_80211w_fail:
+	return _FAIL;
+
+}
+
+static inline void dump_rx_packet(u8 *ptr)
+{
+	int i;
+
+	DBG_871X("#############################\n");
+	for (i = 0; i < 64; i = i+8)
+		DBG_871X("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n", *(ptr+i),
+		*(ptr+i+1), *(ptr+i+2), *(ptr+i+3), *(ptr+i+4), *(ptr+i+5), *(ptr+i+6), *(ptr+i+7));
+	DBG_871X("#############################\n");
+}
+
+sint validate_recv_frame(struct adapter *adapter, union recv_frame *precv_frame);
+sint validate_recv_frame(struct 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;
+
+	/* add version chk */
+	if (ver != 0) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail! (ver!= 0)\n"));
+		retval = _FAIL;
+		DBG_COUNTER(adapter->rx_logs.core_rx_pre_ver_err);
+		goto exit;
+	}
+
+	type =  GetFrameType(ptr);
+	subtype = GetFrameSubType(ptr); /* bit(7)~bit(2) */
+
+	pattrib->to_fr_ds = get_tofr_ds(ptr);
+
+	pattrib->frag_num = GetFragNum(ptr);
+	pattrib->seq_num = GetSequence(ptr);
+
+	pattrib->pw_save = GetPwrMgt(ptr);
+	pattrib->mfrag = GetMFrag(ptr);
+	pattrib->mdata = GetMData(ptr);
+	pattrib->privacy = GetPrivacy(ptr);
+	pattrib->order = GetOrder(ptr);
+{
+	u8 bDumpRxPkt;
+	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);
+		if (validate_80211w_mgmt(adapter, precv_frame) == _FAIL) {
+			retval = _FAIL;
+			DBG_COUNTER(padapter->rx_logs.core_rx_pre_mgmt_err_80211w);
+			break;
+		}
+
+		retval = validate_recv_mgnt_frame(adapter, precv_frame);
+		if (retval == _FAIL) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_mgnt_frame fail\n"));
+			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) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_ctrl_frame fail\n"));
+			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);
+
+		pattrib->qos = (subtype & BIT(7)) ? 1:0;
+		retval = validate_recv_data_frame(adapter, precv_frame);
+		if (retval == _FAIL) {
+			struct recv_priv *precvpriv = &adapter->recvpriv;
+			/* RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail\n")); */
+			precvpriv->rx_drop++;
+			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);
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail! type = 0x%x\n", type));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME validate_recv_data_frame fail! type = 0x%x\n", type);
+		#endif
+		retval = _FAIL;
+		break;
+	}
+
+exit:
+	return retval;
+}
+
+
+/* remove the wlanhdr and add the eth_hdr */
+sint wlanhdr_to_ethhdr(union recv_frame *precvframe);
+sint wlanhdr_to_ethhdr(union recv_frame *precvframe)
+{
+	sint	rmv_len;
+	u16 eth_type, len;
+	u8 bsnaphdr;
+	u8 *psnap_type;
+	struct ieee80211_snap_hdr	*psnap;
+	__be16 be_tmp;
+	sint ret = _SUCCESS;
+	struct 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)) &&
+		(memcmp(psnap_type, SNAP_ETH_TYPE_APPLETALK_AARP, 2))) ||
+		/* 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;
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n ===pattrib->hdrlen: %x,  pattrib->iv_len:%x ===\n\n", pattrib->hdrlen,  pattrib->iv_len));
+
+	memcpy(&be_tmp, ptr+rmv_len, 2);
+	eth_type = ntohs(be_tmp); /* pattrib->ether_type */
+	pattrib->eth_type = eth_type;
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (0x8899 == pattrib->eth_type) {
+		struct sta_info *psta = precvframe->u.hdr.psta;
+
+		DBG_871X("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);
+
+			DBG_871X("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); */
+
+				/* DBG_871X("RC: len = 0x%x, ctrl_type = 0x%x\n", len, ctrl_type); */
+				DBG_871X("RC: len = 0x%x\n", len);
+
+				for (i = 0; i < len ; i++)
+					DBG_871X("0x%x\n", *(ptr+rmv_len+6+i));
+					/* DBG_871X("0x%x\n", *(ptr+rmv_len+8+i)); */
+
+				DBG_871X("RC-end\n");
+			}
+		}
+	}
+#endif /* CONFIG_AUTO_AP_MODE */
+
+	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true)) {
+		ptr += rmv_len;
+		*ptr = 0x87;
+		*(ptr+1) = 0x12;
+
+		eth_type = 0x8712;
+		/*  append rx status for mp test packets */
+		ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr)+2)-24);
+		memcpy(ptr, get_rxmem(precvframe), 24);
+		ptr += 24;
+	} else
+		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);
+	}
+
+	return ret;
+}
+
+/* perform defrag */
+union recv_frame *recvframe_defrag(struct adapter *adapter, struct __queue *defrag_q)
+{
+	struct list_head	 *plist, *phead;
+	u8 *data, wlanhdr_offset;
+	u8 curfragnum;
+	struct recv_frame_hdr *pfhdr, *pnfhdr;
+	union recv_frame *prframe, *pnextrframe;
+	struct __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 (phead != plist) {
+		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, pnfhdr->len);
+
+		pfhdr->attrib.icv_len = pnfhdr->attrib.icv_len;
+		plist = get_next(plist);
+
+	};
+
+	/* free the defrag_q queue and return the prframe */
+	rtw_free_recvframe_queue(defrag_q, pfree_recv_queue);
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("Performance defrag!!!!!\n"));
+
+	return prframe;
+}
+
+/* check if need to defrag, if needed queue the frame to defrag_q */
+union recv_frame *recvframe_chk_defrag(struct adapter *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;
+	struct list_head *phead;
+	union recv_frame *prtnframe = NULL;
+	struct __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 (!list_empty(&pdefrag_q->queue))
+					/* 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); */
+
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("Enqueuq: ismfrag = %d, fragnum = %d\n", ismfrag, fragnum));
+
+			prtnframe = NULL;
+
+		} else{
+			/* can't find this ta's defrag_queue, so free this recv_frame */
+			rtw_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe = NULL;
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("Free because pdefrag_q == NULL: ismfrag = %d, fragnum = %d\n", ismfrag, fragnum));
+		}
+
+	}
+
+	if ((ismfrag == 0) && (fragnum != 0)) {
+		/* the last fragment frame */
+		/* enqueue the last fragment */
+		if (pdefrag_q != NULL) {
+			/* 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 */
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("defrag: ismfrag = %d, fragnum = %d\n", ismfrag, fragnum));
+			precv_frame = recvframe_defrag(padapter, pdefrag_q);
+			prtnframe = precv_frame;
+
+		} else{
+			/* can't find this ta's defrag_queue, so free this recv_frame */
+			rtw_free_recvframe(precv_frame, pfree_recv_queue);
+			prtnframe = NULL;
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("Free because pdefrag_q == NULL: ismfrag = %d, fragnum = %d\n", ismfrag, fragnum));
+		}
+
+	}
+
+
+	if ((prtnframe != NULL) && (prtnframe->u.hdr.attrib.privacy)) {
+		/* after defrag we must check tkip mic code */
+		if (recvframe_chkmic(padapter,  prtnframe) == _FAIL) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chkmic(padapter,  prtnframe) == _FAIL\n"));
+			rtw_free_recvframe(prtnframe, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+	}
+	return prtnframe;
+}
+
+static int amsdu_to_msdu(struct 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;
+	struct __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)) {
+			DBG_871X("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) {
+			DBG_871X("%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) {
+			DBG_871X("ParseSubframe(): Too many Subframes! Packets dropped!\n");
+			break;
+		}
+
+		pdata += nSubframe_Length;
+		a_len -= nSubframe_Length;
+		if (a_len != 0) {
+			padding_len = 4 - ((nSubframe_Length + ETH_HLEN) & (4-1));
+			if (padding_len == 4) {
+				padding_len = 0;
+			}
+
+			if (a_len < padding_len) {
+				DBG_871X("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;
+}
+
+int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num);
+int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num)
+{
+	struct adapter *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 = seq_num;
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d init IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+			preorder_ctrl->indicate_seq, seq_num);
+		#endif
+
+		/* DbgPrint("check_indicate_seq, 1st->indicate_seq =%d\n", precvpriv->indicate_seq); */
+	}
+
+	/* DbgPrint("enter->check_indicate_seq(): IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */
+
+	/*  Drop out the packet which SeqNum is smaller than WinStart */
+	if (SN_LESS(seq_num, preorder_ctrl->indicate_seq)) {
+		/* RT_TRACE(COMP_RX_REORDER, DBG_LOUD, ("CheckRxTsIndicateSeq(): Packet Drop! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, NewSeqNum)); */
+		/* DbgPrint("CheckRxTsIndicateSeq(): Packet Drop! IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */
+
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("%s IndicateSeq: %d > NewSeq: %d\n", __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(seq_num, preorder_ctrl->indicate_seq)) {
+		preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d SN_EQUAL IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+			preorder_ctrl->indicate_seq, seq_num);
+		#endif
+	} else if (SN_LESS(wend, seq_num)) {
+		/* RT_TRACE(COMP_RX_REORDER, DBG_LOUD, ("CheckRxTsIndicateSeq(): Window Shift! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, NewSeqNum)); */
+		/* DbgPrint("CheckRxTsIndicateSeq(): Window Shift! IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */
+
+		/*  boundary situation, when seq_num cross 0xFFF */
+		if (seq_num >= (wsize - 1))
+			preorder_ctrl->indicate_seq = seq_num + 1 - wsize;
+		else
+			preorder_ctrl->indicate_seq = 0xFFF - (wsize - (seq_num + 1)) + 1;
+		pdbgpriv->dbg_rx_ampdu_window_shift_cnt++;
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d SN_LESS(wend, seq_num) IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+			preorder_ctrl->indicate_seq, seq_num);
+		#endif
+	}
+
+	/* DbgPrint("exit->check_indicate_seq(): IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */
+
+	return true;
+}
+
+int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, union recv_frame *prframe);
+int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, union recv_frame *prframe)
+{
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+	struct list_head	*phead, *plist;
+	union recv_frame *pnextrframe;
+	struct rx_pkt_attrib *pnextattrib;
+
+	/* DbgPrint("+enqueue_reorder_recvframe()\n"); */
+
+	/* spin_lock_irqsave(&ppending_recvframe_queue->lock, irql); */
+	/* spin_lock(&ppending_recvframe_queue->lock); */
+
+
+	phead = get_list_head(ppending_recvframe_queue);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+		pnextrframe = LIST_CONTAINOR(plist, union recv_frame, u);
+		pnextattrib = &pnextrframe->u.hdr.attrib;
+
+		if (SN_LESS(pnextattrib->seq_num, pattrib->seq_num))
+			plist = get_next(plist);
+		else if (SN_EQUAL(pnextattrib->seq_num, pattrib->seq_num))
+			/* Duplicate entry is found!! Do not insert current entry. */
+			/* RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("InsertRxReorderList(): Duplicate packet is dropped!! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum)); */
+			/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+			return false;
+		else
+			break;
+
+		/* DbgPrint("enqueue_reorder_recvframe():while\n"); */
+
+	}
+
+
+	/* spin_lock_irqsave(&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); */
+	/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+
+
+	/* RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("InsertRxReorderList(): Pkt insert into buffer!! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum)); */
+	return true;
+
+}
+
+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(struct adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced);
+int recv_indicatepkts_in_order(struct adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced)
+{
+	struct list_head	*phead, *plist;
+	union recv_frame *prframe;
+	struct rx_pkt_attrib *pattrib;
+	/* u8 index = 0; */
+	int bPktInBuf = false;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct __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"); */
+
+	/* spin_lock_irqsave(&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 == true) {
+		pdbgpriv->dbg_rx_ampdu_forced_indicate_count++;
+		if (list_empty(phead)) {
+			/*  spin_unlock_irqrestore(&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
+		DBG_871X("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, pattrib->seq_num);
+		preorder_ctrl->indicate_seq = 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, pattrib->seq_num)) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
+				 ("recv_indicatepkts_in_order: indicate =%d seq =%d amsdu =%d\n",
+				  preorder_ctrl->indicate_seq, pattrib->seq_num, pattrib->amsdu));
+
+			plist = get_next(plist);
+			list_del_init(&(prframe->u.hdr.list));
+
+			if (SN_EQUAL(preorder_ctrl->indicate_seq, pattrib->seq_num)) {
+				preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1) & 0xFFF;
+				#ifdef DBG_RX_SEQ
+				DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+					preorder_ctrl->indicate_seq, pattrib->seq_num);
+				#endif
+			}
+
+			/* Set this as a lock to make sure that only one thread is indicating packet. */
+			/* pTS->RxIndicateState = RXTS_INDICATE_PROCESSING; */
+
+			/*  Indicate packets */
+			/* RT_ASSERT((index<=REORDER_WIN_SIZE), ("RxReorderIndicatePacket(): Rx Reorder buffer full!!\n")); */
+
+
+			/* indicate this recv_frame */
+			/* DbgPrint("recv_indicatepkts_in_order, indicate_seq =%d, seq_num =%d\n", precvpriv->indicate_seq, pattrib->seq_num); */
+			if (!pattrib->amsdu) {
+				/* DBG_871X("recv_indicatepkts_in_order, amsdu!= 1, indicate_seq =%d, seq_num =%d\n", preorder_ctrl->indicate_seq, pattrib->seq_num); */
+
+				if ((padapter->bDriverStopped == false) &&
+				    (padapter->bSurpriseRemoved == false))
+					rtw_recv_indicatepkt(padapter, prframe);/* indicate this recv_frame */
+
+			} else if (pattrib->amsdu == 1) {
+				if (amsdu_to_msdu(padapter, prframe) != _SUCCESS)
+					rtw_free_recvframe(prframe, &precvpriv->free_recv_queue);
+
+			} else{
+				/* error condition; */
+			}
+
+
+			/* Update local variables. */
+			bPktInBuf = false;
+
+		} else{
+			bPktInBuf = true;
+			break;
+		}
+
+		/* DbgPrint("recv_indicatepkts_in_order():while\n"); */
+
+	}
+
+	/* spin_unlock(&ppending_recvframe_queue->lock); */
+	/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+
+	return bPktInBuf;
+}
+
+int recv_indicatepkt_reorder(struct adapter *padapter, union recv_frame *prframe);
+int recv_indicatepkt_reorder(struct adapter *padapter, union recv_frame *prframe)
+{
+	int retval = _SUCCESS;
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+	struct recv_reorder_ctrl *preorder_ctrl = prframe->u.hdr.preorder_ctrl;
+	struct __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. */
+		wlanhdr_to_ethhdr(prframe);
+
+		if (pattrib->qos != 1) {
+			if ((padapter->bDriverStopped == false) &&
+			    (padapter->bSurpriseRemoved == false)) {
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@  recv_indicatepkt_reorder -recv_func recv_indicatepkt\n"));
+
+				rtw_recv_indicatepkt(padapter, prframe);
+				return _SUCCESS;
+
+			}
+
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s pattrib->qos != 1\n", __func__);
+			#endif
+
+			return _FAIL;
+
+		}
+
+		if (preorder_ctrl->enable == false) {
+			/* indicate this recv_frame */
+			preorder_ctrl->indicate_seq = pattrib->seq_num;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __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
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+				preorder_ctrl->indicate_seq, pattrib->seq_num);
+			#endif
+
+			return _SUCCESS;
+		}
+	} else if (pattrib->amsdu == 1) { /* temp filter -> means didn't support A-MSDUs in a A-MPDU */
+		if (preorder_ctrl->enable == false) {
+			preorder_ctrl->indicate_seq = pattrib->seq_num;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __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
+			DBG_871X("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
+				DBG_871X("DBG_RX_DROP_FRAME %s amsdu_to_msdu fail\n", __func__);
+				#endif
+			}
+
+			return retval;
+		}
+	}
+
+	spin_lock_bh(&ppending_recvframe_queue->lock);
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
+		 ("recv_indicatepkt_reorder: indicate =%d seq =%d\n",
+		  preorder_ctrl->indicate_seq, pattrib->seq_num));
+
+	/* s2. check if winstart_b(indicate_seq) needs to been updated */
+	if (!check_indicate_seq(preorder_ctrl, pattrib->seq_num)) {
+		pdbgpriv->dbg_rx_ampdu_drop_count++;
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("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)) {
+		/* DbgPrint("recv_indicatepkt_reorder, enqueue_reorder_recvframe fail!\n"); */
+		/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+		/* return _FAIL; */
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("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) == true) {
+		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+		spin_unlock_bh(&ppending_recvframe_queue->lock);
+	} else{
+		spin_unlock_bh(&ppending_recvframe_queue->lock);
+		del_timer_sync(&preorder_ctrl->reordering_ctrl_timer);
+	}
+
+	return _SUCCESS;
+
+_err_exit:
+	spin_unlock_bh(&ppending_recvframe_queue->lock);
+
+	return _FAIL;
+}
+
+
+void rtw_reordering_ctrl_timeout_handler(void *pcontext)
+{
+	struct recv_reorder_ctrl *preorder_ctrl = (struct recv_reorder_ctrl *)pcontext;
+	struct adapter *padapter = preorder_ctrl->padapter;
+	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
+		return;
+
+	/* DBG_871X("+rtw_reordering_ctrl_timeout_handler() =>\n"); */
+
+	spin_lock_bh(&ppending_recvframe_queue->lock);
+
+	if (recv_indicatepkts_in_order(padapter, preorder_ctrl, true) == true)
+		_set_timer(&preorder_ctrl->reordering_ctrl_timer, REORDER_WAIT_TIME);
+
+	spin_unlock_bh(&ppending_recvframe_queue->lock);
+
+}
+
+int process_recv_indicatepkts(struct adapter *padapter, union recv_frame *prframe);
+int process_recv_indicatepkts(struct 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;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate);
+
+	if (phtpriv->ht_option == true) { /* B/G/N Mode */
+		/* prframe->u.hdr.preorder_ctrl = &precvpriv->recvreorder_ctrl[pattrib->priority]; */
+
+		if (recv_indicatepkt_reorder(padapter, prframe) != _SUCCESS) { /*  including perform A-MPDU Rx Ordering Buffer Control */
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s recv_indicatepkt_reorder error!\n", __func__);
+			#endif
+
+			if ((padapter->bDriverStopped == false) &&
+			    (padapter->bSurpriseRemoved == false)) {
+				retval = _FAIL;
+				return retval;
+			}
+		}
+	} else { /* B/G mode */
+		retval = wlanhdr_to_ethhdr(prframe);
+		if (retval != _SUCCESS) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("wlanhdr_to_ethhdr: drop pkt\n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s wlanhdr_to_ethhdr error!\n", __func__);
+			#endif
+			return retval;
+		}
+
+		if ((padapter->bDriverStopped == false) && (padapter->bSurpriseRemoved == false)) {
+			/* indicate this recv_frame */
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@ process_recv_indicatepkts- recv_func recv_indicatepkt\n"));
+			rtw_recv_indicatepkt(padapter, prframe);
+
+
+		} else{
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@ process_recv_indicatepkts- recv_func free_indicatepkt\n"));
+
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_func:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved));
+			retval = _FAIL;
+			return retval;
+		}
+
+	}
+
+	return retval;
+
+}
+
+static int recv_func_prehandle(struct adapter *padapter, union recv_frame *rframe)
+{
+	int ret = _SUCCESS;
+	struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_pre);
+
+	/* check the frame crtl field and decache */
+	ret = validate_recv_frame(padapter, rframe);
+	if (ret != _SUCCESS) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("recv_func: validate_recv_frame fail! drop pkt\n"));
+		rtw_free_recvframe(rframe, pfree_recv_queue);/* free this recv_frame */
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+static int recv_func_posthandle(struct adapter *padapter, union recv_frame *prframe)
+{
+	int ret = _SUCCESS;
+	union recv_frame *orig_prframe = prframe;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post);
+
+	prframe = decryptor(padapter, prframe);
+	if (prframe == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("decryptor: drop pkt\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s decryptor: drop pkt\n", __func__);
+		#endif
+		ret = _FAIL;
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_err);
+		goto _recv_data_drop;
+	}
+
+	prframe = recvframe_chk_defrag(padapter, prframe);
+	if (prframe == NULL)	{
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chk_defrag: drop pkt\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s recvframe_chk_defrag: drop pkt\n", __func__);
+		#endif
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_defrag_err);
+		goto _recv_data_drop;
+	}
+
+	prframe = portctrl(padapter, prframe);
+	if (prframe == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("portctrl: drop pkt\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s portctrl: drop pkt\n", __func__);
+		#endif
+		ret = _FAIL;
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_portctrl_err);
+		goto _recv_data_drop;
+	}
+
+	count_rx_stats(padapter, prframe, NULL);
+
+	ret = process_recv_indicatepkts(padapter, prframe);
+	if (ret != _SUCCESS) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recv_func: process_recv_indicatepkts fail!\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s process_recv_indicatepkts fail!\n", __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;
+	}
+
+_recv_data_drop:
+	precvpriv->rx_drop++;
+	return ret;
+}
+
+
+int recv_func(struct adapter *padapter, union recv_frame *rframe);
+int recv_func(struct adapter *padapter, union recv_frame *rframe)
+{
+	int ret;
+	struct rx_pkt_attrib *prxattrib = &rframe->u.hdr.attrib;
+	struct recv_priv *recvpriv = &padapter->recvpriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+
+	/* check if need to handle uc_swdec_pending_queue*/
+	if (check_fwstate(mlmepriv, WIFI_STATION_STATE) && psecuritypriv->busetkipkey) {
+		union recv_frame *pending_frame;
+		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)
+			DBG_871X(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 == true) &&
+			psecuritypriv->ndisauthtype == Ndis802_11AuthModeWPAPSK &&
+			!psecuritypriv->busetkipkey) {
+			DBG_COUNTER(padapter->rx_logs.core_rx_enqueue);
+			rtw_enqueue_recvframe(rframe, &padapter->recvpriv.uc_swdec_pending_queue);
+			/* DBG_871X("%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)
+{
+	struct adapter *padapter;
+	struct recv_priv *precvpriv;
+	s32 ret = _SUCCESS;
+
+/* 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("+rtw_recv_entry\n")); */
+
+	padapter = precvframe->u.hdr.adapter;
+
+	precvpriv = &padapter->recvpriv;
+
+	ret = recv_func(padapter, precvframe);
+	if (ret == _FAIL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("rtw_recv_entry: recv_func return fail!!!\n"));
+		goto _recv_entry_drop;
+	}
+
+
+	precvpriv->rx_pkts++;
+
+	return ret;
+
+_recv_entry_drop:
+
+	/* RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("_recv_entry_drop\n")); */
+
+	return ret;
+}
+
+void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS)
+{
+	struct adapter *adapter = (struct adapter *)FunctionContext;
+	struct recv_priv *recvpriv = &adapter->recvpriv;
+
+	u32 tmp_s, tmp_q;
+	u8 avg_signal_strength = 0;
+	u8 avg_signal_qual = 0;
+	u32 num_signal_strength = 0;
+	u32 num_signal_qual = 0;
+	u8 _alpha = 5; /*  this value is based on converging_constant = 5000 and sampling_interval = 1000 */
+
+	if (adapter->recvpriv.is_signal_dbg) {
+		/* update the user specific value, signal_strength_dbg, to signal_strength, rssi */
+		adapter->recvpriv.signal_strength = adapter->recvpriv.signal_strength_dbg;
+		adapter->recvpriv.rssi = (s8)translate_percentage_to_dbm((u8)adapter->recvpriv.signal_strength_dbg);
+	} else {
+
+		if (recvpriv->signal_strength_data.update_req == 0) {/*  update_req is clear, means we got rx */
+			avg_signal_strength = recvpriv->signal_strength_data.avg_val;
+			num_signal_strength = recvpriv->signal_strength_data.total_num;
+			/*  after avg_vals are accquired, we can re-stat the signal values */
+			recvpriv->signal_strength_data.update_req = 1;
+		}
+
+		if (recvpriv->signal_qual_data.update_req == 0) {/*  update_req is clear, means we got rx */
+			avg_signal_qual = recvpriv->signal_qual_data.avg_val;
+			num_signal_qual = recvpriv->signal_qual_data.total_num;
+			/*  after avg_vals are accquired, we can re-stat the signal values */
+			recvpriv->signal_qual_data.update_req = 1;
+		}
+
+		if (num_signal_strength == 0) {
+			if (rtw_get_on_cur_ch_time(adapter) == 0
+				|| jiffies_to_msecs(jiffies - 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) == true
+			|| check_fwstate(&adapter->mlmepriv, _FW_LINKED) == false
+		) {
+			goto set_timer;
+		}
+
+		/* update value of signal_strength, rssi, signal_qual */
+		tmp_s = (avg_signal_strength+(_alpha-1)*recvpriv->signal_strength);
+		if (tmp_s % _alpha)
+			tmp_s = tmp_s/_alpha + 1;
+		else
+			tmp_s = tmp_s/_alpha;
+		if (tmp_s > 100)
+			tmp_s = 100;
+
+		tmp_q = (avg_signal_qual+(_alpha-1)*recvpriv->signal_qual);
+		if (tmp_q % _alpha)
+			tmp_q = tmp_q/_alpha + 1;
+		else
+			tmp_q = tmp_q/_alpha;
+		if (tmp_q > 100)
+			tmp_q = 100;
+
+		recvpriv->signal_strength = tmp_s;
+		recvpriv->rssi = (s8)translate_percentage_to_dbm(tmp_s);
+		recvpriv->signal_qual = tmp_q;
+
+		#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1
+		DBG_871X(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) ? jiffies_to_msecs(jiffies - rtw_get_on_cur_ch_time(adapter)) : 0
+		);
+		#endif
+	}
+
+set_timer:
+	rtw_set_signal_stat_timer(recvpriv);
+
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_rf.c linux/3rdparty/rtl8723bs/core/rtw_rf.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_rf.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_rf.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,64 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#define _RTW_RF_C_
+
+#include <drv_types.h>
+
+
+struct ch_freq {
+	u32 channel;
+	u32 frequency;
+};
+
+static struct ch_freq ch_freq_map[] = {
+	{1, 2412}, {2, 2417}, {3, 2422}, {4, 2427}, {5, 2432},
+	{6, 2437}, {7, 2442}, {8, 2447}, {9, 2452}, {10, 2457},
+	{11, 2462}, {12, 2467}, {13, 2472}, {14, 2484},
+	/*  UNII */
+	{36, 5180}, {40, 5200}, {44, 5220}, {48, 5240}, {52, 5260},
+	{56, 5280}, {60, 5300}, {64, 5320}, {149, 5745}, {153, 5765},
+	{157, 5785}, {161, 5805}, {165, 5825}, {167, 5835}, {169, 5845},
+	{171, 5855}, {173, 5865},
+	/* HiperLAN2 */
+	{100, 5500}, {104, 5520}, {108, 5540}, {112, 5560}, {116, 5580},
+	{120, 5600}, {124, 5620}, {128, 5640}, {132, 5660}, {136, 5680},
+	{140, 5700},
+	/* Japan MMAC */
+	{34, 5170}, {38, 5190}, {42, 5210}, {46, 5230},
+	/*  Japan */
+	{184, 4920}, {188, 4940}, {192, 4960}, {196, 4980},
+	{208, 5040},/* Japan, means J08 */
+	{212, 5060},/* Japan, means J12 */
+	{216, 5080},/* Japan, means J16 */
+};
+
+static int ch_freq_map_num = (sizeof(ch_freq_map) / sizeof(struct ch_freq));
+
+u32 rtw_ch2freq(u32 channel)
+{
+	u8 i;
+	u32 freq = 0;
+
+	for (i = 0; i < ch_freq_map_num; i++) {
+		if (channel == ch_freq_map[i].channel) {
+			freq = ch_freq_map[i].frequency;
+				break;
+		}
+	}
+	if (i == ch_freq_map_num)
+		freq = 2412;
+
+	return freq;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_security.c linux/3rdparty/rtl8723bs/core/rtw_security.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_security.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_security.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,2448 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#define  _RTW_SECURITY_C_
+
+#include <drv_types.h>
+#include <rtw_debug.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)
+{
+	if (value <= _BIP_)
+		return _security_type_str[value];
+	return NULL;
+}
+
+#ifdef DBG_SW_SEC_CNT
+#define WEP_SW_ENC_CNT_INC(sec, ra) \
+	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++;
+
+#define WEP_SW_DEC_CNT_INC(sec, ra) \
+	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++;
+
+#define TKIP_SW_ENC_CNT_INC(sec, ra) \
+	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++;
+
+#define TKIP_SW_DEC_CNT_INC(sec, ra) \
+	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++;
+
+#define AES_SW_ENC_CNT_INC(sec, ra) \
+	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++;
+
+#define AES_SW_DEC_CNT_INC(sec, ra) \
+	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++;
+#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)
+		return;
+	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;
+	}
+}
+
+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(struct 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;
+
+	hw_hdr_offset = TXDESC_OFFSET;
+	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(struct 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 */
+		*((u32 *)crc) = le32_to_cpu(getcrc32(payload, length-4));
+
+		if (crc[3] != payload[length-1] || crc[2] != payload[length-2] || crc[1] != payload[length-3] || crc[0] != payload[length-4]) {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_wep_decrypt:icv error crc[3](%x)!=payload[length-1](%x) || crc[2](%x)!=payload[length-2](%x) || crc[1](%x)!=payload[length-3](%x) || crc[0](%x)!=payload[length-4](%x)\n",
+						crc[3], payload[length-1], crc[2], payload[length-2], crc[1], payload[length-3], crc[0], payload[length-4]));
+		}
+
+		WEP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
+	}
+	return;
+}
+
+/* 3		=====TKIP related ===== */
+
+static u32 secmicgetuint32(u8 *p)
+/*  Convert from Byte[] to Us3232 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 Us3232 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(struct 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;
+
+	hw_hdr_offset = TXDESC_OFFSET;
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
+
+	/* 4 start to encrypt each fragment */
+	if (pattrib->encrypt == _TKIP_) {
+
+/*
+		if (pattrib->psta)
+		{
+			stainfo = pattrib->psta;
+		}
+		else
+		{
+			DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+			stainfo =rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]);
+		}
+*/
+		/* if (stainfo!= NULL) */
+		{
+/*
+			if (!(stainfo->state &_FW_LINKED))
+			{
+				DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
+				return _FAIL;
+			}
+*/
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt: stainfo!= NULL!!!\n"));
+
+			if (IS_MCAST(pattrib->ra))
+				prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+			else
+				/* prwskey =&stainfo->dot118021x_UncstKey.skey[0]; */
+				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;
+					RT_TRACE(_module_rtl871x_security_c_, _drv_info_, ("pattrib->iv_len =%x, pattrib->icv_len =%x\n", 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 {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt: stainfo == NULL!!!\n"));
+			DBG_871X("%s, psta ==NUL\n", __func__);
+			res = _FAIL;
+		}
+*/
+
+	}
+	return res;
+}
+
+
+/* The hlen isn't include the IV */
+u32 rtw_tkip_decrypt(struct 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 unsigned long 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 (jiffies_to_msecs(jiffies - start) > 1000) {
+						if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
+							DBG_871X_LEVEL(_drv_always_, 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) {
+					DBG_871X_LEVEL(_drv_always_, 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;
+
+				/* DBG_871X("rx bc/mc packets, to perform sw rtw_tkip_decrypt\n"); */
+				/* prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; */
+				prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+				prwskeylen = 16;
+			} else{
+				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+				prwskeylen = 16;
+			}
+
+			iv = pframe+prxattrib->hdrlen;
+			payload = pframe+prxattrib->iv_len+prxattrib->hdrlen;
+			length = ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
+
+			GET_TKIP_PN(iv, dot11txpn);
+
+			pnl = (u16)(dot11txpn.val);
+			pnh = (u32)(dot11txpn.val>>16);
+
+			phase1((u16 *)&ttkey[0], prwskey, &prxattrib->ta[0], pnh);
+			phase2(&rc4key[0], prwskey, (unsigned short *)&ttkey[0], pnl);
+
+			/* 4 decrypt payload include icv */
+
+			arcfour_init(&mycontext, rc4key, 16);
+			arcfour_encrypt(&mycontext, payload, payload, length);
+
+			*((u32 *)crc) = le32_to_cpu(getcrc32(payload, length-4));
+
+			if (crc[3] != payload[length-1] || crc[2] != payload[length-2] || crc[1] != payload[length-3] || crc[0] != payload[length-4]) {
+			    RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_wep_decrypt:icv error crc[3](%x)!=payload[length-1](%x) || crc[2](%x)!=payload[length-2](%x) || crc[1](%x)!=payload[length-3](%x) || crc[0](%x)!=payload[length-4](%x)\n",
+						crc[3], payload[length-1], crc[2], payload[length-2], crc[1], payload[length-3], crc[0], payload[length-4]));
+				res = _FAIL;
+			}
+
+			TKIP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
+		} else {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt: stainfo == NULL!!!\n"));
+			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 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;
+
+		/* 802.11w management frame should set management bit(4) */
+		if (frtype == WIFI_MGT_TYPE)
+			mic_iv[1] |= BIT(4);
+
+		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);
+
+		/* 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
+			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] = 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;
+
+	/* 802.11w management frame should set management bit(4) */
+	if (frtype == WIFI_MGT_TYPE)
+		ctr_preload[1] |= BIT(4);
+
+	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)
+{
+	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	frtype  = GetFrameType(pframe);
+	uint	frsubtype  = GetFrameSubType(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;
+
+		} else if ((frtype == WIFI_DATA) && /*  add for CONFIG_IEEE80211W, none 11w also can use */
+		((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(struct 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;
+
+	hw_hdr_offset = TXDESC_OFFSET;
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
+
+	/* 4 start to encrypt each fragment */
+	if ((pattrib->encrypt == _AES_)) {
+		RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_encrypt: stainfo!= NULL!!!\n"));
+
+		if (IS_MCAST(pattrib->ra))
+			prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+		else
+			/* prwskey =&stainfo->dot118021x_UncstKey.skey[0]; */
+			prwskey = pattrib->dot118021x_UncstKey.skey;
+
+		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);
+	}
+	return res;
+}
+
+static sint aes_decipher(u8 *key, uint	hdrlen,
+			u8 *pframe, uint plen)
+{
+	static u8 message[MAX_MSG_SIZE];
+	uint	qc_exists, a4_exists, i, j, payload_remainder,
+			num_blocks, payload_index;
+	sint res = _SUCCESS;
+	u8 pn_vector[6];
+	u8 mic_iv[16];
+	u8 mic_header1[16];
+	u8 mic_header2[16];
+	u8 ctr_preload[16];
+
+		/* Intermediate Buffers */
+	u8 chain_buffer[16];
+	u8 aes_out[16];
+	u8 padded_buffer[16];
+	u8 mic[8];
+
+
+/* 	uint	offset = 0; */
+	uint	frtype  = GetFrameType(pframe);
+	uint	frsubtype  = GetFrameSubType(pframe);
+
+	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;
+			}
+		} else if ((frtype == WIFI_DATA) && /* only for data packet . add for CONFIG_IEEE80211W, none 11w also can use */
+		((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]) {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x)\n",
+						i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]));
+			DBG_871X("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x)\n",
+						i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]);
+			res = _FAIL;
+		}
+	}
+	return res;
+}
+
+u32 rtw_aes_decrypt(struct adapter *padapter, u8 *precvframe)
+{	/*  exclude ICV */
+
+
+	/*static*/
+/* 	unsigned char message[MAX_MSG_SIZE]; */
+
+
+	/* Intermediate Buffers */
+
+
+	sint		length;
+	u8 *pframe, *prwskey;	/*  *payload,*iv */
+	struct	sta_info 	*stainfo;
+	struct	rx_pkt_attrib	 *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
+	struct	security_priv *psecuritypriv = &padapter->securitypriv;
+/* 	struct	recv_priv 	*precvpriv =&padapter->recvpriv; */
+	u32 res = _SUCCESS;
+
+	pframe = (unsigned char *)((union recv_frame *)precvframe)->u.hdr.rx_data;
+	/* 4 start to encrypt each fragment */
+	if ((prxattrib->encrypt == _AES_)) {
+
+		stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]);
+		if (stainfo != NULL) {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_decrypt: stainfo!= NULL!!!\n"));
+
+			if (IS_MCAST(prxattrib->ra)) {
+				static unsigned long start = 0;
+				static u32 no_gkey_bc_cnt = 0;
+				static u32 no_gkey_mc_cnt = 0;
+
+				/* DBG_871X("rx bc/mc packets, to perform sw rtw_aes_decrypt\n"); */
+				/* prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; */
+				if (psecuritypriv->binstallGrpkey == false) {
+					res = _FAIL;
+
+					if (start == 0)
+						start = jiffies;
+
+					if (is_broadcast_mac_addr(prxattrib->ra))
+						no_gkey_bc_cnt++;
+					else
+						no_gkey_mc_cnt++;
+
+					if (jiffies_to_msecs(jiffies - start) > 1000) {
+						if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
+							DBG_871X_LEVEL(_drv_always_, 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) {
+					DBG_871X_LEVEL(_drv_always_, 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) {
+					DBG_871X("not match packet_index =%d, install_index =%d\n"
+					, prxattrib->key_index, psecuritypriv->dot118021XGrpKeyid);
+					res = _FAIL;
+					goto exit;
+				}
+			} else
+				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+
+
+			length = ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
+
+			res = aes_decipher(prwskey, prxattrib->hdrlen, pframe, length);
+
+			AES_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
+		} else {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_decrypt: stainfo == NULL!!!\n"));
+			res = _FAIL;
+		}
+	}
+exit:
+	return res;
+}
+
+u32 rtw_BIP_verify(struct 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 ieee80211_hdr *pwlanhdr;
+	u8 mic[16];
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	__le16 le_tmp;
+	__le64 le_tmp64;
+
+	ori_len = pattrib->pkt_len-WLAN_HDR_A3_LEN+BIP_AAD_SIZE;
+	BIP_AAD = rtw_zmalloc(ori_len);
+
+	if (BIP_AAD == NULL) {
+		DBG_871X("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 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(&le_tmp64, p+4, 6);
+		temp_ipn = le64_to_cpu(le_tmp64);
+		/* BIP packet number should bigger than previous BIP packet */
+		if (temp_ipn <= pmlmeext->mgnt_80211w_IPN_rx) {
+			DBG_871X("replay BIP packet\n");
+			goto BIP_exit;
+		}
+		/* copy key index */
+		memcpy(&le_tmp, p+2, 2);
+		keyid = le16_to_cpu(le_tmp);
+		if (keyid != padapter->securitypriv.dot11wBIPKeyid) {
+			DBG_871X("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_control, 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
+			DBG_871X("BIP MIC error!\n");
+
+	} else
+		res = RTW_RX_HANDLED;
+BIP_exit:
+
+	kfree(BIP_AAD);
+	return res;
+}
+
+/* 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;
+	int r;
+
+	/*
+	 * map byte array block to cipher state
+	 * and add initial round key:
+	 */
+	s0 = GETU32(pt) ^ rk[0];
+	s1 = GETU32(pt +  4) ^ rk[1];
+	s2 = GETU32(pt +  8) ^ rk[2];
+	s3 = GETU32(pt + 12) ^ rk[3];
+
+#define ROUND(i, d, s) \
+d##0 = TE0(s##0) ^ TE1(s##1) ^ TE2(s##2) ^ TE3(s##3) ^ rk[4 * i]; \
+d##1 = TE0(s##1) ^ TE1(s##2) ^ TE2(s##3) ^ TE3(s##0) ^ rk[4 * i + 1]; \
+d##2 = TE0(s##2) ^ TE1(s##3) ^ TE2(s##0) ^ TE3(s##1) ^ rk[4 * i + 2]; \
+d##3 = TE0(s##3) ^ TE1(s##0) ^ TE2(s##1) ^ TE3(s##2) ^ rk[4 * i + 3]
+
+	/* Nr - 1 full rounds: */
+	r = Nr >> 1;
+	for (;;) {
+		ROUND(1, t, s);
+		rk += 8;
+		if (--r == 0)
+			break;
+		ROUND(0, s, t);
+	}
+
+#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);
+	kfree(ctx);
+}
+
+
+/**
+ * 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 */
+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);
+}
+
+/* Restore HW wep key setting according to key_mask */
+void rtw_sec_restore_wep_key(struct 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(struct adapter *adapter, const char *caller)
+{
+	struct security_priv *securitypriv = &(adapter->securitypriv);
+	u8 status = _SUCCESS;
+
+	if (securitypriv->btkip_countermeasure == true) {
+		unsigned long passing_ms = jiffies_to_msecs(jiffies - securitypriv->btkip_countermeasure_time);
+		if (passing_ms > 60*1000) {
+			DBG_871X_LEVEL(_drv_always_, "%s("ADPT_FMT") countermeasure time:%lus > 60s\n",
+				caller, ADPT_ARG(adapter), passing_ms/1000);
+			securitypriv->btkip_countermeasure = false;
+			securitypriv->btkip_countermeasure_time = 0;
+		} else {
+			DBG_871X_LEVEL(_drv_always_, "%s("ADPT_FMT") countermeasure time:%lus < 60s\n",
+				caller, ADPT_ARG(adapter), passing_ms/1000);
+			status = _FAIL;
+		}
+	}
+
+	return status;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_sta_mgt.c linux/3rdparty/rtl8723bs/core/rtw_sta_mgt.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_sta_mgt.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_sta_mgt.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,641 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#define _RTW_STA_MGT_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+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);
+
+	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;
+
+	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->under_exist_checking = 0;
+
+	psta->keep_alive_trycnt = 0;
+}
+
+u32 _rtw_init_sta_priv(struct	sta_priv *pstapriv)
+{
+	struct sta_info *psta;
+	s32 i;
+
+	pstapriv->pallocated_stainfo_buf = vzalloc(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->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 = 3; /*  3*2 = 6 sec */
+	pstapriv->max_num_sta = NUM_STA;
+	return _SUCCESS;
+}
+
+inline int rtw_stainfo_offset(struct sta_priv *stapriv, struct sta_info *sta)
+{
+	int offset = (((u8 *)sta) - stapriv->pstainfo_buf)/sizeof(struct sta_info);
+
+	if (!stainfo_offset_valid(offset))
+		DBG_871X("%s invalid offset(%d), out of range!!!", __func__, offset);
+
+	return offset;
+}
+
+inline struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int offset)
+{
+	if (!stainfo_offset_valid(offset))
+		DBG_871X("%s invalid offset(%d), out of range!!!", __func__, offset);
+
+	return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info));
+}
+
+/*  this function is used to free the memory of lock || sema for all stainfos */
+void kfree_all_stainfo(struct sta_priv *pstapriv);
+void kfree_all_stainfo(struct sta_priv *pstapriv)
+{
+	struct list_head	*plist, *phead;
+	struct sta_info *psta = NULL;
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+
+	phead = get_list_head(&pstapriv->free_sta_queue);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+		psta = LIST_CONTAINOR(plist, struct sta_info, list);
+		plist = get_next(plist);
+	}
+
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+}
+
+void kfree_sta_priv_lock(struct	sta_priv *pstapriv);
+void kfree_sta_priv_lock(struct	sta_priv *pstapriv)
+{
+	 kfree_all_stainfo(pstapriv); /* be done before free sta_hash_lock */
+}
+
+u32 _rtw_free_sta_priv(struct	sta_priv *pstapriv)
+{
+	struct list_head	*phead, *plist;
+	struct sta_info *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int	index;
+
+	if (pstapriv) {
+
+		/*delete all reordering_ctrl_timer		*/
+		spin_lock_bh(&pstapriv->sta_hash_lock);
+		for (index = 0; index < NUM_STA; index++) {
+			phead = &(pstapriv->sta_hash[index]);
+			plist = get_next(phead);
+
+			while (phead != plist) {
+				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];
+					del_timer_sync(&preorder_ctrl->reordering_ctrl_timer);
+				}
+			}
+		}
+		spin_unlock_bh(&pstapriv->sta_hash_lock);
+		/*===============================*/
+
+		kfree_sta_priv_lock(pstapriv);
+
+		if (pstapriv->pallocated_stainfo_buf)
+			vfree(pstapriv->pallocated_stainfo_buf);
+
+	}
+	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)
+{
+	uint tmp_aid;
+	s32	index;
+	struct list_head	*phash_list;
+	struct sta_info *psta;
+	struct __queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int i = 0;
+	u16  wRxSeqInitialValue = 0xffff;
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+
+	/* spin_lock_bh(&(pfree_sta_queue->lock)); */
+	spin_lock_bh(&(pstapriv->sta_hash_lock));
+	if (list_empty(&pfree_sta_queue->queue)) {
+		/* spin_unlock_bh(&(pfree_sta_queue->lock)); */
+		spin_unlock_bh(&(pstapriv->sta_hash_lock));
+		psta = NULL;
+		return psta;
+	} else{
+		psta = LIST_CONTAINOR(get_next(&pfree_sta_queue->queue), struct sta_info, list);
+
+		list_del_init(&(psta->list));
+
+		/* spin_unlock_bh(&(pfree_sta_queue->lock)); */
+
+		tmp_aid = psta->aid;
+
+		_rtw_init_stainfo(psta);
+
+		psta->padapter = pstapriv->padapter;
+
+		memcpy(psta->hwaddr, hwaddr, ETH_ALEN);
+
+		index = wifi_mac_hash(hwaddr);
+
+		RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_info_, ("rtw_alloc_stainfo: index  = %x", index));
+
+		if (index >= NUM_STA) {
+			RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_, ("ERROR => rtw_alloc_stainfo: index >= NUM_STA"));
+			spin_unlock_bh(&(pstapriv->sta_hash_lock));
+			psta = NULL;
+			goto exit;
+		}
+		phash_list = &(pstapriv->sta_hash[index]);
+
+		/* spin_lock_bh(&(pstapriv->sta_hash_lock)); */
+
+		list_add_tail(&psta->hash_list, phash_list);
+
+		pstapriv->asoc_sta_count++;
+
+		/* spin_unlock_bh(&(pstapriv->sta_hash_lock)); */
+
+/*  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);
+		}
+
+		RT_TRACE(
+			_module_rtl871x_sta_mgt_c_,
+			_drv_info_, (
+				"alloc number_%d stainfo  with hwaddr = %x %x %x %x %x %x \n",
+				pstapriv->asoc_sta_count,
+				hwaddr[0],
+				hwaddr[1],
+				hwaddr[2],
+				hwaddr[3],
+				hwaddr[4],
+				hwaddr[5]
+			)
+		);
+
+		init_addba_retry_timer(pstapriv->padapter, psta);
+
+		/* for A-MPDU Rx reordering buffer control */
+		for (i = 0; i < 16 ; i++) {
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+			preorder_ctrl->padapter = pstapriv->padapter;
+
+			preorder_ctrl->enable = false;
+
+			preorder_ctrl->indicate_seq = 0xffff;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d\n", __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; */
+
+			_rtw_init_queue(&preorder_ctrl->pending_recvframe_queue);
+
+			rtw_init_recv_timer(preorder_ctrl);
+		}
+
+
+		/* init for DM */
+		psta->rssi_stat.UndecoratedSmoothedPWDB = (-1);
+		psta->rssi_stat.UndecoratedSmoothedCCK = (-1);
+
+		/* init for the sequence number of received management frame */
+		psta->RxMgmtFrameSeqNum = 0xffff;
+		spin_unlock_bh(&(pstapriv->sta_hash_lock));
+		/* alloc mac id for non-bc/mc station, */
+		rtw_alloc_macid(pstapriv->padapter, psta);
+
+	}
+
+exit:
+
+
+	return psta;
+}
+
+/*  using pstapriv->sta_hash_lock to protect */
+u32 rtw_free_stainfo(struct adapter *padapter, struct sta_info *psta)
+{
+	int i;
+	struct __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;
+
+	if (psta == NULL)
+		goto exit;
+
+
+	spin_lock_bh(&psta->lock);
+	psta->state &= ~_FW_LINKED;
+	spin_unlock_bh(&psta->lock);
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+
+
+	pstaxmitpriv = &psta->sta_xmitpriv;
+
+	/* list_del_init(&psta->sleep_list); */
+
+	/* list_del_init(&psta->wakeup_list); */
+
+	spin_lock_bh(&pxmitpriv->lock);
+
+	rtw_free_xmitframe_queue(pxmitpriv, &psta->sleep_q);
+	psta->sleepq_len = 0;
+
+	/* vo */
+	/* spin_lock_bh(&(pxmitpriv->vo_pending.lock)); */
+	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;
+	pstaxmitpriv->vo_q.qcnt = 0;
+	/* spin_unlock_bh(&(pxmitpriv->vo_pending.lock)); */
+
+	/* vi */
+	/* spin_lock_bh(&(pxmitpriv->vi_pending.lock)); */
+	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;
+	pstaxmitpriv->vi_q.qcnt = 0;
+	/* spin_unlock_bh(&(pxmitpriv->vi_pending.lock)); */
+
+	/* be */
+	/* spin_lock_bh(&(pxmitpriv->be_pending.lock)); */
+	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;
+	pstaxmitpriv->be_q.qcnt = 0;
+	/* spin_unlock_bh(&(pxmitpriv->be_pending.lock)); */
+
+	/* bk */
+	/* spin_lock_bh(&(pxmitpriv->bk_pending.lock)); */
+	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;
+	pstaxmitpriv->bk_q.qcnt = 0;
+	/* spin_unlock_bh(&(pxmitpriv->bk_pending.lock)); */
+
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	list_del_init(&psta->hash_list);
+	RT_TRACE(
+		_module_rtl871x_sta_mgt_c_,
+		_drv_err_, (
+			"\n free number_%d stainfo  with hwaddr = 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x \n",
+			pstapriv->asoc_sta_count,
+			psta->hwaddr[0],
+			psta->hwaddr[1],
+			psta->hwaddr[2],
+			psta->hwaddr[3],
+			psta->hwaddr[4],
+			psta->hwaddr[5]
+		)
+	);
+	pstapriv->asoc_sta_count--;
+
+
+	/*  re-init sta_info; 20061114 will be init in alloc_stainfo */
+	/* _rtw_init_sta_xmit_priv(&psta->sta_xmitpriv); */
+	/* _rtw_init_sta_recv_priv(&psta->sta_recvpriv); */
+
+	del_timer_sync(&psta->addba_retry_timer);
+
+	/* for A-MPDU Rx reordering buffer control, cancel reordering_ctrl_timer */
+	for (i = 0; i < 16 ; i++) {
+		struct list_head	*phead, *plist;
+		union recv_frame *prframe;
+		struct __queue *ppending_recvframe_queue;
+		struct __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+		preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+		del_timer_sync(&preorder_ctrl->reordering_ctrl_timer);
+
+
+		ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+		spin_lock_bh(&ppending_recvframe_queue->lock);
+
+		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);
+		}
+
+		spin_unlock_bh(&ppending_recvframe_queue->lock);
+
+	}
+
+	if (!(psta->state & WIFI_AP_STATE))
+		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, false);
+
+
+	/* release mac id for non-bc/mc station, */
+	rtw_release_macid(pstapriv->padapter, psta);
+
+/*
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	list_del_init(&psta->asoc_list);
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+*/
+	spin_lock_bh(&pstapriv->auth_list_lock);
+	if (!list_empty(&psta->auth_list)) {
+		list_del_init(&psta->auth_list);
+		pstapriv->auth_list_cnt--;
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	psta->expire_to = 0;
+	psta->sleepq_ac_len = 0;
+	psta->qos_info = 0;
+
+	psta->max_sp_len = 0;
+	psta->uapsd_bk = 0;
+	psta->uapsd_be = 0;
+	psta->uapsd_vi = 0;
+	psta->uapsd_vo = 0;
+
+	psta->has_legacy_ac = 0;
+
+	pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+	pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+	if ((psta->aid > 0) && (pstapriv->sta_aid[psta->aid - 1] == psta)) {
+		pstapriv->sta_aid[psta->aid - 1] = NULL;
+		psta->aid = 0;
+	}
+
+	psta->under_exist_checking = 0;
+
+	/* spin_lock_bh(&(pfree_sta_queue->lock)); */
+	list_add_tail(&psta->list, get_list_head(pfree_sta_queue));
+	/* spin_unlock_bh(&(pfree_sta_queue->lock)); */
+
+exit:
+	return _SUCCESS;
+}
+
+/*  free all stainfo which in sta_hash[all] */
+void rtw_free_all_stainfo(struct adapter *padapter)
+{
+	struct list_head	*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);
+
+	if (pstapriv->asoc_sta_count == 1)
+		return;
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+
+	for (index = 0; index < NUM_STA; index++) {
+		phead = &(pstapriv->sta_hash[index]);
+		plist = get_next(phead);
+
+		while (phead != plist) {
+			psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+			plist = get_next(plist);
+
+			if (pbcmc_stainfo != psta)
+				rtw_free_stainfo(padapter, psta);
+
+		}
+	}
+
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+}
+
+/* any station allocated can be searched by hash list */
+struct sta_info *rtw_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
+{
+	struct list_head	*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);
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+
+	phead = &(pstapriv->sta_hash[index]);
+	plist = get_next(phead);
+
+
+	while (phead != plist) {
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+		if ((!memcmp(psta->hwaddr, addr, ETH_ALEN)))
+		 /*  if found the matched address */
+			break;
+
+		psta = NULL;
+		plist = get_next(plist);
+	}
+
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+	return psta;
+}
+
+u32 rtw_init_bcmc_stainfo(struct 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;
+	/* struct __queue	*pstapending = &padapter->xmitpriv.bm_pending; */
+
+	psta = rtw_alloc_stainfo(pstapriv, bcast_addr);
+
+	if (psta == NULL) {
+		res = _FAIL;
+		RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_, ("rtw_alloc_stainfo fail"));
+		goto exit;
+	}
+
+	/*  default broadcast & multicast use macid 1 */
+	psta->mac_id = 1;
+
+	ptxservq = &(psta->sta_xmitpriv.be_q);
+exit:
+	return _SUCCESS;
+}
+
+
+struct sta_info *rtw_get_bcmc_stainfo(struct 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;
+}
+
+u8 rtw_access_ctrl(struct adapter *padapter, u8 *mac_addr)
+{
+	u8 res = true;
+	struct list_head	*plist, *phead;
+	struct rtw_wlan_acl_node *paclnode;
+	u8 match = false;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
+	struct __queue	*pacl_node_q = &pacl_list->acl_node_q;
+
+	spin_lock_bh(&(pacl_node_q->lock));
+	phead = get_list_head(pacl_node_q);
+	plist = get_next(phead);
+	while (phead != plist) {
+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);
+		plist = get_next(plist);
+
+		if (!memcmp(paclnode->addr, mac_addr, ETH_ALEN))
+			if (paclnode->valid == true) {
+				match = true;
+				break;
+			}
+
+	}
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+
+	if (pacl_list->mode == 1) /* accept unless in deny list */
+		res = (match == true) ?  false:true;
+
+	else if (pacl_list->mode == 2)/* deny unless in accept list */
+		res = (match == true) ?  true:false;
+	else
+		 res = true;
+
+	return res;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_wlan_util.c linux/3rdparty/rtl8723bs/core/rtw_wlan_util.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_wlan_util.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_wlan_util.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,2328 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#define _RTW_WLAN_UTIL_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_com_h2c.h>
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+#include <linux/inetdevice.h>
+#endif
+
+static unsigned char ARTHEROS_OUI1[] = {0x00, 0x03, 0x7f};
+static unsigned char ARTHEROS_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};
+
+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};
+static unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
+static unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
+
+extern unsigned char RTW_WPA_OUI[];
+extern unsigned char WPA_TKIP_CIPHER[4];
+
+#define R2T_PHY_DELAY	(0)
+
+/* define WAIT_FOR_BCN_TO_MIN	(3000) */
+#define WAIT_FOR_BCN_TO_MIN	(6000)
+#define WAIT_FOR_BCN_TO_MAX	(20000)
+
+#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
+};
+
+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;
+}
+
+u8 networktype_to_raid_ex(struct adapter *adapter, struct sta_info *psta)
+{
+	u8 raid, cur_rf_type, rf_type = RF_1T1R;
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&cur_rf_type));
+
+	if (cur_rf_type == RF_1T1R) {
+		rf_type = RF_1T1R;
+	} else if (IsSupportedVHT(psta->wireless_mode)) {
+		if (psta->ra_mask & 0xffc00000)
+			rf_type = RF_2T2R;
+	} else if (IsSupportedHT(psta->wireless_mode)) {
+		if (psta->ra_mask & 0xfff00000)
+			rf_type = RF_2T2R;
+	}
+
+	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 (rf_type == RF_2T2R)
+			raid = RATEID_IDX_GN_N2SS;
+		else
+			raid = RATEID_IDX_GN_N1SS;
+		break;
+	case WIRELESS_11B_24N:
+	case WIRELESS_11BG_24N:
+		if (psta->bw_mode == CHANNEL_WIDTH_20) {
+			if (rf_type == RF_2T2R)
+				raid = RATEID_IDX_BGN_20M_2SS_BN;
+			else
+				raid = RATEID_IDX_BGN_20M_1SS_BN;
+		} else {
+			if (rf_type == RF_2T2R)
+				raid = RATEID_IDX_BGN_40M_2SS;
+			else
+				raid = RATEID_IDX_BGN_40M_1SS;
+		}
+		break;
+	default:
+		raid = RATEID_IDX_BGN_40M_2SS;
+		break;
+
+	}
+	return raid;
+
+}
+
+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(struct adapter *padapter, unsigned char rate);
+int is_basicrate(struct 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(struct adapter *padapter, unsigned char *rateset);
+unsigned int ratetbl2rateset(struct adapter *padapter, unsigned char *rateset)
+{
+	int i;
+	unsigned char rate;
+	unsigned int	len = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	for (i = 0; i < NumRates; i++) {
+		rate = pmlmeext->datarate[i];
+
+		switch (rate) {
+		case 0xff:
+			return len;
+
+		case 0xfe:
+			continue;
+
+		default:
+			rate = ratetbl_val_2wifirate(rate);
+
+			if (is_basicrate(padapter, rate) == true)
+				rate |= IEEE80211_BASIC_RATE_MASK;
+
+			rateset[len] = rate;
+			len++;
+			break;
+		}
+	}
+	return len;
+}
+
+void get_rate_set(struct 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(struct adapter *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 Save_DM_Func_Flag(struct adapter *padapter)
+{
+	u8 bSaveFlag = true;
+	rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
+}
+
+void Restore_DM_Func_Flag(struct adapter *padapter)
+{
+	u8 bSaveFlag = false;
+	rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
+}
+
+void Switch_DM_Func(struct adapter *padapter, u32 mode, u8 enable)
+{
+	if (enable == true)
+		rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_SET, (u8 *)(&mode));
+	else
+		rtw_hal_set_hwreg(padapter, HW_VAR_DM_FUNC_CLR, (u8 *)(&mode));
+}
+
+static void Set_NETYPE0_MSR(struct adapter *padapter, u8 type)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_MEDIA_STATUS, (u8 *)(&type));
+}
+
+void Set_MSR(struct adapter *padapter, u8 type)
+{
+	Set_NETYPE0_MSR(padapter, type);
+}
+
+inline u8 rtw_get_oper_ch(struct adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_channel;
+}
+
+inline void rtw_set_oper_ch(struct 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++) {
+			struct 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, "]");
+		}
+
+		DBG_871X(FUNC_ADPT_FMT" %s\n", FUNC_ADPT_ARG(adapter), msg);
+#endif /* DBG_CH_SWITCH */
+	}
+
+	dvobj->oper_channel = ch;
+}
+
+inline u8 rtw_get_oper_bw(struct adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_bwmode;
+}
+
+inline void rtw_set_oper_bw(struct adapter *adapter, u8 bw)
+{
+	adapter_to_dvobj(adapter)->oper_bwmode = bw;
+}
+
+inline u8 rtw_get_oper_choffset(struct adapter *adapter)
+{
+	return adapter_to_dvobj(adapter)->oper_ch_offset;
+}
+
+inline void rtw_set_oper_choffset(struct adapter *adapter, u8 offset)
+{
+	adapter_to_dvobj(adapter)->oper_ch_offset = 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;
+		if ((channel == 52) || (channel == 56) || (channel == 60) || (channel == 64))
+			center_ch = 58;
+		if ((channel == 100) || (channel == 104) || (channel == 108) || (channel == 112))
+			center_ch = 106;
+		if ((channel == 116) || (channel == 120) || (channel == 124) || (channel == 128))
+			center_ch = 122;
+		if ((channel == 132) || (channel == 136) || (channel == 140) || (channel == 144))
+			center_ch = 138;
+		if ((channel == 149) || (channel == 153) || (channel == 157) || (channel == 161))
+			center_ch = 155;
+		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;
+	}
+
+	return center_ch;
+}
+
+inline unsigned long rtw_get_on_cur_ch_time(struct 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 SelectChannel(struct adapter *padapter, unsigned char channel)
+{
+	if (mutex_lock_interruptible(&(adapter_to_dvobj(padapter)->setch_mutex)))
+		return;
+
+	/* saved channel info */
+	rtw_set_oper_ch(padapter, channel);
+
+	rtw_hal_set_chan(padapter, channel);
+
+	mutex_unlock(&(adapter_to_dvobj(padapter)->setch_mutex));
+}
+
+void set_channel_bwmode(struct adapter *padapter, unsigned char channel, unsigned char channel_offset, unsigned short bwmode)
+{
+	u8 center_ch, chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	if (padapter->bNotifyChannelChange)
+		DBG_871X("[%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;
+	}
+
+	/* set Channel */
+	if (mutex_lock_interruptible(&(adapter_to_dvobj(padapter)->setch_mutex)))
+		return;
+
+	/* saved channel/bw info */
+	rtw_set_oper_ch(padapter, channel);
+	rtw_set_oper_bw(padapter, bwmode);
+	rtw_set_oper_choffset(padapter, channel_offset);
+
+	rtw_hal_set_chnl_bw(padapter, center_ch, bwmode, channel_offset, chnl_offset80); /*  set center channel */
+
+	mutex_unlock(&(adapter_to_dvobj(padapter)->setch_mutex));
+}
+
+__inline u8 *get_my_bssid(struct wlan_bssid_ex *pnetwork)
+{
+	return pnetwork->MacAddress;
+}
+
+u16 get_beacon_interval(struct 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(struct 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(struct 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(struct adapter *padapter)
+{
+	unsigned int i;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
+		if (pmlmeinfo->FW_sta_info[i].status == 1)
+			return _FAIL;
+	}
+
+	return true;
+
+}
+
+unsigned int decide_wait_for_beacon_timeout(unsigned int bcn_interval)
+{
+	if ((bcn_interval << 2) < WAIT_FOR_BCN_TO_MIN)
+		return WAIT_FOR_BCN_TO_MIN;
+	else if ((bcn_interval << 2) > WAIT_FOR_BCN_TO_MAX)
+		return WAIT_FOR_BCN_TO_MAX;
+	else
+		return ((bcn_interval << 2));
+}
+
+void invalidate_cam_all(struct adapter *padapter)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_CAM_INVALID_ALL, NULL);
+
+	spin_lock_bh(&cam_ctl->lock);
+	cam_ctl->bitmap = 0;
+	memset(dvobj->cam_cache, 0, sizeof(struct cam_entry_cache)*TOTAL_CAM_ENTRY);
+	spin_unlock_bh(&cam_ctl->lock);
+}
+
+static u32 _ReadCAM(struct adapter *padapter, u32 addr)
+{
+	u32 count = 0, cmd;
+	cmd = CAM_POLLINIG | addr;
+	rtw_write32(padapter, RWCAM, cmd);
+
+	do {
+		if (0 == (rtw_read32(padapter, REG_CAMCMD) & CAM_POLLINIG))
+			break;
+	} while (count++ < 100);
+
+	return rtw_read32(padapter, REG_CAMREAD);
+}
+void read_cam(struct adapter *padapter, u8 entry, u8 *get_key)
+{
+	u32 j, addr, cmd;
+	addr = entry << 3;
+
+	/* DBG_8192C("********* DUMP CAM Entry_#%02d***************\n", entry); */
+	for (j = 0; j < 6; j++) {
+		cmd = _ReadCAM(padapter, addr+j);
+		/* DBG_8192C("offset:0x%02x => 0x%08x\n", addr+j, cmd); */
+		if (j > 1) /* get key from cam */
+			memcpy(get_key+(j-2)*4, &cmd, 4);
+	}
+	/* DBG_8192C("*********************************\n"); */
+}
+
+void _write_cam(struct adapter *padapter, u8 entry, u16 ctrl, u8 *mac, u8 *key)
+{
+	unsigned int i, val, addr;
+	int j;
+	u32 cam_val[2];
+
+	addr = entry << 3;
+
+	for (j = 5; j >= 0; j--) {
+		switch (j) {
+		case 0:
+			val = (ctrl | (mac[0] << 16) | (mac[1] << 24));
+			break;
+		case 1:
+			val = (mac[2] | (mac[3] << 8) | (mac[4] << 16) | (mac[5] << 24));
+			break;
+		default:
+			i = (j - 2) << 2;
+			val = (key[i] | (key[i+1] << 8) | (key[i+2] << 16) | (key[i+3] << 24));
+			break;
+		}
+
+		cam_val[0] = val;
+		cam_val[1] = addr + (unsigned int)j;
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_CAM_WRITE, (u8 *)cam_val);
+	}
+}
+
+void _clear_cam_entry(struct 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};
+
+	_write_cam(padapter, entry, 0, null_sta, null_key);
+}
+
+inline void write_cam(struct adapter *adapter, u8 id, u16 ctrl, u8 *mac, u8 *key)
+{
+	_write_cam(adapter, id, ctrl, mac, key);
+	write_cam_cache(adapter, id, ctrl, mac, key);
+}
+
+inline void clear_cam_entry(struct adapter *adapter, u8 id)
+{
+	_clear_cam_entry(adapter, id);
+	clear_cam_cache(adapter, id);
+}
+
+inline void write_cam_from_cache(struct adapter *adapter, u8 id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	struct cam_entry_cache cache;
+
+	spin_lock_bh(&cam_ctl->lock);
+	memcpy(&cache, &dvobj->cam_cache[id], sizeof(struct cam_entry_cache));
+	spin_unlock_bh(&cam_ctl->lock);
+
+	_write_cam(adapter, id, cache.ctrl, cache.mac, cache.key);
+}
+
+void write_cam_cache(struct 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;
+
+	spin_lock_bh(&cam_ctl->lock);
+
+	dvobj->cam_cache[id].ctrl = ctrl;
+	memcpy(dvobj->cam_cache[id].mac, mac, ETH_ALEN);
+	memcpy(dvobj->cam_cache[id].key, key, 16);
+
+	spin_unlock_bh(&cam_ctl->lock);
+}
+
+void clear_cam_cache(struct adapter *adapter, u8 id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+
+	spin_lock_bh(&cam_ctl->lock);
+
+	memset(&(dvobj->cam_cache[id]), 0, sizeof(struct cam_entry_cache));
+
+	spin_unlock_bh(&cam_ctl->lock);
+}
+
+static bool _rtw_camid_is_gk(struct 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 >= TOTAL_CAM_ENTRY)
+		goto exit;
+
+	if (!(cam_ctl->bitmap & BIT(cam_id)))
+		goto exit;
+
+	ret = (dvobj->cam_cache[cam_id].ctrl&BIT6)?true:false;
+
+exit:
+	return ret;
+}
+
+static s16 _rtw_camid_search(struct adapter *adapter, u8 *addr, s16 kid)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	int i;
+	s16 cam_id = -1;
+
+	for (i = 0; i < TOTAL_CAM_ENTRY; i++) {
+		if (addr && memcmp(dvobj->cam_cache[i].mac, addr, ETH_ALEN))
+			continue;
+		if (kid >= 0 && kid != (dvobj->cam_cache[i].ctrl&0x03))
+			continue;
+
+		cam_id = i;
+		break;
+	}
+
+	if (addr)
+		DBG_871X(FUNC_ADPT_FMT" addr:"MAC_FMT" kid:%d, return cam_id:%d\n"
+			 , FUNC_ADPT_ARG(adapter), MAC_ARG(addr), kid, cam_id);
+	else
+		DBG_871X(FUNC_ADPT_FMT" addr:%p kid:%d, return cam_id:%d\n"
+			 , FUNC_ADPT_ARG(adapter), addr, kid, cam_id);
+
+	return cam_id;
+}
+
+s16 rtw_camid_search(struct adapter *adapter, u8 *addr, s16 kid)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	s16 cam_id = -1;
+
+	spin_lock_bh(&cam_ctl->lock);
+	cam_id = _rtw_camid_search(adapter, addr, kid);
+	spin_unlock_bh(&cam_ctl->lock);
+
+	return cam_id;
+}
+
+s16 rtw_camid_alloc(struct adapter *adapter, struct sta_info *sta, u8 kid)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	s16 cam_id = -1;
+	struct mlme_ext_info *mlmeinfo;
+
+	spin_lock_bh(&cam_ctl->lock);
+
+	mlmeinfo = &adapter->mlmeextpriv.mlmext_info;
+
+	if ((((mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) || ((mlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE))
+		&& !sta) {
+		/* AP/Ad-hoc mode group key: static alloction to default key by key ID */
+		if (kid > 3) {
+			DBG_871X_LEVEL(_drv_always_, 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 {
+		int i;
+		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);
+		}
+
+		i = _rtw_camid_search(adapter, addr, kid);
+		if (i >= 0) {
+			/* Fix issue that pairwise and group key have same key id. Pairwise key first, group key can overwirte group only(ex: rekey) */
+			if (sta || _rtw_camid_is_gk(adapter, i) == true)
+				cam_id = i;
+			else
+				DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" group key id:%u the same key id as pairwise key\n"
+					, FUNC_ADPT_ARG(adapter), kid);
+			goto bitmap_handle;
+		}
+
+		for (i = 4; i < TOTAL_CAM_ENTRY; i++)
+			if (!(cam_ctl->bitmap & BIT(i)))
+				break;
+
+		if (i == TOTAL_CAM_ENTRY) {
+			if (sta)
+				DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" pairwise key with "MAC_FMT" id:%u no room\n"
+				, FUNC_ADPT_ARG(adapter), MAC_ARG(sta->hwaddr), kid);
+			else
+				DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" group key id:%u no room\n"
+				, FUNC_ADPT_ARG(adapter), kid);
+			rtw_warn_on(1);
+			goto bitmap_handle;
+		}
+
+		cam_id = i;
+	}
+
+bitmap_handle:
+	if (cam_id >= 0)
+		cam_ctl->bitmap |= BIT(cam_id);
+
+	spin_unlock_bh(&cam_ctl->lock);
+
+	return cam_id;
+}
+
+void rtw_camid_free(struct adapter *adapter, u8 cam_id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+
+	spin_lock_bh(&cam_ctl->lock);
+
+	if (cam_id < TOTAL_CAM_ENTRY)
+		cam_ctl->bitmap &= ~(BIT(cam_id));
+
+	spin_unlock_bh(&cam_ctl->lock);
+}
+
+int allocate_fw_sta_entry(struct adapter *padapter)
+{
+	unsigned int mac_id;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	for (mac_id = IBSS_START_MAC_ID; mac_id < NUM_STA; mac_id++) {
+		if (pmlmeinfo->FW_sta_info[mac_id].status == 0) {
+			pmlmeinfo->FW_sta_info[mac_id].status = 1;
+			pmlmeinfo->FW_sta_info[mac_id].retry = 0;
+			break;
+		}
+	}
+
+	return mac_id;
+}
+
+void flush_all_cam_entry(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	invalidate_cam_all(padapter);
+	/* clear default key related key search setting */
+	rtw_hal_set_hwreg(padapter, HW_VAR_SEC_DK_CFG, (u8 *)false);
+
+	memset((u8 *)(pmlmeinfo->FW_sta_info), 0, sizeof(pmlmeinfo->FW_sta_info));
+
+}
+
+int WMM_param_handler(struct adapter *padapter, struct ndis_80211_var_ie *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(struct 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 (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;
+			}
+
+			DBG_871X("WMM(%x): %x, %x\n", ACI, ACM, acParm);
+		}
+
+		if (padapter->registrypriv.acm_method == 1)
+			rtw_hal_set_hwreg(padapter, HW_VAR_ACM_CTRL, (u8 *)(&acm_mask));
+		else
+			padapter->mlmepriv.acm_mask = acm_mask;
+
+		inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
+
+		if (pregpriv->wifi_spec == 1) {
+			u32 j, tmp, change_inx = 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];
+			DBG_871X("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]);
+		}
+	}
+}
+
+static void bwmode_update_check(struct adapter *padapter, struct ndis_80211_var_ie *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 (pmlmeext->cur_channel > 14) {
+		if ((pregistrypriv->bw_mode & 0xf0) > 0)
+			cbw40_enable = 1;
+	} else
+		if ((pregistrypriv->bw_mode & 0x0f) > 0)
+			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)) {
+		pmlmeinfo->bwmode_updated = true;
+
+		pmlmeext->cur_bwmode = new_bwmode;
+		pmlmeext->cur_ch_offset = new_ch_offset;
+
+		/* update HT info also */
+		HT_info_handler(padapter, pIE);
+	} else
+		pmlmeinfo->bwmode_updated = false;
+
+
+	if (true == pmlmeinfo->bwmode_updated) {
+		struct sta_info *psta;
+		struct 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);
+		}
+	}
+}
+
+void HT_caps_handler(struct adapter *padapter, struct ndis_80211_var_ie *pIE)
+{
+	unsigned int	i;
+	u8 rf_type;
+	u8 max_AMPDU_len, min_MPDU_spacing;
+	u8 cur_ldpc_cap = 0, cur_stbc_cap = 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;
+
+	if (pIE == NULL)
+		return;
+
+	if (phtpriv->ht_option == false)
+		return;
+
+	pmlmeinfo->HT_caps_enable = 1;
+
+	for (i = 0; i < (pIE->Length); i++) {
+		if (i != 2) {
+			/* 	Commented by Albert 2010/07/12 */
+			/* 	Got the endian issue here. */
+			pmlmeinfo->HT_caps.u.HT_cap[i] &= (pIE->data[i]);
+		} else{
+			/* modify from  fw by Thomas 2010/11/17 */
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (pIE->data[i] & 0x3))
+				max_AMPDU_len = (pIE->data[i] & 0x3);
+			else
+				max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3);
+
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (pIE->data[i] & 0x1c))
+				min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c);
+			else
+				min_MPDU_spacing = (pIE->data[i] & 0x1c);
+
+			pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para = max_AMPDU_len | min_MPDU_spacing;
+		}
+	}
+	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+	/* 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 (rf_type) {
+	case RF_1T1R:
+	case RF_1T2R:
+		set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R);
+		break;
+	case RF_2T2R:
+	default:
+#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
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R);
+#else /* CONFIG_DISABLE_MCS13TO15 */
+		set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R);
+#endif /* CONFIG_DISABLE_MCS13TO15 */
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		/*  Config STBC setting */
+		if (TEST_FLAG(phtpriv->stbc_cap, STBC_HT_ENABLE_TX) && GET_HT_CAPABILITY_ELE_TX_STBC(pIE->data)) {
+			SET_FLAG(cur_stbc_cap, STBC_HT_ENABLE_TX);
+			DBG_871X("Enable HT Tx STBC !\n");
+		}
+		phtpriv->stbc_cap = cur_stbc_cap;
+	} else {
+		/*  Config LDPC Coding Capability */
+		if (TEST_FLAG(phtpriv->ldpc_cap, LDPC_HT_ENABLE_TX) && GET_HT_CAPABILITY_ELE_LDPC_CAP(pIE->data)) {
+			SET_FLAG(cur_ldpc_cap, (LDPC_HT_ENABLE_TX | LDPC_HT_CAP_TX));
+			DBG_871X("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_CAPABILITY_ELE_RX_STBC(pIE->data)) {
+			SET_FLAG(cur_stbc_cap, (STBC_HT_ENABLE_TX | STBC_HT_CAP_TX));
+			DBG_871X("Enable HT Tx STBC!\n");
+		}
+		phtpriv->stbc_cap = cur_stbc_cap;
+	}
+}
+
+void HT_info_handler(struct adapter *padapter, struct ndis_80211_var_ie *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(struct adapter *padapter)
+{
+	unsigned char 	max_AMPDU_len;
+	unsigned char 	min_MPDU_spacing;
+	/* struct registry_priv  *pregpriv = &padapter->registrypriv; */
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	DBG_871X("%s\n", __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(struct adapter *padapter, struct ndis_80211_var_ie *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(struct adapter *padapter, struct sta_info *psta)
+{
+	struct registry_priv  *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	switch (pregpriv->vrtl_carrier_sense) {/* 0:off 1:on 2:auto */
+	case 0: /* off */
+		psta->rtsen = 0;
+		psta->cts2self = 0;
+		break;
+
+	case 1: /* on */
+		if (pregpriv->vcs_type == 1) { /* 1:RTS/CTS 2:CTS to self */
+			psta->rtsen = 1;
+			psta->cts2self = 0;
+		} else{
+			psta->rtsen = 0;
+			psta->cts2self = 1;
+		}
+		break;
+
+	case 2: /* auto */
+	default:
+		if ((pmlmeinfo->ERP_enable) && (pmlmeinfo->ERP_IE & BIT(1))) {
+			if (pregpriv->vcs_type == 1) {
+				psta->rtsen = 1;
+				psta->cts2self = 0;
+			} else{
+				psta->rtsen = 0;
+				psta->cts2self = 1;
+			}
+		} else{
+			psta->rtsen = 0;
+			psta->cts2self = 0;
+		}
+		break;
+	}
+}
+
+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;
+	}
+}
+
+int rtw_check_bcn_info(struct adapter *Adapter, u8 *pframe, u32 packet_len)
+{
+	unsigned int		len;
+	unsigned char 	*p;
+	unsigned short	val16, subtype;
+	struct wlan_network *cur_network = &(Adapter->mlmepriv.cur_network);
+	/* u8 wpa_ie[255], rsn_ie[255]; */
+	u16 wpa_len = 0, rsn_len = 0;
+	u8 encryp_protocol = 0;
+	struct wlan_bssid_ex *bssid;
+	int group_cipher = 0, pairwise_cipher = 0, is_8021x = 0;
+	unsigned char *pbuf;
+	u32 wpa_ielen = 0;
+	u8 *pbssid = GetAddr3Ptr(pframe);
+	u32 hidden_ssid = 0;
+	struct HT_info_element *pht_info = NULL;
+	struct rtw_ieee80211_ht_cap *pht_cap = NULL;
+	u32 bcn_channel;
+	unsigned short	ht_cap_info;
+	unsigned char ht_info_infos_0;
+	struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
+
+	if (is_client_associated_to_ap(Adapter) == false)
+		return true;
+
+	len = packet_len - sizeof(struct ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ) {
+		DBG_871X("%s IE too long for survey event\n", __func__);
+		return _FAIL;
+	}
+
+	if (memcmp(cur_network->network.MacAddress, pbssid, 6)) {
+		DBG_871X("Oops: rtw_check_network_encrypt linked but recv other bssid bcn\n" MAC_FMT MAC_FMT,
+				MAC_ARG(pbssid), MAC_ARG(cur_network->network.MacAddress));
+		return true;
+	}
+
+	bssid = (struct wlan_bssid_ex *)rtw_zmalloc(sizeof(struct wlan_bssid_ex));
+	if (bssid == NULL) {
+		DBG_871X("%s rtw_zmalloc fail !!!\n", __func__);
+		return true;
+	}
+
+	if ((pmlmepriv->timeBcnInfoChkStart != 0) && (jiffies_to_msecs(jiffies - pmlmepriv->timeBcnInfoChkStart) > DISCONNECT_BY_CHK_BCN_FAIL_OBSERV_PERIOD_IN_MS)) {
+		pmlmepriv->timeBcnInfoChkStart = 0;
+		pmlmepriv->NumOfBcnInfoChkFail = 0;
+	}
+
+	subtype = GetFrameSubType(pframe) >> 4;
+
+	if (subtype == WIFI_BEACON)
+		bssid->Reserved[0] = 1;
+
+	bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+
+	/* below is to copy the information element */
+	bssid->IELength = len;
+	memcpy(bssid->IEs, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->IELength);
+
+	/* check bw and channel offset */
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+			pht_cap = (struct rtw_ieee80211_ht_cap *)(p + 2);
+			ht_cap_info = le16_to_cpu(pht_cap->cap_info);
+	} else {
+			ht_cap_info = 0;
+	}
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+	if (p && len > 0) {
+			pht_info = (struct HT_info_element *)(p + 2);
+			ht_info_infos_0 = pht_info->infos[0];
+	} else {
+			ht_info_infos_0 = 0;
+	}
+	if (ht_cap_info != cur_network->BcnInfo.ht_cap_info ||
+		((ht_info_infos_0&0x03) != (cur_network->BcnInfo.ht_info_infos_0&0x03))) {
+			DBG_871X("%s bcn now: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
+							ht_cap_info, ht_info_infos_0);
+			DBG_871X("%s bcn link: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
+							cur_network->BcnInfo.ht_cap_info, cur_network->BcnInfo.ht_info_infos_0);
+			DBG_871X("%s bw mode change\n", __func__);
+			{
+				/* bcn_info_update */
+				cur_network->BcnInfo.ht_cap_info = ht_cap_info;
+				cur_network->BcnInfo.ht_info_infos_0 = ht_info_infos_0;
+				/* to do : need to check that whether modify related register of BB or not */
+			}
+			/* goto _mismatch; */
+	}
+
+	/* Checking for channel */
+	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _DSSET_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+	if (p) {
+			bcn_channel = *(p + 2);
+	} else {/* In 5G, some ap do not have DSSET IE checking HT info for channel */
+			rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+			if (pht_info) {
+					bcn_channel = pht_info->primary_channel;
+			} else { /* we don't find channel IE, so don't check it */
+					/* DBG_871X("Oops: %s we don't find channel IE, so don't check it\n", __func__); */
+					bcn_channel = Adapter->mlmeextpriv.cur_channel;
+			}
+	}
+	if (bcn_channel != Adapter->mlmeextpriv.cur_channel) {
+			DBG_871X("%s beacon channel:%d cur channel:%d disconnect\n", __func__,
+						   bcn_channel, Adapter->mlmeextpriv.cur_channel);
+			goto _mismatch;
+	}
+
+	/* checking SSID */
+	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _SSID_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
+	if (p == NULL) {
+		DBG_871X("%s marc: cannot find SSID for survey event\n", __func__);
+		hidden_ssid = true;
+	} else {
+		hidden_ssid = false;
+	}
+
+	if ((NULL != p) && (false == hidden_ssid && (*(p + 1)))) {
+		memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
+		bssid->Ssid.SsidLength = *(p + 1);
+	} else {
+		bssid->Ssid.SsidLength = 0;
+		bssid->Ssid.Ssid[0] = '\0';
+	}
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s bssid.Ssid.Ssid:%s bssid.Ssid.SsidLength:%d "
+				"cur_network->network.Ssid.Ssid:%s len:%d\n", __func__, bssid->Ssid.Ssid,
+				bssid->Ssid.SsidLength, cur_network->network.Ssid.Ssid,
+				cur_network->network.Ssid.SsidLength));
+
+	if (memcmp(bssid->Ssid.Ssid, cur_network->network.Ssid.Ssid, 32) ||
+			bssid->Ssid.SsidLength != cur_network->network.Ssid.SsidLength) {
+		if (bssid->Ssid.Ssid[0] != '\0' && bssid->Ssid.SsidLength != 0) { /* not hidden ssid */
+			DBG_871X("%s(), SSID is not match\n", __func__);
+			goto _mismatch;
+		}
+	}
+
+	/* check encryption info */
+	val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
+
+	if (val16 & BIT(4))
+		bssid->Privacy = 1;
+	else
+		bssid->Privacy = 0;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+			("%s(): cur_network->network.Privacy is %d, bssid.Privacy is %d\n",
+			 __func__, cur_network->network.Privacy, bssid->Privacy));
+	if (cur_network->network.Privacy != bssid->Privacy) {
+		DBG_871X("%s(), privacy is not match\n", __func__);
+		goto _mismatch;
+	}
+
+	rtw_get_sec_ie(bssid->IEs, bssid->IELength, NULL, &rsn_len, NULL, &wpa_len);
+
+	if (rsn_len > 0) {
+		encryp_protocol = ENCRYP_PROTOCOL_WPA2;
+	} else if (wpa_len > 0) {
+		encryp_protocol = ENCRYP_PROTOCOL_WPA;
+	} else {
+		if (bssid->Privacy)
+			encryp_protocol = ENCRYP_PROTOCOL_WEP;
+	}
+
+	if (cur_network->BcnInfo.encryp_protocol != encryp_protocol) {
+		DBG_871X("%s(): enctyp is not match\n", __func__);
+		goto _mismatch;
+	}
+
+	if (encryp_protocol == ENCRYP_PROTOCOL_WPA || encryp_protocol == ENCRYP_PROTOCOL_WPA2) {
+		pbuf = rtw_get_wpa_ie(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12);
+		if (pbuf && (wpa_ielen > 0)) {
+			if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+						("%s pnetwork->pairwise_cipher: %d, group_cipher is %d, is_8021x is %d\n", __func__,
+						 pairwise_cipher, group_cipher, is_8021x));
+			}
+		} else {
+			pbuf = rtw_get_wpa2_ie(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12);
+
+			if (pbuf && (wpa_ielen > 0)) {
+				if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
+					RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+							("%s pnetwork->pairwise_cipher: %d, pnetwork->group_cipher is %d, is_802x is %d\n",
+							 __func__, pairwise_cipher, group_cipher, is_8021x));
+				}
+			}
+		}
+
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+				("%s cur_network->group_cipher is %d: %d\n", __func__, cur_network->BcnInfo.group_cipher, group_cipher));
+		if (pairwise_cipher != cur_network->BcnInfo.pairwise_cipher || group_cipher != cur_network->BcnInfo.group_cipher) {
+			DBG_871X("%s pairwise_cipher(%x:%x) or group_cipher(%x:%x) is not match\n", __func__,
+					pairwise_cipher, cur_network->BcnInfo.pairwise_cipher,
+					group_cipher, cur_network->BcnInfo.group_cipher);
+			goto _mismatch;
+		}
+
+		if (is_8021x != cur_network->BcnInfo.is_8021x) {
+			DBG_871X("%s authentication is not match\n", __func__);
+			goto _mismatch;
+		}
+	}
+
+	kfree((u8 *)bssid);
+	return _SUCCESS;
+
+_mismatch:
+	kfree((u8 *)bssid);
+
+	if (pmlmepriv->NumOfBcnInfoChkFail == 0)
+		pmlmepriv->timeBcnInfoChkStart = jiffies;
+
+	pmlmepriv->NumOfBcnInfoChkFail++;
+	DBG_871X("%s by "ADPT_FMT" - NumOfChkFail = %d (SeqNum of this Beacon frame = %d).\n", __func__, ADPT_ARG(Adapter), pmlmepriv->NumOfBcnInfoChkFail, GetSequence(pframe));
+
+	if ((pmlmepriv->timeBcnInfoChkStart != 0) && (jiffies_to_msecs(jiffies - pmlmepriv->timeBcnInfoChkStart) <= DISCONNECT_BY_CHK_BCN_FAIL_OBSERV_PERIOD_IN_MS)
+		&& (pmlmepriv->NumOfBcnInfoChkFail >= DISCONNECT_BY_CHK_BCN_FAIL_THRESHOLD)) {
+		DBG_871X("%s by "ADPT_FMT" - NumOfChkFail = %d >= threshold : %d (in %d ms), return FAIL.\n", __func__, ADPT_ARG(Adapter), pmlmepriv->NumOfBcnInfoChkFail,
+			DISCONNECT_BY_CHK_BCN_FAIL_THRESHOLD, jiffies_to_msecs(jiffies - pmlmepriv->timeBcnInfoChkStart));
+		pmlmepriv->timeBcnInfoChkStart = 0;
+		pmlmepriv->NumOfBcnInfoChkFail = 0;
+		return _FAIL;
+	}
+
+	return _SUCCESS;
+}
+
+void update_beacon_info(struct adapter *padapter, u8 *pframe, uint pkt_len, struct sta_info *psta)
+{
+	unsigned int i;
+	unsigned int len;
+	struct ndis_80211_var_ie *pIE;
+
+	len = pkt_len - (_BEACON_IE_OFFSET_ + WLAN_HDR_A3_LEN);
+
+	for (i = 0; i < len;) {
+		pIE = (struct ndis_80211_var_ie *)(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;
+
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+}
+
+unsigned int is_ap_in_tkip(struct adapter *padapter)
+{
+	u32 i;
+	struct ndis_80211_var_ie *pIE;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex		*cur_network = &(pmlmeinfo->network);
+
+	if (rtw_get_capability((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = sizeof(struct ndis_802_11_fix_ie); i < pmlmeinfo->network.IELength;) {
+			pIE = (struct ndis_80211_var_ie *)(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;
+
+}
+
+int support_short_GI(struct 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(struct adapter *padapter, struct sta_info *psta)
+{
+	rtw_hal_update_ra_mask(psta, 0);
+}
+
+void enable_rate_adaptive(struct adapter *padapter, struct sta_info *psta);
+void enable_rate_adaptive(struct adapter *padapter, struct sta_info *psta)
+{
+	Update_RA_Entry(padapter, psta);
+}
+
+void set_sta_rate(struct adapter *padapter, struct sta_info *psta)
+{
+	/* rate adaptive */
+	enable_rate_adaptive(padapter, psta);
+}
+
+unsigned char check_assoc_AP(u8 *pframe, uint len)
+{
+	unsigned int	i;
+	struct ndis_80211_var_ie *pIE;
+
+	for (i = sizeof(struct ndis_802_11_fix_ie); i < len;) {
+		pIE = (struct ndis_80211_var_ie *)(pframe + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			if ((!memcmp(pIE->data, ARTHEROS_OUI1, 3)) || (!memcmp(pIE->data, ARTHEROS_OUI2, 3))) {
+				DBG_871X("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))) {
+				DBG_871X("link to Broadcom AP\n");
+				return HT_IOT_PEER_BROADCOM;
+			} else if (!memcmp(pIE->data, MARVELL_OUI, 3)) {
+				DBG_871X("link to Marvell AP\n");
+				return HT_IOT_PEER_MARVELL;
+			} else if (!memcmp(pIE->data, RALINK_OUI, 3)) {
+				DBG_871X("link to Ralink AP\n");
+				return HT_IOT_PEER_RALINK;
+			} else if (!memcmp(pIE->data, CISCO_OUI, 3)) {
+				DBG_871X("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;
+							DBG_871X("link to Realtek JAGUAR_BCUTAP\n");
+						}
+						if (pIE->data[6] & RT_HT_CAP_USE_JAGUAR_CCUT) {
+							Vender = HT_IOT_PEER_REALTEK_JAGUAR_CCUTAP;
+							DBG_871X("link to Realtek JAGUAR_CCUTAP\n");
+						}
+					}
+				}
+
+				DBG_871X("link to Realtek AP\n");
+				return Vender;
+			} else if (!memcmp(pIE->data, AIRGOCAP_OUI, 3)) {
+				DBG_871X("link to Airgo Cap\n");
+				return HT_IOT_PEER_AIRGO;
+			} else
+				break;
+
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+	DBG_871X("link to new AP\n");
+	return HT_IOT_PEER_UNKNOWN;
+}
+
+void update_IOT_info(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	switch (pmlmeinfo->assoc_AP_vendor) {
+	case HT_IOT_PEER_MARVELL:
+		pmlmeinfo->turboMode_cts2self = 1;
+		pmlmeinfo->turboMode_rtsen = 0;
+		break;
+
+	case HT_IOT_PEER_RALINK:
+		pmlmeinfo->turboMode_cts2self = 0;
+		pmlmeinfo->turboMode_rtsen = 1;
+		/* disable high power */
+		Switch_DM_Func(padapter, (~DYNAMIC_BB_DYNAMIC_TXPWR), false);
+		break;
+	case HT_IOT_PEER_REALTEK:
+		/* rtw_write16(padapter, 0x4cc, 0xffff); */
+		/* rtw_write16(padapter, 0x546, 0x01c0); */
+		/* disable high power */
+		Switch_DM_Func(padapter, (~DYNAMIC_BB_DYNAMIC_TXPWR), false);
+		break;
+	default:
+		pmlmeinfo->turboMode_cts2self = 0;
+		pmlmeinfo->turboMode_rtsen = 1;
+		break;
+	}
+
+}
+
+void update_capinfo(struct adapter *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);
+
+}
+
+void update_wireless_mode(struct 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);
+	struct wlan_bssid_ex		*cur_network = &(pmlmeinfo->network);
+	unsigned char 		*rate = cur_network->SupportedRates;
+
+	ratelen = rtw_get_rateset_len(cur_network->SupportedRates);
+
+	if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable))
+		pmlmeinfo->HT_enable = 1;
+
+	if (pmlmeext->cur_channel > 14) {
+		if (pmlmeinfo->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)) == true)
+			network_type |= WIRELESS_11B;
+		else if ((cckrates_included(rate, ratelen)) == true)
+			network_type |= WIRELESS_11BG;
+		else
+			network_type |= WIRELESS_11G;
+	}
+
+	pmlmeext->cur_wireless_mode = network_type & padapter->registrypriv.wireless_mode;
+
+	SIFS_Timer = 0x0a0a0808; /* 0x0808 -> for CCK, 0x0a0a -> for OFDM */
+													/* change this value if having IOT issues. */
+
+	padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_RESP_SIFS,  (u8 *)&SIFS_Timer);
+
+	padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_WIRELESS_MODE,  (u8 *)&(pmlmeext->cur_wireless_mode));
+
+	if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
+		update_mgnt_tx_rate(padapter, IEEE80211_CCK_RATE_1MB);
+	 else
+		update_mgnt_tx_rate(padapter, IEEE80211_OFDM_RATE_6MB);
+}
+
+void 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 update_sta_support_rate(struct adapter *padapter, u8 *pvar_ie, uint var_ie_len, int cam_idx)
+{
+	unsigned int	ie_len;
+	struct ndis_80211_var_ie *pIE;
+	int	supportRateNum = 0;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pIE = (struct ndis_80211_var_ie *)rtw_get_ie(pvar_ie, _SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
+	if (pIE == NULL)
+		return _FAIL;
+
+	memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, pIE->data, ie_len);
+	supportRateNum = ie_len;
+
+	pIE = (struct ndis_80211_var_ie *)rtw_get_ie(pvar_ie, _EXT_SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
+	if (pIE)
+		memcpy((pmlmeinfo->FW_sta_info[cam_idx].SupportedRates + supportRateNum), pIE->data, ie_len);
+
+	return _SUCCESS;
+
+}
+
+void process_addba_req(struct adapter *padapter, u8 *paddba_req, u8 *addr)
+{
+	struct sta_info *psta;
+	u16 tid, start_seq, param;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct ADDBA_request *preq = (struct ADDBA_request *)paddba_req;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	psta = rtw_get_stainfo(pstapriv, addr);
+
+	if (psta) {
+		start_seq = le16_to_cpu(preq->BA_starting_seqctrl) >> 4;
+
+		param = le16_to_cpu(preq->BA_para_set);
+		tid = (param>>2)&0x0f;
+
+		preorder_ctrl = &psta->recvreorder_ctrl[tid];
+
+		#ifdef CONFIG_UPDATE_INDICATE_SEQ_WHILE_PROCESS_ADDBA_REQ
+		preorder_ctrl->indicate_seq = start_seq;
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, start_seq: %d\n", __func__, __LINE__,
+			preorder_ctrl->indicate_seq, start_seq);
+		#endif
+		#else
+		preorder_ctrl->indicate_seq = 0xffff;
+		#endif
+
+		preorder_ctrl->enable = (pmlmeinfo->bAcceptAddbaReq == true) ? true : false;
+	}
+
+}
+
+void update_TSF(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len)
+{
+	u8 *pIE;
+	__le32 *pbuf;
+
+	pIE = pframe + sizeof(struct 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(struct 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 ieee80211_hdr_3addr);
+	pbuf = (__le32 *)pIE;
+
+	tsf = le32_to_cpu(*(pbuf+1));
+	tsf = tsf << 32;
+	tsf |= le32_to_cpu(*pbuf);
+
+	/* DBG_871X("%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; */
+
+/*
+	DBG_871X("%s(): (a)bcn_cnt = %d\n", __func__, pmlmeext->bcn_cnt);
+
+
+	for (i = 0; i<9; i++)
+	{
+		DBG_871X("%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 == true)) {
+		u8 ratio_20_delay, ratio_80_delay;
+		u8 DrvBcnEarly, DrvBcnTimeOut;
+
+		ratio_20_delay = 0;
+		ratio_80_delay = 0;
+		DrvBcnEarly = 0xff;
+		DrvBcnTimeOut = 0xff;
+
+		DBG_871X("%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;
+
+
+			DBG_871X("%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;
+				DBG_871X("%s(): DrvBcnEarly = %d\n", __func__, DrvBcnEarly);
+			}
+
+			if (ratio_80_delay > 80 && DrvBcnTimeOut == 0xff) {
+				DrvBcnTimeOut = i;
+				DBG_871X("%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(struct adapter *padapter)
+{
+	rtw_hal_bcn_related_reg_setting(padapter);
+}
+
+void rtw_alloc_macid(struct adapter *padapter, struct sta_info *psta)
+{
+	int i;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter);
+
+
+	if (!memcmp(psta->hwaddr, bc_addr, ETH_ALEN))
+		return;
+
+	if (!memcmp(psta->hwaddr, myid(&padapter->eeprompriv), ETH_ALEN)) {
+		psta->mac_id = NUM_STA;
+		return;
+	}
+
+	spin_lock_bh(&pdvobj->lock);
+	for (i = 0; i < NUM_STA; i++) {
+		if (pdvobj->macid[i] == false) {
+			pdvobj->macid[i]  = true;
+			break;
+		}
+	}
+	spin_unlock_bh(&pdvobj->lock);
+
+	if (i > (NUM_STA-1)) {
+		psta->mac_id = NUM_STA;
+		DBG_871X("  no room for more MACIDs\n");
+	} else{
+		psta->mac_id = i;
+		DBG_871X("%s = %d\n", __func__, psta->mac_id);
+	}
+
+}
+
+void rtw_release_macid(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter);
+
+
+	if (!memcmp(psta->hwaddr, bc_addr, ETH_ALEN))
+		return;
+
+	if (!memcmp(psta->hwaddr, myid(&padapter->eeprompriv), ETH_ALEN))
+		return;
+
+	spin_lock_bh(&pdvobj->lock);
+	if (psta->mac_id < NUM_STA && psta->mac_id != 1) {
+		if (pdvobj->macid[psta->mac_id] == true) {
+			DBG_871X("%s = %d\n", __func__, psta->mac_id);
+			pdvobj->macid[psta->mac_id] = false;
+			psta->mac_id = NUM_STA;
+		}
+
+	}
+	spin_unlock_bh(&pdvobj->lock);
+
+}
+/* For 8188E RA */
+u8 rtw_search_max_mac_id(struct adapter *padapter)
+{
+	u8 max_mac_id = 0;
+	struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter);
+	int i;
+	spin_lock_bh(&pdvobj->lock);
+	for (i = (NUM_STA-1); i >= 0 ; i--) {
+		if (pdvobj->macid[i] == true)
+			break;
+	}
+	max_mac_id = i;
+	spin_unlock_bh(&pdvobj->lock);
+
+	return max_mac_id;
+
+}
+
+struct adapter *dvobj_get_port0_adapter(struct dvobj_priv *dvobj)
+{
+	if (get_iface_type(dvobj->padapters[i]) != IFACE_PORT0)
+		return NULL;
+
+	return dvobj->padapters;
+}
+
+#ifdef CONFIG_GPIO_API
+int rtw_get_gpio(struct net_device *netdev, int gpio_num)
+{
+	u8 value;
+	u8 direction;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(netdev);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter);
+
+	rtw_ps_deny(adapter, PS_DENY_IOCTL);
+
+	DBG_871X("rf_pwrstate = 0x%02x\n", pwrpriv->rf_pwrstate);
+	LeaveAllPowerSaveModeDirect(adapter);
+
+	/* Read GPIO Direction */
+	direction = (rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) & BIT(gpio_num)) >> gpio_num;
+
+	/* According the direction to read register value */
+	if (direction)
+		value = (rtw_read8(adapter, REG_GPIO_PIN_CTRL + 1) & BIT(gpio_num)) >> gpio_num;
+	else
+		value = (rtw_read8(adapter, REG_GPIO_PIN_CTRL) & BIT(gpio_num)) >> gpio_num;
+
+	rtw_ps_deny_cancel(adapter, PS_DENY_IOCTL);
+	DBG_871X("%s direction =%d value =%d\n", __func__, direction, value);
+
+	return value;
+}
+EXPORT_SYMBOL(rtw_get_gpio);
+
+int  rtw_set_gpio_output_value(struct net_device *netdev, int gpio_num, bool isHigh)
+{
+	u8 direction = 0;
+	u8 res = -1;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(netdev);
+
+	/* Check GPIO is 4~7 */
+	if (gpio_num > 7 || gpio_num < 4) {
+		DBG_871X("%s The gpio number does not included 4~7.\n", __func__);
+		return -1;
+	}
+
+	rtw_ps_deny(adapter, PS_DENY_IOCTL);
+
+	LeaveAllPowerSaveModeDirect(adapter);
+
+	/* Read GPIO direction */
+	direction = (rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) & BIT(gpio_num)) >> gpio_num;
+
+	/* If GPIO is output direction, setting value. */
+	if (direction) {
+		if (isHigh)
+			rtw_write8(adapter, REG_GPIO_PIN_CTRL + 1, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 1) | BIT(gpio_num));
+		else
+			rtw_write8(adapter, REG_GPIO_PIN_CTRL + 1, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 1) & ~BIT(gpio_num));
+
+		DBG_871X("%s Set gpio %x[%d]=%d\n", __func__, REG_GPIO_PIN_CTRL+1, gpio_num, isHigh);
+		res = 0;
+	} else{
+		DBG_871X("%s The gpio is input, not be set!\n", __func__);
+		res = -1;
+	}
+
+	rtw_ps_deny_cancel(adapter, PS_DENY_IOCTL);
+	return res;
+}
+EXPORT_SYMBOL(rtw_set_gpio_output_value);
+
+int rtw_config_gpio(struct net_device *netdev, int gpio_num, bool isOutput)
+{
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(netdev);
+
+	if (gpio_num > 7 || gpio_num < 4) {
+		DBG_871X("%s The gpio number does not included 4~7.\n", __func__);
+		return -1;
+	}
+
+	DBG_871X("%s gpio_num =%d direction =%d\n", __func__, gpio_num, isOutput);
+
+	rtw_ps_deny(adapter, PS_DENY_IOCTL);
+
+	LeaveAllPowerSaveModeDirect(adapter);
+
+	if (isOutput)
+		rtw_write8(adapter, REG_GPIO_PIN_CTRL + 2, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) | BIT(gpio_num));
+	else
+		rtw_write8(adapter, REG_GPIO_PIN_CTRL + 2, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) & ~BIT(gpio_num));
+
+	rtw_ps_deny_cancel(adapter, PS_DENY_IOCTL);
+
+	return 0;
+}
+EXPORT_SYMBOL(rtw_config_gpio);
+#endif
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+void rtw_get_current_ip_address(struct adapter *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;
+				DBG_871X("%s: %d.%d.%d.%d ==========\n", __func__,
+						ipaddress[0], ipaddress[1], ipaddress[2], ipaddress[3]);
+				memcpy(pcurrentip, ipaddress, 4);
+			}
+		}
+	}
+}
+#endif
+#ifdef CONFIG_WOWLAN
+void rtw_get_sec_iv(struct adapter *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);
+	DBG_871X("%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 != _NO_PRIVACY_ && psta->dot11txpn.val > 0)
+			psta->dot11txpn.val--;
+		AES_IV(pcur_dot11txpn, psta->dot11txpn, 0);
+
+		DBG_871X("%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]);
+	}
+}
+void rtw_set_sec_pn(struct adapter *padapter)
+{
+		struct sta_info         *psta;
+		struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
+		struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
+		struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+		struct security_priv *psecpriv = &padapter->securitypriv;
+
+		psta = rtw_get_stainfo(&padapter->stapriv,
+		get_my_bssid(&pmlmeinfo->network));
+
+		if (psta) {
+			if (pwrpriv->wowlan_fw_iv > psta->dot11txpn.val) {
+				if (psecpriv->dot11PrivacyAlgrthm != _NO_PRIVACY_)
+					psta->dot11txpn.val = pwrpriv->wowlan_fw_iv + 2;
+			} else {
+				DBG_871X("%s(): FW IV is smaller than driver\n", __func__);
+				psta->dot11txpn.val += 2;
+			}
+			DBG_871X("%s: dot11txpn: 0x%016llx\n", __func__, psta->dot11txpn.val);
+		}
+}
+#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"
+
+#endif /* CONFIG_PNO_SUPPORT */
diff -Nurp linux.old/3rdparty/rtl8723bs/core/rtw_xmit.c linux/3rdparty/rtl8723bs/core/rtw_xmit.c
--- linux.old/3rdparty/rtl8723bs/core/rtw_xmit.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/core/rtw_xmit.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,3100 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#define _RTW_XMIT_C_
+
+#include <drv_types.h>
+#include <rtw_debug.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, struct 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 vzalloc(). */
+	/* 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 = vzalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
+
+	if (pxmitpriv->pallocated_frame_buf  == NULL) {
+		pxmitpriv->pxmit_frame_buf = NULL;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_frame fail!\n"));
+		res = _FAIL;
+		goto exit;
+	}
+	pxmitpriv->pxmit_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_frame_buf), 4);
+	/* pxmitpriv->pxmit_frame_buf = pxmitpriv->pallocated_frame_buf + 4 - */
+	/* 						((SIZE_PTR) (pxmitpriv->pallocated_frame_buf) &3); */
+
+	pxframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf;
+
+	for (i = 0; i < NR_XMITFRAME; i++) {
+		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 = vzalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
+
+	if (pxmitpriv->pallocated_xmitbuf  == NULL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_buf fail!\n"));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pxmitpriv->pxmitbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmitbuf), 4);
+	/* pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 - */
+	/* 						((SIZE_PTR) (pxmitpriv->pallocated_xmitbuf) &3); */
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
+
+	for (i = 0; i < NR_XMITBUFF; i++) {
+		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) {
+			msleep(10);
+			res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true);
+			if (res == _FAIL)
+				goto exit;
+		}
+
+		pxmitbuf->phead = pxmitbuf->pbuf;
+		pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMITBUF_SZ;
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+
+		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 = vzalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_frame) + 4);
+
+	if (pxmitpriv->xframe_ext_alloc_addr  == NULL) {
+		pxmitpriv->xframe_ext = NULL;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xframe_ext fail!\n"));
+		res = _FAIL;
+		goto exit;
+	}
+	pxmitpriv->xframe_ext = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->xframe_ext_alloc_addr), 4);
+	pxframe = (struct xmit_frame *)pxmitpriv->xframe_ext;
+
+	for (i = 0; i < 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 = vzalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_buf) + 4);
+
+	if (pxmitpriv->pallocated_xmit_extbuf  == NULL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_extbuf fail!\n"));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmit_extbuf), 4);
+
+	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
+
+	for (i = 0; i < 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;
+		}
+
+		pxmitbuf->phead = pxmitbuf->pbuf;
+		pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMIT_EXTBUF_SZ;
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+
+		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->phead = pxmitbuf->pbuf;
+			pxmitbuf->pend = pxmitbuf->pbuf + MAX_CMDBUF_SZ;
+			pxmitbuf->len = 0;
+			pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+			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->ack_tx = false;
+	mutex_init(&pxmitpriv->ack_tx_mutex);
+	rtw_sctx_init(&pxmitpriv->ack_tx_ops, 0);
+
+	rtw_hal_init_xmit_priv(padapter);
+
+exit:
+	return res;
+}
+
+void _rtw_free_xmit_priv(struct xmit_priv *pxmitpriv)
+{
+       int i;
+      struct 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)
+		return;
+
+	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)
+		vfree(pxmitpriv->pallocated_frame_buf);
+
+
+	if (pxmitpriv->pallocated_xmitbuf)
+		vfree(pxmitpriv->pallocated_xmitbuf);
+
+	/* free xframe_ext queue,  the same count as extbuf  */
+	pxmitframe = (struct xmit_frame *)pxmitpriv->xframe_ext;
+	if (pxmitframe) {
+		for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
+			rtw_os_xmit_complete(padapter, pxmitframe);
+			pxmitframe++;
+		}
+	}
+	if (pxmitpriv->xframe_ext_alloc_addr)
+		vfree(pxmitpriv->xframe_ext_alloc_addr);
+
+	/*  free xmit extension buff */
+	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) {
+		vfree(pxmitpriv->pallocated_xmit_extbuf);
+	}
+
+	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);
+
+	mutex_destroy(&pxmitpriv->ack_tx_mutex);
+}
+
+u8 query_ra_short_GI(struct sta_info *psta)
+{
+	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 (psta->bw_mode) {
+	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(struct 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->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 == true) &&
+				(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 == true) {
+				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)
+		pattrib->vcs_mode = padapter->driver_vcs_type;
+}
+
+static void update_attrib_phy_info(struct adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)
+{
+	struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
+
+	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;
+
+	if (mlmeext->cur_bwmode < psta->bw_mode)
+		pattrib->bwmode = mlmeext->cur_bwmode;
+	else
+		pattrib->bwmode = psta->bw_mode;
+
+	pattrib->sgi = query_ra_short_GI(psta);
+
+	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;
+
+	/* if (pattrib->ht_en && psta->htpriv.ampdu_enable) */
+	/*  */
+	/* 	if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority)) */
+	/* 		pattrib->ampdu_en = true; */
+	/*  */
+
+
+	pattrib->retry_ctrl = false;
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (psta->isrc && psta->pid > 0)
+		pattrib->pctrl = true;
+#endif
+
+}
+
+static s32 update_attrib_sec_info(struct 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 == true) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("\n psta->ieee8021x_blocked == true\n"));
+
+		pattrib->encrypt = 0;
+
+		if ((pattrib->ether_type != 0x888e) && (check_fwstate(pmlmepriv, WIFI_MP_STATE) == false)) {
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("\npsta->ieee8021x_blocked == true,  pattrib->ether_type(%.4x) != 0x888e\n", pattrib->ether_type));
+			#ifdef DBG_TX_DROP_FRAME
+			DBG_871X("DBG_TX_DROP_FRAME %s psta->ieee8021x_blocked == true,  pattrib->ether_type(%04x) != 0x888e\n", __func__, pattrib->ether_type);
+			#endif
+			res = _FAIL;
+			goto exit;
+		}
+	} else{
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast);
+
+		switch (psecuritypriv->dot11AuthAlgrthm) {
+		case dot11AuthAlgrthm_Open:
+		case dot11AuthAlgrthm_Shared:
+		case dot11AuthAlgrthm_Auto:
+			pattrib->key_idx = (u8)psecuritypriv->dot11PrivacyKeyIndex;
+			break;
+		case dot11AuthAlgrthm_8021X:
+			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_;
+
+	}
+
+	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
+			DBG_871X("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;
+
+	default:
+		pattrib->iv_len = 0;
+		pattrib->icv_len = 0;
+		break;
+	}
+
+	if (pattrib->encrypt > 0)
+		memcpy(pattrib->dot118021x_UncstKey.skey, psta->dot118021x_UncstKey.skey, 16);
+
+	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+		("update_attrib: encrypt =%d  securitypriv.sw_encrypt =%d\n",
+		pattrib->encrypt, padapter->securitypriv.sw_encrypt));
+
+	if (pattrib->encrypt &&
+		((padapter->securitypriv.sw_encrypt == true) || (psecuritypriv->hw_decrypted == false))) {
+		pattrib->bswenc = true;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+			("update_attrib: encrypt =%d securitypriv.hw_decrypted =%d bswenc =true\n",
+			pattrib->encrypt, padapter->securitypriv.sw_encrypt));
+	} else {
+		pattrib->bswenc = false;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("update_attrib: bswenc =false\n"));
+	}
+
+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:
+		DBG_871X("qos_acm(): invalid pattrib->priority: %d!!!\n", priority);
+		break;
+	}
+
+	return change_priority;
+}
+
+static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
+{
+	struct ethhdr etherhdr;
+	struct iphdr ip_hdr;
+	s32 UserPriority = 0;
+
+
+	_rtw_open_pktfile(ppktfile->pkt, ppktfile);
+	_rtw_pktfile_read(ppktfile, (unsigned char *)&etherhdr, ETH_HLEN);
+
+	/*  get UserPriority from IP hdr */
+	if (pattrib->ether_type == 0x0800) {
+		_rtw_pktfile_read(ppktfile, (u8 *)&ip_hdr, sizeof(ip_hdr));
+/* 		UserPriority = (ntohs(ip_hdr.tos) >> 5) & 0x3; */
+		UserPriority = ip_hdr.tos >> 5;
+	}
+	pattrib->priority = UserPriority;
+	pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN;
+	pattrib->subtype = WIFI_QOS_DATA_TYPE;
+}
+
+static s32 update_attrib(struct 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 mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv 	*pqospriv = &pmlmepriv->qospriv;
+	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) == true) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_adhoc);
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, 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);
+
+	pattrib->pktlen = pktfile.pkt_len;
+
+	if (ETH_P_IP == pattrib->ether_type) {
+		/*  The following is for DHCP and ARP packet, we use cck1M to tx these packets and let LPS awake some time */
+		/*  to prevent DHCP protocol fail */
+
+		u8 tmp[24];
+
+		_rtw_pktfile_read(&pktfile, &tmp[0], 24);
+
+		pattrib->dhcp_pkt = 0;
+		if (pktfile.pkt_len > 282) {/* MINIMUM_DHCP_PACKET_SIZE) { */
+			if (ETH_P_IP == pattrib->ether_type) {/*  IP header */
+				if (((tmp[21] == 68) && (tmp[23] == 67)) ||
+					((tmp[21] == 67) && (tmp[23] == 68))) {
+					/*  68 : UDP BOOTP client */
+					/*  67 : UDP BOOTP server */
+					RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("======================update_attrib: get DHCP Packet\n"));
+					pattrib->dhcp_pkt = 1;
+					DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_dhcp);
+				}
+			}
+		}
+
+		/* for parsing ICMP pakcets */
+		{
+			struct iphdr *piphdr = (struct iphdr *)tmp;
+
+			pattrib->icmp_pkt = 0;
+			if (piphdr->protocol == 0x1) { /*  protocol type in ip header 0x1 is ICMP */
+				pattrib->icmp_pkt = 1;
+				DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_icmp);
+			}
+		}
+
+
+	} else if (0x888e == pattrib->ether_type) {
+		DBG_871X_LEVEL(_drv_always_, "send eapol packet\n");
+	}
+
+	if ((pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1))
+		rtw_set_scan_deny(padapter, 3000);
+
+	/*  If EAPOL , ARP , OR DHCP packet, driver must be in active mode. */
+	if (pattrib->icmp_pkt == 1)
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1);
+	else if (pattrib->dhcp_pkt == 1) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_active);
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SPECIAL_PACKET, 1);
+	}
+
+	bmcast = IS_MCAST(pattrib->ra);
+
+	/*  get sta_info */
+	if (bmcast) {
+		psta = rtw_get_bcmc_stainfo(padapter);
+	} else {
+		psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+		if (psta == NULL)	{ /*  if we cannot get psta => drop the pkt */
+			DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_sta);
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT"\n", MAC_ARG(pattrib->ra)));
+			#ifdef DBG_TX_DROP_FRAME
+			DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra));
+			#endif
+			res = _FAIL;
+			goto exit;
+		} else if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) && (!(psta->state & _FW_LINKED))) {
+			DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_ap_link);
+			res = _FAIL;
+			goto exit;
+		}
+	}
+
+	if (psta == NULL) {
+		/*  if we cannot get psta => drop the pkt */
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sta);
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT "\n", MAC_ARG(pattrib->ra)));
+		#ifdef DBG_TX_DROP_FRAME
+		DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra));
+		#endif
+		res = _FAIL;
+		goto exit;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_link);
+		DBG_871X("%s, psta("MAC_FMT")->state(0x%x) != _FW_LINKED\n", __func__, MAC_ARG(psta->hwaddr), psta->state);
+		return _FAIL;
+	}
+
+
+
+	/* 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);
+
+	/* DBG_8192C("%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{
+		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 */
+
+	rtw_set_tx_chksum_offload(pkt, pattrib);
+
+exit:
+	return res;
+}
+
+static s32 xmitframe_addmic(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	sint			curfragnum, length;
+	u8 *pframe, *payload, mic[8];
+	struct	mic_data		micdata;
+	/* struct	sta_info 	*stainfo; */
+	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
+	struct	security_priv *psecuritypriv = &padapter->securitypriv;
+	struct	xmit_priv 	*pxmitpriv = &padapter->xmitpriv;
+	u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
+	u8 hw_hdr_offset = 0;
+	sint bmcst = IS_MCAST(pattrib->ra);
+
+/*
+	if (pattrib->psta)
+	{
+		stainfo = pattrib->psta;
+	}
+	else
+	{
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		stainfo =rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]);
+	}
+
+	if (stainfo == NULL)
+	{
+		DBG_871X("%s, psta ==NUL\n", __func__);
+		return _FAIL;
+	}
+
+	if (!(stainfo->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
+		return _FAIL;
+	}
+*/
+
+	hw_hdr_offset = TXDESC_OFFSET;
+
+	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"); */
+					/* msleep(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"); */
+					/* msleep(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));
+				RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("===curfragnum =%d, pframe = 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x,!!!\n",
+					curfragnum, *payload, *(payload+1), *(payload+2), *(payload+3), *(payload+4), *(payload+5), *(payload+6), *(payload+7)));
+
+				payload = payload+pattrib->hdrlen+pattrib->iv_len;
+				RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("curfragnum =%d pattrib->hdrlen =%d pattrib->iv_len =%d", curfragnum, pattrib->hdrlen, pattrib->iv_len));
+				if ((curfragnum+1) == pattrib->nr_frags) {
+					length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-((pattrib->bswenc) ? pattrib->icv_len : 0);
+					rtw_secmicappend(&micdata, payload, length);
+					payload = payload+length;
+				} else{
+					length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-((pattrib->bswenc) ? pattrib->icv_len : 0);
+					rtw_secmicappend(&micdata, payload, length);
+					payload = payload+length+pattrib->icv_len;
+					RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("curfragnum =%d length =%d pattrib->icv_len =%d", curfragnum, length, pattrib->icv_len));
+				}
+			}
+			rtw_secgetmic(&micdata, &(mic[0]));
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: before add mic code!!!\n"));
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: pattrib->last_txcmdsz =%d!!!\n", pattrib->last_txcmdsz));
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: mic[0]= 0x%.2x , mic[1]= 0x%.2x , mic[2]= 0x%.2x , mic[3]= 0x%.2x\n\
+  mic[4]= 0x%.2x , mic[5]= 0x%.2x , mic[6]= 0x%.2x , mic[7]= 0x%.2x !!!!\n",
+				mic[0], mic[1], mic[2], mic[3], mic[4], mic[5], mic[6], mic[7]));
+			/* add mic code  and add the mic code length in last_txcmdsz */
+
+			memcpy(payload, &(mic[0]), 8);
+			pattrib->last_txcmdsz += 8;
+
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("\n ========last pkt ========\n"));
+			payload = payload-pattrib->last_txcmdsz+8;
+			for (curfragnum = 0; curfragnum < pattrib->last_txcmdsz; curfragnum = curfragnum+8)
+					RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, (" %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x ",
+					*(payload+curfragnum), *(payload+curfragnum+1), *(payload+curfragnum+2), *(payload+curfragnum+3),
+					*(payload+curfragnum+4), *(payload+curfragnum+5), *(payload+curfragnum+6), *(payload+curfragnum+7)));
+			}
+/*
+			else {
+				RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: rtw_get_stainfo == NULL!!!\n"));
+			}
+*/
+	}
+	return _SUCCESS;
+}
+
+static s32 xmitframe_swencrypt(struct 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) {
+		/* DBG_871X("start xmitframe_swencrypt\n"); */
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("### xmitframe_swencrypt\n"));
+		switch (pattrib->encrypt) {
+		case _WEP40_:
+		case _WEP104_:
+			rtw_wep_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		case _TKIP_:
+			rtw_tkip_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		case _AES_:
+			rtw_aes_encrypt(padapter, (u8 *)pxmitframe);
+			break;
+		default:
+				break;
+		}
+
+	} else
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_, ("### xmitframe_hwencrypt\n"));
+
+	return _SUCCESS;
+}
+
+s32 rtw_make_wlanhdr(struct adapter *padapter, u8 *hdr, struct pkt_attrib *pattrib)
+{
+	u16 *qc;
+
+	struct ieee80211_hdr *pwlanhdr = (struct 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_control;
+
+	memset(hdr, 0, WLANHDR_OFFSET);
+
+	SetFrameSubType(fctrl, pattrib->subtype);
+
+	if (pattrib->subtype & WIFI_DATA_TYPE) {
+		if ((check_fwstate(pmlmepriv,  WIFI_STATION_STATE) == true)) {
+			/* to_ds = 1, fr_ds = 0; */
+
+			{
+				/*  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->src, ETH_ALEN);
+				memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+			}
+
+			if (pqospriv->qos_option)
+				qos_option = true;
+
+		} else if ((check_fwstate(pmlmepriv,  WIFI_AP_STATE) == true)) {
+			/* to_ds = 0, fr_ds = 1; */
+			SetFrDs(fctrl);
+			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) == true) ||
+		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
+			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 {
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("fw_state:%x is not allowed to xmit frame\n", get_fwstate(pmlmepriv)));
+			res = _FAIL;
+			goto exit;
+		}
+
+		if (pattrib->mdata)
+			SetMData(fctrl);
+
+		if (pattrib->encrypt)
+			SetPrivacy(fctrl);
+
+		if (qos_option) {
+			qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
+
+			if (pattrib->priority)
+				SetPriority(qc, pattrib->priority);
+
+			SetEOSP(qc, pattrib->eosp);
+
+			SetAckpolicy(qc, pattrib->ack_policy);
+		}
+
+		/* TODO: fill HT Control Field */
+
+		/* Update Seq Num will be handled by f/w */
+		{
+			struct sta_info *psta;
+			psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+			if (pattrib->psta != psta) {
+				DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+				return _FAIL;
+			}
+
+			if (psta == NULL) {
+				DBG_871X("%s, psta ==NUL\n", __func__);
+				return _FAIL;
+			}
+
+			if (!(psta->state & _FW_LINKED)) {
+				DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+				return _FAIL;
+			}
+
+
+			if (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);
+
+				/* check if enable ampdu */
+				if (pattrib->ht_en && psta->htpriv.ampdu_enable)
+					if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority))
+						pattrib->ampdu_en = true;
+
+
+				/* re-check if enable ampdu by BA_starting_seqctrl */
+				if (pattrib->ampdu_en == true) {
+					u16 tx_seq;
+
+					tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f];
+
+					/* check BA_starting_seqctrl */
+					if (SN_LESS(pattrib->seqnum, tx_seq)) {
+						/* DBG_871X("tx ampdu seqnum(%d) < tx_seq(%d)\n", pattrib->seqnum, tx_seq); */
+						pattrib->ampdu_en = false;/* AGG BK */
+					} else if (SN_EQUAL(pattrib->seqnum, tx_seq)) {
+						psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (tx_seq+1)&0xfff;
+
+						pattrib->ampdu_en = true;/* AGG EN */
+					} else{
+						/* DBG_871X("tx ampdu over run\n"); */
+						psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum+1)&0xfff;
+						pattrib->ampdu_en = true;/* AGG EN */
+					}
+
+				}
+			}
+		}
+
+	} else{
+
+	}
+
+exit:
+	return res;
+}
+
+s32 rtw_txframes_pending(struct adapter *padapter)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	return ((!list_empty(&pxmitpriv->be_pending.queue)) ||
+			 (!list_empty(&pxmitpriv->bk_pending.queue)) ||
+			 (!list_empty(&pxmitpriv->vi_pending.queue)) ||
+			 (!list_empty(&pxmitpriv->vo_pending.queue)));
+}
+
+/*
+ * Calculate wlan 802.11 packet MAX size from pkt_attrib
+ * This function doesn't consider fragment case
+ */
+u32 rtw_calculate_wlan_pkt_size_by_attribue(struct pkt_attrib *pattrib)
+{
+	u32 len = 0;
+
+	len = pattrib->hdrlen + pattrib->iv_len; /*  WLAN Header and IV */
+	len += SNAP_SIZE + sizeof(u16); /*  LLC */
+	len += pattrib->pktlen;
+	if (pattrib->encrypt == _TKIP_)
+		len += 8; /*  MIC */
+	len += ((pattrib->bswenc) ? pattrib->icv_len : 0); /*  ICV */
+
+	return len;
+}
+
+/*
+
+This sub-routine will perform all the following:
+
+1. remove 802.3 header.
+2. create wlan_header, based on the info in pxmitframe
+3. append sta's iv/ext-iv
+4. append LLC
+5. move frag chunk from pframe to pxmitframe->mem
+6. apply sw-encrypt, if necessary.
+
+*/
+s32 rtw_xmitframe_coalesce(struct 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
+	{
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	}
+
+	if (psta == NULL)
+  {
+
+		DBG_871X("%s, psta ==NUL\n", __func__);
+		return _FAIL;
+	}
+
+
+	if (!(psta->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return _FAIL;
+	}
+*/
+	if (pxmitframe->buf_addr == NULL) {
+		DBG_8192C("==> %s buf_addr == NULL\n", __func__);
+		return _FAIL;
+	}
+
+	pbuf_start = pxmitframe->buf_addr;
+
+	hw_hdr_offset = TXDESC_OFFSET;
+	mem_start = pbuf_start +	hw_hdr_offset;
+
+	if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n"));
+		DBG_8192C("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n");
+		res = _FAIL;
+		goto exit;
+	}
+
+	_rtw_open_pktfile(pkt, &pktfile);
+	_rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen);
+
+	frg_inx = 0;
+	frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */
+
+	while (1) {
+		llc_sz = 0;
+
+		mpdu_len = frg_len;
+
+		pframe = mem_start;
+
+		SetMFrag(mem_start);
+
+		pframe += pattrib->hdrlen;
+		mpdu_len -= pattrib->hdrlen;
+
+		/* adding icv, if necessary... */
+		if (pattrib->iv_len) {
+			memcpy(pframe, pattrib->iv, pattrib->iv_len);
+
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
+				 ("rtw_xmitframe_coalesce: keyid =%d pattrib->iv[3]=%.2x pframe =%.2x %.2x %.2x %.2x\n",
+				  padapter->securitypriv.dot11PrivacyKeyIndex, pattrib->iv[3], *pframe, *(pframe+1), *(pframe+2), *(pframe+3)));
+
+			pframe += pattrib->iv_len;
+
+			mpdu_len -= pattrib->iv_len;
+		}
+
+		if (frg_inx == 0) {
+			llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
+			pframe += llc_sz;
+			mpdu_len -= llc_sz;
+		}
+
+		if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+			mpdu_len -= pattrib->icv_len;
+		}
+
+
+		if (bmcst) {
+			/*  don't do fragment to broadcat/multicast packets */
+			mem_sz = _rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen);
+		} else {
+			mem_sz = _rtw_pktfile_read(&pktfile, pframe, mpdu_len);
+		}
+
+		pframe += mem_sz;
+
+		if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
+			memcpy(pframe, pattrib->icv, pattrib->icv_len);
+			pframe += pattrib->icv_len;
+		}
+
+		frg_inx++;
+
+		if (bmcst || (rtw_endofpktfile(&pktfile) == true)) {
+			pattrib->nr_frags = frg_inx;
+
+			pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + ((pattrib->nr_frags == 1) ? llc_sz:0) +
+					((pattrib->bswenc) ? pattrib->icv_len : 0) + mem_sz;
+
+			ClearMFrag(mem_start);
+
+			break;
+		} else
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("%s: There're still something in packet!\n", __func__));
+
+		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) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n"));
+		DBG_8192C("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n");
+		res = _FAIL;
+		goto exit;
+	}
+
+	xmitframe_swencrypt(padapter, pxmitframe);
+
+	if (bmcst == false)
+		update_attrib_vcs_info(padapter, pxmitframe);
+	else
+		pattrib->vcs_mode = NONE_VCS;
+
+exit:
+	return res;
+}
+
+/* broadcast or multicast management pkt use BIP, unicast management pkt use CCMP encryption */
+s32 rtw_mgmt_xmitframe_coalesce(struct adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe)
+{
+	u8 *pframe, *mem_start = NULL, *tmp_buf = NULL;
+	u8 subtype;
+	struct sta_info 	*psta = NULL;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	s32 bmcst = IS_MCAST(pattrib->ra);
+	u8 *BIP_AAD = NULL;
+	u8 *MGMT_body = NULL;
+
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ieee80211_hdr	*pwlanhdr;
+	u8 MME[_MME_IE_LENGTH_];
+	u32 ori_len;
+	mem_start = pframe = (u8 *)(pxmitframe->buf_addr) + TXDESC_OFFSET;
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	ori_len = BIP_AAD_SIZE+pattrib->pktlen;
+	tmp_buf = BIP_AAD = rtw_zmalloc(ori_len);
+	subtype = GetFrameSubType(pframe); /* bit(7)~bit(2) */
+
+	if (BIP_AAD == NULL)
+		return _FAIL;
+
+	spin_lock_bh(&padapter->security_key_mutex);
+
+	/* only support station mode */
+	if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE) || !check_fwstate(pmlmepriv, _FW_LINKED))
+		goto xmitframe_coalesce_success;
+
+	/* IGTK key is not install, it may not support 802.11w */
+	if (padapter->securitypriv.binstallBIPkey != true) {
+		DBG_871X("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, 18);
+
+		/* other types doesn't need the BIP */
+		if (GetFrameSubType(pframe) != WIFI_DEAUTH && GetFrameSubType(pframe) != WIFI_DISASSOC)
+			goto xmitframe_coalesce_fail;
+
+		MGMT_body = pframe + sizeof(struct 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 ieee80211_hdr_3addr);
+
+		/* conscruct AAD, copy frame control field */
+		memcpy(BIP_AAD, &pwlanhdr->frame_control, 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) {
+
+				DBG_871X("%s, psta ==NUL\n", __func__);
+				goto xmitframe_coalesce_fail;
+			}
+
+			if (!(psta->state & _FW_LINKED) || pxmitframe->buf_addr == NULL) {
+				DBG_871X("%s, not _FW_LINKED or addr null\n", __func__);
+				goto xmitframe_coalesce_fail;
+			}
+
+			/* DBG_871X("%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 */
+			if (pattrib->encrypt > 0)
+				memcpy(pattrib->dot118021x_UncstKey.skey, psta->dot118021x_UncstKey.skey, 16);
+			/* 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:
+	spin_unlock_bh(&padapter->security_key_mutex);
+	kfree(BIP_AAD);
+	return _SUCCESS;
+
+xmitframe_coalesce_fail:
+	spin_unlock_bh(&padapter->security_key_mutex);
+	kfree(BIP_AAD);
+	return _FAIL;
+}
+
+/* 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(struct 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(struct adapter *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) {
+		pkt_num = pxmitframe->agg_num;
+
+		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;
+		}
+	}
+}
+
+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;
+
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+		pxmitbuf->agg_num = 0;
+		pxmitbuf->pg_num = 0;
+
+		if (pxmitbuf->sctx) {
+			DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+		}
+	} else
+		DBG_871X("%s fail, no xmitbuf available !!!\n", __func__);
+
+	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) {
+		DBG_871X("%s, alloc xmitframe fail\n", __func__);
+		return NULL;
+	}
+
+	pxmitbuf = __rtw_alloc_cmd_xmitbuf(pxmitpriv, buf_type);
+	if (pxmitbuf == NULL) {
+		DBG_871X("%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;
+
+	pxmitbuf->priv_data = pcmdframe;
+
+	return pcmdframe;
+
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv)
+{
+	_irqL irqL;
+	struct xmit_buf *pxmitbuf =  NULL;
+	struct list_head *plist, *phead;
+	struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+
+	spin_lock_irqsave(&pfree_queue->lock, irqL);
+
+	if (list_empty(&pfree_queue->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
+		DBG_871X("DBG_XMIT_BUF_EXT ALLOC no =%d,  free_xmit_extbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt);
+		#endif
+
+
+		pxmitbuf->priv_data = NULL;
+
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+		pxmitbuf->agg_num = 1;
+
+		if (pxmitbuf->sctx) {
+			DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+		}
+
+	}
+
+	spin_unlock_irqrestore(&pfree_queue->lock, irqL);
+
+	return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	_irqL irqL;
+	struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+
+	if (pxmitbuf == NULL)
+		return _FAIL;
+
+	spin_lock_irqsave(&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
+	DBG_871X("DBG_XMIT_BUF_EXT FREE no =%d, free_xmit_extbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt);
+	#endif
+
+	spin_unlock_irqrestore(&pfree_queue->lock, irqL);
+
+	return _SUCCESS;
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv)
+{
+	_irqL irqL;
+	struct xmit_buf *pxmitbuf =  NULL;
+	struct list_head *plist, *phead;
+	struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+	/* DBG_871X("+rtw_alloc_xmitbuf\n"); */
+
+	spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL);
+
+	if (list_empty(&pfree_xmitbuf_queue->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
+		DBG_871X("DBG_XMIT_BUF ALLOC no =%d,  free_xmitbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt);
+		#endif
+		/* DBG_871X("alloc, free_xmitbuf_cnt =%d\n", pxmitpriv->free_xmitbuf_cnt); */
+
+		pxmitbuf->priv_data = NULL;
+
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+		pxmitbuf->agg_num = 0;
+		pxmitbuf->pg_num = 0;
+
+		if (pxmitbuf->sctx) {
+			DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+		}
+	}
+	#ifdef DBG_XMIT_BUF
+	else
+		DBG_871X("DBG_XMIT_BUF rtw_alloc_xmitbuf return NULL\n");
+	#endif
+
+	spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
+
+	return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	_irqL irqL;
+	struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+	/* DBG_871X("+rtw_free_xmitbuf\n"); */
+
+	if (pxmitbuf == NULL)
+		return _FAIL;
+
+	if (pxmitbuf->sctx) {
+		DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
+		rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE);
+	}
+
+	if (pxmitbuf->buf_tag == XMITBUF_CMD) {
+	} else if (pxmitbuf->buf_tag == XMITBUF_MGNT) {
+		rtw_free_xmitbuf_ext(pxmitpriv, pxmitbuf);
+	} else{
+		spin_lock_irqsave(&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++;
+		/* DBG_871X("FREE, free_xmitbuf_cnt =%d\n", pxmitpriv->free_xmitbuf_cnt); */
+		#ifdef DBG_XMIT_BUF
+		DBG_871X("DBG_XMIT_BUF FREE no =%d, free_xmitbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt);
+		#endif
+		spin_unlock_irqrestore(&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->pg_num = 1;
+		pxframe->agg_num = 1;
+		pxframe->ack_report = 0;
+	}
+}
+
+/*
+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
+	*/
+
+	struct xmit_frame *pxframe = NULL;
+	struct list_head *plist, *phead;
+	struct __queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
+
+	spin_lock_bh(&pfree_xmit_queue->lock);
+
+	if (list_empty(&pfree_xmit_queue->queue)) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe:%d\n", pxmitpriv->free_xmitframe_cnt));
+		pxframe =  NULL;
+	} else {
+		phead = get_list_head(pfree_xmit_queue);
+
+		plist = get_next(phead);
+
+		pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		list_del_init(&(pxframe->list));
+		pxmitpriv->free_xmitframe_cnt--;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe():free_xmitframe_cnt =%d\n", pxmitpriv->free_xmitframe_cnt));
+	}
+
+	spin_unlock_bh(&pfree_xmit_queue->lock);
+
+	rtw_init_xmitframe(pxframe);
+	return pxframe;
+}
+
+struct xmit_frame *rtw_alloc_xmitframe_ext(struct xmit_priv *pxmitpriv)
+{
+	struct xmit_frame *pxframe = NULL;
+	struct list_head *plist, *phead;
+	struct __queue *queue = &pxmitpriv->free_xframe_ext_queue;
+
+	spin_lock_bh(&queue->lock);
+
+	if (list_empty(&queue->queue)) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe_ext:%d\n", pxmitpriv->free_xframe_ext_cnt));
+		pxframe =  NULL;
+	} else {
+		phead = get_list_head(queue);
+		plist = get_next(phead);
+		pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		list_del_init(&(pxframe->list));
+		pxmitpriv->free_xframe_ext_cnt--;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe_ext():free_xmitframe_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt));
+	}
+
+	spin_unlock_bh(&queue->lock);
+
+	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);
+
+	DBG_871X("################## %s ##################\n", __func__);
+
+exit:
+	return pxframe;
+}
+
+s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe)
+{
+	struct __queue *queue = NULL;
+	struct adapter *padapter = pxmitpriv->adapter;
+	_pkt *pndis_pkt = NULL;
+
+	if (pxmitframe == NULL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("======rtw_free_xmitframe():pxmitframe == NULL!!!!!!!!!!\n"));
+		goto exit;
+	}
+
+	if (pxmitframe->pkt) {
+		pndis_pkt = pxmitframe->pkt;
+		pxmitframe->pkt = NULL;
+	}
+
+	if (pxmitframe->alloc_addr) {
+		DBG_871X("################## %s with alloc_addr ##################\n", __func__);
+		kfree(pxmitframe->alloc_addr);
+		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 {
+
+	}
+
+	spin_lock_bh(&queue->lock);
+
+	list_del_init(&pxmitframe->list);
+	list_add_tail(&pxmitframe->list, get_list_head(queue));
+	if (pxmitframe->ext_tag == 0) {
+		pxmitpriv->free_xmitframe_cnt++;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe():free_xmitframe_cnt =%d\n", pxmitpriv->free_xmitframe_cnt));
+	} else if (pxmitframe->ext_tag == 1) {
+		pxmitpriv->free_xframe_ext_cnt++;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe():free_xframe_ext_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt));
+	} else {
+	}
+
+	spin_unlock_bh(&queue->lock);
+
+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, struct __queue *pframequeue)
+{
+	struct list_head	*plist, *phead;
+	struct	xmit_frame	*pxmitframe;
+
+	spin_lock_bh(&(pframequeue->lock));
+
+	phead = get_list_head(pframequeue);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+
+		pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		plist = get_next(plist);
+
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+
+	}
+	spin_unlock_bh(&(pframequeue->lock));
+}
+
+s32 rtw_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	DBG_COUNTER(padapter->tx_logs.core_tx_enqueue);
+	if (rtw_xmit_classifier(padapter, pxmitframe) == _FAIL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+			 ("rtw_xmitframe_enqueue: drop xmit pkt for classifier fail\n"));
+/* 		pxmitframe->pkt = NULL; */
+		return _FAIL;
+	}
+
+	return _SUCCESS;
+}
+
+struct tx_servq *rtw_get_sta_pending(struct 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;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : BK\n"));
+		break;
+
+	case 4:
+	case 5:
+		ptxservq = &(psta->sta_xmitpriv.vi_q);
+		*(ac) = 1;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : VI\n"));
+		break;
+
+	case 6:
+	case 7:
+		ptxservq = &(psta->sta_xmitpriv.vo_q);
+		*(ac) = 0;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : VO\n"));
+		break;
+
+	case 0:
+	case 3:
+	default:
+		ptxservq = &(psta->sta_xmitpriv.be_q);
+		*(ac) = 2;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : BE\n"));
+	break;
+
+	}
+
+	return ptxservq;
+}
+
+/*
+ * Will enqueue pxmitframe to the proper queue,
+ * and indicate it to xx_pending list.....
+ */
+s32 rtw_xmit_classifier(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	/* _irqL irqL0; */
+	u8 ac_index;
+	struct sta_info *psta;
+	struct tx_servq	*ptxservq;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct 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 {
+		DBG_871X("%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);
+		DBG_871X("%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;
+		DBG_8192C("rtw_xmit_classifier: psta == NULL\n");
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmit_classifier: psta == NULL\n"));
+		goto exit;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_fwlink);
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return _FAIL;
+	}
+
+	ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+	/* spin_lock_irqsave(&pstapending->lock, irqL0); */
+
+	if (list_empty(&ptxservq->tx_pending)) {
+		list_add_tail(&ptxservq->tx_pending, get_list_head(phwxmits[ac_index].sta_queue));
+	}
+
+	/* spin_lock_irqsave(&ptxservq->sta_pending.lock, irqL1); */
+
+	list_add_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending));
+	ptxservq->qcnt++;
+	phwxmits[ac_index].accnt++;
+
+	/* spin_unlock_irqrestore(&ptxservq->sta_pending.lock, irqL1); */
+
+	/* spin_unlock_irqrestore(&pstapending->lock, irqL0); */
+
+exit:
+
+	return res;
+}
+
+void rtw_alloc_hwxmits(struct 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) {
+		DBG_871X("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(struct adapter *padapter)
+{
+	struct hw_xmit *hwxmits;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	hwxmits = pxmitpriv->hwxmits;
+	if (hwxmits)
+		kfree((u8 *)hwxmits);
+}
+
+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;
+	}
+}
+
+u32 rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe)
+{
+	u32 addr;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+
+	switch (pattrib->qsel) {
+	case 0:
+	case 3:
+		addr = BE_QUEUE_INX;
+		break;
+	case 1:
+	case 2:
+		addr = BK_QUEUE_INX;
+		break;
+	case 4:
+	case 5:
+		addr = VI_QUEUE_INX;
+		break;
+	case 6:
+	case 7:
+		addr = VO_QUEUE_INX;
+		break;
+	case 0x10:
+		addr = BCN_QUEUE_INX;
+		break;
+	case 0x11:/* BC/MC in PS (HIQ) */
+		addr = HIGH_QUEUE_INX;
+		break;
+	case 0x12:
+	default:
+		addr = MGT_QUEUE_INX;
+		break;
+
+	}
+
+	return addr;
+
+}
+
+static void do_queue_select(struct adapter	*padapter, struct pkt_attrib *pattrib)
+{
+	u8 qsel;
+
+	qsel = pattrib->priority;
+	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("### do_queue_select priority =%d , qsel = %d\n", pattrib->priority, qsel));
+
+	pattrib->qsel = qsel;
+}
+
+/*
+ * The main transmit(tx) entry
+ *
+ * Return
+ *1	enqueue
+ *0	success, hardware will handle this xmit frame(packet)
+ *<0	fail
+ */
+s32 rtw_xmit(struct adapter *padapter, _pkt **ppkt)
+{
+	static unsigned long start = 0;
+	static u32 drop_cnt = 0;
+
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_frame *pxmitframe = NULL;
+
+	s32 res;
+
+	DBG_COUNTER(padapter->tx_logs.core_tx);
+
+	if (start == 0)
+		start = jiffies;
+
+	pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
+
+	if (jiffies_to_msecs(jiffies - start) > 2000) {
+		if (drop_cnt)
+			DBG_871X("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++;
+		RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: no more pxmitframe\n"));
+		DBG_COUNTER(padapter->tx_logs.core_tx_err_pxmitframe);
+		return -1;
+	}
+
+	res = update_attrib(padapter, *ppkt, &pxmitframe->attrib);
+
+	if (res == _FAIL) {
+		RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: update attrib fail\n"));
+		#ifdef DBG_TX_DROP_FRAME
+		DBG_871X("DBG_TX_DROP_FRAME %s update attrib fail\n", __func__);
+		#endif
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+		return -1;
+	}
+	pxmitframe->pkt = *ppkt;
+
+	do_queue_select(padapter, &pxmitframe->attrib);
+
+	spin_lock_bh(&pxmitpriv->lock);
+	if (xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe) == true) {
+		spin_unlock_bh(&pxmitpriv->lock);
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue);
+		return 1;
+	}
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	/* pre_xmitframe */
+	if (rtw_hal_xmit(padapter, pxmitframe) == false)
+		return 1;
+
+	return 0;
+}
+
+#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;
+	struct adapter *adapter = xmitframe->padapter;
+	struct registry_priv *registry = &adapter->registrypriv;
+
+	if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_SPECIAL) {
+
+		struct pkt_attrib *attrib = &xmitframe->attrib;
+
+		if (attrib->ether_type == 0x0806
+			|| attrib->ether_type == 0x888e
+			|| attrib->dhcp_pkt
+		) {
+			DBG_871X(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;
+}
+
+sint xmitframe_enqueue_for_sleeping_sta(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	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;
+
+	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
+	{
+		DBG_871X("%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);
+		DBG_871X("%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);
+		DBG_871X("%s, psta ==NUL\n", __func__);
+		return false;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_link);
+		DBG_871X("%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);
+		/* DBG_871X("directly xmit pspoll_triggered packet\n"); */
+
+		/* pattrib->triggered = 0; */
+		if (bmcst && xmitframe_hiq_filter(pxmitframe) == true)
+			pattrib->qsel = 0x11;/* HIQ */
+
+		return ret;
+	}
+
+
+	if (bmcst) {
+		spin_lock_bh(&psta->sleep_q.lock);
+
+		if (pstapriv->sta_dz_bitmap) { /* if anyone sta is in ps mode */
+			/* pattrib->qsel = 0x11;HIQ */
+
+			list_del_init(&pxmitframe->list);
+
+			/* spin_lock_bh(&psta->sleep_q.lock); */
+
+			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);
+
+			/* DBG_871X("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
+
+			if (update_tim == true) {
+				update_beacon(padapter, _TIM_IE_, NULL, true);
+			} else {
+				chk_bmc_sleepq_cmd(padapter);
+			}
+
+			/* spin_unlock_bh(&psta->sleep_q.lock); */
+
+			ret = true;
+
+			DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_mcast);
+
+		}
+
+		spin_unlock_bh(&psta->sleep_q.lock);
+
+		return ret;
+
+	}
+
+
+	spin_lock_bh(&psta->sleep_q.lock);
+
+	if (psta->state&WIFI_SLEEP_STATE) {
+		u8 wmmps_ac = 0;
+
+		if (pstapriv->sta_dz_bitmap & BIT(psta->aid)) {
+			list_del_init(&pxmitframe->list);
+
+			/* spin_lock_bh(&psta->sleep_q.lock); */
+
+			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);
+
+				/* DBG_871X("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
+
+				if (update_tim == true)
+					/* DBG_871X("sleepq_len == 1, update BCNTIM\n"); */
+					/* upate BCN for TIM IE */
+					update_beacon(padapter, _TIM_IE_, NULL, true);
+			}
+
+			/* spin_unlock_bh(&psta->sleep_q.lock); */
+
+			/* 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);
+		}
+
+	}
+
+	spin_unlock_bh(&psta->sleep_q.lock);
+
+	return ret;
+
+}
+
+static void dequeue_xmitframes_to_sleeping_queue(struct adapter *padapter, struct sta_info *psta, struct __queue *pframequeue)
+{
+	sint ret;
+	struct list_head	*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 (phead != plist) {
+		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 (true == ret) {
+			ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+			ptxservq->qcnt--;
+			phwxmits[ac_index].accnt--;
+		} else {
+			/* DBG_871X("xmitframe_enqueue_for_sleeping_sta return false\n"); */
+		}
+
+	}
+
+}
+
+void stop_sta_xmit(struct adapter *padapter, struct sta_info *psta)
+{
+	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);
+
+
+	spin_lock_bh(&pxmitpriv->lock);
+
+	psta->state |= WIFI_SLEEP_STATE;
+
+	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));
+
+	/* 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));
+
+	spin_unlock_bh(&pxmitpriv->lock);
+}
+
+void wakeup_sta_to_xmit(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 update_mask = 0, wmmps_ac = 0;
+	struct sta_info *psta_bmc;
+	struct list_head	*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);
+
+
+	/* spin_lock_bh(&psta->sleep_q.lock); */
+	spin_lock_bh(&pxmitpriv->lock);
+
+	xmitframe_phead = get_list_head(&psta->sleep_q);
+	xmitframe_plist = get_next(xmitframe_phead);
+
+	while (xmitframe_phead != xmitframe_plist) {
+		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;
+
+/*
+		spin_unlock_bh(&psta->sleep_q.lock);
+		if (rtw_hal_xmit(padapter, pxmitframe) == true)
+		{
+			rtw_os_xmit_complete(padapter, pxmitframe);
+		}
+		spin_lock_bh(&psta->sleep_q.lock);
+*/
+		rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+
+	}
+
+	if (psta->sleepq_len == 0) {
+		if (pstapriv->tim_bitmap & BIT(psta->aid)) {
+			/* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */
+			/* upate BCN for TIM IE */
+			/* update_BCNTIM(padapter); */
+			update_mask = BIT(0);
+		}
+
+		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) {
+			DBG_871X("%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 (xmitframe_phead != xmitframe_plist) {
+			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;
+/*
+			spin_unlock_bh(&psta_bmc->sleep_q.lock);
+			if (rtw_hal_xmit(padapter, pxmitframe) == true)
+			{
+				rtw_os_xmit_complete(padapter, pxmitframe);
+			}
+			spin_lock_bh(&psta_bmc->sleep_q.lock);
+
+*/
+			rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+		}
+
+		if (psta_bmc->sleepq_len == 0) {
+			if (pstapriv->tim_bitmap & BIT(0)) {
+				/* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */
+				/* upate BCN for TIM IE */
+				/* update_BCNTIM(padapter); */
+				update_mask |= BIT(1);
+			}
+			pstapriv->tim_bitmap &= ~BIT(0);
+			pstapriv->sta_dz_bitmap &= ~BIT(0);
+		}
+
+	}
+
+_exit:
+
+	/* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	if (update_mask)
+		/* update_BCNTIM(padapter); */
+		/* printk("%s => call update_beacon\n", __func__); */
+		update_beacon(padapter, _TIM_IE_, NULL, true);
+
+}
+
+void xmit_delivery_enabled_frames(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 wmmps_ac = 0;
+	struct list_head	*xmitframe_plist, *xmitframe_phead;
+	struct xmit_frame *pxmitframe = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+
+	/* spin_lock_bh(&psta->sleep_q.lock); */
+	spin_lock_bh(&pxmitpriv->lock);
+
+	xmitframe_phead = get_list_head(&psta->sleep_q);
+	xmitframe_plist = get_next(xmitframe_phead);
+
+	while (xmitframe_phead != xmitframe_plist) {
+		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)) {
+			pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+			/* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */
+			/* upate BCN for TIM IE */
+			/* update_BCNTIM(padapter); */
+			update_beacon(padapter, _TIM_IE_, NULL, true);
+			/* update_mask = BIT(0); */
+		}
+
+	}
+
+	/* spin_unlock_bh(&psta->sleep_q.lock); */
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	return;
+}
+
+void enqueue_pending_xmitbuf(
+	struct xmit_priv *pxmitpriv,
+	struct xmit_buf *pxmitbuf)
+{
+	struct __queue *pqueue;
+	struct adapter *pri_adapter = pxmitpriv->adapter;
+
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	spin_lock_bh(&pqueue->lock);
+	list_del_init(&pxmitbuf->list);
+	list_add_tail(&pxmitbuf->list, get_list_head(pqueue));
+	spin_unlock_bh(&pqueue->lock);
+
+	up(&(pri_adapter->xmitpriv.xmit_sema));
+}
+
+void enqueue_pending_xmitbuf_to_head(
+	struct xmit_priv *pxmitpriv,
+	struct xmit_buf *pxmitbuf)
+{
+	struct __queue *pqueue;
+
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	spin_lock_bh(&pqueue->lock);
+	list_del_init(&pxmitbuf->list);
+	list_add(&pxmitbuf->list, get_list_head(pqueue));
+	spin_unlock_bh(&pqueue->lock);
+}
+
+struct xmit_buf *dequeue_pending_xmitbuf(
+	struct xmit_priv *pxmitpriv)
+{
+	struct xmit_buf *pxmitbuf;
+	struct __queue *pqueue;
+
+
+	pxmitbuf = NULL;
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	spin_lock_bh(&pqueue->lock);
+
+	if (!list_empty(&pqueue->queue)) {
+		struct list_head *plist, *phead;
+
+		phead = get_list_head(pqueue);
+		plist = get_next(phead);
+		pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
+		list_del_init(&pxmitbuf->list);
+	}
+
+	spin_unlock_bh(&pqueue->lock);
+
+	return pxmitbuf;
+}
+
+struct xmit_buf *dequeue_pending_xmitbuf_under_survey(
+	struct xmit_priv *pxmitpriv)
+{
+	struct xmit_buf *pxmitbuf;
+	struct __queue *pqueue;
+
+
+	pxmitbuf = NULL;
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	spin_lock_bh(&pqueue->lock);
+
+	if (!list_empty(&pqueue->queue)) {
+		struct list_head *plist, *phead;
+		u8 type;
+
+		phead = get_list_head(pqueue);
+		plist = phead;
+		do {
+			plist = get_next(plist);
+			if (plist == phead)
+				break;
+
+			pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
+
+			type = GetFrameSubType(pxmitbuf->pbuf + TXDESC_OFFSET);
+
+			if ((type == WIFI_PROBEREQ) ||
+				(type == WIFI_DATA_NULL) ||
+				(type == WIFI_QOS_DATA_NULL)) {
+				list_del_init(&pxmitbuf->list);
+				break;
+			}
+			pxmitbuf = NULL;
+		} while (1);
+	}
+
+	spin_unlock_bh(&pqueue->lock);
+
+	return pxmitbuf;
+}
+
+sint check_pending_xmitbuf(
+	struct xmit_priv *pxmitpriv)
+{
+	struct __queue *pqueue;
+	sint	ret = false;
+
+	pqueue = &pxmitpriv->pending_xmitbuf_queue;
+
+	spin_lock_bh(&pqueue->lock);
+
+	if (!list_empty(&pqueue->queue))
+		ret = true;
+
+	spin_unlock_bh(&pqueue->lock);
+
+	return ret;
+}
+
+int rtw_xmit_thread(void *context)
+{
+	s32 err;
+	struct adapter *padapter;
+
+
+	err = _SUCCESS;
+	padapter = (struct adapter *)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();
+}
+
+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;
+		DBG_871X("%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))
+			DBG_871X("%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);
+}
+
+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 {
+		DBG_871X("%s ack_tx not set\n", __func__);
+	}
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/Hal8723BPwrSeq.c linux/3rdparty/rtl8723bs/hal/Hal8723BPwrSeq.c
--- linux.old/3rdparty/rtl8723bs/hal/Hal8723BPwrSeq.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/Hal8723BPwrSeq.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,138 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+
+/*
+*
+This file includes all kinds of Power Action event for RTL8723B
+and corresponding hardware configurtions which are released from HW SD.
+
+Major Change History:
+	When       Who               What
+	---------- ---------------   -------------------------------
+	2011-08-08 Roger            Create.
+
+*/
+
+#include "Hal8723BPwrSeq.h"
+
+/* drivers should parse below arrays and do the corresponding actions */
+/* 3 Power on  Array */
+WLAN_PWR_CFG rtl8723B_power_on_flow[
+	RTL8723B_TRANS_CARDEMU_TO_ACT_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_CARDEMU_TO_ACT
+	RTL8723B_TRANS_END
+};
+
+/* 3Radio off GPIO Array */
+WLAN_PWR_CFG rtl8723B_radio_off_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_END
+};
+
+/* 3Card Disable Array */
+WLAN_PWR_CFG rtl8723B_card_disable_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_CARDDIS
+	RTL8723B_TRANS_END
+};
+
+/* 3 Card Enable Array */
+WLAN_PWR_CFG rtl8723B_card_enable_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_CARDDIS_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_ACT
+	RTL8723B_TRANS_END
+};
+
+/* 3Suspend Array */
+WLAN_PWR_CFG rtl8723B_suspend_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_SUS
+	RTL8723B_TRANS_END
+};
+
+/* 3 Resume Array */
+WLAN_PWR_CFG rtl8723B_resume_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_SUS_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_ACT
+	RTL8723B_TRANS_END
+};
+
+/* 3HWPDN Array */
+WLAN_PWR_CFG rtl8723B_hwpdn_flow[
+	RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+
+	RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+
+	RTL8723B_TRANS_END_STEPS
+] = {
+	RTL8723B_TRANS_ACT_TO_CARDEMU
+	RTL8723B_TRANS_CARDEMU_TO_PDN
+	RTL8723B_TRANS_END
+};
+
+/* 3 Enter LPS */
+WLAN_PWR_CFG rtl8723B_enter_lps_flow[
+	RTL8723B_TRANS_ACT_TO_LPS_STEPS+RTL8723B_TRANS_END_STEPS
+] = {
+	/* FW behavior */
+	RTL8723B_TRANS_ACT_TO_LPS
+	RTL8723B_TRANS_END
+};
+
+/* 3 Leave LPS */
+WLAN_PWR_CFG rtl8723B_leave_lps_flow[
+	RTL8723B_TRANS_LPS_TO_ACT_STEPS+RTL8723B_TRANS_END_STEPS
+] = {
+	/* FW behavior */
+	RTL8723B_TRANS_LPS_TO_ACT
+	RTL8723B_TRANS_END
+};
+
+/* 3 Enter SW LPS */
+WLAN_PWR_CFG rtl8723B_enter_swlps_flow[
+	RTL8723B_TRANS_ACT_TO_SWLPS_STEPS+RTL8723B_TRANS_END_STEPS
+] = {
+	/* SW behavior */
+	RTL8723B_TRANS_ACT_TO_SWLPS
+	RTL8723B_TRANS_END
+};
+
+/* 3 Leave SW LPS */
+WLAN_PWR_CFG rtl8723B_leave_swlps_flow[
+	RTL8723B_TRANS_SWLPS_TO_ACT_STEPS+RTL8723B_TRANS_END_STEPS
+] = {
+	/* SW behavior */
+	RTL8723B_TRANS_SWLPS_TO_ACT
+	RTL8723B_TRANS_END
+};
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/Hal8723BReg.h linux/3rdparty/rtl8723bs/hal/Hal8723BReg.h
--- linux.old/3rdparty/rtl8723bs/hal/Hal8723BReg.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/Hal8723BReg.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,442 @@
+/*****************************************************************************
+ *Copyright(c) 2009,  RealTEK Technology Inc. All Right Reserved.
+ *
+ * Module:	__INC_HAL8723BREG_H
+ *
+ *
+ * Note:	1. Define Mac register address and corresponding bit mask map
+ *
+ *
+ * Export:	Constants, macro, functions(API), global variables(None).
+ *
+ * Abbrev:
+ *
+ * History:
+ *	Data		Who		Remark
+ *
+ *****************************************************************************/
+#ifndef __INC_HAL8723BREG_H
+#define __INC_HAL8723BREG_H
+
+
+
+/*  */
+/*  */
+/*  */
+
+/*  */
+/*  */
+/* 	0x0000h ~ 0x00FFh	System Configuration */
+/*  */
+/*  */
+#define REG_SYS_ISO_CTRL_8723B			0x0000	/*  2 Byte */
+#define REG_SYS_FUNC_EN_8723B			0x0002	/*  2 Byte */
+#define REG_APS_FSMCO_8723B			0x0004	/*  4 Byte */
+#define REG_SYS_CLKR_8723B				0x0008	/*  2 Byte */
+#define REG_9346CR_8723B				0x000A	/*  2 Byte */
+#define REG_EE_VPD_8723B				0x000C	/*  2 Byte */
+#define REG_AFE_MISC_8723B				0x0010	/*  1 Byte */
+#define REG_SPS0_CTRL_8723B				0x0011	/*  7 Byte */
+#define REG_SPS_OCP_CFG_8723B			0x0018	/*  4 Byte */
+#define REG_RSV_CTRL_8723B				0x001C	/*  3 Byte */
+#define REG_RF_CTRL_8723B				0x001F	/*  1 Byte */
+#define REG_LPLDO_CTRL_8723B			0x0023	/*  1 Byte */
+#define REG_AFE_XTAL_CTRL_8723B		0x0024	/*  4 Byte */
+#define REG_AFE_PLL_CTRL_8723B			0x0028	/*  4 Byte */
+#define REG_MAC_PLL_CTRL_EXT_8723B		0x002c	/*  4 Byte */
+#define REG_EFUSE_CTRL_8723B			0x0030
+#define REG_EFUSE_TEST_8723B			0x0034
+#define REG_PWR_DATA_8723B				0x0038
+#define REG_CAL_TIMER_8723B				0x003C
+#define REG_ACLK_MON_8723B				0x003E
+#define REG_GPIO_MUXCFG_8723B			0x0040
+#define REG_GPIO_IO_SEL_8723B			0x0042
+#define REG_MAC_PINMUX_CFG_8723B		0x0043
+#define REG_GPIO_PIN_CTRL_8723B			0x0044
+#define REG_GPIO_INTM_8723B				0x0048
+#define REG_LEDCFG0_8723B				0x004C
+#define REG_LEDCFG1_8723B				0x004D
+#define REG_LEDCFG2_8723B				0x004E
+#define REG_LEDCFG3_8723B				0x004F
+#define REG_FSIMR_8723B					0x0050
+#define REG_FSISR_8723B					0x0054
+#define REG_HSIMR_8723B					0x0058
+#define REG_HSISR_8723B					0x005c
+#define REG_GPIO_EXT_CTRL				0x0060
+#define REG_MULTI_FUNC_CTRL_8723B		0x0068
+#define REG_GPIO_STATUS_8723B			0x006C
+#define REG_SDIO_CTRL_8723B				0x0070
+#define REG_OPT_CTRL_8723B				0x0074
+#define REG_AFE_XTAL_CTRL_EXT_8723B	0x0078
+#define REG_MCUFWDL_8723B				0x0080
+#define REG_BT_PATCH_STATUS_8723B		0x0088
+#define REG_HIMR0_8723B					0x00B0
+#define REG_HISR0_8723B					0x00B4
+#define REG_HIMR1_8723B					0x00B8
+#define REG_HISR1_8723B					0x00BC
+#define REG_PMC_DBG_CTRL2_8723B			0x00CC
+#define	REG_EFUSE_BURN_GNT_8723B		0x00CF
+#define REG_HPON_FSM_8723B				0x00EC
+#define REG_SYS_CFG_8723B				0x00F0
+#define REG_SYS_CFG1_8723B				0x00FC
+#define REG_ROM_VERSION					0x00FD
+
+/*  */
+/*  */
+/* 	0x0100h ~ 0x01FFh	MACTOP General Configuration */
+/*  */
+/*  */
+#define REG_CR_8723B						0x0100
+#define REG_PBP_8723B					0x0104
+#define REG_PKT_BUFF_ACCESS_CTRL_8723B	0x0106
+#define REG_TRXDMA_CTRL_8723B			0x010C
+#define REG_TRXFF_BNDY_8723B			0x0114
+#define REG_TRXFF_STATUS_8723B			0x0118
+#define REG_RXFF_PTR_8723B				0x011C
+#define REG_CPWM_8723B					0x012F
+#define REG_FWIMR_8723B					0x0130
+#define REG_FWISR_8723B					0x0134
+#define REG_FTIMR_8723B					0x0138
+#define REG_PKTBUF_DBG_CTRL_8723B		0x0140
+#define REG_RXPKTBUF_CTRL_8723B		0x0142
+#define REG_PKTBUF_DBG_DATA_L_8723B	0x0144
+#define REG_PKTBUF_DBG_DATA_H_8723B	0x0148
+
+#define REG_TC0_CTRL_8723B				0x0150
+#define REG_TC1_CTRL_8723B				0x0154
+#define REG_TC2_CTRL_8723B				0x0158
+#define REG_TC3_CTRL_8723B				0x015C
+#define REG_TC4_CTRL_8723B				0x0160
+#define REG_TCUNIT_BASE_8723B			0x0164
+#define REG_RSVD3_8723B					0x0168
+#define REG_C2HEVT_MSG_NORMAL_8723B	0x01A0
+#define REG_C2HEVT_CMD_SEQ_88XX		0x01A1
+#define REG_C2hEVT_CMD_CONTENT_88XX	0x01A2
+#define REG_C2HEVT_CMD_LEN_88XX		0x01AE
+#define REG_C2HEVT_CLEAR_8723B			0x01AF
+#define REG_MCUTST_1_8723B				0x01C0
+#define REG_MCUTST_WOWLAN_8723B		0x01C7
+#define REG_FMETHR_8723B				0x01C8
+#define REG_HMETFR_8723B				0x01CC
+#define REG_HMEBOX_0_8723B				0x01D0
+#define REG_HMEBOX_1_8723B				0x01D4
+#define REG_HMEBOX_2_8723B				0x01D8
+#define REG_HMEBOX_3_8723B				0x01DC
+#define REG_LLT_INIT_8723B				0x01E0
+#define REG_HMEBOX_EXT0_8723B			0x01F0
+#define REG_HMEBOX_EXT1_8723B			0x01F4
+#define REG_HMEBOX_EXT2_8723B			0x01F8
+#define REG_HMEBOX_EXT3_8723B			0x01FC
+
+/*  */
+/*  */
+/* 	0x0200h ~ 0x027Fh	TXDMA Configuration */
+/*  */
+/*  */
+#define REG_RQPN_8723B					0x0200
+#define REG_FIFOPAGE_8723B				0x0204
+#define REG_DWBCN0_CTRL_8723B			REG_TDECTRL
+#define REG_TXDMA_OFFSET_CHK_8723B	0x020C
+#define REG_TXDMA_STATUS_8723B		0x0210
+#define REG_RQPN_NPQ_8723B			0x0214
+#define REG_DWBCN1_CTRL_8723B			0x0228
+
+
+/*  */
+/*  */
+/* 	0x0280h ~ 0x02FFh	RXDMA Configuration */
+/*  */
+/*  */
+#define REG_RXDMA_AGG_PG_TH_8723B		0x0280
+#define REG_FW_UPD_RDPTR_8723B		0x0284 /*  FW shall update this register before FW write RXPKT_RELEASE_POLL to 1 */
+#define REG_RXDMA_CONTROL_8723B		0x0286 /*  Control the RX DMA. */
+#define REG_RXPKT_NUM_8723B			0x0287 /*  The number of packets in RXPKTBUF. */
+#define REG_RXDMA_STATUS_8723B			0x0288
+#define REG_RXDMA_PRO_8723B			0x0290
+#define REG_EARLY_MODE_CONTROL_8723B	0x02BC
+#define REG_RSVD5_8723B					0x02F0
+#define REG_RSVD6_8723B					0x02F4
+
+
+/*  */
+/*  */
+/* 	0x0300h ~ 0x03FFh	PCIe */
+/*  */
+/*  */
+#define	REG_PCIE_CTRL_REG_8723B		0x0300
+#define	REG_INT_MIG_8723B				0x0304	/*  Interrupt Migration */
+#define	REG_BCNQ_DESA_8723B			0x0308	/*  TX Beacon Descriptor Address */
+#define	REG_HQ_DESA_8723B				0x0310	/*  TX High Queue Descriptor Address */
+#define	REG_MGQ_DESA_8723B			0x0318	/*  TX Manage Queue Descriptor Address */
+#define	REG_VOQ_DESA_8723B			0x0320	/*  TX VO Queue Descriptor Address */
+#define	REG_VIQ_DESA_8723B				0x0328	/*  TX VI Queue Descriptor Address */
+#define	REG_BEQ_DESA_8723B			0x0330	/*  TX BE Queue Descriptor Address */
+#define	REG_BKQ_DESA_8723B			0x0338	/*  TX BK Queue Descriptor Address */
+#define	REG_RX_DESA_8723B				0x0340	/*  RX Queue	Descriptor Address */
+#define	REG_DBI_WDATA_8723B			0x0348	/*  DBI Write Data */
+#define	REG_DBI_RDATA_8723B			0x034C	/*  DBI Read Data */
+#define	REG_DBI_ADDR_8723B				0x0350	/*  DBI Address */
+#define	REG_DBI_FLAG_8723B				0x0352	/*  DBI Read/Write Flag */
+#define	REG_MDIO_WDATA_8723B		0x0354	/*  MDIO for Write PCIE PHY */
+#define	REG_MDIO_RDATA_8723B			0x0356	/*  MDIO for Reads PCIE PHY */
+#define	REG_MDIO_CTL_8723B			0x0358	/*  MDIO for Control */
+#define	REG_DBG_SEL_8723B				0x0360	/*  Debug Selection Register */
+#define	REG_PCIE_HRPWM_8723B			0x0361	/* PCIe RPWM */
+#define	REG_PCIE_HCPWM_8723B			0x0363	/* PCIe CPWM */
+#define	REG_PCIE_MULTIFET_CTRL_8723B	0x036A	/* PCIE Multi-Fethc Control */
+
+/*  spec version 11 */
+/*  */
+/*  */
+/* 	0x0400h ~ 0x047Fh	Protocol Configuration */
+/*  */
+/*  */
+#define REG_VOQ_INFORMATION_8723B		0x0400
+#define REG_VIQ_INFORMATION_8723B		0x0404
+#define REG_BEQ_INFORMATION_8723B		0x0408
+#define REG_BKQ_INFORMATION_8723B		0x040C
+#define REG_MGQ_INFORMATION_8723B		0x0410
+#define REG_HGQ_INFORMATION_8723B		0x0414
+#define REG_BCNQ_INFORMATION_8723B	0x0418
+#define REG_TXPKT_EMPTY_8723B			0x041A
+
+#define REG_FWHW_TXQ_CTRL_8723B		0x0420
+#define REG_HWSEQ_CTRL_8723B			0x0423
+#define REG_TXPKTBUF_BCNQ_BDNY_8723B	0x0424
+#define REG_TXPKTBUF_MGQ_BDNY_8723B	0x0425
+#define REG_LIFECTRL_CTRL_8723B			0x0426
+#define REG_MULTI_BCNQ_OFFSET_8723B	0x0427
+#define REG_SPEC_SIFS_8723B				0x0428
+#define REG_RL_8723B						0x042A
+#define REG_TXBF_CTRL_8723B				0x042C
+#define REG_DARFRC_8723B				0x0430
+#define REG_RARFRC_8723B				0x0438
+#define REG_RRSR_8723B					0x0440
+#define REG_ARFR0_8723B					0x0444
+#define REG_ARFR1_8723B					0x044C
+#define REG_CCK_CHECK_8723B				0x0454
+#define REG_AMPDU_MAX_TIME_8723B		0x0456
+#define REG_TXPKTBUF_BCNQ_BDNY1_8723B	0x0457
+
+#define REG_AMPDU_MAX_LENGTH_8723B	0x0458
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD_8723B	0x045D
+#define REG_NDPA_OPT_CTRL_8723B		0x045F
+#define REG_FAST_EDCA_CTRL_8723B		0x0460
+#define REG_RD_RESP_PKT_TH_8723B		0x0463
+#define REG_DATA_SC_8723B				0x0483
+#define REG_TXRPT_START_OFFSET		0x04AC
+#define REG_POWER_STAGE1_8723B		0x04B4
+#define REG_POWER_STAGE2_8723B		0x04B8
+#define REG_AMPDU_BURST_MODE_8723B	0x04BC
+#define REG_PKT_VO_VI_LIFE_TIME_8723B	0x04C0
+#define REG_PKT_BE_BK_LIFE_TIME_8723B	0x04C2
+#define REG_STBC_SETTING_8723B			0x04C4
+#define REG_HT_SINGLE_AMPDU_8723B		0x04C7
+#define REG_PROT_MODE_CTRL_8723B		0x04C8
+#define REG_MAX_AGGR_NUM_8723B		0x04CA
+#define REG_RTS_MAX_AGGR_NUM_8723B	0x04CB
+#define REG_BAR_MODE_CTRL_8723B		0x04CC
+#define REG_RA_TRY_RATE_AGG_LMT_8723B	0x04CF
+#define REG_MACID_PKT_DROP0_8723B		0x04D0
+#define REG_MACID_PKT_SLEEP_8723B		0x04D4
+
+/*  */
+/*  */
+/* 	0x0500h ~ 0x05FFh	EDCA Configuration */
+/*  */
+/*  */
+#define REG_EDCA_VO_PARAM_8723B		0x0500
+#define REG_EDCA_VI_PARAM_8723B		0x0504
+#define REG_EDCA_BE_PARAM_8723B		0x0508
+#define REG_EDCA_BK_PARAM_8723B		0x050C
+#define REG_BCNTCFG_8723B				0x0510
+#define REG_PIFS_8723B					0x0512
+#define REG_RDG_PIFS_8723B				0x0513
+#define REG_SIFS_CTX_8723B				0x0514
+#define REG_SIFS_TRX_8723B				0x0516
+#define REG_AGGR_BREAK_TIME_8723B		0x051A
+#define REG_SLOT_8723B					0x051B
+#define REG_TX_PTCL_CTRL_8723B			0x0520
+#define REG_TXPAUSE_8723B				0x0522
+#define REG_DIS_TXREQ_CLR_8723B		0x0523
+#define REG_RD_CTRL_8723B				0x0524
+/*  */
+/*  Format for offset 540h-542h: */
+/* 	[3:0]:   TBTT prohibit setup in unit of 32us. The time for HW getting beacon content before TBTT. */
+/* 	[7:4]:   Reserved. */
+/* 	[19:8]:  TBTT prohibit hold in unit of 32us. The time for HW holding to send the beacon packet. */
+/* 	[23:20]: Reserved */
+/*  Description: */
+/* 	              | */
+/*      |<--Setup--|--Hold------------>| */
+/* 	--------------|---------------------- */
+/*                 | */
+/*                TBTT */
+/*  Note: We cannot update beacon content to HW or send any AC packets during the time between Setup and Hold. */
+/*  Described by Designer Tim and Bruce, 2011-01-14. */
+/*  */
+#define REG_TBTT_PROHIBIT_8723B		0x0540
+#define REG_RD_NAV_NXT_8723B		0x0544
+#define REG_NAV_PROT_LEN_8723B		0x0546
+#define REG_BCN_CTRL_8723B		0x0550
+#define REG_BCN_CTRL_1_8723B		0x0551
+#define REG_MBID_NUM_8723B		0x0552
+#define REG_DUAL_TSF_RST_8723B		0x0553
+#define REG_BCN_INTERVAL_8723B		0x0554
+#define REG_DRVERLYINT_8723B		0x0558
+#define REG_BCNDMATIM_8723B		0x0559
+#define REG_ATIMWND_8723B		0x055A
+#define REG_USTIME_TSF_8723B		0x055C
+#define REG_BCN_MAX_ERR_8723B		0x055D
+#define REG_RXTSF_OFFSET_CCK_8723B	0x055E
+#define REG_RXTSF_OFFSET_OFDM_8723B	0x055F
+#define REG_TSFTR_8723B			0x0560
+#define REG_CTWND_8723B			0x0572
+#define REG_SECONDARY_CCA_CTRL_8723B	0x0577
+#define REG_PSTIMER_8723B		0x0580
+#define REG_TIMER0_8723B		0x0584
+#define REG_TIMER1_8723B		0x0588
+#define REG_ACMHWCTRL_8723B		0x05C0
+#define REG_SCH_TXCMD_8723B		0x05F8
+
+/* 	0x0600h ~ 0x07FFh	WMAC Configuration */
+#define REG_MAC_CR_8723B		0x0600
+#define REG_TCR_8723B			0x0604
+#define REG_RCR_8723B			0x0608
+#define REG_RX_PKT_LIMIT_8723B		0x060C
+#define REG_RX_DLK_TIME_8723B		0x060D
+#define REG_RX_DRVINFO_SZ_8723B		0x060F
+
+#define REG_MACID_8723B			0x0610
+#define REG_BSSID_8723B			0x0618
+#define REG_MAR_8723B			0x0620
+#define REG_MBIDCAMCFG_8723B		0x0628
+
+#define REG_USTIME_EDCA_8723B		0x0638
+#define REG_MAC_SPEC_SIFS_8723B		0x063A
+#define REG_RESP_SIFP_CCK_8723B		0x063C
+#define REG_RESP_SIFS_OFDM_8723B	0x063E
+#define REG_ACKTO_8723B			0x0640
+#define REG_CTS2TO_8723B		0x0641
+#define REG_EIFS_8723B			0x0642
+
+#define REG_NAV_UPPER_8723B		0x0652	/*  unit of 128 */
+#define REG_TRXPTCL_CTL_8723B		0x0668
+
+/*  Security */
+#define REG_CAMCMD_8723B		0x0670
+#define REG_CAMWRITE_8723B		0x0674
+#define REG_CAMREAD_8723B		0x0678
+#define REG_CAMDBG_8723B		0x067C
+#define REG_SECCFG_8723B		0x0680
+
+/*  Power */
+#define REG_WOW_CTRL_8723B		0x0690
+#define REG_PS_RX_INFO_8723B		0x0692
+#define REG_UAPSD_TID_8723B		0x0693
+#define REG_WKFMCAM_CMD_8723B		0x0698
+#define REG_WKFMCAM_NUM_8723B		0x0698
+#define REG_WKFMCAM_RWD_8723B		0x069C
+#define REG_RXFLTMAP0_8723B		0x06A0
+#define REG_RXFLTMAP1_8723B		0x06A2
+#define REG_RXFLTMAP2_8723B		0x06A4
+#define REG_BCN_PSR_RPT_8723B		0x06A8
+#define REG_BT_COEX_TABLE_8723B		0x06C0
+#define REG_BFMER0_INFO_8723B		0x06E4
+#define REG_BFMER1_INFO_8723B		0x06EC
+#define REG_CSI_RPT_PARAM_BW20_8723B	0x06F4
+#define REG_CSI_RPT_PARAM_BW40_8723B	0x06F8
+#define REG_CSI_RPT_PARAM_BW80_8723B	0x06FC
+
+/*  Hardware Port 2 */
+#define REG_MACID1_8723B		0x0700
+#define REG_BSSID1_8723B		0x0708
+#define REG_BFMEE_SEL_8723B		0x0714
+#define REG_SND_PTCL_CTRL_8723B		0x0718
+
+
+/* 	Redifine 8192C register definition for compatibility */
+
+/*  TODO: use these definition when using REG_xxx naming rule. */
+/*  NOTE: DO NOT Remove these definition. Use later. */
+#define	EFUSE_CTRL_8723B	REG_EFUSE_CTRL_8723B	/*  E-Fuse Control. */
+#define	EFUSE_TEST_8723B	REG_EFUSE_TEST_8723B	/*  E-Fuse Test. */
+#define	MSR_8723B		(REG_CR_8723B + 2)	/*  Media Status register */
+#define	ISR_8723B		REG_HISR0_8723B
+#define	TSFR_8723B		REG_TSFTR_8723B		/*  Timing Sync Function Timer Register. */
+
+#define PBP_8723B		REG_PBP_8723B
+
+/*  Redifine MACID register, to compatible prior ICs. */
+#define	IDR0_8723B		REG_MACID_8723B		/*  MAC ID Register, Offset 0x0050-0x0053 */
+#define	IDR4_8723B		(REG_MACID_8723B + 4)	/*  MAC ID Register, Offset 0x0054-0x0055 */
+
+/*  9. Security Control Registers	(Offset:) */
+#define	RWCAM_8723B		REG_CAMCMD_8723B	/* IN 8190 Data Sheet is called CAMcmd */
+#define	WCAMI_8723B		REG_CAMWRITE_8723B	/*  Software write CAM input content */
+#define	RCAMO_8723B		REG_CAMREAD_8723B	/*  Software read/write CAM config */
+#define	CAMDBG_8723B		REG_CAMDBG_8723B
+#define	SECR_8723B		REG_SECCFG_8723B	/* Security Configuration Register */
+
+/*        8195 IMR/ISR bits		(offset 0xB0,  8bits) */
+#define	IMR_DISABLED_8723B		0
+/*  IMR DW0(0x00B0-00B3) Bit 0-31 */
+#define	IMR_TIMER2_8723B		BIT31	/*  Timeout interrupt 2 */
+#define	IMR_TIMER1_8723B		BIT30	/*  Timeout interrupt 1 */
+#define	IMR_PSTIMEOUT_8723B		BIT29	/*  Power Save Time Out Interrupt */
+#define	IMR_GTINT4_8723B		BIT28	/*  When GTIMER4 expires, this bit is set to 1 */
+#define	IMR_GTINT3_8723B		BIT27	/*  When GTIMER3 expires, this bit is set to 1 */
+#define	IMR_TXBCN0ERR_8723B		BIT26	/*  Transmit Beacon0 Error */
+#define	IMR_TXBCN0OK_8723B		BIT25	/*  Transmit Beacon0 OK */
+#define	IMR_TSF_BIT32_TOGGLE_8723B	BIT24	/*  TSF Timer BIT32 toggle indication interrupt */
+#define	IMR_BCNDMAINT0_8723B		BIT20	/*  Beacon DMA Interrupt 0 */
+#define	IMR_BCNDERR0_8723B		BIT16	/*  Beacon Queue DMA OK0 */
+#define	IMR_HSISR_IND_ON_INT_8723B	BIT15	/*  HSISR Indicator (HSIMR & HSISR is true, this bit is set to 1) */
+#define	IMR_BCNDMAINT_E_8723B		BIT14	/*  Beacon DMA Interrupt Extension for Win7 */
+#define	IMR_ATIMEND_8723B		BIT12	/*  CTWidnow End or ATIM Window End */
+#define	IMR_C2HCMD_8723B		BIT10	/*  CPU to Host Command INT Status, Write 1 clear */
+#define	IMR_CPWM2_8723B			BIT9	/*  CPU power Mode exchange INT Status, Write 1 clear */
+#define	IMR_CPWM_8723B			BIT8	/*  CPU power Mode exchange INT Status, Write 1 clear */
+#define	IMR_HIGHDOK_8723B		BIT7	/*  High Queue DMA OK */
+#define	IMR_MGNTDOK_8723B		BIT6	/*  Management Queue DMA OK */
+#define	IMR_BKDOK_8723B			BIT5	/*  AC_BK DMA OK */
+#define	IMR_BEDOK_8723B			BIT4	/*  AC_BE DMA OK */
+#define	IMR_VIDOK_8723B			BIT3	/*  AC_VI DMA OK */
+#define	IMR_VODOK_8723B			BIT2	/*  AC_VO DMA OK */
+#define	IMR_RDU_8723B			BIT1	/*  Rx Descriptor Unavailable */
+#define	IMR_ROK_8723B			BIT0	/*  Receive DMA OK */
+
+/*  IMR DW1(0x00B4-00B7) Bit 0-31 */
+#define	IMR_BCNDMAINT7_8723B		BIT27	/*  Beacon DMA Interrupt 7 */
+#define	IMR_BCNDMAINT6_8723B		BIT26	/*  Beacon DMA Interrupt 6 */
+#define	IMR_BCNDMAINT5_8723B		BIT25	/*  Beacon DMA Interrupt 5 */
+#define	IMR_BCNDMAINT4_8723B		BIT24	/*  Beacon DMA Interrupt 4 */
+#define	IMR_BCNDMAINT3_8723B		BIT23	/*  Beacon DMA Interrupt 3 */
+#define	IMR_BCNDMAINT2_8723B		BIT22	/*  Beacon DMA Interrupt 2 */
+#define	IMR_BCNDMAINT1_8723B		BIT21	/*  Beacon DMA Interrupt 1 */
+#define	IMR_BCNDOK7_8723B		BIT20	/*  Beacon Queue DMA OK Interrup 7 */
+#define	IMR_BCNDOK6_8723B		BIT19	/*  Beacon Queue DMA OK Interrup 6 */
+#define	IMR_BCNDOK5_8723B		BIT18	/*  Beacon Queue DMA OK Interrup 5 */
+#define	IMR_BCNDOK4_8723B		BIT17	/*  Beacon Queue DMA OK Interrup 4 */
+#define	IMR_BCNDOK3_8723B		BIT16	/*  Beacon Queue DMA OK Interrup 3 */
+#define	IMR_BCNDOK2_8723B		BIT15	/*  Beacon Queue DMA OK Interrup 2 */
+#define	IMR_BCNDOK1_8723B		BIT14	/*  Beacon Queue DMA OK Interrup 1 */
+#define	IMR_ATIMEND_E_8723B		BIT13	/*  ATIM Window End Extension for Win7 */
+#define	IMR_TXERR_8723B			BIT11	/*  Tx Error Flag Interrupt Status, write 1 clear. */
+#define	IMR_RXERR_8723B			BIT10	/*  Rx Error Flag INT Status, Write 1 clear */
+#define	IMR_TXFOVW_8723B		BIT9	/*  Transmit FIFO Overflow */
+#define	IMR_RXFOVW_8723B		BIT8	/*  Receive FIFO Overflow */
+
+/* 2 ACMHWCTRL 0x05C0 */
+#define	AcmHw_HwEn_8723B		BIT(0)
+#define	AcmHw_VoqEn_8723B		BIT(1)
+#define	AcmHw_ViqEn_8723B		BIT(2)
+#define	AcmHw_BeqEn_8723B		BIT(3)
+#define	AcmHw_VoqStatus_8723B		BIT(5)
+#define	AcmHw_ViqStatus_8723B		BIT(6)
+#define	AcmHw_BeqStatus_8723B		BIT(7)
+
+/*        8195 (RCR) Receive Configuration Register	(Offset 0x608, 32 bits) */
+#define	RCR_TCPOFLD_EN			BIT25	/*  Enable TCP checksum offload */
+
+#endif /*  #ifndef __INC_HAL8723BREG_H */
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalBtc8723b1Ant.c linux/3rdparty/rtl8723bs/hal/HalBtc8723b1Ant.c
--- linux.old/3rdparty/rtl8723bs/hal/HalBtc8723b1Ant.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalBtc8723b1Ant.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,3779 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+
+#include "Mp_Precomp.h"
+
+/*  Global variables, these are static variables */
+static COEX_DM_8723B_1ANT GLCoexDm8723b1Ant;
+static PCOEX_DM_8723B_1ANT pCoexDm = &GLCoexDm8723b1Ant;
+static COEX_STA_8723B_1ANT GLCoexSta8723b1Ant;
+static PCOEX_STA_8723B_1ANT	pCoexSta = &GLCoexSta8723b1Ant;
+
+static const char *const GLBtInfoSrc8723b1Ant[] = {
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+static u32 GLCoexVerDate8723b1Ant = 20140507;
+static u32 GLCoexVer8723b1Ant = 0x4e;
+
+/*  local function proto type if needed */
+/*  local function start with halbtc8723b1ant_ */
+static u8 halbtc8723b1ant_BtRssiState(
+	u8 levelNum, u8 rssiThresh, u8 rssiThresh1
+)
+{
+	s32 btRssi = 0;
+	u8 btRssiState = pCoexSta->preBtRssiState;
+
+	btRssi = pCoexSta->btRssi;
+
+	if (levelNum == 2) {
+		if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT)) {
+
+				btRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to High\n")
+				);
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state stay at Low\n")
+				);
+			}
+		} else {
+			if (btRssi < rssiThresh) {
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to Low\n")
+				);
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state stay at High\n")
+				);
+			}
+		}
+	} else if (levelNum == 3) {
+		if (rssiThresh > rssiThresh1) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_BT_RSSI_STATE,
+				("[BTCoex], BT Rssi thresh error!!\n")
+			);
+			return pCoexSta->preBtRssiState;
+		}
+
+		if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT)) {
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to Medium\n")
+				);
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state stay at Low\n")
+				);
+			}
+		} else if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_MEDIUM) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_MEDIUM)
+		) {
+			if (btRssi >= (rssiThresh1+BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT)) {
+				btRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to High\n")
+				);
+			} else if (btRssi < rssiThresh) {
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to Low\n")
+				);
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state stay at Medium\n")
+				);
+			}
+		} else {
+			if (btRssi < rssiThresh1) {
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to Medium\n")
+				);
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state stay at High\n")
+				);
+			}
+		}
+	}
+
+	pCoexSta->preBtRssiState = btRssiState;
+
+	return btRssiState;
+}
+
+static void halbtc8723b1ant_UpdateRaMask(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u32 disRateMask
+)
+{
+	pCoexDm->curRaMask = disRateMask;
+
+	if (bForceExec || (pCoexDm->preRaMask != pCoexDm->curRaMask))
+		pBtCoexist->fBtcSet(
+			pBtCoexist,
+			BTC_SET_ACT_UPDATE_RAMASK,
+			&pCoexDm->curRaMask
+		);
+	pCoexDm->preRaMask = pCoexDm->curRaMask;
+}
+
+static void halbtc8723b1ant_AutoRateFallbackRetry(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 type
+)
+{
+	bool bWifiUnderBMode = false;
+
+	pCoexDm->curArfrType = type;
+
+	if (bForceExec || (pCoexDm->preArfrType != pCoexDm->curArfrType)) {
+		switch (pCoexDm->curArfrType) {
+		case 0:	/*  normal mode */
+			pBtCoexist->fBtcWrite4Byte(
+				pBtCoexist, 0x430, pCoexDm->backupArfrCnt1
+			);
+			pBtCoexist->fBtcWrite4Byte(
+				pBtCoexist, 0x434, pCoexDm->backupArfrCnt2
+			);
+			break;
+		case 1:
+			pBtCoexist->fBtcGet(
+				pBtCoexist, BTC_GET_BL_WIFI_UNDER_B_MODE, &bWifiUnderBMode
+			);
+			if (bWifiUnderBMode) {
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x430, 0x0);
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x434, 0x01010101);
+			} else {
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x430, 0x0);
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x434, 0x04030201);
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	pCoexDm->preArfrType = pCoexDm->curArfrType;
+}
+
+static void halbtc8723b1ant_RetryLimit(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 type
+)
+{
+	pCoexDm->curRetryLimitType = type;
+
+	if (
+		bForceExec ||
+		(pCoexDm->preRetryLimitType != pCoexDm->curRetryLimitType)
+	) {
+		switch (pCoexDm->curRetryLimitType) {
+		case 0:	/*  normal mode */
+			pBtCoexist->fBtcWrite2Byte(
+				pBtCoexist, 0x42a, pCoexDm->backupRetryLimit
+			);
+			break;
+		case 1:	/*  retry limit =8 */
+			pBtCoexist->fBtcWrite2Byte(pBtCoexist, 0x42a, 0x0808);
+			break;
+		default:
+			break;
+		}
+	}
+
+	pCoexDm->preRetryLimitType = pCoexDm->curRetryLimitType;
+}
+
+static void halbtc8723b1ant_AmpduMaxTime(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 type
+)
+{
+	pCoexDm->curAmpduTimeType = type;
+
+	if (
+		bForceExec || (pCoexDm->preAmpduTimeType != pCoexDm->curAmpduTimeType)
+	) {
+		switch (pCoexDm->curAmpduTimeType) {
+		case 0:	/*  normal mode */
+			pBtCoexist->fBtcWrite1Byte(
+				pBtCoexist, 0x456, pCoexDm->backupAmpduMaxTime
+			);
+			break;
+		case 1:	/*  AMPDU timw = 0x38 * 32us */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x456, 0x38);
+			break;
+		default:
+			break;
+		}
+	}
+
+	pCoexDm->preAmpduTimeType = pCoexDm->curAmpduTimeType;
+}
+
+static void halbtc8723b1ant_LimitedTx(
+	PBTC_COEXIST pBtCoexist,
+	bool bForceExec,
+	u8 raMaskType,
+	u8 arfrType,
+	u8 retryLimitType,
+	u8 ampduTimeType
+)
+{
+	switch (raMaskType) {
+	case 0:	/*  normal mode */
+		halbtc8723b1ant_UpdateRaMask(pBtCoexist, bForceExec, 0x0);
+		break;
+	case 1:	/*  disable cck 1/2 */
+		halbtc8723b1ant_UpdateRaMask(pBtCoexist, bForceExec, 0x00000003);
+		break;
+	case 2:	/*  disable cck 1/2/5.5, ofdm 6/9/12/18/24, mcs 0/1/2/3/4 */
+		halbtc8723b1ant_UpdateRaMask(pBtCoexist, bForceExec, 0x0001f1f7);
+		break;
+	default:
+		break;
+	}
+
+	halbtc8723b1ant_AutoRateFallbackRetry(pBtCoexist, bForceExec, arfrType);
+	halbtc8723b1ant_RetryLimit(pBtCoexist, bForceExec, retryLimitType);
+	halbtc8723b1ant_AmpduMaxTime(pBtCoexist, bForceExec, ampduTimeType);
+}
+
+static void halbtc8723b1ant_LimitedRx(
+	PBTC_COEXIST pBtCoexist,
+	bool bForceExec,
+	bool bRejApAggPkt,
+	bool bBtCtrlAggBufSize,
+	u8 aggBufSize
+)
+{
+	bool bRejectRxAgg = bRejApAggPkt;
+	bool bBtCtrlRxAggSize = bBtCtrlAggBufSize;
+	u8 rxAggSize = aggBufSize;
+
+	/*  */
+	/* 	Rx Aggregation related setting */
+	/*  */
+	pBtCoexist->fBtcSet(
+		pBtCoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT, &bRejectRxAgg
+	);
+	/*  decide BT control aggregation buf size or not */
+	pBtCoexist->fBtcSet(
+		pBtCoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE, &bBtCtrlRxAggSize
+	);
+	/*  aggregation buf size, only work when BT control Rx aggregation size. */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_U1_AGG_BUF_SIZE, &rxAggSize);
+	/*  real update aggregation setting */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
+
+
+}
+
+static void halbtc8723b1ant_QueryBtInfo(PBTC_COEXIST pBtCoexist)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	pCoexSta->bC2hBtInfoReqSent = true;
+
+	H2C_Parameter[0] |= BIT0;	/*  trigger */
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		("[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n", H2C_Parameter[0])
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x61, 1, H2C_Parameter);
+}
+
+static void halbtc8723b1ant_MonitorBtCtr(PBTC_COEXIST pBtCoexist)
+{
+	u32 regHPTxRx, regLPTxRx, u4Tmp;
+	u32 regHPTx = 0, regHPRx = 0, regLPTx = 0, regLPRx = 0;
+	static u8 NumOfBtCounterChk;
+
+       /* to avoid 0x76e[3] = 1 (WLAN_Act control by PTA) during IPS */
+	/* if (! (pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x76e) & 0x8)) */
+
+	if (pCoexSta->bUnderIps) {
+		pCoexSta->highPriorityTx = 65535;
+		pCoexSta->highPriorityRx = 65535;
+		pCoexSta->lowPriorityTx = 65535;
+		pCoexSta->lowPriorityRx = 65535;
+		return;
+	}
+
+	regHPTxRx = 0x770;
+	regLPTxRx = 0x774;
+
+	u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, regHPTxRx);
+	regHPTx = u4Tmp & bMaskLWord;
+	regHPRx = (u4Tmp & bMaskHWord)>>16;
+
+	u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, regLPTxRx);
+	regLPTx = u4Tmp & bMaskLWord;
+	regLPRx = (u4Tmp & bMaskHWord)>>16;
+
+	pCoexSta->highPriorityTx = regHPTx;
+	pCoexSta->highPriorityRx = regHPRx;
+	pCoexSta->lowPriorityTx = regLPTx;
+	pCoexSta->lowPriorityRx = regLPRx;
+
+	if ((pCoexSta->lowPriorityTx >= 1050) && (!pCoexSta->bC2hBtInquiryPage))
+		pCoexSta->popEventCnt++;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE,
+		(
+			"[BTCoex], Hi-Pri Rx/Tx: %d/%d, Lo-Pri Rx/Tx: %d/%d\n",
+			regHPRx,
+			regHPTx,
+			regLPRx,
+			regLPTx
+		)
+	);
+
+	/*  reset counter */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0xc);
+
+	if ((regHPTx == 0) && (regHPRx == 0) && (regLPTx == 0) && (regLPRx == 0)) {
+		NumOfBtCounterChk++;
+		if (NumOfBtCounterChk >= 3) {
+			halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+			NumOfBtCounterChk = 0;
+		}
+	}
+}
+
+
+static void halbtc8723b1ant_MonitorWiFiCtr(PBTC_COEXIST pBtCoexist)
+{
+	s32	wifiRssi = 0;
+	bool bWifiBusy = false, bWifiUnderBMode = false;
+	static u8 nCCKLockCounter = 0;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_BL_WIFI_UNDER_B_MODE, &bWifiUnderBMode
+	);
+
+	if (pCoexSta->bUnderIps) {
+		pCoexSta->nCRCOK_CCK = 0;
+		pCoexSta->nCRCOK_11g = 0;
+		pCoexSta->nCRCOK_11n = 0;
+		pCoexSta->nCRCOK_11nAgg = 0;
+
+		pCoexSta->nCRCErr_CCK = 0;
+		pCoexSta->nCRCErr_11g = 0;
+		pCoexSta->nCRCErr_11n = 0;
+		pCoexSta->nCRCErr_11nAgg = 0;
+	} else {
+		pCoexSta->nCRCOK_CCK	= pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xf88);
+		pCoexSta->nCRCOK_11g	= pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xf94);
+		pCoexSta->nCRCOK_11n	= pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xf90);
+		pCoexSta->nCRCOK_11nAgg = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xfb8);
+
+		pCoexSta->nCRCErr_CCK	 = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xf84);
+		pCoexSta->nCRCErr_11g	 = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xf96);
+		pCoexSta->nCRCErr_11n	 = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xf92);
+		pCoexSta->nCRCErr_11nAgg = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0xfba);
+	}
+
+
+	/* reset counter */
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0xf16, 0x1, 0x1);
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0xf16, 0x1, 0x0);
+
+	if (bWifiBusy && (wifiRssi >= 30) && !bWifiUnderBMode) {
+		if (
+			(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_BUSY) ||
+			(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY) ||
+			(pCoexDm->btStatus == BT_8723B_1ANT_BT_STATUS_SCO_BUSY)
+		) {
+			if (
+				pCoexSta->nCRCOK_CCK > (
+					pCoexSta->nCRCOK_11g +
+					pCoexSta->nCRCOK_11n +
+					pCoexSta->nCRCOK_11nAgg
+				)
+			) {
+				if (nCCKLockCounter < 5)
+				 nCCKLockCounter++;
+			} else {
+				if (nCCKLockCounter > 0)
+				 nCCKLockCounter--;
+			}
+
+		} else {
+			if (nCCKLockCounter > 0)
+			  nCCKLockCounter--;
+		}
+	} else {
+		if (nCCKLockCounter > 0)
+			nCCKLockCounter--;
+	}
+
+	if (!pCoexSta->bPreCCKLock) {
+
+		if (nCCKLockCounter >= 5)
+		 pCoexSta->bCCKLock = true;
+		else
+		 pCoexSta->bCCKLock = false;
+	} else {
+		if (nCCKLockCounter == 0)
+		 pCoexSta->bCCKLock = false;
+		else
+		 pCoexSta->bCCKLock = true;
+	}
+
+	pCoexSta->bPreCCKLock =  pCoexSta->bCCKLock;
+
+
+}
+
+static bool halbtc8723b1ant_IsWifiStatusChanged(PBTC_COEXIST pBtCoexist)
+{
+	static bool	bPreWifiBusy = false, bPreUnder4way = false, bPreBtHsOn = false;
+	bool bWifiBusy = false, bUnder4way = false, bBtHsOn = false;
+	bool bWifiConnected = false;
+
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected
+	);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &bUnder4way
+	);
+
+	if (bWifiConnected) {
+		if (bWifiBusy != bPreWifiBusy) {
+			bPreWifiBusy = bWifiBusy;
+			return true;
+		}
+
+		if (bUnder4way != bPreUnder4way) {
+			bPreUnder4way = bUnder4way;
+			return true;
+		}
+
+		if (bBtHsOn != bPreBtHsOn) {
+			bPreBtHsOn = bBtHsOn;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static void halbtc8723b1ant_UpdateBtLinkInfo(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bBtHsOn = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+
+	pBtLinkInfo->bBtLinkExist = pCoexSta->bBtLinkExist;
+	pBtLinkInfo->bScoExist = pCoexSta->bScoExist;
+	pBtLinkInfo->bA2dpExist = pCoexSta->bA2dpExist;
+	pBtLinkInfo->bPanExist = pCoexSta->bPanExist;
+	pBtLinkInfo->bHidExist = pCoexSta->bHidExist;
+
+	/*  work around for HS mode. */
+	if (bBtHsOn) {
+		pBtLinkInfo->bPanExist = true;
+		pBtLinkInfo->bBtLinkExist = true;
+	}
+
+	/*  check if Sco only */
+	if (
+		pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bScoOnly = true;
+	else
+		pBtLinkInfo->bScoOnly = false;
+
+	/*  check if A2dp only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bA2dpOnly = true;
+	else
+		pBtLinkInfo->bA2dpOnly = false;
+
+	/*  check if Pan only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bPanOnly = true;
+	else
+		pBtLinkInfo->bPanOnly = false;
+
+	/*  check if Hid only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bHidOnly = true;
+	else
+		pBtLinkInfo->bHidOnly = false;
+}
+
+static u8 halbtc8723b1ant_ActionAlgorithm(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bBtHsOn = false;
+	u8 algorithm = BT_8723B_1ANT_COEX_ALGO_UNDEFINED;
+	u8 numOfDiffProfile = 0;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+
+	if (!pBtLinkInfo->bBtLinkExist) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], No BT link exists!!!\n")
+		);
+		return algorithm;
+	}
+
+	if (pBtLinkInfo->bScoExist)
+		numOfDiffProfile++;
+	if (pBtLinkInfo->bHidExist)
+		numOfDiffProfile++;
+	if (pBtLinkInfo->bPanExist)
+		numOfDiffProfile++;
+	if (pBtLinkInfo->bA2dpExist)
+		numOfDiffProfile++;
+
+	if (numOfDiffProfile == 1) {
+		if (pBtLinkInfo->bScoExist) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE,
+				("[BTCoex], BT Profile = SCO only\n")
+			);
+			algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+		} else {
+			if (pBtLinkInfo->bHidExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = HID only\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = A2DP only\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_A2DP;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = PAN(HS) only\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = PAN(EDR) only\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 2) {
+		if (pBtLinkInfo->bScoExist) {
+			if (pBtLinkInfo->bHidExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = SCO + HID\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = SCO + A2DP ==> SCO\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = SCO + PAN(HS)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = SCO + PAN(EDR)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (pBtLinkInfo->bHidExist && pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = HID + A2DP\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+			} else if (pBtLinkInfo->bHidExist && pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = HID + PAN(HS)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = HID + PAN(EDR)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (pBtLinkInfo->bPanExist && pBtLinkInfo->bA2dpExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = A2DP + PAN(HS)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = A2DP + PAN(EDR)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 3) {
+		if (pBtLinkInfo->bScoExist) {
+			if (pBtLinkInfo->bHidExist && pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = SCO + HID + A2DP ==> HID\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (
+				pBtLinkInfo->bHidExist && pBtLinkInfo->bPanExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT Profile = SCO + HID + PAN(HS)\n"));
+					algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT Profile = SCO + HID + PAN(EDR)\n"));
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (pBtLinkInfo->bPanExist && pBtLinkInfo->bA2dpExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT Profile = SCO + A2DP + PAN(HS)\n"));
+					algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = SCO + A2DP + PAN(EDR) ==> HID\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = HID + A2DP + PAN(HS)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = HID + A2DP + PAN(EDR)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	} else if (numOfDiffProfile >= 3) {
+		if (pBtLinkInfo->bScoExist) {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], Error!!! BT Profile = SCO + HID + A2DP + PAN(HS)\n")
+					);
+
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+static void halbtc8723b1ant_SetSwPenaltyTxRateAdaptive(
+	PBTC_COEXIST pBtCoexist, bool bLowPenaltyRa
+)
+{
+	u8 	H2C_Parameter[6] = {0};
+
+	H2C_Parameter[0] = 0x6;	/*  opCode, 0x6 = Retry_Penalty */
+
+	if (bLowPenaltyRa) {
+		H2C_Parameter[1] |= BIT0;
+		H2C_Parameter[2] = 0x00;  /* normal rate except MCS7/6/5, OFDM54/48/36 */
+		H2C_Parameter[3] = 0xf7;  /* MCS7 or OFDM54 */
+		H2C_Parameter[4] = 0xf8;  /* MCS6 or OFDM48 */
+		H2C_Parameter[5] = 0xf9;	/* MCS5 or OFDM36 */
+	}
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], set WiFi Low-Penalty Retry: %s",
+			(bLowPenaltyRa ? "ON!!" : "OFF!!")
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x69, 6, H2C_Parameter);
+}
+
+static void halbtc8723b1ant_LowPenaltyRa(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bLowPenaltyRa
+)
+{
+	pCoexDm->bCurLowPenaltyRa = bLowPenaltyRa;
+
+	if (!bForceExec) {
+		if (pCoexDm->bPreLowPenaltyRa == pCoexDm->bCurLowPenaltyRa)
+			return;
+	}
+	halbtc8723b1ant_SetSwPenaltyTxRateAdaptive(
+		pBtCoexist, pCoexDm->bCurLowPenaltyRa
+	);
+
+	pCoexDm->bPreLowPenaltyRa = pCoexDm->bCurLowPenaltyRa;
+}
+
+static void halbtc8723b1ant_SetCoexTable(
+	PBTC_COEXIST pBtCoexist,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c0 = 0x%x\n", val0x6c0)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c4 = 0x%x\n", val0x6c4)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c4, val0x6c4);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c8 = 0x%x\n", val0x6c8)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6cc = 0x%x\n", val0x6cc)
+	);
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cc, val0x6cc);
+}
+
+static void halbtc8723b1ant_CoexTable(
+	PBTC_COEXIST pBtCoexist,
+	bool bForceExec,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s write Coex Table 0x6c0 = 0x%x, 0x6c4 = 0x%x, 0x6cc = 0x%x\n",
+			(bForceExec ? "force to" : ""),
+			val0x6c0, val0x6c4, val0x6cc
+		)
+	);
+	pCoexDm->curVal0x6c0 = val0x6c0;
+	pCoexDm->curVal0x6c4 = val0x6c4;
+	pCoexDm->curVal0x6c8 = val0x6c8;
+	pCoexDm->curVal0x6cc = val0x6cc;
+
+	if (!bForceExec) {
+		if (
+			(pCoexDm->preVal0x6c0 == pCoexDm->curVal0x6c0) &&
+		    (pCoexDm->preVal0x6c4 == pCoexDm->curVal0x6c4) &&
+		    (pCoexDm->preVal0x6c8 == pCoexDm->curVal0x6c8) &&
+		    (pCoexDm->preVal0x6cc == pCoexDm->curVal0x6cc)
+		)
+			return;
+	}
+
+	halbtc8723b1ant_SetCoexTable(
+		pBtCoexist, val0x6c0, val0x6c4, val0x6c8, val0x6cc
+	);
+
+	pCoexDm->preVal0x6c0 = pCoexDm->curVal0x6c0;
+	pCoexDm->preVal0x6c4 = pCoexDm->curVal0x6c4;
+	pCoexDm->preVal0x6c8 = pCoexDm->curVal0x6c8;
+	pCoexDm->preVal0x6cc = pCoexDm->curVal0x6cc;
+}
+
+static void halbtc8723b1ant_CoexTableWithType(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 type
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE,
+		("[BTCoex], ********** CoexTable(%d) **********\n", type)
+	);
+
+	pCoexSta->nCoexTableType = type;
+
+	switch (type) {
+	case 0:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x55555555, 0x55555555, 0xffffff, 0x3
+		);
+		break;
+	case 1:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x55555555, 0x5a5a5a5a, 0xffffff, 0x3
+		);
+		break;
+	case 2:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x5a5a5a5a, 0x5a5a5a5a, 0xffffff, 0x3
+		);
+		break;
+	case 3:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0xaaaa5555, 0xaaaa5a5a, 0xffffff, 0x3
+		);
+		break;
+	case 4:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x55555555, 0xaaaa5a5a, 0xffffff, 0x3
+		);
+		break;
+	case 5:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x5a5a5a5a, 0xaaaa5a5a, 0xffffff, 0x3
+		);
+		break;
+	case 6:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0x55555555, 0xaaaaaaaa, 0xffffff, 0x3
+		);
+		break;
+	case 7:
+		halbtc8723b1ant_CoexTable(
+			pBtCoexist, bForceExec, 0xaaaaaaaa, 0xaaaaaaaa, 0xffffff, 0x3
+		);
+		break;
+	default:
+		break;
+	}
+}
+
+static void halbtc8723b1ant_SetFwIgnoreWlanAct(
+	PBTC_COEXIST pBtCoexist, bool bEnable
+)
+{
+	u8 H2C_Parameter[1] = {0};
+
+	if (bEnable)
+		H2C_Parameter[0] |= BIT0; /* function enable */
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
+			H2C_Parameter[0]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x63, 1, H2C_Parameter);
+}
+
+static void halbtc8723b1ant_IgnoreWlanAct(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bEnable
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s turn Ignore WlanAct %s\n",
+			(bForceExec ? "force to" : ""),
+			(bEnable ? "ON" : "OFF")
+		)
+	);
+	pCoexDm->bCurIgnoreWlanAct = bEnable;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n",
+				pCoexDm->bPreIgnoreWlanAct,
+				pCoexDm->bCurIgnoreWlanAct
+			)
+		);
+
+		if (pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
+			return;
+	}
+	halbtc8723b1ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
+
+	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
+}
+
+static void halbtc8723b1ant_SetLpsRpwm(
+	PBTC_COEXIST pBtCoexist, u8 lpsVal, u8 rpwmVal
+)
+{
+	u8 lps = lpsVal;
+	u8 rpwm = rpwmVal;
+
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_U1_LPS_VAL, &lps);
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_U1_RPWM_VAL, &rpwm);
+}
+
+static void halbtc8723b1ant_LpsRpwm(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 lpsVal, u8 rpwmVal
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s set lps/rpwm = 0x%x/0x%x\n",
+			(bForceExec ? "force to" : ""),
+			lpsVal,
+			rpwmVal
+		)
+	);
+	pCoexDm->curLps = lpsVal;
+	pCoexDm->curRpwm = rpwmVal;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], LPS-RxBeaconMode = 0x%x , LPS-RPWM = 0x%x!!\n",
+				pCoexDm->curLps,
+				pCoexDm->curRpwm
+			)
+		);
+
+		if (
+			(pCoexDm->preLps == pCoexDm->curLps) &&
+			(pCoexDm->preRpwm == pCoexDm->curRpwm)
+		) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE_FW_DETAIL,
+				(
+					"[BTCoex], LPS-RPWM_Last = 0x%x , LPS-RPWM_Now = 0x%x!!\n",
+					pCoexDm->preRpwm,
+					pCoexDm->curRpwm
+				)
+			);
+
+			return;
+		}
+	}
+	halbtc8723b1ant_SetLpsRpwm(pBtCoexist, lpsVal, rpwmVal);
+
+	pCoexDm->preLps = pCoexDm->curLps;
+	pCoexDm->preRpwm = pCoexDm->curRpwm;
+}
+
+static void halbtc8723b1ant_SwMechanism(
+	PBTC_COEXIST pBtCoexist, bool bLowPenaltyRA
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_BT_MONITOR,
+		("[BTCoex], SM[LpRA] = %d\n", bLowPenaltyRA)
+	);
+
+	halbtc8723b1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, bLowPenaltyRA);
+}
+
+static void halbtc8723b1ant_SetAntPath(
+	PBTC_COEXIST pBtCoexist, u8 antPosType, bool bInitHwCfg, bool bWifiOff
+)
+{
+	PBTC_BOARD_INFO pBoardInfo = &pBtCoexist->boardInfo;
+	u32 fwVer = 0, u4Tmp = 0, cntBtCalChk = 0;
+	bool bPgExtSwitch = false;
+	bool bUseExtSwitch = false;
+	bool bIsInMpMode = false;
+	u8 H2C_Parameter[2] = {0}, u1Tmp = 0;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_EXT_SWITCH, &bPgExtSwitch);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer); /*  [31:16]=fw ver, [15:0]=fw sub ver */
+
+	if ((fwVer > 0 && fwVer < 0xc0000) || bPgExtSwitch)
+		bUseExtSwitch = true;
+
+	if (bInitHwCfg) {
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x780); /* WiFi TRx Mask on */
+		pBtCoexist->fBtcSetBtReg(pBtCoexist, BTC_BT_REG_RF, 0x3c, 0x15); /* BT TRx Mask on */
+
+		if (fwVer >= 0x180000) {
+			/* Use H2C to set GNT_BT to HIGH */
+			H2C_Parameter[0] = 1;
+			pBtCoexist->fBtcFillH2c(pBtCoexist, 0x6E, 1, H2C_Parameter);
+		} else /*  set grant_bt to high */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x18);
+
+		/* set wlan_act control by PTA */
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0x4);
+
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x1); /* BT select s0/s1 is controlled by WiFi */
+
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x39, 0x8, 0x1);
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x974, 0xff);
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x944, 0x3, 0x3);
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x930, 0x77);
+	} else if (bWifiOff) {
+		if (fwVer >= 0x180000) {
+			/* Use H2C to set GNT_BT to HIGH */
+			H2C_Parameter[0] = 1;
+			pBtCoexist->fBtcFillH2c(pBtCoexist, 0x6E, 1, H2C_Parameter);
+		} else /*  set grant_bt to high */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x18);
+
+		/* set wlan_act to always low */
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0x4);
+
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_IS_IN_MP_MODE, &bIsInMpMode);
+		if (!bIsInMpMode)
+			pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x0); /* BT select s0/s1 is controlled by BT */
+		else
+			pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x1); /* BT select s0/s1 is controlled by WiFi */
+
+		/*  0x4c[24:23]= 00, Set Antenna control by BT_RFE_CTRL	BT Vendor 0xac = 0xf002 */
+		u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+		u4Tmp &= ~BIT23;
+		u4Tmp &= ~BIT24;
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x4c, u4Tmp);
+	} else {
+		/* Use H2C to set GNT_BT to LOW */
+		if (fwVer >= 0x180000) {
+			if (pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x765) != 0) {
+				H2C_Parameter[0] = 0;
+				pBtCoexist->fBtcFillH2c(pBtCoexist, 0x6E, 1, H2C_Parameter);
+			}
+		} else {
+			/*  BT calibration check */
+			while (cntBtCalChk <= 20) {
+				u1Tmp = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x49d);
+				cntBtCalChk++;
+
+				if (u1Tmp & BIT0) {
+					BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ########### BT is calibrating (wait cnt =%d) ###########\n", cntBtCalChk));
+					mdelay(50);
+				} else {
+					BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ********** BT is NOT calibrating (wait cnt =%d)**********\n", cntBtCalChk));
+					break;
+				}
+			}
+
+			/*  set grant_bt to PTA */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x0);
+		}
+
+		if (pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x76e) != 0xc)
+			/* set wlan_act control by PTA */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0xc);
+	}
+
+	if (bUseExtSwitch) {
+		if (bInitHwCfg) {
+			/*  0x4c[23]= 0, 0x4c[24]= 1  Antenna control by WL/BT */
+			u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+			u4Tmp &= ~BIT23;
+			u4Tmp |= BIT24;
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x4c, u4Tmp);
+
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0); /*  fixed internal switch S1->WiFi, S0->BT */
+
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT) {
+				/* tell firmware "no antenna inverse" */
+				H2C_Parameter[0] = 0;
+				H2C_Parameter[1] = 1;  /* ext switch type */
+				pBtCoexist->fBtcFillH2c(pBtCoexist, 0x65, 2, H2C_Parameter);
+			} else {
+				/* tell firmware "antenna inverse" */
+				H2C_Parameter[0] = 1;
+				H2C_Parameter[1] = 1;  /* ext switch type */
+				pBtCoexist->fBtcFillH2c(pBtCoexist, 0x65, 2, H2C_Parameter);
+			}
+		}
+
+
+		/*  ext switch setting */
+		switch (antPosType) {
+		case BTC_ANT_PATH_WIFI:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x1);
+			else
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x2);
+			break;
+		case BTC_ANT_PATH_BT:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x2);
+			else
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x1);
+			break;
+		default:
+		case BTC_ANT_PATH_PTA:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x1);
+			else
+				pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x2);
+			break;
+		}
+
+	} else {
+		if (bInitHwCfg) {
+			/*  0x4c[23]= 1, 0x4c[24]= 0  Antenna control by 0x64 */
+			u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+			u4Tmp |= BIT23;
+			u4Tmp &= ~BIT24;
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x4c, u4Tmp);
+
+			/* Fix Ext switch Main->S1, Aux->S0 */
+			pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x64, 0x1, 0x0);
+
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT) {
+
+				/* tell firmware "no antenna inverse" */
+				H2C_Parameter[0] = 0;
+				H2C_Parameter[1] = 0;  /* internal switch type */
+				pBtCoexist->fBtcFillH2c(pBtCoexist, 0x65, 2, H2C_Parameter);
+			} else {
+
+				/* tell firmware "antenna inverse" */
+				H2C_Parameter[0] = 1;
+				H2C_Parameter[1] = 0;  /* internal switch type */
+				pBtCoexist->fBtcFillH2c(pBtCoexist, 0x65, 2, H2C_Parameter);
+			}
+		}
+
+
+		/*  internal switch setting */
+		switch (antPosType) {
+		case BTC_ANT_PATH_WIFI:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+			else
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x280);
+			break;
+		case BTC_ANT_PATH_BT:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x280);
+			else
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+			break;
+		default:
+		case BTC_ANT_PATH_PTA:
+			if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT)
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x200);
+			else
+				pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x80);
+			break;
+		}
+	}
+}
+
+static void halbtc8723b1ant_SetFwPstdma(
+	PBTC_COEXIST pBtCoexist, u8 byte1, u8 byte2, u8 byte3, u8 byte4, u8 byte5
+)
+{
+	u8 H2C_Parameter[5] = {0};
+	u8 realByte1 = byte1, realByte5 = byte5;
+	bool bApEnable = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE, &bApEnable);
+
+	if (bApEnable) {
+		if (byte1&BIT4 && !(byte1&BIT5)) {
+			BTC_PRINT(
+				BTC_MSG_INTERFACE,
+				INTF_NOTIFY,
+				("[BTCoex], FW for 1Ant AP mode\n")
+			);
+			realByte1 &= ~BIT4;
+			realByte1 |= BIT5;
+
+			realByte5 |= BIT5;
+			realByte5 &= ~BIT6;
+		}
+	}
+
+	H2C_Parameter[0] = realByte1;
+	H2C_Parameter[1] = byte2;
+	H2C_Parameter[2] = byte3;
+	H2C_Parameter[3] = byte4;
+	H2C_Parameter[4] = realByte5;
+
+	pCoexDm->psTdmaPara[0] = realByte1;
+	pCoexDm->psTdmaPara[1] = byte2;
+	pCoexDm->psTdmaPara[2] = byte3;
+	pCoexDm->psTdmaPara[3] = byte4;
+	pCoexDm->psTdmaPara[4] = realByte5;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], PS-TDMA H2C cmd = 0x%x%08x\n",
+			H2C_Parameter[0],
+			H2C_Parameter[1]<<24|
+			H2C_Parameter[2]<<16|
+			H2C_Parameter[3]<<8|
+			H2C_Parameter[4]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x60, 5, H2C_Parameter);
+}
+
+
+static void halbtc8723b1ant_PsTdma(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bTurnOn, u8 type
+)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bWifiBusy = false;
+	u8 rssiAdjustVal = 0;
+	u8 psTdmaByte4Val = 0x50, psTdmaByte0Val = 0x51, psTdmaByte3Val =  0x10;
+	s8 nWiFiDurationAdjust = 0x0;
+	/* u32 		fwVer = 0; */
+
+	/* BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn %s PS TDMA, type =%d\n", */
+	/* 	(bForceExec? "force to":""), (bTurnOn? "ON":"OFF"), type)); */
+	pCoexDm->bCurPsTdmaOn = bTurnOn;
+	pCoexDm->curPsTdma = type;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+
+	if (pCoexDm->bCurPsTdmaOn) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			(
+				"[BTCoex], ********** TDMA(on, %d) **********\n",
+				pCoexDm->curPsTdma
+			)
+		);
+	} else {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			(
+				"[BTCoex], ********** TDMA(off, %d) **********\n",
+				pCoexDm->curPsTdma
+			)
+		);
+	}
+
+	if (!bForceExec) {
+		if (
+			(pCoexDm->bPrePsTdmaOn == pCoexDm->bCurPsTdmaOn) &&
+			(pCoexDm->prePsTdma == pCoexDm->curPsTdma)
+		)
+			return;
+	}
+
+	if (pCoexSta->nScanAPNum <= 5)
+		nWiFiDurationAdjust = 5;
+	else if  (pCoexSta->nScanAPNum >= 40)
+		nWiFiDurationAdjust = -15;
+	else if  (pCoexSta->nScanAPNum >= 20)
+		nWiFiDurationAdjust = -10;
+
+	if (!pCoexSta->bForceLpsOn) { /* only for A2DP-only case 1/2/9/11 */
+		psTdmaByte0Val = 0x61;  /* no null-pkt */
+		psTdmaByte3Val = 0x11; /*  no tx-pause at BT-slot */
+		psTdmaByte4Val = 0x10; /*  0x778 = d/1 toggle */
+	}
+
+
+	if (bTurnOn) {
+		if (pBtLinkInfo->bSlaveRole == true)
+			psTdmaByte4Val = psTdmaByte4Val | 0x1;  /* 0x778 = 0x1 at wifi slot (no blocking BT Low-Pri pkts) */
+
+
+		switch (type) {
+		default:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x1a, 0x1a, 0x0, psTdmaByte4Val
+			);
+			break;
+		case 1:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist,
+				psTdmaByte0Val,
+				0x3a+nWiFiDurationAdjust,
+				0x03,
+				psTdmaByte3Val,
+				psTdmaByte4Val
+			);
+			break;
+		case 2:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist,
+				psTdmaByte0Val,
+				0x2d+nWiFiDurationAdjust,
+				0x03,
+				psTdmaByte3Val,
+				psTdmaByte4Val
+			);
+			break;
+		case 3:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x1d, 0x1d, 0x0, 0x10
+			);
+			break;
+		case 4:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x93, 0x15, 0x3, 0x14, 0x0
+			);
+			break;
+		case 5:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x15, 0x3, 0x11, 0x10
+			);
+			break;
+		case 6:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x20, 0x3, 0x11, 0x11
+			);
+			break;
+		case 7:
+			halbtc8723b1ant_SetFwPstdma(pBtCoexist, 0x13, 0xc, 0x5, 0x0, 0x0);
+			break;
+		case 8:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x93, 0x25, 0x3, 0x10, 0x0
+			);
+			break;
+		case 9:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist,
+				psTdmaByte0Val,
+				0x21,
+				0x3,
+				psTdmaByte3Val,
+				psTdmaByte4Val
+			);
+			break;
+		case 10:
+			halbtc8723b1ant_SetFwPstdma(pBtCoexist, 0x13, 0xa, 0xa, 0x0, 0x40);
+			break;
+		case 11:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist,
+				psTdmaByte0Val,
+				0x21,
+				0x03,
+				psTdmaByte3Val,
+				psTdmaByte4Val
+			);
+			break;
+		case 12:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x0a, 0x0a, 0x0, 0x50
+			);
+			break;
+		case 13:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x12, 0x12, 0x0, 0x10
+			);
+			break;
+		case 14:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x21, 0x3, 0x10, psTdmaByte4Val
+			);
+			break;
+		case 15:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x13, 0xa, 0x3, 0x8, 0x0
+			);
+			break;
+		case 16:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x93, 0x15, 0x3, 0x10, 0x0
+			);
+			break;
+		case 18:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x93, 0x25, 0x3, 0x10, 0x0
+			);
+			break;
+		case 20:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x3f, 0x03, 0x11, 0x10
+
+			);
+			break;
+		case 21:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x25, 0x03, 0x11, 0x11
+			);
+			break;
+		case 22:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x25, 0x03, 0x11, 0x10
+			);
+			break;
+		case 23:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xe3, 0x25, 0x3, 0x31, 0x18
+			);
+			break;
+		case 24:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xe3, 0x15, 0x3, 0x31, 0x18
+			);
+			break;
+		case 25:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xe3, 0xa, 0x3, 0x31, 0x18
+			);
+			break;
+		case 26:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xe3, 0xa, 0x3, 0x31, 0x18
+			);
+			break;
+		case 27:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xe3, 0x25, 0x3, 0x31, 0x98
+			);
+			break;
+		case 28:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x69, 0x25, 0x3, 0x31, 0x0
+			);
+			break;
+		case 29:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xab, 0x1a, 0x1a, 0x1, 0x10
+			);
+			break;
+		case 30:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x51, 0x30, 0x3, 0x10, 0x10
+			);
+			break;
+		case 31:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xd3, 0x1a, 0x1a, 0x0, 0x58
+			);
+			break;
+		case 32:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x61, 0x35, 0x3, 0x11, 0x11
+			);
+			break;
+		case 33:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xa3, 0x25, 0x3, 0x30, 0x90
+			);
+			break;
+		case 34:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x53, 0x1a, 0x1a, 0x0, 0x10
+			);
+			break;
+		case 35:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x63, 0x1a, 0x1a, 0x0, 0x10
+			);
+			break;
+		case 36:
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0xd3, 0x12, 0x3, 0x14, 0x50
+			);
+			break;
+		case 40: /*  SoftAP only with no sta associated, BT disable , TDMA mode for power saving */
+			/* here softap mode screen off will cost 70-80mA for phone */
+			halbtc8723b1ant_SetFwPstdma(
+				pBtCoexist, 0x23, 0x18, 0x00, 0x10, 0x24
+			);
+			break;
+		}
+	} else {
+
+		/*  disable PS tdma */
+		switch (type) {
+		case 8: /* PTA Control */
+			halbtc8723b1ant_SetFwPstdma(pBtCoexist, 0x8, 0x0, 0x0, 0x0, 0x0);
+			halbtc8723b1ant_SetAntPath(
+				pBtCoexist, BTC_ANT_PATH_PTA, false, false
+			);
+			break;
+		case 0:
+		default:  /* Software control, Antenna at BT side */
+			halbtc8723b1ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
+			halbtc8723b1ant_SetAntPath(
+				pBtCoexist, BTC_ANT_PATH_BT, false, false
+			);
+			break;
+		case 9:   /* Software control, Antenna at WiFi side */
+			halbtc8723b1ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
+			halbtc8723b1ant_SetAntPath(
+				pBtCoexist, BTC_ANT_PATH_WIFI, false, false
+			);
+			break;
+		}
+	}
+
+	rssiAdjustVal = 0;
+	pBtCoexist->fBtcSet(
+		pBtCoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE, &rssiAdjustVal
+	);
+
+	/*  update pre state */
+	pCoexDm->bPrePsTdmaOn = pCoexDm->bCurPsTdmaOn;
+	pCoexDm->prePsTdma = pCoexDm->curPsTdma;
+}
+
+static bool halbtc8723b1ant_IsCommonAction(PBTC_COEXIST pBtCoexist)
+{
+	bool bCommon = false, bWifiConnected = false, bWifiBusy = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+
+	if (
+		!bWifiConnected &&
+		BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus
+	) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], Wifi non connected-idle + BT non connected-idle!!\n")
+		);
+
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		bWifiConnected &&
+		(BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus)
+	) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], Wifi connected + BT non connected-idle!!\n")
+		);
+
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		!bWifiConnected &&
+		(BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)
+	) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], Wifi non connected-idle + BT connected-idle!!\n")
+		);
+
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		bWifiConnected &&
+		(BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)
+	) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT connected-idle!!\n"));
+
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		!bWifiConnected &&
+		(BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE != pCoexDm->btStatus)
+	) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], Wifi non connected-idle + BT Busy!!\n")
+		);
+
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else {
+		if (bWifiBusy) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE,
+				("[BTCoex], Wifi Connected-Busy + BT Busy!!\n")
+			);
+		} else {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE,
+				("[BTCoex], Wifi Connected-Idle + BT Busy!!\n")
+			);
+		}
+
+		bCommon = false;
+	}
+
+	return bCommon;
+}
+
+
+static void halbtc8723b1ant_TdmaDurationAdjustForAcl(
+	PBTC_COEXIST pBtCoexist, u8 wifiStatus
+)
+{
+	static s32 up, dn, m, n, WaitCount;
+	s32 result;   /* 0: no change, +1: increase WiFi duration, -1: decrease WiFi duration */
+	u8 retryCount = 0, btInfoExt;
+	bool bWifiBusy = false;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		("[BTCoex], TdmaDurationAdjustForAcl()\n")
+	);
+
+	if (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY == wifiStatus)
+		bWifiBusy = true;
+	else
+		bWifiBusy = false;
+
+	if (
+		(BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN == wifiStatus) ||
+		(BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN == wifiStatus) ||
+		(BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT == wifiStatus)
+	) {
+		if (
+			pCoexDm->curPsTdma != 1 &&
+			pCoexDm->curPsTdma != 2 &&
+			pCoexDm->curPsTdma != 3 &&
+			pCoexDm->curPsTdma != 9
+		) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+			pCoexDm->psTdmaDuAdjType = 9;
+
+			up = 0;
+			dn = 0;
+			m = 1;
+			n = 3;
+			result = 0;
+			WaitCount = 0;
+		}
+		return;
+	}
+
+	if (!pCoexDm->bAutoTdmaAdjust) {
+		pCoexDm->bAutoTdmaAdjust = true;
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			("[BTCoex], first run TdmaDurationAdjust()!!\n")
+		);
+
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+		pCoexDm->psTdmaDuAdjType = 2;
+		/*  */
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		WaitCount = 0;
+	} else {
+		/* accquire the BT TRx retry count from BT_Info byte2 */
+		retryCount = pCoexSta->btRetryCnt;
+		btInfoExt = pCoexSta->btInfoExt;
+		/* BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], retryCount = %d\n", retryCount)); */
+		/* BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], up =%d, dn =%d, m =%d, n =%d, WaitCount =%d\n", */
+		/* 	up, dn, m, n, WaitCount)); */
+
+		if (pCoexSta->lowPriorityTx > 1050 || pCoexSta->lowPriorityRx > 1250)
+			retryCount++;
+
+		result = 0;
+		WaitCount++;
+
+		if (retryCount == 0) { /*  no retry in the last 2-second duration */
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;
+
+			if (up >= n) { /*  if 連續 n 個2秒 retry count為0, 則調寬WiFi duration */
+				WaitCount = 0;
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE_FW_DETAIL,
+					("[BTCoex], Increase wifi duration!!\n")
+				);
+			}
+		} else if (retryCount <= 3) { /*  <=3 retry in the last 2-second duration */
+			up--;
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2) { /*  if 連續 2 個2秒 retry count< 3, 則調窄WiFi duration */
+				if (WaitCount <= 2)
+					m++; /*  避免一直在兩個level中來回 */
+				else
+					m = 1;
+
+				if (m >= 20) /* m 最大值 = 20 ' 最大120秒 recheck是否調整 WiFi duration. */
+					m = 20;
+
+				n = 3*m;
+				up = 0;
+				dn = 0;
+				WaitCount = 0;
+				result = -1;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
+			}
+		} else { /* retry count > 3, 只要1次 retry count > 3, 則調窄WiFi duration */
+			if (WaitCount == 1)
+				m++; /*  避免一直在兩個level中來回 */
+			else
+				m = 1;
+
+			if (m >= 20) /* m 最大值 = 20 ' 最大120秒 recheck是否調整 WiFi duration. */
+				m = 20;
+
+			n = 3*m;
+			up = 0;
+			dn = 0;
+			WaitCount = 0;
+			result = -1;
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE_FW_DETAIL,
+				("[BTCoex], Decrease wifi duration for retryCounter>3!!\n")
+			);
+		}
+
+		if (result == -1) {
+			if (
+				BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(btInfoExt) &&
+				((pCoexDm->curPsTdma == 1) || (pCoexDm->curPsTdma == 2))
+			) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			} else if (pCoexDm->curPsTdma == 1) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+				pCoexDm->psTdmaDuAdjType = 2;
+			} else if (pCoexDm->curPsTdma == 2) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			} else if (pCoexDm->curPsTdma == 9) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+				pCoexDm->psTdmaDuAdjType = 11;
+			}
+		} else if (result == 1) {
+			if (
+				BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(btInfoExt) &&
+				((pCoexDm->curPsTdma == 1) || (pCoexDm->curPsTdma == 2))
+			) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			} else if (pCoexDm->curPsTdma == 11) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+				pCoexDm->psTdmaDuAdjType = 9;
+			} else if (pCoexDm->curPsTdma == 9) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+				pCoexDm->psTdmaDuAdjType = 2;
+			} else if (pCoexDm->curPsTdma == 2) {
+				halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+				pCoexDm->psTdmaDuAdjType = 1;
+			}
+		} else {	  /* no change */
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE_FW_DETAIL,
+				(
+					"[BTCoex], ********** TDMA(on, %d) **********\n",
+					pCoexDm->curPsTdma
+				)
+			);
+		}
+
+		if (
+			pCoexDm->curPsTdma != 1 &&
+			pCoexDm->curPsTdma != 2 &&
+			pCoexDm->curPsTdma != 9 &&
+			pCoexDm->curPsTdma != 11
+		) /*  recover to previous adjust type */
+			halbtc8723b1ant_PsTdma(
+				pBtCoexist, NORMAL_EXEC, true, pCoexDm->psTdmaDuAdjType
+			);
+	}
+}
+
+static void halbtc8723b1ant_PsTdmaCheckForPowerSaveState(
+	PBTC_COEXIST pBtCoexist, bool bNewPsState
+)
+{
+	u8 lpsMode = 0x0;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_LPS_MODE, &lpsMode);
+
+	if (lpsMode) {	/*  already under LPS state */
+		if (bNewPsState) {
+			/*  keep state under LPS, do nothing. */
+		} else /*  will leave LPS state, turn off psTdma first */
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 0);
+	} else {						/*  NO PS state */
+		if (bNewPsState) /*  will enter LPS state, turn off psTdma first */
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 0);
+		else {
+			/*  keep state under NO PS state, do nothing. */
+		}
+	}
+}
+
+static void halbtc8723b1ant_PowerSaveState(
+	PBTC_COEXIST pBtCoexist, u8 psType, u8 lpsVal, u8 rpwmVal
+)
+{
+	bool bLowPwrDisable = false;
+
+	switch (psType) {
+	case BTC_PS_WIFI_NATIVE:
+		/*  recover to original 32k low power setting */
+		bLowPwrDisable = false;
+		pBtCoexist->fBtcSet(
+			pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable
+		);
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
+		pCoexSta->bForceLpsOn = false;
+		break;
+	case BTC_PS_LPS_ON:
+		halbtc8723b1ant_PsTdmaCheckForPowerSaveState(pBtCoexist, true);
+		halbtc8723b1ant_LpsRpwm(pBtCoexist, NORMAL_EXEC, lpsVal, rpwmVal);
+		/*  when coex force to enter LPS, do not enter 32k low power. */
+		bLowPwrDisable = true;
+		pBtCoexist->fBtcSet(
+			pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable
+		);
+		/*  power save must executed before psTdma. */
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+		pCoexSta->bForceLpsOn = true;
+		break;
+	case BTC_PS_LPS_OFF:
+		halbtc8723b1ant_PsTdmaCheckForPowerSaveState(pBtCoexist, false);
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+		pCoexSta->bForceLpsOn = false;
+		break;
+	default:
+		break;
+	}
+}
+
+/*  */
+/*  */
+/* 	Software Coex Mechanism start */
+/*  */
+/*  */
+
+/*  */
+/*  */
+/* 	Non-Software Coex Mechanism start */
+/*  */
+/*  */
+static void halbtc8723b1ant_ActionWifiMultiPort(PBTC_COEXIST pBtCoexist)
+{
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+	halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+}
+
+static void halbtc8723b1ant_ActionHs(PBTC_COEXIST pBtCoexist)
+{
+	halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+	halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+}
+
+static void halbtc8723b1ant_ActionBtInquiry(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bWifiConnected = false;
+	bool bApEnable = false;
+	bool bWifiBusy = false;
+	bool bBtBusy = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE, &bApEnable);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
+
+	if (!bWifiConnected && !pCoexSta->bWiFiIsHighPriTask) {
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+	} else if (
+		pBtLinkInfo->bScoExist ||
+		pBtLinkInfo->bHidExist ||
+		pBtLinkInfo->bA2dpExist
+	) {
+		/*  SCO/HID/A2DP busy */
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else if (pBtLinkInfo->bPanExist || bWifiBusy) {
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 20);
+
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else {
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+	}
+}
+
+static void halbtc8723b1ant_ActionBtScoHidOnlyBusy(
+	PBTC_COEXIST pBtCoexist, u8 wifiStatus
+)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bWifiConnected = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+	/*  tdma and coex table */
+
+	if (pBtLinkInfo->bScoExist) {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 5);
+	} else { /* HID */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 5);
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiConnectedBtAclBusy(
+	PBTC_COEXIST pBtCoexist, u8 wifiStatus
+)
+{
+	u8 btRssiState;
+
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	btRssiState = halbtc8723b1ant_BtRssiState(2, 28, 0);
+
+	if ((pCoexSta->lowPriorityRx >= 1000) && (pCoexSta->lowPriorityRx != 65535))
+		pBtLinkInfo->bSlaveRole = true;
+	else
+		pBtLinkInfo->bSlaveRole = false;
+
+	if (pBtLinkInfo->bHidOnly) { /* HID */
+		halbtc8723b1ant_ActionBtScoHidOnlyBusy(pBtCoexist, wifiStatus);
+		pCoexDm->bAutoTdmaAdjust = false;
+		return;
+	} else if (pBtLinkInfo->bA2dpOnly) { /* A2DP */
+		if (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE == wifiStatus) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+			pCoexDm->bAutoTdmaAdjust = false;
+		} else {
+			halbtc8723b1ant_TdmaDurationAdjustForAcl(pBtCoexist, wifiStatus);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+			pCoexDm->bAutoTdmaAdjust = true;
+		}
+	} else if (pBtLinkInfo->bHidExist && pBtLinkInfo->bA2dpExist) { /* HID+A2DP */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+		pCoexDm->bAutoTdmaAdjust = false;
+
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else if (
+		pBtLinkInfo->bPanOnly ||
+		(pBtLinkInfo->bHidExist && pBtLinkInfo->bPanExist)
+	) { /* PAN(OPP, FTP), HID+PAN(OPP, FTP) */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		pCoexDm->bAutoTdmaAdjust = false;
+	} else if (
+		(pBtLinkInfo->bA2dpExist && pBtLinkInfo->bPanExist) ||
+		(pBtLinkInfo->bHidExist && pBtLinkInfo->bA2dpExist && pBtLinkInfo->bPanExist)
+	) { /* A2DP+PAN(OPP, FTP), HID+A2DP+PAN(OPP, FTP) */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		pCoexDm->bAutoTdmaAdjust = false;
+	} else {
+		/* BT no-profile busy (0x9) */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		pCoexDm->bAutoTdmaAdjust = false;
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiNotConnected(PBTC_COEXIST pBtCoexist)
+{
+	/*  power save state */
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	halbtc8723b1ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 8);
+	halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+}
+
+static void halbtc8723b1ant_ActionWifiNotConnectedScan(
+	PBTC_COEXIST pBtCoexist
+)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) {
+		if (pBtLinkInfo->bA2dpExist) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		} else if (pBtLinkInfo->bA2dpExist && pBtLinkInfo->bPanExist) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 22);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		} else {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 20);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		}
+	} else if (
+		(BT_8723B_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+	) {
+		halbtc8723b1ant_ActionBtScoHidOnlyBusy(
+			pBtCoexist, BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN
+		);
+	} else {
+		/* Bryant Add */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiNotConnectedAssoAuth(
+	PBTC_COEXIST pBtCoexist
+)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (
+		(pBtLinkInfo->bScoExist) ||
+		(pBtLinkInfo->bHidExist) ||
+		(pBtLinkInfo->bA2dpExist)
+	) {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else if (pBtLinkInfo->bPanExist) {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 20);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiConnectedScan(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) {
+		if (pBtLinkInfo->bA2dpExist) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		} else if (pBtLinkInfo->bA2dpExist && pBtLinkInfo->bPanExist) {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 22);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		} else {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 20);
+			halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+		}
+	} else if (
+		(BT_8723B_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+	) {
+		halbtc8723b1ant_ActionBtScoHidOnlyBusy(
+			pBtCoexist, BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN
+		);
+	} else {
+		/* Bryant Add */
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiConnectedSpecialPacket(
+	PBTC_COEXIST pBtCoexist
+)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (
+		(pBtLinkInfo->bScoExist) ||
+		(pBtLinkInfo->bHidExist) ||
+		(pBtLinkInfo->bA2dpExist)
+	) {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else if (pBtLinkInfo->bPanExist) {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 20);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 4);
+	} else {
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void halbtc8723b1ant_ActionWifiConnected(PBTC_COEXIST pBtCoexist)
+{
+	bool bWifiBusy = false;
+	bool bScan = false, bLink = false, bRoam = false;
+	bool bUnder4way = false, bApEnable = false;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE,
+		("[BTCoex], CoexForWifiConnect() ===>\n")
+	);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &bUnder4way);
+	if (bUnder4way) {
+		halbtc8723b1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], CoexForWifiConnect(), return for wifi is under 4way<===\n")
+		);
+		return;
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+	if (bScan || bLink || bRoam) {
+		if (bScan)
+			halbtc8723b1ant_ActionWifiConnectedScan(pBtCoexist);
+		else
+			halbtc8723b1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n")
+		);
+		return;
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE, &bApEnable);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+
+	/*  power save state */
+	if (
+		!bApEnable &&
+		BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus &&
+		!pBtCoexist->btLinkInfo.bHidOnly
+	) {
+		if (pBtCoexist->btLinkInfo.bA2dpOnly) { /* A2DP */
+			if (!bWifiBusy)
+				halbtc8723b1ant_PowerSaveState(
+					pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0
+				);
+			else { /* busy */
+				if  (pCoexSta->nScanAPNum >= BT_8723B_1ANT_WIFI_NOISY_THRESH)  /* no force LPS, no PS-TDMA, use pure TDMA */
+					halbtc8723b1ant_PowerSaveState(
+						pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0
+					);
+				else
+					halbtc8723b1ant_PowerSaveState(
+						pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x4
+					);
+			}
+		} else if (
+			(pCoexSta->bPanExist == false) &&
+			(pCoexSta->bA2dpExist == false) &&
+			(pCoexSta->bHidExist == false)
+		)
+			halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		else
+			halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x4);
+	} else
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (!bWifiBusy) {
+		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) {
+			halbtc8723b1ant_ActionWifiConnectedBtAclBusy(
+				pBtCoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE
+			);
+		} else if (
+			(BT_8723B_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+			(BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+		) {
+			halbtc8723b1ant_ActionBtScoHidOnlyBusy(pBtCoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
+		} else {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+
+			if ((pCoexSta->highPriorityTx) + (pCoexSta->highPriorityRx) <= 60)
+				halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+			else
+				halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+		}
+	} else {
+		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) {
+			halbtc8723b1ant_ActionWifiConnectedBtAclBusy(
+				pBtCoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY
+			);
+		} else if (
+			(BT_8723B_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+			(BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+		) {
+			halbtc8723b1ant_ActionBtScoHidOnlyBusy(
+				pBtCoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY
+			);
+		} else {
+			halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 8);
+
+			if ((pCoexSta->highPriorityTx) + (pCoexSta->highPriorityRx) <= 60)
+				halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+			else
+				halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+		}
+	}
+}
+
+static void halbtc8723b1ant_RunSwCoexistMechanism(PBTC_COEXIST pBtCoexist)
+{
+	u8 algorithm = 0;
+
+	algorithm = halbtc8723b1ant_ActionAlgorithm(pBtCoexist);
+	pCoexDm->curAlgorithm = algorithm;
+
+	if (halbtc8723b1ant_IsCommonAction(pBtCoexist)) {
+
+	} else {
+		switch (pCoexDm->curAlgorithm) {
+		case BT_8723B_1ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = SCO.\n"));
+			/* halbtc8723b1ant_ActionSco(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID.\n"));
+			/* halbtc8723b1ant_ActionHid(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP.\n"));
+			/* halbtc8723b1ant_ActionA2dp(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP+PAN(HS).\n"));
+			/* halbtc8723b1ant_ActionA2dpPanHs(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR).\n"));
+			/* halbtc8723b1ant_ActionPanEdr(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HS mode.\n"));
+			/* halbtc8723b1ant_ActionPanHs(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN+A2DP.\n"));
+			/* halbtc8723b1ant_ActionPanEdrA2dp(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR)+HID.\n"));
+			/* halbtc8723b1ant_ActionPanEdrHid(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP+PAN.\n"));
+			/* halbtc8723b1ant_ActionHidA2dpPanEdr(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP.\n"));
+			/* halbtc8723b1ant_ActionHidA2dp(pBtCoexist); */
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = coexist All Off!!\n"));
+			break;
+		}
+		pCoexDm->preAlgorithm = pCoexDm->curAlgorithm;
+	}
+}
+
+static void halbtc8723b1ant_RunCoexistMechanism(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bWifiConnected = false, bBtHsOn = false;
+	bool bIncreaseScanDevNum = false;
+	bool bBtCtrlAggBufSize = false;
+	u8 aggBufSize = 5;
+	u32 wifiLinkStatus = 0;
+	u32 numOfWifiLink = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism() ===>\n"));
+
+	if (pBtCoexist->bManualControl) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n"));
+		return;
+	}
+
+	if (pBtCoexist->bStopCoexDm) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Stop Coex DM <===\n"));
+		return;
+	}
+
+	if (pCoexSta->bUnderIps) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is under IPS !!!\n"));
+		return;
+	}
+
+	if (
+		(BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+	){
+		bIncreaseScanDevNum = true;
+	}
+
+	pBtCoexist->fBtcSet(
+		pBtCoexist,
+		BTC_SET_BL_INC_SCAN_DEV_NUM,
+		&bIncreaseScanDevNum
+	);
+	pBtCoexist->fBtcGet(
+		pBtCoexist,
+		BTC_GET_BL_WIFI_CONNECTED,
+		&bWifiConnected
+	);
+
+	pBtCoexist->fBtcGet(
+		pBtCoexist,
+		BTC_GET_U4_WIFI_LINK_STATUS,
+		&wifiLinkStatus
+	);
+	numOfWifiLink = wifiLinkStatus>>16;
+
+	if ((numOfWifiLink >= 2) || (wifiLinkStatus&WIFI_P2P_GO_CONNECTED)) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE,
+			INTF_NOTIFY,
+			(
+				"############# [BTCoex],  Multi-Port numOfWifiLink = %d, wifiLinkStatus = 0x%x\n",
+				numOfWifiLink,
+				wifiLinkStatus
+			)
+		);
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, bBtCtrlAggBufSize, aggBufSize);
+
+		if ((pBtLinkInfo->bA2dpExist) && (pCoexSta->bC2hBtInquiryPage)) {
+			BTC_PRINT(
+				BTC_MSG_INTERFACE,
+				INTF_NOTIFY,
+				("############# [BTCoex],  BT Is Inquirying\n")
+			);
+			halbtc8723b1ant_ActionBtInquiry(pBtCoexist);
+		} else
+			halbtc8723b1ant_ActionWifiMultiPort(pBtCoexist);
+
+		return;
+	}
+
+	if ((pBtLinkInfo->bBtLinkExist) && (bWifiConnected)) {
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 1, 1, 0, 1);
+
+		if (pBtLinkInfo->bScoExist)
+			halbtc8723b1ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, true, 0x5);
+		else
+			halbtc8723b1ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, true, 0x8);
+
+		halbtc8723b1ant_SwMechanism(pBtCoexist, true);
+		halbtc8723b1ant_RunSwCoexistMechanism(pBtCoexist);  /* just print debug message */
+	} else {
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 0, 0, 0, 0);
+
+		halbtc8723b1ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x5);
+
+		halbtc8723b1ant_SwMechanism(pBtCoexist, false);
+		halbtc8723b1ant_RunSwCoexistMechanism(pBtCoexist); /* just print debug message */
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	if (pCoexSta->bC2hBtInquiryPage) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE,
+			INTF_NOTIFY,
+			("############# [BTCoex],  BT Is Inquirying\n")
+		);
+		halbtc8723b1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else if (bBtHsOn) {
+		halbtc8723b1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+
+	if (!bWifiConnected) {
+		bool bScan = false, bLink = false, bRoam = false;
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is non connected-idle !!!\n"));
+
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+
+		if (bScan || bLink || bRoam) {
+			 if (bScan)
+				halbtc8723b1ant_ActionWifiNotConnectedScan(pBtCoexist);
+			 else
+				halbtc8723b1ant_ActionWifiNotConnectedAssoAuth(pBtCoexist);
+		} else
+			halbtc8723b1ant_ActionWifiNotConnected(pBtCoexist);
+	} else /*  wifi LPS/Busy */
+		halbtc8723b1ant_ActionWifiConnected(pBtCoexist);
+}
+
+static void halbtc8723b1ant_InitCoexDm(PBTC_COEXIST pBtCoexist)
+{
+	/*  force to reset coex mechanism */
+
+	/*  sw all off */
+	halbtc8723b1ant_SwMechanism(pBtCoexist, false);
+
+	/* halbtc8723b1ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 8); */
+	halbtc8723b1ant_CoexTableWithType(pBtCoexist, FORCE_EXEC, 0);
+
+	pCoexSta->popEventCnt = 0;
+}
+
+static void halbtc8723b1ant_InitHwConfig(
+	PBTC_COEXIST pBtCoexist,
+	bool bBackUp,
+	bool bWifiOnly
+)
+{
+	u32 u4Tmp = 0;/*  fwVer; */
+	u8 u1Tmpa = 0, u1Tmpb = 0;
+
+	BTC_PRINT(
+		BTC_MSG_INTERFACE,
+		INTF_INIT,
+		("[BTCoex], 1Ant Init HW Config!!\n")
+	);
+
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x550, 0x8, 0x1);  /* enable TBTT nterrupt */
+
+	/*  0x790[5:0]= 0x5 */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x790, 0x5);
+
+	/*  Enable counter statistics */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x778, 0x1);
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x40, 0x20, 0x1);
+
+	/* Antenna config */
+	if (bWifiOnly) {
+		halbtc8723b1ant_SetAntPath(pBtCoexist, BTC_ANT_PATH_WIFI, true, false);
+		halbtc8723b1ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 9);
+	} else
+		halbtc8723b1ant_SetAntPath(pBtCoexist, BTC_ANT_PATH_BT, true, false);
+
+	/*  PTA parameter */
+	halbtc8723b1ant_CoexTableWithType(pBtCoexist, FORCE_EXEC, 0);
+
+	u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+	u1Tmpa = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x765);
+	u1Tmpb = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x67);
+
+	BTC_PRINT(
+		BTC_MSG_INTERFACE,
+		INTF_NOTIFY,
+		(
+			"############# [BTCoex], 0x948 = 0x%x, 0x765 = 0x%x, 0x67 = 0x%x\n",
+			u4Tmp,
+			u1Tmpa,
+			u1Tmpb
+		)
+	);
+}
+
+/*  */
+/*  work around function start with wa_halbtc8723b1ant_ */
+/*  */
+/*  */
+/*  extern function start with EXhalbtc8723b1ant_ */
+/*  */
+void EXhalbtc8723b1ant_PowerOnSetting(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BOARD_INFO pBoardInfo = &pBtCoexist->boardInfo;
+	u8 u1Tmp = 0x0;
+	u16 u2Tmp = 0x0;
+
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x67, 0x20);
+
+	/*  enable BB, REG_SYS_FUNC_EN such that we can write 0x948 correctly. */
+	u2Tmp = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0x2);
+	pBtCoexist->fBtcWrite2Byte(pBtCoexist, 0x2, u2Tmp|BIT0|BIT1);
+
+	/*  set GRAN_BT = 1 */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x18);
+	/*  set WLAN_ACT = 0 */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0x4);
+
+	/*  */
+	/*  S0 or S1 setting and Local register setting(By the setting fw can get ant number, S0/S1, ... info) */
+	/*  Local setting bit define */
+	/* 	BIT0: "0" for no antenna inverse; "1" for antenna inverse */
+	/* 	BIT1: "0" for internal switch; "1" for external switch */
+	/* 	BIT2: "0" for one antenna; "1" for two antenna */
+	/*  NOTE: here default all internal switch and 1-antenna ==> BIT1 = 0 and BIT2 = 0 */
+	if (pBtCoexist->chipInterface == BTC_INTF_USB) {
+		/*  fixed at S0 for USB interface */
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+
+		u1Tmp |= 0x1;	/*  antenna inverse */
+		pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0xfe08, u1Tmp);
+
+		pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_AUX_PORT;
+	} else {
+		/*  for PCIE and SDIO interface, we check efuse 0xc3[6] */
+		if (pBoardInfo->singleAntPath == 0) {
+			/*  set to S1 */
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x280);
+			pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_MAIN_PORT;
+		} else if (pBoardInfo->singleAntPath == 1) {
+			/*  set to S0 */
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+			u1Tmp |= 0x1;	/*  antenna inverse */
+			pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_AUX_PORT;
+		}
+
+		if (pBtCoexist->chipInterface == BTC_INTF_PCI)
+			pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0x384, u1Tmp);
+		else if (pBtCoexist->chipInterface == BTC_INTF_SDIO)
+			pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0x60, u1Tmp);
+	}
+}
+
+void EXhalbtc8723b1ant_InitHwConfig(PBTC_COEXIST pBtCoexist, bool bWifiOnly)
+{
+	halbtc8723b1ant_InitHwConfig(pBtCoexist, true, bWifiOnly);
+}
+
+void EXhalbtc8723b1ant_InitCoexDm(PBTC_COEXIST pBtCoexist)
+{
+	BTC_PRINT(
+		BTC_MSG_INTERFACE,
+		INTF_INIT,
+		("[BTCoex], Coex Mechanism Init!!\n")
+	);
+
+	pBtCoexist->bStopCoexDm = false;
+
+	halbtc8723b1ant_InitCoexDm(pBtCoexist);
+
+	halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+}
+
+void EXhalbtc8723b1ant_DisplayCoexInfo(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BOARD_INFO pBoardInfo = &pBtCoexist->boardInfo;
+	PBTC_STACK_INFO pStackInfo = &pBtCoexist->stackInfo;
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	u8 *cliBuf = pBtCoexist->cliBuf;
+	u8 u1Tmp[4], i, btInfoExt, psTdmaCase = 0;
+	u16 u2Tmp[4];
+	u32 u4Tmp[4];
+	bool bRoam = false;
+	bool bScan = false;
+	bool bLink = false;
+	bool bWifiUnder5G = false;
+	bool bWifiUnderBMode = false;
+	bool bBtHsOn = false;
+	bool bWifiBusy = false;
+	s32 wifiRssi = 0, btHsRssi = 0;
+	u32 wifiBw, wifiTrafficDir, faOfdm, faCck, wifiLinkStatus;
+	u8 wifiDot11Chnl, wifiHsChnl;
+	u32 fwVer = 0, btPatchVer = 0;
+	static u8 PopReportIn10s = 0;
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n ============[BT Coexist info]============"
+	);
+	CL_PRINTF(cliBuf);
+
+	if (pBtCoexist->bManualControl) {
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n ============[Under Manual Control]============"
+		);
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n =========================================="
+		);
+		CL_PRINTF(cliBuf);
+	}
+	if (pBtCoexist->bStopCoexDm) {
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n ============[Coex is STOPPED]============"
+		);
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n =========================================="
+		);
+		CL_PRINTF(cliBuf);
+	}
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d", "Ant PG Num/ Ant Mech/ Ant Pos:", \
+		pBoardInfo->pgAntNum,
+		pBoardInfo->btdmAntNum,
+		pBoardInfo->btdmAntPos
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
+		((pStackInfo->bProfileNotified) ? "Yes" : "No"),
+		pStackInfo->hciVersion
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)", "CoexVer/ FwVer/ PatchVer", \
+		GLCoexVerDate8723b1Ant,
+		GLCoexVer8723b1Ant,
+		fwVer,
+		btPatchVer,
+		btPatchVer
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
+		wifiDot11Chnl,
+		wifiHsChnl,
+		bBtHsOn
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
+		pCoexDm->wifiChnlInfo[0],
+		pCoexDm->wifiChnlInfo[1],
+		pCoexDm->wifiChnlInfo[2]
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d", "Wifi rssi/ HS rssi", \
+		wifiRssi-100, btHsRssi-100
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d/ %s", "Wifi bLink/ bRoam/ bScan/ bHi-Pri", \
+		bLink, bRoam, bScan, ((pCoexSta->bWiFiIsHighPriTask) ? "1" : "0")
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_UNDER_5G, &bWifiUnder5G);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir
+	);
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_BL_WIFI_UNDER_B_MODE, &bWifiUnderBMode
+	);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s / %s/ %s/ AP =%d/ %s ", "Wifi status", \
+		(bWifiUnder5G ? "5G" : "2.4G"),
+		((bWifiUnderBMode) ? "11b" : ((BTC_WIFI_BW_LEGACY == wifiBw) ? "11bg" : (((BTC_WIFI_BW_HT40 == wifiBw) ? "HT40" : "HT20")))),
+		((!bWifiBusy) ? "idle" : ((BTC_WIFI_TRAFFIC_TX == wifiTrafficDir) ? "uplink" : "downlink")),
+		pCoexSta->nScanAPNum,
+		(pCoexSta->bCCKLock) ? "Lock" : "noLock"
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_U4_WIFI_LINK_STATUS, &wifiLinkStatus
+	);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d/ %d/ %d", "sta/vwifi/hs/p2pGo/p2pGc", \
+		((wifiLinkStatus&WIFI_STA_CONNECTED) ? 1 : 0),
+		((wifiLinkStatus&WIFI_AP_CONNECTED) ? 1 : 0),
+		((wifiLinkStatus&WIFI_HS_CONNECTED) ? 1 : 0),
+		((wifiLinkStatus&WIFI_P2P_GO_CONNECTED) ? 1 : 0),
+		((wifiLinkStatus&WIFI_P2P_GC_CONNECTED) ? 1 : 0)
+	);
+	CL_PRINTF(cliBuf);
+
+
+	PopReportIn10s++;
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = [%s/ %d/ %d/ %d] ", "BT [status/ rssi/ retryCnt/ popCnt]", \
+		((pBtCoexist->btInfo.bBtDisabled) ? ("disabled") : ((pCoexSta->bC2hBtInquiryPage) ? ("inquiry/page scan") : ((BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus) ? "non-connected idle" :
+		((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) ? "connected-idle" : "busy")))),
+		pCoexSta->btRssi, pCoexSta->btRetryCnt, pCoexSta->popEventCnt
+	);
+	CL_PRINTF(cliBuf);
+
+	if (PopReportIn10s >= 5) {
+		pCoexSta->popEventCnt = 0;
+		PopReportIn10s = 0;
+	}
+
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
+		pBtLinkInfo->bScoExist,
+		pBtLinkInfo->bHidExist,
+		pBtLinkInfo->bPanExist,
+		pBtLinkInfo->bA2dpExist
+	);
+	CL_PRINTF(cliBuf);
+
+	if (pStackInfo->bProfileNotified) {
+		pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_BT_LINK_INFO);
+	} else {
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "BT Role", \
+		(pBtLinkInfo->bSlaveRole) ? "Slave" : "Master");
+		CL_PRINTF(cliBuf);
+	}
+
+
+	btInfoExt = pCoexSta->btInfoExt;
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s", "BT Info A2DP rate", \
+		(btInfoExt&BIT0) ? "Basic rate" : "EDR rate"
+	);
+	CL_PRINTF(cliBuf);
+
+	for (i = 0; i < BT_INFO_SRC_8723B_1ANT_MAX; i++) {
+		if (pCoexSta->btInfoC2hCnt[i]) {
+			CL_SPRINTF(
+				cliBuf,
+				BT_TMP_BUF_SIZE,
+				"\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8723b1Ant[i], \
+				pCoexSta->btInfoC2h[i][0], pCoexSta->btInfoC2h[i][1],
+				pCoexSta->btInfoC2h[i][2], pCoexSta->btInfoC2h[i][3],
+				pCoexSta->btInfoC2h[i][4], pCoexSta->btInfoC2h[i][5],
+				pCoexSta->btInfoC2h[i][6], pCoexSta->btInfoC2hCnt[i]
+			);
+			CL_PRINTF(cliBuf);
+		}
+	}
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s/%s, (0x%x/0x%x)", "PS state, IPS/LPS, (lps/rpwm)", \
+		(pCoexSta->bUnderIps ? "IPS ON" : "IPS OFF"),
+		(pCoexSta->bUnderLps ? "LPS ON" : "LPS OFF"),
+		pBtCoexist->btInfo.lpsVal,
+		pBtCoexist->btInfo.rpwmVal
+	);
+	CL_PRINTF(cliBuf);
+	pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	if (!pBtCoexist->bManualControl) {
+		/*  Sw mechanism */
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n %-35s", "============[Sw mechanism]============"
+		);
+		CL_PRINTF(cliBuf);
+
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n %-35s = %d", "SM[LowPenaltyRA]", \
+			pCoexDm->bCurLowPenaltyRa
+		);
+		CL_PRINTF(cliBuf);
+
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n %-35s = %s/ %s/ %d ", "DelBA/ BtCtrlAgg/ AggSize", \
+			(pBtCoexist->btInfo.bRejectAggPkt ? "Yes" : "No"),
+			(pBtCoexist->btInfo.bBtCtrlAggBufSize ? "Yes" : "No"),
+			pBtCoexist->btInfo.aggBufSize
+		);
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n %-35s = 0x%x ", "Rate Mask", \
+			pBtCoexist->btInfo.raMask
+		);
+		CL_PRINTF(cliBuf);
+
+		/*  Fw mechanism */
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
+		CL_PRINTF(cliBuf);
+
+		psTdmaCase = pCoexDm->curPsTdma;
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)", "PS TDMA", \
+			pCoexDm->psTdmaPara[0], pCoexDm->psTdmaPara[1],
+			pCoexDm->psTdmaPara[2], pCoexDm->psTdmaPara[3],
+			pCoexDm->psTdmaPara[4], psTdmaCase, pCoexDm->bAutoTdmaAdjust);
+		CL_PRINTF(cliBuf);
+
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d", "Coex Table Type", \
+			pCoexSta->nCoexTableType);
+		CL_PRINTF(cliBuf);
+
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d", "IgnWlanAct", \
+			pCoexDm->bCurIgnoreWlanAct);
+		CL_PRINTF(cliBuf);
+
+		/*
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Latest error condition(should be 0)", \
+			pCoexDm->errorCondition);
+		CL_PRINTF(cliBuf);
+		*/
+	}
+
+	/*  Hw setting */
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x", "backup ARFR1/ARFR2/RL/AMaxTime", \
+		pCoexDm->backupArfrCnt1, pCoexDm->backupArfrCnt2, pCoexDm->backupRetryLimit, pCoexDm->backupAmpduMaxTime);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x430);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x434);
+	u2Tmp[0] = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0x42a);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x456);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x", "0x430/0x434/0x42a/0x456", \
+		u4Tmp[0], u4Tmp[1], u2Tmp[0], u1Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x778);
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6cc);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x880);
+	CL_SPRINTF(
+		cliBuf, BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x778/0x6cc/0x880[29:25]", \
+		u1Tmp[0], u4Tmp[0],  (u4Tmp[1]&0x3e000000) >> 25
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x67);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x764);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x76e);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x948/ 0x67[5] / 0x764 / 0x76e", \
+		u4Tmp[0], ((u1Tmp[0]&0x20) >> 5), (u4Tmp[1] & 0xffff), u1Tmp[1]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x92c);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x930);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x944);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]", \
+		u4Tmp[0]&0x3, u4Tmp[1]&0xff, u4Tmp[2]&0x3
+	);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x39);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x40);
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+	u1Tmp[2] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x64);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x38[11]/0x40/0x4c[24:23]/0x64[0]", \
+		((u1Tmp[0] & 0x8)>>3),
+		u1Tmp[1],
+		((u4Tmp[0]&0x01800000)>>23),
+		u1Tmp[2]&0x1
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x550);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x522);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
+		u4Tmp[0], u1Tmp[0]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xc50);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x49c);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x", "0xc50(dig)/0x49c(null-drop)", \
+		u4Tmp[0]&0xff, u1Tmp[0]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda0);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda4);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda8);
+	u4Tmp[3] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xcf0);
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0xa5b);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0xa5c);
+
+	faOfdm =
+		((u4Tmp[0]&0xffff0000) >> 16) +
+		((u4Tmp[1]&0xffff0000) >> 16) +
+		(u4Tmp[1] & 0xffff) +  (u4Tmp[2] & 0xffff) + \
+		((u4Tmp[3]&0xffff0000) >> 16) + (u4Tmp[3] & 0xffff);
+	faCck = (u1Tmp[0] << 8) + u1Tmp[1];
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "OFDM-CCA/OFDM-FA/CCK-FA", \
+		u4Tmp[0]&0xffff, faOfdm, faCck
+	);
+	CL_PRINTF(cliBuf);
+
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d/ %d", "CRC_OK CCK/11g/11n/11n-Agg", \
+		pCoexSta->nCRCOK_CCK,
+		pCoexSta->nCRCOK_11g,
+		pCoexSta->nCRCOK_11n,
+		pCoexSta->nCRCOK_11nAgg
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d/ %d", "CRC_Err CCK/11g/11n/11n-Agg", \
+		pCoexSta->nCRCErr_CCK,
+		pCoexSta->nCRCErr_11g,
+		pCoexSta->nCRCErr_11n,
+		pCoexSta->nCRCErr_11nAgg
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c0);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c4);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c8);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8(coexTable)", \
+		u4Tmp[0], u4Tmp[1], u4Tmp[2]);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d", "0x770(high-pri rx/tx)", \
+		pCoexSta->highPriorityRx, pCoexSta->highPriorityTx
+	);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d", "0x774(low-pri rx/tx)", \
+		pCoexSta->lowPriorityRx, pCoexSta->lowPriorityTx
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+
+void EXhalbtc8723b1ant_IpsNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (pBtCoexist->bManualControl ||	pBtCoexist->bStopCoexDm)
+		return;
+
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n")
+		);
+		pCoexSta->bUnderIps = true;
+
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 0);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8723b1ant_SetAntPath(pBtCoexist, BTC_ANT_PATH_BT, false, true);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n")
+		);
+		pCoexSta->bUnderIps = false;
+
+		halbtc8723b1ant_InitHwConfig(pBtCoexist, false, false);
+		halbtc8723b1ant_InitCoexDm(pBtCoexist);
+		halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_LpsNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (pBtCoexist->bManualControl || pBtCoexist->bStopCoexDm)
+		return;
+
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n")
+		);
+		pCoexSta->bUnderLps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n")
+		);
+		pCoexSta->bUnderLps = false;
+	}
+}
+
+void EXhalbtc8723b1ant_ScanNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	bool bWifiConnected = false, bBtHsOn = false;
+	u32 wifiLinkStatus = 0;
+	u32 numOfWifiLink = 0;
+	bool bBtCtrlAggBufSize = false;
+	u8 aggBufSize = 5;
+
+	u8 u1Tmpa, u1Tmpb;
+	u32 u4Tmp;
+
+	if (pBtCoexist->bManualControl || pBtCoexist->bStopCoexDm)
+		return;
+
+	if (BTC_SCAN_START == type) {
+		pCoexSta->bWiFiIsHighPriTask = true;
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n")
+		);
+
+		halbtc8723b1ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 8);  /* Force antenna setup for no scan result issue */
+		u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+		u1Tmpa = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x765);
+		u1Tmpb = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x67);
+
+
+		BTC_PRINT(
+			BTC_MSG_INTERFACE,
+			INTF_NOTIFY,
+			(
+				"[BTCoex], 0x948 = 0x%x, 0x765 = 0x%x, 0x67 = 0x%x\n",
+				u4Tmp,
+				u1Tmpa,
+				u1Tmpb
+			)
+		);
+	} else {
+		pCoexSta->bWiFiIsHighPriTask = false;
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n")
+		);
+
+		pBtCoexist->fBtcGet(
+			pBtCoexist, BTC_GET_U1_AP_NUM, &pCoexSta->nScanAPNum
+		);
+	}
+
+	if (pBtCoexist->btInfo.bBtDisabled)
+		return;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+	halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_LINK_STATUS, &wifiLinkStatus);
+	numOfWifiLink = wifiLinkStatus>>16;
+
+	if (numOfWifiLink >= 2) {
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_LimitedRx(
+			pBtCoexist, NORMAL_EXEC, false, bBtCtrlAggBufSize, aggBufSize
+		);
+		halbtc8723b1ant_ActionWifiMultiPort(pBtCoexist);
+		return;
+	}
+
+	if (pCoexSta->bC2hBtInquiryPage) {
+		halbtc8723b1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else if (bBtHsOn) {
+		halbtc8723b1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+	if (BTC_SCAN_START == type) {
+		/* BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n")); */
+		if (!bWifiConnected)	/*  non-connected scan */
+			halbtc8723b1ant_ActionWifiNotConnectedScan(pBtCoexist);
+		else	/*  wifi is connected */
+			halbtc8723b1ant_ActionWifiConnectedScan(pBtCoexist);
+	} else if (BTC_SCAN_FINISH == type) {
+		/* BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n")); */
+		if (!bWifiConnected)	/*  non-connected scan */
+			halbtc8723b1ant_ActionWifiNotConnected(pBtCoexist);
+		else
+			halbtc8723b1ant_ActionWifiConnected(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_ConnectNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	bool bWifiConnected = false, bBtHsOn = false;
+	u32 wifiLinkStatus = 0;
+	u32 numOfWifiLink = 0;
+	bool bBtCtrlAggBufSize = false;
+	u8 aggBufSize = 5;
+
+	if (
+		pBtCoexist->bManualControl ||
+		pBtCoexist->bStopCoexDm ||
+		pBtCoexist->btInfo.bBtDisabled
+	)
+		return;
+
+	if (BTC_ASSOCIATE_START == type) {
+		pCoexSta->bWiFiIsHighPriTask = true;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
+		 pCoexDm->nArpCnt = 0;
+	} else {
+		pCoexSta->bWiFiIsHighPriTask = false;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
+		/* pCoexDm->nArpCnt = 0; */
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_LINK_STATUS, &wifiLinkStatus);
+	numOfWifiLink = wifiLinkStatus>>16;
+	if (numOfWifiLink >= 2) {
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, bBtCtrlAggBufSize, aggBufSize);
+		halbtc8723b1ant_ActionWifiMultiPort(pBtCoexist);
+		return;
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	if (pCoexSta->bC2hBtInquiryPage) {
+		halbtc8723b1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else if (bBtHsOn) {
+		halbtc8723b1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+	if (BTC_ASSOCIATE_START == type) {
+		/* BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n")); */
+		halbtc8723b1ant_ActionWifiNotConnectedAssoAuth(pBtCoexist);
+	} else if (BTC_ASSOCIATE_FINISH == type) {
+		/* BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n")); */
+
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+		if (!bWifiConnected) /*  non-connected scan */
+			halbtc8723b1ant_ActionWifiNotConnected(pBtCoexist);
+		else
+			halbtc8723b1ant_ActionWifiConnected(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_MediaStatusNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	u8 H2C_Parameter[3] = {0};
+	u32 wifiBw;
+	u8 wifiCentralChnl;
+	bool bWifiUnderBMode = false;
+
+	if (
+		pBtCoexist->bManualControl ||
+		pBtCoexist->bStopCoexDm ||
+		pBtCoexist->btInfo.bBtDisabled
+	)
+		return;
+
+	if (BTC_MEDIA_CONNECT == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA connect notify\n"));
+
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_UNDER_B_MODE, &bWifiUnderBMode);
+
+		/* Set CCK Tx/Rx high Pri except 11b mode */
+		if (bWifiUnderBMode) {
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cd, 0x00); /* CCK Tx */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cf, 0x00); /* CCK Rx */
+		} else {
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cd, 0x10); /* CCK Tx */
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cf, 0x10); /* CCK Rx */
+		}
+
+		pCoexDm->backupArfrCnt1 = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x430);
+		pCoexDm->backupArfrCnt2 = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x434);
+		pCoexDm->backupRetryLimit = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0x42a);
+		pCoexDm->backupAmpduMaxTime = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x456);
+	} else {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA disconnect notify\n"));
+		pCoexDm->nArpCnt = 0;
+
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cd, 0x0); /* CCK Tx */
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cf, 0x0); /* CCK Rx */
+	}
+
+	/*  only 2.4G we need to inform bt the chnl mask */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifiCentralChnl);
+	if ((BTC_MEDIA_CONNECT == type) && (wifiCentralChnl <= 14)) {
+		/* H2C_Parameter[0] = 0x1; */
+		H2C_Parameter[0] = 0x0;
+		H2C_Parameter[1] = wifiCentralChnl;
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+		if (BTC_WIFI_BW_HT40 == wifiBw)
+			H2C_Parameter[2] = 0x30;
+		else
+			H2C_Parameter[2] = 0x20;
+	}
+
+	pCoexDm->wifiChnlInfo[0] = H2C_Parameter[0];
+	pCoexDm->wifiChnlInfo[1] = H2C_Parameter[1];
+	pCoexDm->wifiChnlInfo[2] = H2C_Parameter[2];
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], FW write 0x66 = 0x%x\n",
+			H2C_Parameter[0]<<16 | H2C_Parameter[1]<<8 | H2C_Parameter[2]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x66, 3, H2C_Parameter);
+}
+
+void EXhalbtc8723b1ant_SpecialPacketNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	bool bBtHsOn = false;
+	u32 wifiLinkStatus = 0;
+	u32 numOfWifiLink = 0;
+	bool bBtCtrlAggBufSize = false;
+	u8 aggBufSize = 5;
+
+	if (
+		pBtCoexist->bManualControl ||
+		pBtCoexist->bStopCoexDm ||
+		pBtCoexist->btInfo.bBtDisabled
+	)
+		return;
+
+	if (
+		BTC_PACKET_DHCP == type ||
+		BTC_PACKET_EAPOL == type ||
+		BTC_PACKET_ARP == type
+	) {
+		if (BTC_PACKET_ARP == type) {
+			BTC_PRINT(
+				BTC_MSG_INTERFACE,
+				INTF_NOTIFY,
+				("[BTCoex], special Packet ARP notify\n")
+			);
+
+			pCoexDm->nArpCnt++;
+			BTC_PRINT(
+				BTC_MSG_INTERFACE,
+				INTF_NOTIFY,
+				("[BTCoex], ARP Packet Count = %d\n", pCoexDm->nArpCnt)
+			);
+
+			if (pCoexDm->nArpCnt >= 10) /*  if APR PKT > 10 after connect, do not go to ActionWifiConnectedSpecialPacket(pBtCoexist) */
+				pCoexSta->bWiFiIsHighPriTask = false;
+			else
+				pCoexSta->bWiFiIsHighPriTask = true;
+		} else {
+			pCoexSta->bWiFiIsHighPriTask = true;
+			BTC_PRINT(
+				BTC_MSG_INTERFACE,
+				INTF_NOTIFY,
+				("[BTCoex], special Packet DHCP or EAPOL notify\n")
+			);
+		}
+	} else {
+		pCoexSta->bWiFiIsHighPriTask = false;
+		BTC_PRINT(
+			BTC_MSG_INTERFACE,
+			INTF_NOTIFY,
+			("[BTCoex], special Packet [Type = %d] notify\n", type)
+		);
+	}
+
+	pCoexSta->specialPktPeriodCnt = 0;
+
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_U4_WIFI_LINK_STATUS, &wifiLinkStatus
+	);
+	numOfWifiLink = wifiLinkStatus>>16;
+
+	if (numOfWifiLink >= 2) {
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_LimitedRx(
+			pBtCoexist, NORMAL_EXEC, false, bBtCtrlAggBufSize, aggBufSize
+		);
+		halbtc8723b1ant_ActionWifiMultiPort(pBtCoexist);
+		return;
+	}
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	if (pCoexSta->bC2hBtInquiryPage) {
+		halbtc8723b1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else if (bBtHsOn) {
+		halbtc8723b1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+	if (
+		BTC_PACKET_DHCP == type ||
+		BTC_PACKET_EAPOL == type ||
+		((BTC_PACKET_ARP == type) && (pCoexSta->bWiFiIsHighPriTask))
+	)
+		halbtc8723b1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
+}
+
+void EXhalbtc8723b1ant_BtInfoNotify(
+	PBTC_COEXIST pBtCoexist, u8 *tmpBuf, u8 length
+)
+{
+	u8 btInfo = 0;
+	u8 i, rspSource = 0;
+	bool bWifiConnected = false;
+	bool bBtBusy = false;
+
+	pCoexSta->bC2hBtInfoReqSent = false;
+
+	rspSource = tmpBuf[0]&0xf;
+	if (rspSource >= BT_INFO_SRC_8723B_1ANT_MAX)
+		rspSource = BT_INFO_SRC_8723B_1ANT_WIFI_FW;
+	pCoexSta->btInfoC2hCnt[rspSource]++;
+
+	BTC_PRINT(
+		BTC_MSG_INTERFACE,
+		INTF_NOTIFY,
+		("[BTCoex], Bt info[%d], length =%d, hex data =[",
+		rspSource,
+		length)
+	);
+	for (i = 0; i < length; i++) {
+		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
+		if (i == 1)
+			btInfo = tmpBuf[i];
+		if (i == length-1)
+			BTC_PRINT(
+				BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x]\n", tmpBuf[i])
+			);
+		else
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x, ", tmpBuf[i]));
+	}
+
+	if (BT_INFO_SRC_8723B_1ANT_WIFI_FW != rspSource) {
+		pCoexSta->btRetryCnt = pCoexSta->btInfoC2h[rspSource][2]&0xf;
+
+		if (pCoexSta->btRetryCnt >= 1)
+			pCoexSta->popEventCnt++;
+
+		if (pCoexSta->btInfoC2h[rspSource][2]&0x20)
+			pCoexSta->bC2hBtPage = true;
+		else
+			pCoexSta->bC2hBtPage = false;
+
+		pCoexSta->btRssi = pCoexSta->btInfoC2h[rspSource][3]*2-90;
+		/* pCoexSta->btInfoC2h[rspSource][3]*2+10; */
+
+		pCoexSta->btInfoExt = pCoexSta->btInfoC2h[rspSource][4];
+
+		pCoexSta->bBtTxRxMask = (pCoexSta->btInfoC2h[rspSource][2]&0x40);
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_TX_RX_MASK, &pCoexSta->bBtTxRxMask);
+
+		if (!pCoexSta->bBtTxRxMask) {
+			/* BT into is responded by BT FW and BT RF REG 0x3C != 0x15 => Need to switch BT TRx Mask */
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Switch BT TRx Mask since BT RF REG 0x3C != 0x15\n"));
+			pBtCoexist->fBtcSetBtReg(pBtCoexist, BTC_BT_REG_RF, 0x3c, 0x15);
+		}
+
+		/*  Here we need to resend some wifi info to BT */
+		/*  because bt is reset and loss of the info. */
+		if (pCoexSta->btInfoExt & BIT1) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE,
+				("[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n")
+			);
+			pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+			if (bWifiConnected)
+				EXhalbtc8723b1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_CONNECT);
+			else
+				EXhalbtc8723b1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+		}
+
+		if (pCoexSta->btInfoExt & BIT3) {
+			if (!pBtCoexist->bManualControl && !pBtCoexist->bStopCoexDm) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT ext info bit3 check, set BT NOT to ignore Wlan active!!\n")
+				);
+				halbtc8723b1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, false);
+			}
+		} else {
+			/*  BT already NOT ignore Wlan active, do nothing here. */
+		}
+	}
+
+	/*  check BIT2 first ==> check if bt is under inquiry or page scan */
+	if (btInfo & BT_INFO_8723B_1ANT_B_INQ_PAGE)
+		pCoexSta->bC2hBtInquiryPage = true;
+	else
+		pCoexSta->bC2hBtInquiryPage = false;
+
+	/*  set link exist status */
+	if (!(btInfo&BT_INFO_8723B_1ANT_B_CONNECTION)) {
+		pCoexSta->bBtLinkExist = false;
+		pCoexSta->bPanExist = false;
+		pCoexSta->bA2dpExist = false;
+		pCoexSta->bHidExist = false;
+		pCoexSta->bScoExist = false;
+	} else {	/*  connection exists */
+		pCoexSta->bBtLinkExist = true;
+		if (btInfo & BT_INFO_8723B_1ANT_B_FTP)
+			pCoexSta->bPanExist = true;
+		else
+			pCoexSta->bPanExist = false;
+
+		if (btInfo & BT_INFO_8723B_1ANT_B_A2DP)
+			pCoexSta->bA2dpExist = true;
+		else
+			pCoexSta->bA2dpExist = false;
+
+		if (btInfo & BT_INFO_8723B_1ANT_B_HID)
+			pCoexSta->bHidExist = true;
+		else
+			pCoexSta->bHidExist = false;
+
+		if (btInfo & BT_INFO_8723B_1ANT_B_SCO_ESCO)
+			pCoexSta->bScoExist = true;
+		else
+			pCoexSta->bScoExist = false;
+	}
+
+	halbtc8723b1ant_UpdateBtLinkInfo(pBtCoexist);
+
+	btInfo = btInfo & 0x1f;  /* mask profile bit for connect-ilde identification (for CSR case: A2DP idle --> 0x41) */
+
+	if (!(btInfo&BT_INFO_8723B_1ANT_B_CONNECTION)) {
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Non-Connected idle!!!\n"));
+	} else if (btInfo == BT_INFO_8723B_1ANT_B_CONNECTION)	{
+		/*  connection exists but no busy */
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n"));
+	} else if (
+		(btInfo&BT_INFO_8723B_1ANT_B_SCO_ESCO) ||
+		(btInfo&BT_INFO_8723B_1ANT_B_SCO_BUSY)
+	) {
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_SCO_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT SCO busy!!!\n"));
+	} else if (btInfo&BT_INFO_8723B_1ANT_B_ACL_BUSY) {
+		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY != pCoexDm->btStatus)
+			pCoexDm->bAutoTdmaAdjust = false;
+
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT ACL busy!!!\n"));
+	} else {
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Non-Defined state!!!\n"));
+	}
+
+	if (
+		(BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+	)
+		bBtBusy = true;
+	else
+		bBtBusy = false;
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
+
+	halbtc8723b1ant_RunCoexistMechanism(pBtCoexist);
+}
+
+void EXhalbtc8723b1ant_HaltNotify(PBTC_COEXIST pBtCoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Halt notify\n"));
+
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+	halbtc8723b1ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 0);
+	halbtc8723b1ant_SetAntPath(pBtCoexist, BTC_ANT_PATH_BT, false, true);
+
+	halbtc8723b1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
+
+	EXhalbtc8723b1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+
+	pBtCoexist->bStopCoexDm = true;
+}
+
+void EXhalbtc8723b1ant_PnpNotify(PBTC_COEXIST pBtCoexist, u8 pnpState)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify\n"));
+
+	if (BTC_WIFI_PNP_SLEEP == pnpState) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify to SLEEP\n"));
+
+		halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 0);
+		halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+		halbtc8723b1ant_SetAntPath(pBtCoexist, BTC_ANT_PATH_BT, false, true);
+
+		pBtCoexist->bStopCoexDm = true;
+	} else if (BTC_WIFI_PNP_WAKE_UP == pnpState) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify to WAKE UP\n"));
+		pBtCoexist->bStopCoexDm = false;
+		halbtc8723b1ant_InitHwConfig(pBtCoexist, false, false);
+		halbtc8723b1ant_InitCoexDm(pBtCoexist);
+		halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_Periodical(PBTC_COEXIST pBtCoexist)
+{
+	static u8 disVerInfoCnt = 0;
+	u32 fwVer = 0, btPatchVer = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ==========================Periodical ===========================\n"));
+
+	if (disVerInfoCnt <= 5) {
+		disVerInfoCnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n", \
+			GLCoexVerDate8723b1Ant, GLCoexVer8723b1Ant, fwVer, btPatchVer, btPatchVer));
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
+	}
+
+	halbtc8723b1ant_MonitorBtCtr(pBtCoexist);
+	halbtc8723b1ant_MonitorWiFiCtr(pBtCoexist);
+
+	if (
+		halbtc8723b1ant_IsWifiStatusChanged(pBtCoexist) ||
+		pCoexDm->bAutoTdmaAdjust
+	)
+		halbtc8723b1ant_RunCoexistMechanism(pBtCoexist);
+
+	pCoexSta->specialPktPeriodCnt++;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalBtc8723b1Ant.h linux/3rdparty/rtl8723bs/hal/HalBtc8723b1Ant.h
--- linux.old/3rdparty/rtl8723bs/hal/HalBtc8723b1Ant.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalBtc8723b1Ant.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,193 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+/*  The following is for 8723B 1ANT BT Co-exist definition */
+#define	BT_INFO_8723B_1ANT_B_FTP		BIT7
+#define	BT_INFO_8723B_1ANT_B_A2DP		BIT6
+#define	BT_INFO_8723B_1ANT_B_HID		BIT5
+#define	BT_INFO_8723B_1ANT_B_SCO_BUSY		BIT4
+#define	BT_INFO_8723B_1ANT_B_ACL_BUSY		BIT3
+#define	BT_INFO_8723B_1ANT_B_INQ_PAGE		BIT2
+#define	BT_INFO_8723B_1ANT_B_SCO_ESCO		BIT1
+#define	BT_INFO_8723B_1ANT_B_CONNECTION		BIT0
+
+#define	BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
+		(((_BT_INFO_EXT_&BIT0)) ? true : false)
+
+#define	BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT 2
+
+#define  BT_8723B_1ANT_WIFI_NOISY_THRESH 30   /* max: 255 */
+
+typedef enum _BT_INFO_SRC_8723B_1ANT {
+	BT_INFO_SRC_8723B_1ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8723B_1ANT_BT_RSP				= 0x1,
+	BT_INFO_SRC_8723B_1ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8723B_1ANT_MAX
+} BT_INFO_SRC_8723B_1ANT, *PBT_INFO_SRC_8723B_1ANT;
+
+typedef enum _BT_8723B_1ANT_BT_STATUS {
+	BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
+	BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
+	BT_8723B_1ANT_BT_STATUS_INQ_PAGE				= 0x2,
+	BT_8723B_1ANT_BT_STATUS_ACL_BUSY				= 0x3,
+	BT_8723B_1ANT_BT_STATUS_SCO_BUSY				= 0x4,
+	BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY			= 0x5,
+	BT_8723B_1ANT_BT_STATUS_MAX
+} BT_8723B_1ANT_BT_STATUS, *PBT_8723B_1ANT_BT_STATUS;
+
+typedef enum _BT_8723B_1ANT_WIFI_STATUS {
+	BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_IDLE           = 0x0,
+	BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN = 0x1,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN               = 0x2,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT        = 0x3,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE               = 0x4,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY               = 0x5,
+	BT_8723B_1ANT_WIFI_STATUS_MAX
+} BT_8723B_1ANT_WIFI_STATUS, *PBT_8723B_1ANT_WIFI_STATUS;
+
+typedef enum _BT_8723B_1ANT_COEX_ALGO {
+	BT_8723B_1ANT_COEX_ALGO_UNDEFINED		= 0x0,
+	BT_8723B_1ANT_COEX_ALGO_SCO				= 0x1,
+	BT_8723B_1ANT_COEX_ALGO_HID				= 0x2,
+	BT_8723B_1ANT_COEX_ALGO_A2DP			= 0x3,
+	BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
+	BT_8723B_1ANT_COEX_ALGO_PANEDR			= 0x5,
+	BT_8723B_1ANT_COEX_ALGO_PANHS			= 0x6,
+	BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
+	BT_8723B_1ANT_COEX_ALGO_PANEDR_HID		= 0x8,
+	BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x9,
+	BT_8723B_1ANT_COEX_ALGO_HID_A2DP		= 0xa,
+	BT_8723B_1ANT_COEX_ALGO_MAX				= 0xb,
+} BT_8723B_1ANT_COEX_ALGO, *PBT_8723B_1ANT_COEX_ALGO;
+
+typedef struct _COEX_DM_8723B_1ANT {
+	/*  fw mechanism */
+	bool bCurIgnoreWlanAct;
+	bool bPreIgnoreWlanAct;
+	u8 prePsTdma;
+	u8 curPsTdma;
+	u8 psTdmaPara[5];
+	u8 psTdmaDuAdjType;
+	bool bAutoTdmaAdjust;
+	bool bPrePsTdmaOn;
+	bool bCurPsTdmaOn;
+	bool bPreBtAutoReport;
+	bool bCurBtAutoReport;
+	u8 preLps;
+	u8 curLps;
+	u8 preRpwm;
+	u8 curRpwm;
+
+	/*  sw mechanism */
+	bool bPreLowPenaltyRa;
+	bool bCurLowPenaltyRa;
+	u32 preVal0x6c0;
+	u32 curVal0x6c0;
+	u32 preVal0x6c4;
+	u32 curVal0x6c4;
+	u32 preVal0x6c8;
+	u32 curVal0x6c8;
+	u8 preVal0x6cc;
+	u8 curVal0x6cc;
+	bool bLimitedDig;
+
+	u32 backupArfrCnt1;	/*  Auto Rate Fallback Retry cnt */
+	u32 backupArfrCnt2;	/*  Auto Rate Fallback Retry cnt */
+	u16 backupRetryLimit;
+	u8 backupAmpduMaxTime;
+
+	/*  algorithm related */
+	u8 preAlgorithm;
+	u8 curAlgorithm;
+	u8 btStatus;
+	u8 wifiChnlInfo[3];
+
+	u32 preRaMask;
+	u32 curRaMask;
+	u8 preArfrType;
+	u8 curArfrType;
+	u8 preRetryLimitType;
+	u8 curRetryLimitType;
+	u8 preAmpduTimeType;
+	u8 curAmpduTimeType;
+	u32 nArpCnt;
+
+	u8 errorCondition;
+} COEX_DM_8723B_1ANT, *PCOEX_DM_8723B_1ANT;
+
+typedef struct _COEX_STA_8723B_1ANT {
+	bool bBtLinkExist;
+	bool bScoExist;
+	bool bA2dpExist;
+	bool bHidExist;
+	bool bPanExist;
+
+	bool bUnderLps;
+	bool bUnderIps;
+	u32 specialPktPeriodCnt;
+	u32 highPriorityTx;
+	u32 highPriorityRx;
+	u32 lowPriorityTx;
+	u32 lowPriorityRx;
+	s8 btRssi;
+	bool bBtTxRxMask;
+	u8 preBtRssiState;
+	u8 preWifiRssiState[4];
+	bool bC2hBtInfoReqSent;
+	u8 btInfoC2h[BT_INFO_SRC_8723B_1ANT_MAX][10];
+	u32 btInfoC2hCnt[BT_INFO_SRC_8723B_1ANT_MAX];
+	bool bC2hBtInquiryPage;
+	bool bC2hBtPage; /* Add for win8.1 page out issue */
+	bool bWiFiIsHighPriTask; /* Add for win8.1 page out issue */
+	u8 btRetryCnt;
+	u8 btInfoExt;
+	u32 popEventCnt;
+	u8 nScanAPNum;
+
+	u32 nCRCOK_CCK;
+	u32 nCRCOK_11g;
+	u32 nCRCOK_11n;
+	u32 nCRCOK_11nAgg;
+
+	u32 nCRCErr_CCK;
+	u32 nCRCErr_11g;
+	u32 nCRCErr_11n;
+	u32 nCRCErr_11nAgg;
+
+	bool bCCKLock;
+	bool bPreCCKLock;
+	u8 nCoexTableType;
+
+	bool bForceLpsOn;
+} COEX_STA_8723B_1ANT, *PCOEX_STA_8723B_1ANT;
+
+/*  */
+/*  The following is interface which will notify coex module. */
+/*  */
+void EXhalbtc8723b1ant_PowerOnSetting(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b1ant_InitHwConfig(PBTC_COEXIST pBtCoexist, bool bWifiOnly);
+void EXhalbtc8723b1ant_InitCoexDm(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b1ant_IpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_LpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_ScanNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_ConnectNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_MediaStatusNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_SpecialPacketNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_BtInfoNotify(
+	PBTC_COEXIST pBtCoexist, u8 *tmpBuf, u8 length
+);
+void EXhalbtc8723b1ant_HaltNotify(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b1ant_PnpNotify(PBTC_COEXIST pBtCoexist, u8 pnpState);
+void EXhalbtc8723b1ant_Periodical(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b1ant_DisplayCoexInfo(PBTC_COEXIST pBtCoexist);
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalBtc8723b2Ant.c linux/3rdparty/rtl8723bs/hal/HalBtc8723b2Ant.c
--- linux.old/3rdparty/rtl8723bs/hal/HalBtc8723b2Ant.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalBtc8723b2Ant.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,3729 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+
+#include "Mp_Precomp.h"
+
+/*  Global variables, these are static variables */
+static COEX_DM_8723B_2ANT GLCoexDm8723b2Ant;
+static PCOEX_DM_8723B_2ANT pCoexDm = &GLCoexDm8723b2Ant;
+static COEX_STA_8723B_2ANT GLCoexSta8723b2Ant;
+static PCOEX_STA_8723B_2ANT pCoexSta = &GLCoexSta8723b2Ant;
+
+static const char *const GLBtInfoSrc8723b2Ant[] = {
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+static u32 GLCoexVerDate8723b2Ant = 20131211;
+static u32 GLCoexVer8723b2Ant = 0x40;
+
+/*  local function start with halbtc8723b2ant_ */
+static u8 halbtc8723b2ant_BtRssiState(
+	u8 levelNum, u8 rssiThresh, u8 rssiThresh1
+)
+{
+	s32 btRssi = 0;
+	u8 btRssiState = pCoexSta->preBtRssiState;
+
+	btRssi = pCoexSta->btRssi;
+
+	if (levelNum == 2) {
+		if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				btRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
+			}
+		} else {
+			if (btRssi < rssiThresh) {
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
+			}
+		}
+	} else if (levelNum == 3) {
+		if (rssiThresh > rssiThresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi thresh error!!\n"));
+			return pCoexSta->preBtRssiState;
+		}
+
+		if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
+			}
+		} else if (
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_MEDIUM) ||
+			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_MEDIUM)
+		) {
+			if (btRssi >= (rssiThresh1+BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				btRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
+			} else if (btRssi < rssiThresh) {
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Medium\n"));
+			}
+		} else {
+			if (btRssi < rssiThresh1) {
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
+			}
+		}
+	}
+
+	pCoexSta->preBtRssiState = btRssiState;
+
+	return btRssiState;
+}
+
+static u8 halbtc8723b2ant_WifiRssiState(
+	PBTC_COEXIST pBtCoexist,
+	u8 index,
+	u8 levelNum,
+	u8 rssiThresh,
+	u8 rssiThresh1
+)
+{
+	s32 wifiRssi = 0;
+	u8 wifiRssiState = pCoexSta->preWifiRssiState[index];
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+
+	if (levelNum == 2) {
+		if (
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (wifiRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				wifiRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
+			}
+		} else {
+			if (wifiRssi < rssiThresh) {
+				wifiRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
+			}
+		}
+	} else if (levelNum == 3) {
+		if (rssiThresh > rssiThresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI thresh error!!\n"));
+			return pCoexSta->preWifiRssiState[index];
+		}
+
+		if (
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW)
+		) {
+			if (wifiRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
+			}
+		} else if (
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_MEDIUM) ||
+			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_MEDIUM)
+		) {
+			if (wifiRssi >= (rssiThresh1+BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT)) {
+				wifiRssiState = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
+			} else if (wifiRssi < rssiThresh) {
+				wifiRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Medium\n"));
+			}
+		} else {
+			if (wifiRssi < rssiThresh1) {
+				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
+			}
+		}
+	}
+
+	pCoexSta->preWifiRssiState[index] = wifiRssiState;
+
+	return wifiRssiState;
+}
+
+static void halbtc8723b2ant_LimitedRx(
+	PBTC_COEXIST pBtCoexist,
+	bool bForceExec,
+	bool bRejApAggPkt,
+	bool bBtCtrlAggBufSize,
+	u8 aggBufSize
+)
+{
+	bool bRejectRxAgg = bRejApAggPkt;
+	bool bBtCtrlRxAggSize = bBtCtrlAggBufSize;
+	u8 rxAggSize = aggBufSize;
+
+	/*  */
+	/* 	Rx Aggregation related setting */
+	/*  */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT, &bRejectRxAgg);
+	/*  decide BT control aggregation buf size or not */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE, &bBtCtrlRxAggSize);
+	/*  aggregation buf size, only work when BT control Rx aggregation size. */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_U1_AGG_BUF_SIZE, &rxAggSize);
+	/*  real update aggregation setting */
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
+}
+
+static void halbtc8723b2ant_MonitorBtCtr(PBTC_COEXIST pBtCoexist)
+{
+	u32 regHPTxRx, regLPTxRx, u4Tmp;
+	u32 regHPTx = 0, regHPRx = 0, regLPTx = 0, regLPRx = 0;
+
+	regHPTxRx = 0x770;
+	regLPTxRx = 0x774;
+
+	u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, regHPTxRx);
+	regHPTx = u4Tmp & bMaskLWord;
+	regHPRx = (u4Tmp & bMaskHWord)>>16;
+
+	u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, regLPTxRx);
+	regLPTx = u4Tmp & bMaskLWord;
+	regLPRx = (u4Tmp & bMaskHWord)>>16;
+
+	pCoexSta->highPriorityTx = regHPTx;
+	pCoexSta->highPriorityRx = regHPRx;
+	pCoexSta->lowPriorityTx = regLPTx;
+	pCoexSta->lowPriorityRx = regLPRx;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_BT_MONITOR,
+		(
+			"[BTCoex], High Priority Tx/Rx (reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
+			regHPTxRx,
+			regHPTx,
+			regHPTx,
+			regHPRx,
+			regHPRx
+		)
+	);
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_BT_MONITOR,
+		(
+			"[BTCoex], Low Priority Tx/Rx (reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
+			regLPTxRx,
+			regLPTx,
+			regLPTx,
+			regLPRx,
+			regLPRx
+		)
+	);
+
+	/*  reset counter */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0xc);
+}
+
+static void halbtc8723b2ant_QueryBtInfo(PBTC_COEXIST pBtCoexist)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	pCoexSta->bC2hBtInfoReqSent = true;
+
+	H2C_Parameter[0] |= BIT0;	/*  trigger */
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		("[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n", H2C_Parameter[0])
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x61, 1, H2C_Parameter);
+}
+
+static bool halbtc8723b2ant_IsWifiStatusChanged(PBTC_COEXIST pBtCoexist)
+{
+	static bool	bPreWifiBusy = false, bPreUnder4way = false, bPreBtHsOn = false;
+	bool bWifiBusy = false, bUnder4way = false, bBtHsOn = false;
+	bool bWifiConnected = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &bUnder4way);
+
+	if (bWifiConnected) {
+		if (bWifiBusy != bPreWifiBusy) {
+			bPreWifiBusy = bWifiBusy;
+			return true;
+		}
+
+		if (bUnder4way != bPreUnder4way) {
+			bPreUnder4way = bUnder4way;
+			return true;
+		}
+
+		if (bBtHsOn != bPreBtHsOn) {
+			bPreBtHsOn = bBtHsOn;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static void halbtc8723b2ant_UpdateBtLinkInfo(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bBtHsOn = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+
+	pBtLinkInfo->bBtLinkExist = pCoexSta->bBtLinkExist;
+	pBtLinkInfo->bScoExist = pCoexSta->bScoExist;
+	pBtLinkInfo->bA2dpExist = pCoexSta->bA2dpExist;
+	pBtLinkInfo->bPanExist = pCoexSta->bPanExist;
+	pBtLinkInfo->bHidExist = pCoexSta->bHidExist;
+
+	/*  work around for HS mode. */
+	if (bBtHsOn) {
+		pBtLinkInfo->bPanExist = true;
+		pBtLinkInfo->bBtLinkExist = true;
+	}
+
+	/*  check if Sco only */
+	if (
+		pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bScoOnly = true;
+	else
+		pBtLinkInfo->bScoOnly = false;
+
+	/*  check if A2dp only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bA2dpOnly = true;
+	else
+		pBtLinkInfo->bA2dpOnly = false;
+
+	/*  check if Pan only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		pBtLinkInfo->bPanExist &&
+		!pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bPanOnly = true;
+	else
+		pBtLinkInfo->bPanOnly = false;
+
+	/*  check if Hid only */
+	if (
+		!pBtLinkInfo->bScoExist &&
+		!pBtLinkInfo->bA2dpExist &&
+		!pBtLinkInfo->bPanExist &&
+		pBtLinkInfo->bHidExist
+	)
+		pBtLinkInfo->bHidOnly = true;
+	else
+		pBtLinkInfo->bHidOnly = false;
+}
+
+static u8 halbtc8723b2ant_ActionAlgorithm(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	bool bBtHsOn = false;
+	u8 algorithm = BT_8723B_2ANT_COEX_ALGO_UNDEFINED;
+	u8 numOfDiffProfile = 0;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+
+	if (!pBtLinkInfo->bBtLinkExist) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], No BT link exists!!!\n"));
+		return algorithm;
+	}
+
+	if (pBtLinkInfo->bScoExist)
+		numOfDiffProfile++;
+
+	if (pBtLinkInfo->bHidExist)
+		numOfDiffProfile++;
+
+	if (pBtLinkInfo->bPanExist)
+		numOfDiffProfile++;
+
+	if (pBtLinkInfo->bA2dpExist)
+		numOfDiffProfile++;
+
+	if (numOfDiffProfile == 1) {
+		if (pBtLinkInfo->bScoExist) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO only\n"));
+			algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
+		} else {
+			if (pBtLinkInfo->bHidExist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID only\n"));
+				algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP only\n"));
+				algorithm = BT_8723B_2ANT_COEX_ALGO_A2DP;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(HS) only\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(EDR) only\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 2) {
+		if (pBtLinkInfo->bScoExist) {
+			if (pBtLinkInfo->bHidExist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID\n"));
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP ==> SCO\n"));
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(HS)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(EDR)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP\n"));
+				algorithm = BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
+			} else if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(HS)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(EDR)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(HS)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(EDR)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 3) {
+		if (pBtLinkInfo->bScoExist) {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], SCO + HID + A2DP ==> HID\n")
+				);
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], SCO + HID + PAN(HS)\n")
+					);
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], SCO + HID + PAN(EDR)\n")
+					);
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(HS)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(HS)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(EDR)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	} else if (numOfDiffProfile >= 3) {
+		if (pBtLinkInfo->bScoExist) {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Error!!! SCO + HID + A2DP + PAN(HS)\n"));
+
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+static void halbtc8723b2ant_SetFwDacSwingLevel(
+	PBTC_COEXIST pBtCoexist, u8 dacSwingLvl
+)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	/*  There are several type of dacswing */
+	/*  0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
+	H2C_Parameter[0] = dacSwingLvl;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		("[BTCoex], Set Dac Swing Level = 0x%x\n", dacSwingLvl)
+	);
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		("[BTCoex], FW write 0x64 = 0x%x\n", H2C_Parameter[0])
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x64, 1, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_SetFwDecBtPwr(
+	PBTC_COEXIST pBtCoexist, u8 decBtPwrLvl
+)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	H2C_Parameter[0] = decBtPwrLvl;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], decrease Bt Power level = %d, FW write 0x62 = 0x%x\n",
+			decBtPwrLvl,
+			H2C_Parameter[0]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x62, 1, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_DecBtPwr(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 decBtPwrLvl
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s Dec BT power level = %d\n",
+			(bForceExec ? "force to" : ""),
+			decBtPwrLvl
+		)
+	);
+	pCoexDm->curBtDecPwrLvl = decBtPwrLvl;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], preBtDecPwrLvl =%d, curBtDecPwrLvl =%d\n",
+				pCoexDm->preBtDecPwrLvl,
+				pCoexDm->curBtDecPwrLvl
+			)
+		);
+
+		if (pCoexDm->preBtDecPwrLvl == pCoexDm->curBtDecPwrLvl)
+			return;
+	}
+	halbtc8723b2ant_SetFwDecBtPwr(pBtCoexist, pCoexDm->curBtDecPwrLvl);
+
+	pCoexDm->preBtDecPwrLvl = pCoexDm->curBtDecPwrLvl;
+}
+
+static void halbtc8723b2ant_FwDacSwingLvl(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 fwDacSwingLvl
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s set FW Dac Swing level = %d\n",
+			(bForceExec ? "force to" : ""),
+			fwDacSwingLvl
+		)
+	);
+	pCoexDm->curFwDacSwingLvl = fwDacSwingLvl;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], preFwDacSwingLvl =%d, curFwDacSwingLvl =%d\n",
+				pCoexDm->preFwDacSwingLvl,
+				pCoexDm->curFwDacSwingLvl
+			)
+		);
+
+		if (pCoexDm->preFwDacSwingLvl == pCoexDm->curFwDacSwingLvl)
+			return;
+	}
+
+	halbtc8723b2ant_SetFwDacSwingLevel(pBtCoexist, pCoexDm->curFwDacSwingLvl);
+
+	pCoexDm->preFwDacSwingLvl = pCoexDm->curFwDacSwingLvl;
+}
+
+static void halbtc8723b2ant_SetSwRfRxLpfCorner(
+	PBTC_COEXIST pBtCoexist,
+	bool bRxRfShrinkOn
+)
+{
+	if (bRxRfShrinkOn) {
+		/* Shrink RF Rx LPF corner */
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_EXEC,
+			("[BTCoex], Shrink RF Rx LPF corner!!\n")
+		);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, 0xffffc);
+	} else {
+		/* Resume RF Rx LPF corner */
+		/*  After initialized, we can use pCoexDm->btRf0x1eBackup */
+		if (pBtCoexist->bInitilized) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Resume RF Rx LPF corner!!\n"));
+			pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, pCoexDm->btRf0x1eBackup);
+		}
+	}
+}
+
+static void halbtc8723b2ant_RfShrink(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bRxRfShrinkOn
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s turn Rx RF Shrink = %s\n",
+			(bForceExec ? "force to" : ""),
+			(bRxRfShrinkOn ? "ON" : "OFF")
+		)
+	);
+	pCoexDm->bCurRfRxLpfShrink = bRxRfShrinkOn;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], bPreRfRxLpfShrink =%d, bCurRfRxLpfShrink =%d\n",
+				pCoexDm->bPreRfRxLpfShrink,
+				pCoexDm->bCurRfRxLpfShrink
+			)
+		);
+
+		if (pCoexDm->bPreRfRxLpfShrink == pCoexDm->bCurRfRxLpfShrink)
+			return;
+	}
+	halbtc8723b2ant_SetSwRfRxLpfCorner(pBtCoexist, pCoexDm->bCurRfRxLpfShrink);
+
+	pCoexDm->bPreRfRxLpfShrink = pCoexDm->bCurRfRxLpfShrink;
+}
+
+static void halbtc8723b2ant_SetSwPenaltyTxRateAdaptive(
+	PBTC_COEXIST pBtCoexist, bool bLowPenaltyRa
+)
+{
+	u8 	H2C_Parameter[6] = {0};
+
+	H2C_Parameter[0] = 0x6;	/*  opCode, 0x6 = Retry_Penalty */
+
+	if (bLowPenaltyRa) {
+		H2C_Parameter[1] |= BIT0;
+		H2C_Parameter[2] = 0x00;  /* normal rate except MCS7/6/5, OFDM54/48/36 */
+		H2C_Parameter[3] = 0xf7;  /* MCS7 or OFDM54 */
+		H2C_Parameter[4] = 0xf8;  /* MCS6 or OFDM48 */
+		H2C_Parameter[5] = 0xf9;	/* MCS5 or OFDM36 */
+	}
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], set WiFi Low-Penalty Retry: %s",
+			(bLowPenaltyRa ? "ON!!" : "OFF!!")
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x69, 6, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_LowPenaltyRa(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bLowPenaltyRa
+)
+{
+	/* return; */
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s turn LowPenaltyRA = %s\n",
+			(bForceExec ? "force to" : ""),
+			(bLowPenaltyRa ? "ON" : "OFF")
+		)
+	);
+	pCoexDm->bCurLowPenaltyRa = bLowPenaltyRa;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], bPreLowPenaltyRa =%d, bCurLowPenaltyRa =%d\n",
+				pCoexDm->bPreLowPenaltyRa,
+				pCoexDm->bCurLowPenaltyRa
+			)
+		);
+
+		if (pCoexDm->bPreLowPenaltyRa == pCoexDm->bCurLowPenaltyRa)
+			return;
+	}
+	halbtc8723b2ant_SetSwPenaltyTxRateAdaptive(pBtCoexist, pCoexDm->bCurLowPenaltyRa);
+
+	pCoexDm->bPreLowPenaltyRa = pCoexDm->bCurLowPenaltyRa;
+}
+
+static void halbtc8723b2ant_SetDacSwingReg(PBTC_COEXIST pBtCoexist, u32 level)
+{
+	u8 val = (u8)level;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], Write SwDacSwing = 0x%x\n", level)
+	);
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x883, 0x3e, val);
+}
+
+static void halbtc8723b2ant_SetSwFullTimeDacSwing(
+	PBTC_COEXIST pBtCoexist, bool bSwDacSwingOn, u32 swDacSwingLvl
+)
+{
+	if (bSwDacSwingOn)
+		halbtc8723b2ant_SetDacSwingReg(pBtCoexist, swDacSwingLvl);
+	else
+		halbtc8723b2ant_SetDacSwingReg(pBtCoexist, 0x18);
+}
+
+
+static void halbtc8723b2ant_DacSwing(
+	PBTC_COEXIST pBtCoexist,
+	bool bForceExec,
+	bool bDacSwingOn,
+	u32 dacSwingLvl
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s turn DacSwing =%s, dacSwingLvl = 0x%x\n",
+			(bForceExec ? "force to" : ""),
+			(bDacSwingOn ? "ON" : "OFF"),
+			dacSwingLvl
+		)
+	);
+	pCoexDm->bCurDacSwingOn = bDacSwingOn;
+	pCoexDm->curDacSwingLvl = dacSwingLvl;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], bPreDacSwingOn =%d, preDacSwingLvl = 0x%x, bCurDacSwingOn =%d, curDacSwingLvl = 0x%x\n",
+				pCoexDm->bPreDacSwingOn,
+				pCoexDm->preDacSwingLvl,
+				pCoexDm->bCurDacSwingOn,
+				pCoexDm->curDacSwingLvl
+			)
+		);
+
+		if ((pCoexDm->bPreDacSwingOn == pCoexDm->bCurDacSwingOn) &&
+			(pCoexDm->preDacSwingLvl == pCoexDm->curDacSwingLvl))
+			return;
+	}
+	mdelay(30);
+	halbtc8723b2ant_SetSwFullTimeDacSwing(pBtCoexist, bDacSwingOn, dacSwingLvl);
+
+	pCoexDm->bPreDacSwingOn = pCoexDm->bCurDacSwingOn;
+	pCoexDm->preDacSwingLvl = pCoexDm->curDacSwingLvl;
+}
+
+static void halbtc8723b2ant_SetAgcTable(
+	PBTC_COEXIST pBtCoexist, bool bAgcTableEn
+)
+{
+	u8 rssiAdjustVal = 0;
+
+	/* BB AGC Gain Table */
+	if (bAgcTableEn) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB Agc Table On!\n"));
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x6e1A0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x6d1B0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x6c1C0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x6b1D0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x6a1E0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x691F0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0x68200001);
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB Agc Table Off!\n"));
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xaa1A0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa91B0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa81C0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa71D0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa61E0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa51F0001);
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0xc78, 0xa4200001);
+	}
+
+
+	/* RF Gain */
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0xef, 0xfffff, 0x02000);
+	if (bAgcTableEn) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table On!\n"));
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x38fff);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x38ffe);
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table Off!\n"));
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x380c3);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x28ce6);
+	}
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0xef, 0xfffff, 0x0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0xed, 0xfffff, 0x1);
+	if (bAgcTableEn) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table On!\n"));
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x40, 0xfffff, 0x38fff);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x40, 0xfffff, 0x38ffe);
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table Off!\n"));
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x40, 0xfffff, 0x380c3);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x40, 0xfffff, 0x28ce6);
+	}
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0xed, 0xfffff, 0x0);
+
+	/*  set rssiAdjustVal for wifi module. */
+	if (bAgcTableEn)
+		rssiAdjustVal = 8;
+
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON, &rssiAdjustVal);
+}
+
+static void halbtc8723b2ant_AgcTable(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bAgcTableEn
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s %s Agc Table\n",
+			(bForceExec ? "force to" : ""),
+			(bAgcTableEn ? "Enable" : "Disable")
+		)
+	);
+	pCoexDm->bCurAgcTableEn = bAgcTableEn;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], bPreAgcTableEn =%d, bCurAgcTableEn =%d\n",
+				pCoexDm->bPreAgcTableEn,
+				pCoexDm->bCurAgcTableEn
+			)
+		);
+
+		if (pCoexDm->bPreAgcTableEn == pCoexDm->bCurAgcTableEn)
+			return;
+	}
+	halbtc8723b2ant_SetAgcTable(pBtCoexist, bAgcTableEn);
+
+	pCoexDm->bPreAgcTableEn = pCoexDm->bCurAgcTableEn;
+}
+
+static void halbtc8723b2ant_SetCoexTable(
+	PBTC_COEXIST pBtCoexist,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c0 = 0x%x\n", val0x6c0)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c4 = 0x%x\n", val0x6c4)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c4, val0x6c4);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c8 = 0x%x\n", val0x6c8)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6cc = 0x%x\n", val0x6cc)
+	);
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cc, val0x6cc);
+}
+
+static void halbtc8723b2ant_CoexTable(
+	PBTC_COEXIST pBtCoexist,
+	bool bForceExec,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s write Coex Table 0x6c0 = 0x%x, 0x6c4 = 0x%x, 0x6c8 = 0x%x, 0x6cc = 0x%x\n",
+			(bForceExec ? "force to" : ""),
+			val0x6c0,
+			val0x6c4,
+			val0x6c8,
+			val0x6cc
+		)
+	);
+	pCoexDm->curVal0x6c0 = val0x6c0;
+	pCoexDm->curVal0x6c4 = val0x6c4;
+	pCoexDm->curVal0x6c8 = val0x6c8;
+	pCoexDm->curVal0x6cc = val0x6cc;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], preVal0x6c0 = 0x%x, preVal0x6c4 = 0x%x, preVal0x6c8 = 0x%x, preVal0x6cc = 0x%x !!\n",
+				pCoexDm->preVal0x6c0,
+				pCoexDm->preVal0x6c4,
+				pCoexDm->preVal0x6c8,
+				pCoexDm->preVal0x6cc
+			)
+		);
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], curVal0x6c0 = 0x%x, curVal0x6c4 = 0x%x, curVal0x6c8 = 0x%x, curVal0x6cc = 0x%x !!\n",
+				pCoexDm->curVal0x6c0,
+				pCoexDm->curVal0x6c4,
+				pCoexDm->curVal0x6c8,
+				pCoexDm->curVal0x6cc
+			)
+		);
+
+		if (
+			(pCoexDm->preVal0x6c0 == pCoexDm->curVal0x6c0) &&
+			(pCoexDm->preVal0x6c4 == pCoexDm->curVal0x6c4) &&
+			(pCoexDm->preVal0x6c8 == pCoexDm->curVal0x6c8) &&
+			(pCoexDm->preVal0x6cc == pCoexDm->curVal0x6cc)
+		)
+			return;
+	}
+	halbtc8723b2ant_SetCoexTable(pBtCoexist, val0x6c0, val0x6c4, val0x6c8, val0x6cc);
+
+	pCoexDm->preVal0x6c0 = pCoexDm->curVal0x6c0;
+	pCoexDm->preVal0x6c4 = pCoexDm->curVal0x6c4;
+	pCoexDm->preVal0x6c8 = pCoexDm->curVal0x6c8;
+	pCoexDm->preVal0x6cc = pCoexDm->curVal0x6cc;
+}
+
+static void halbtc8723b2ant_CoexTableWithType(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 type
+)
+{
+	switch (type) {
+	case 0:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55555555, 0x55555555, 0xffff, 0x3);
+		break;
+	case 1:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
+		break;
+	case 2:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x5a5a5a5a, 0x5a5a5a5a, 0xffff, 0x3);
+		break;
+	case 3:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0xaaaaaaaa, 0xaaaaaaaa, 0xffff, 0x3);
+		break;
+	case 4:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0xffffffff, 0xffffffff, 0xffff, 0x3);
+		break;
+	case 5:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x5fff5fff, 0x5fff5fff, 0xffff, 0x3);
+		break;
+	case 6:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5a5a5a5a, 0xffff, 0x3);
+		break;
+	case 7:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0xfafafafa, 0xffff, 0x3);
+		break;
+	case 8:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x5aea5aea, 0x5aea5aea, 0xffff, 0x3);
+		break;
+	case 9:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5aea5aea, 0xffff, 0x3);
+		break;
+	case 10:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5aff5aff, 0xffff, 0x3);
+		break;
+	case 11:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5a5f5a5f, 0xffff, 0x3);
+		break;
+	case 12:
+		halbtc8723b2ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5f5f5f5f, 0xffff, 0x3);
+		break;
+	default:
+		break;
+	}
+}
+
+static void halbtc8723b2ant_SetFwIgnoreWlanAct(
+	PBTC_COEXIST pBtCoexist, bool bEnable
+)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	if (bEnable)
+		H2C_Parameter[0] |= BIT0;		/*  function enable */
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
+			H2C_Parameter[0]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x63, 1, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_IgnoreWlanAct(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bEnable
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s turn Ignore WlanAct %s\n",
+			(bForceExec ? "force to" : ""),
+			(bEnable ? "ON" : "OFF")
+		)
+	);
+
+	pCoexDm->bCurIgnoreWlanAct = bEnable;
+
+	if (!bForceExec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n",
+			pCoexDm->bPreIgnoreWlanAct, pCoexDm->bCurIgnoreWlanAct));
+
+		if (pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
+			return;
+	}
+	halbtc8723b2ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
+
+	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
+}
+
+static void halbtc8723b2ant_SetFwPstdma(
+	PBTC_COEXIST pBtCoexist,
+	u8 byte1,
+	u8 byte2,
+	u8 byte3,
+	u8 byte4,
+	u8 byte5
+)
+{
+	u8 	H2C_Parameter[5] = {0};
+
+	H2C_Parameter[0] = byte1;
+	H2C_Parameter[1] = byte2;
+	H2C_Parameter[2] = byte3;
+	H2C_Parameter[3] = byte4;
+	H2C_Parameter[4] = byte5;
+
+	pCoexDm->psTdmaPara[0] = byte1;
+	pCoexDm->psTdmaPara[1] = byte2;
+	pCoexDm->psTdmaPara[2] = byte3;
+	pCoexDm->psTdmaPara[3] = byte4;
+	pCoexDm->psTdmaPara[4] = byte5;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], FW write 0x60(5bytes) = 0x%x%08x\n",
+			H2C_Parameter[0],
+			H2C_Parameter[1]<<24|
+			H2C_Parameter[2]<<16|
+			H2C_Parameter[3]<<8|
+			H2C_Parameter[4]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x60, 5, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_SwMechanism1(
+	PBTC_COEXIST pBtCoexist,
+	bool bShrinkRxLPF,
+	bool bLowPenaltyRA,
+	bool bLimitedDIG,
+	bool bBTLNAConstrain
+)
+{
+	halbtc8723b2ant_RfShrink(pBtCoexist, NORMAL_EXEC, bShrinkRxLPF);
+	halbtc8723b2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, bLowPenaltyRA);
+}
+
+static void halbtc8723b2ant_SwMechanism2(
+	PBTC_COEXIST pBtCoexist,
+	bool bAGCTableShift,
+	bool bADCBackOff,
+	bool bSWDACSwing,
+	u32 dacSwingLvl
+)
+{
+	halbtc8723b2ant_AgcTable(pBtCoexist, NORMAL_EXEC, bAGCTableShift);
+	halbtc8723b2ant_DacSwing(pBtCoexist, NORMAL_EXEC, bSWDACSwing, dacSwingLvl);
+}
+
+static void halbtc8723b2ant_SetAntPath(
+	PBTC_COEXIST pBtCoexist, u8 antPosType, bool bInitHwCfg, bool bWifiOff
+)
+{
+	PBTC_BOARD_INFO pBoardInfo = &pBtCoexist->boardInfo;
+	u32 fwVer = 0, u4Tmp = 0;
+	bool bPgExtSwitch = false;
+	bool bUseExtSwitch = false;
+	u8 	H2C_Parameter[2] = {0};
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_EXT_SWITCH, &bPgExtSwitch);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);	/*  [31:16]=fw ver, [15:0]=fw sub ver */
+
+	if ((fwVer > 0 && fwVer < 0xc0000) || bPgExtSwitch)
+		bUseExtSwitch = true;
+
+	if (bInitHwCfg) {
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x39, 0x8, 0x1);
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x974, 0xff);
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x944, 0x3, 0x3);
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x930, 0x77);
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x1);
+
+		if (fwVer >= 0x180000) {
+			/* Use H2C to set GNT_BT to LOW */
+			H2C_Parameter[0] = 0;
+			pBtCoexist->fBtcFillH2c(pBtCoexist, 0x6E, 1, H2C_Parameter);
+		} else {
+			pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x0);
+		}
+
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0); /* WiFi TRx Mask off */
+		pBtCoexist->fBtcSetBtReg(pBtCoexist, BTC_BT_REG_RF, 0x3c, 0x01); /* BT TRx Mask off */
+
+		if (pBoardInfo->btdmAntPos == BTC_ANTENNA_AT_MAIN_PORT) {
+			/* tell firmware "no antenna inverse" */
+			H2C_Parameter[0] = 0;
+		} else {
+			/* tell firmware "antenna inverse" */
+			H2C_Parameter[0] = 1;
+		}
+
+		if (bUseExtSwitch) {
+			/* ext switch type */
+			H2C_Parameter[1] = 1;
+		} else {
+			/* int switch type */
+			H2C_Parameter[1] = 0;
+		}
+		pBtCoexist->fBtcFillH2c(pBtCoexist, 0x65, 2, H2C_Parameter);
+	}
+
+	/*  ext switch setting */
+	if (bUseExtSwitch) {
+		if (bInitHwCfg) {
+			/*  0x4c[23]= 0, 0x4c[24]= 1  Antenna control by WL/BT */
+			u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+			u4Tmp &= ~BIT23;
+			u4Tmp |= BIT24;
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x4c, u4Tmp);
+		}
+
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0); /*  fixed internal switch S1->WiFi, S0->BT */
+		switch (antPosType) {
+		case BTC_ANT_WIFI_AT_MAIN:
+			pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x1);	/*  ext switch main at wifi */
+			break;
+		case BTC_ANT_WIFI_AT_AUX:
+			pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x92c, 0x3, 0x2);	/*  ext switch aux at wifi */
+			break;
+		}
+	} else { /*  internal switch */
+		if (bInitHwCfg) {
+			/*  0x4c[23]= 0, 0x4c[24]= 1  Antenna control by WL/BT */
+			u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+			u4Tmp |= BIT23;
+			u4Tmp &= ~BIT24;
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x4c, u4Tmp);
+		}
+
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x64, 0x1, 0x0); /* fixed external switch S1->Main, S0->Aux */
+		switch (antPosType) {
+		case BTC_ANT_WIFI_AT_MAIN:
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0); /*  fixed internal switch S1->WiFi, S0->BT */
+			break;
+		case BTC_ANT_WIFI_AT_AUX:
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x280); /*  fixed internal switch S0->WiFi, S1->BT */
+			break;
+		}
+	}
+}
+
+static void halbtc8723b2ant_PsTdma(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bTurnOn, u8 type
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s turn %s PS TDMA, type =%d\n",
+			(bForceExec ? "force to" : ""),
+			(bTurnOn ? "ON" : "OFF"),
+			type
+		)
+	);
+	pCoexDm->bCurPsTdmaOn = bTurnOn;
+	pCoexDm->curPsTdma = type;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n",
+				pCoexDm->bPrePsTdmaOn,
+				pCoexDm->bCurPsTdmaOn
+			)
+		);
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n",
+				pCoexDm->prePsTdma, pCoexDm->curPsTdma
+			)
+		);
+
+		if (
+			(pCoexDm->bPrePsTdmaOn == pCoexDm->bCurPsTdmaOn) &&
+			(pCoexDm->prePsTdma == pCoexDm->curPsTdma)
+		)
+			return;
+	}
+
+	if (bTurnOn) {
+		switch (type) {
+		case 1:
+		default:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
+			break;
+		case 2:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0xe1, 0x90);
+			break;
+		case 3:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1c, 0x3, 0xf1, 0x90);
+			break;
+		case 4:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x10, 0x03, 0xf1, 0x90);
+			break;
+		case 5:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x90);
+			break;
+		case 6:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0x60, 0x90);
+			break;
+		case 7:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1c, 0x3, 0x70, 0x90);
+			break;
+		case 8:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xa3, 0x10, 0x3, 0x70, 0x90);
+			break;
+		case 9:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
+			break;
+		case 10:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0xe1, 0x90);
+			break;
+		case 11:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0xe1, 0x90);
+			break;
+		case 12:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0xe1, 0x90);
+			break;
+		case 13:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x90);
+			break;
+		case 14:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0x60, 0x90);
+			break;
+		case 15:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0x60, 0x90);
+			break;
+		case 16:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0x60, 0x90);
+			break;
+		case 17:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xa3, 0x2f, 0x2f, 0x60, 0x90);
+			break;
+		case 18:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0xe1, 0x90);
+			break;
+		case 19:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x25, 0xe1, 0x90);
+			break;
+		case 20:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x25, 0x60, 0x90);
+			break;
+		case 21:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x15, 0x03, 0x70, 0x90);
+			break;
+		case 71:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
+			break;
+		}
+	} else {
+		/*  disable PS tdma */
+		switch (type) {
+		case 0:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x40, 0x0);
+			break;
+		case 1:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x48, 0x0);
+			break;
+		default:
+			halbtc8723b2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x40, 0x0);
+			break;
+		}
+	}
+
+	/*  update pre state */
+	pCoexDm->bPrePsTdmaOn = pCoexDm->bCurPsTdmaOn;
+	pCoexDm->prePsTdma = pCoexDm->curPsTdma;
+}
+
+static void halbtc8723b2ant_CoexAllOff(PBTC_COEXIST pBtCoexist)
+{
+	/*  fw all off */
+	halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+	halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	/*  sw all off */
+	halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+	halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+
+	/*  hw all off */
+	/* pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0); */
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+}
+
+static void halbtc8723b2ant_InitCoexDm(PBTC_COEXIST pBtCoexist)
+{
+	/*  force to reset coex mechanism */
+
+	halbtc8723b2ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 1);
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, FORCE_EXEC, 6);
+	halbtc8723b2ant_DecBtPwr(pBtCoexist, FORCE_EXEC, 0);
+
+	halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+	halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+}
+
+static void halbtc8723b2ant_ActionBtInquiry(PBTC_COEXIST pBtCoexist)
+{
+	bool bWifiConnected = false;
+	bool bLowPwrDisable = true;
+
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+	if (bWifiConnected) {
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+	} else {
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+	}
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, FORCE_EXEC, 6);
+	halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+	halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+
+	pCoexDm->bNeedRecover0x948 = true;
+	pCoexDm->backup0x948 = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+
+	halbtc8723b2ant_SetAntPath(pBtCoexist, BTC_ANT_WIFI_AT_AUX, false, false);
+}
+
+static bool halbtc8723b2ant_IsCommonAction(PBTC_COEXIST pBtCoexist)
+{
+	u8 btRssiState = BTC_RSSI_STATE_HIGH;
+	bool bCommon = false, bWifiConnected = false, bWifiBusy = false;
+	bool bBtHsOn = false, bLowPwrDisable = false;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+
+	if (!bWifiConnected) {
+		bLowPwrDisable = false;
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+		halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non-connected idle!!\n"));
+
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+		halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+		halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+		halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+
+		bCommon = true;
+	} else {
+		if (BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus) {
+			bLowPwrDisable = false;
+			pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+			halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT non connected-idle!!\n"));
+
+			pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+			halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+			halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+			halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0xb);
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+
+			bCommon = true;
+		} else if (BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) {
+			bLowPwrDisable = true;
+			pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+
+			if (bBtHsOn)
+				return false;
+
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT connected-idle!!\n"));
+			halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+			pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+			halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+			halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+			halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0xb);
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+
+			bCommon = true;
+		} else {
+			bLowPwrDisable = true;
+			pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
+
+			if (bWifiBusy) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi Connected-Busy + BT Busy!!\n"));
+				bCommon = false;
+			} else {
+				if (bBtHsOn)
+					return false;
+
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi Connected-Idle + BT Busy!!\n"));
+				btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+				halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+				pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+				halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+				halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 21);
+				halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0xb);
+
+				if (BTC_RSSI_HIGH(btRssiState))
+					halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+				else
+					halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+				halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+				halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+				bCommon = true;
+			}
+		}
+	}
+
+	return bCommon;
+}
+
+static void halbtc8723b2ant_TdmaDurationAdjust(
+	PBTC_COEXIST pBtCoexist, bool bScoHid, bool bTxPause, u8 maxInterval
+)
+{
+	static s32 up, dn, m, n, WaitCount;
+	s32 result;   /* 0: no change, +1: increase WiFi duration, -1: decrease WiFi duration */
+	u8 retryCount = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], TdmaDurationAdjust()\n"));
+
+	if (!pCoexDm->bAutoTdmaAdjust) {
+		pCoexDm->bAutoTdmaAdjust = true;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], first run TdmaDurationAdjust()!!\n"));
+		{
+			if (bScoHid) {
+				if (bTxPause) {
+					if (maxInterval == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+						pCoexDm->psTdmaDuAdjType = 13;
+					} else if (maxInterval == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					} else if (maxInterval == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+				} else {
+					if (maxInterval == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+						pCoexDm->psTdmaDuAdjType = 9;
+					} else if (maxInterval == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					} else if (maxInterval == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+				}
+			} else {
+				if (bTxPause) {
+					if (maxInterval == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+						pCoexDm->psTdmaDuAdjType = 5;
+					} else if (maxInterval == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (maxInterval == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+				} else {
+					if (maxInterval == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+						pCoexDm->psTdmaDuAdjType = 1;
+					} else if (maxInterval == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (maxInterval == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+				}
+			}
+		}
+		/*  */
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		WaitCount = 0;
+	} else {
+		/* accquire the BT TRx retry count from BT_Info byte2 */
+		retryCount = pCoexSta->btRetryCnt;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], retryCount = %d\n", retryCount));
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], up =%d, dn =%d, m =%d, n =%d, WaitCount =%d\n",
+				up, dn, m, n, WaitCount
+			)
+		);
+		result = 0;
+		WaitCount++;
+
+		if (retryCount == 0) { /*  no retry in the last 2-second duration */
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;
+
+			if (up >= n) { /*  if 連續 n 個2秒 retry count為0, 則調寬WiFi duration */
+				WaitCount = 0;
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Increase wifi duration!!\n"));
+			}
+		} else if (retryCount <= 3) { /*  <=3 retry in the last 2-second duration */
+			up--;
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2) { /*  if 連續 2 個2秒 retry count< 3, 則調窄WiFi duration */
+				if (WaitCount <= 2)
+					m++; /*  避免一直在兩個level中來回 */
+				else
+					m = 1;
+
+				if (m >= 20) /* m 最大值 = 20 ' 最大120秒 recheck是否調整 WiFi duration. */
+					m = 20;
+
+				n = 3*m;
+				up = 0;
+				dn = 0;
+				WaitCount = 0;
+				result = -1;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
+			}
+		} else { /* retry count > 3, 只要1次 retry count > 3, 則調窄WiFi duration */
+			if (WaitCount == 1)
+				m++; /*  避免一直在兩個level中來回 */
+			else
+				m = 1;
+
+			if (m >= 20) /* m 最大值 = 20 ' 最大120秒 recheck是否調整 WiFi duration. */
+				m = 20;
+
+			n = 3*m;
+			up = 0;
+			dn = 0;
+			WaitCount = 0;
+			result = -1;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter>3!!\n"));
+		}
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], max Interval = %d\n", maxInterval));
+		if (maxInterval == 1) {
+			if (bTxPause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
+
+				if (pCoexDm->curPsTdma == 71) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+					pCoexDm->psTdmaDuAdjType = 5;
+				} else if (pCoexDm->curPsTdma == 1) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+					pCoexDm->psTdmaDuAdjType = 5;
+				} else if (pCoexDm->curPsTdma == 2) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+					pCoexDm->psTdmaDuAdjType = 6;
+				} else if (pCoexDm->curPsTdma == 3) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				} else if (pCoexDm->curPsTdma == 4) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+					pCoexDm->psTdmaDuAdjType = 8;
+				}
+
+				if (pCoexDm->curPsTdma == 9) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+					pCoexDm->psTdmaDuAdjType = 13;
+				} else if (pCoexDm->curPsTdma == 10) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+					pCoexDm->psTdmaDuAdjType = 14;
+				} else if (pCoexDm->curPsTdma == 11) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				} else if (pCoexDm->curPsTdma == 12) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+					pCoexDm->psTdmaDuAdjType = 16;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 5) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+						pCoexDm->psTdmaDuAdjType = 8;
+					} else if (pCoexDm->curPsTdma == 13) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+						pCoexDm->psTdmaDuAdjType = 16;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 8) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+						pCoexDm->psTdmaDuAdjType = 5;
+					} else if (pCoexDm->curPsTdma == 16) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+						pCoexDm->psTdmaDuAdjType = 13;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
+				if (pCoexDm->curPsTdma == 5) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 71);
+					pCoexDm->psTdmaDuAdjType = 71;
+				} else if (pCoexDm->curPsTdma == 6) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+					pCoexDm->psTdmaDuAdjType = 2;
+				} else if (pCoexDm->curPsTdma == 7) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				} else if (pCoexDm->curPsTdma == 8) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+					pCoexDm->psTdmaDuAdjType = 4;
+				}
+
+				if (pCoexDm->curPsTdma == 13) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+					pCoexDm->psTdmaDuAdjType = 9;
+				} else if (pCoexDm->curPsTdma == 14) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+					pCoexDm->psTdmaDuAdjType = 10;
+				} else if (pCoexDm->curPsTdma == 15) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				} else if (pCoexDm->curPsTdma == 16) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+					pCoexDm->psTdmaDuAdjType = 12;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 71) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+						pCoexDm->psTdmaDuAdjType = 1;
+					} else if (pCoexDm->curPsTdma == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+						pCoexDm->psTdmaDuAdjType = 4;
+					} else if (pCoexDm->curPsTdma == 9) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+						pCoexDm->psTdmaDuAdjType = 12;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 4) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+						pCoexDm->psTdmaDuAdjType = 1;
+					} else if (pCoexDm->curPsTdma == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 71);
+						pCoexDm->psTdmaDuAdjType = 71;
+					} else if (pCoexDm->curPsTdma == 12) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+						pCoexDm->psTdmaDuAdjType = 9;
+					}
+				}
+			}
+		} else if (maxInterval == 2) {
+			if (bTxPause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
+				if (pCoexDm->curPsTdma == 1) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+					pCoexDm->psTdmaDuAdjType = 6;
+				} else if (pCoexDm->curPsTdma == 2) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+					pCoexDm->psTdmaDuAdjType = 6;
+				} else if (pCoexDm->curPsTdma == 3) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				} else if (pCoexDm->curPsTdma == 4) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+					pCoexDm->psTdmaDuAdjType = 8;
+				}
+
+				if (pCoexDm->curPsTdma == 9) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+					pCoexDm->psTdmaDuAdjType = 14;
+				} else if (pCoexDm->curPsTdma == 10) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+					pCoexDm->psTdmaDuAdjType = 14;
+				} else if (pCoexDm->curPsTdma == 11) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				} else if (pCoexDm->curPsTdma == 12) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+					pCoexDm->psTdmaDuAdjType = 16;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 5) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+						pCoexDm->psTdmaDuAdjType = 8;
+					} else if (pCoexDm->curPsTdma == 13) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+						pCoexDm->psTdmaDuAdjType = 16;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 8) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (pCoexDm->curPsTdma == 16) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
+				if (pCoexDm->curPsTdma == 5) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+					pCoexDm->psTdmaDuAdjType = 2;
+				} else if (pCoexDm->curPsTdma == 6) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+					pCoexDm->psTdmaDuAdjType = 2;
+				} else if (pCoexDm->curPsTdma == 7) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				} else if (pCoexDm->curPsTdma == 8) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+					pCoexDm->psTdmaDuAdjType = 4;
+				}
+
+				if (pCoexDm->curPsTdma == 13) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+					pCoexDm->psTdmaDuAdjType = 10;
+				} else if (pCoexDm->curPsTdma == 14) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+					pCoexDm->psTdmaDuAdjType = 10;
+				} else if (pCoexDm->curPsTdma == 15) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				} else if (pCoexDm->curPsTdma == 16) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+					pCoexDm->psTdmaDuAdjType = 12;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+						pCoexDm->psTdmaDuAdjType = 4;
+					} else if (pCoexDm->curPsTdma == 9) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+						pCoexDm->psTdmaDuAdjType = 12;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 4) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (pCoexDm->curPsTdma == 12) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					}
+				}
+			}
+		} else if (maxInterval == 3) {
+			if (bTxPause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
+				if (pCoexDm->curPsTdma == 1) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				} else if (pCoexDm->curPsTdma == 2) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				} else if (pCoexDm->curPsTdma == 3) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				} else if (pCoexDm->curPsTdma == 4) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+					pCoexDm->psTdmaDuAdjType = 8;
+				}
+
+				if (pCoexDm->curPsTdma == 9) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				} else if (pCoexDm->curPsTdma == 10) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				} else if (pCoexDm->curPsTdma == 11) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				} else if (pCoexDm->curPsTdma == 12) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+					pCoexDm->psTdmaDuAdjType = 16;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 5) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+						pCoexDm->psTdmaDuAdjType = 8;
+					} else if (pCoexDm->curPsTdma == 13) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+						pCoexDm->psTdmaDuAdjType = 16;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 8) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 16) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
+				if (pCoexDm->curPsTdma == 5) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				} else if (pCoexDm->curPsTdma == 6) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				} else if (pCoexDm->curPsTdma == 7) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				} else if (pCoexDm->curPsTdma == 8) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+					pCoexDm->psTdmaDuAdjType = 4;
+				}
+
+				if (pCoexDm->curPsTdma == 13) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				} else if (pCoexDm->curPsTdma == 14) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				} else if (pCoexDm->curPsTdma == 15) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				} else if (pCoexDm->curPsTdma == 16) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+					pCoexDm->psTdmaDuAdjType = 12;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+						pCoexDm->psTdmaDuAdjType = 4;
+					} else if (pCoexDm->curPsTdma == 9) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+						pCoexDm->psTdmaDuAdjType = 12;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 4) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 12) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+				}
+			}
+		}
+	}
+
+	/*  if current PsTdma not match with the recorded one (when scan, dhcp...), */
+	/*  then we have to adjust it back to the previous record one. */
+	if (pCoexDm->curPsTdma != pCoexDm->psTdmaDuAdjType) {
+		bool bScan = false, bLink = false, bRoam = false;
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], PsTdma type dismatch!!!, curPsTdma =%d, recordPsTdma =%d\n",
+				pCoexDm->curPsTdma,
+				pCoexDm->psTdmaDuAdjType
+			)
+		);
+
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+
+		if (!bScan && !bLink && !bRoam)
+			halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, pCoexDm->psTdmaDuAdjType);
+		else {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n"));
+		}
+	}
+}
+
+/*  SCO only or SCO+PAN(HS) */
+static void halbtc8723b2ant_ActionSco(PBTC_COEXIST pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 4);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if (BTC_WIFI_BW_LEGACY == wifiBw) /* for SCO quality at 11b/g mode */
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+	else  /* for SCO quality & wifi performance balance at 11n mode */
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 8);
+
+	halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 0); /* for voice quality */
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, true, 0x4);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, true, 0x4);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, true, 0x4);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, true, 0x4);
+		}
+	}
+}
+
+
+static void halbtc8723b2ant_ActionHid(PBTC_COEXIST pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if (BTC_WIFI_BW_LEGACY == wifiBw) /* for HID at 11b/g mode */
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+	else  /* for HID quality & wifi performance balance at 11n mode */
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 9);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	)
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+	else
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
+static void halbtc8723b2ant_ActionA2dp(PBTC_COEXIST pBtCoexist)
+{
+	u8 wifiRssiState, wifiRssiState1, btRssiState;
+	u32 wifiBw;
+	u8 apNum = 0;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	wifiRssiState1 = halbtc8723b2ant_WifiRssiState(pBtCoexist, 1, 2, 40, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_AP_NUM, &apNum);
+
+	/*  define the office environment */
+	if (apNum >= 10 && BTC_RSSI_HIGH(wifiRssiState1)) {
+		/* DbgPrint(" AP#>10(%d)\n", apNum); */
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+		halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+		halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+
+		/*  sw mechanism */
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+		if (BTC_WIFI_BW_HT40 == wifiBw) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, true, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, true, 0x18);
+		}
+		return;
+	}
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	)
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, false, 1);
+	else
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, true, 1);
+
+	/*  sw mechanism */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_ActionA2dpPanHs(PBTC_COEXIST pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+
+	halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, true, 2);
+
+	/*  sw mechanism */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_ActionPanEdr(PBTC_COEXIST pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 10);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	)
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+	else
+		halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+
+	/*  sw mechanism */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+
+/* PAN(HS) only */
+static void halbtc8723b2ant_ActionPanHs(PBTC_COEXIST pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+
+	halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, false, 1);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/* PAN(EDR)+A2DP */
+static void halbtc8723b2ant_ActionPanEdrA2dp(PBTC_COEXIST pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	) {
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 12);
+		if (BTC_WIFI_BW_HT40 == wifiBw)
+			halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, true, 3);
+		else
+			halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, false, 3);
+	} else {
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, false, true, 3);
+	}
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, false, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_ActionPanEdrHid(PBTC_COEXIST pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	) {
+		if (BTC_WIFI_BW_HT40 == wifiBw) {
+			halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 3);
+			halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 11);
+			pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x780);
+		} else {
+			halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+			halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+			pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+		}
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, false, 2);
+	} else {
+		halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+		halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 11);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, true, 2);
+	}
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/*  HID+A2DP+PAN(EDR) */
+static void halbtc8723b2ant_ActionHidA2dpPanEdr(PBTC_COEXIST pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	if (BTC_RSSI_HIGH(btRssiState))
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+	else
+		halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	) {
+		if (BTC_WIFI_BW_HT40 == wifiBw)
+			halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, true, 2);
+		else
+			halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, false, 3);
+	} else
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, true, 3);
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_ActionHidA2dp(PBTC_COEXIST pBtCoexist)
+{
+	u8 wifiRssiState, btRssiState;
+	u32 wifiBw;
+	u8 apNum = 0;
+
+	wifiRssiState = halbtc8723b2ant_WifiRssiState(pBtCoexist, 0, 2, 15, 0);
+	/* btRssiState = halbtc8723b2ant_BtRssiState(2, 29, 0); */
+	btRssiState = halbtc8723b2ant_BtRssiState(3, 29, 37);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, true, 0x5);
+
+	halbtc8723b2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 6);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	if (BTC_WIFI_BW_LEGACY == wifiBw) {
+		if (BTC_RSSI_HIGH(btRssiState))
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+		else if (BTC_RSSI_MEDIUM(btRssiState))
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+		else
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+	} else {
+		/*  only 802.11N mode we have to dec bt power to 4 degree */
+		if (BTC_RSSI_HIGH(btRssiState)) {
+			pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_AP_NUM, &apNum);
+			/*  need to check ap Number of Not */
+			if (apNum < 10)
+				halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 4);
+			else
+				halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+		} else if (BTC_RSSI_MEDIUM(btRssiState))
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
+		else
+			halbtc8723b2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
+	}
+
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);
+
+	if (
+		(btRssiState == BTC_RSSI_STATE_HIGH) ||
+		(btRssiState == BTC_RSSI_STATE_STAY_HIGH)
+	)
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, false, 2);
+	else
+		halbtc8723b2ant_TdmaDurationAdjust(pBtCoexist, true, true, 2);
+
+	/*  sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifiBw) {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, true, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	} else {
+		if (
+			(wifiRssiState == BTC_RSSI_STATE_HIGH) ||
+			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH)
+		) {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, true, false, false, 0x18);
+		} else {
+			halbtc8723b2ant_SwMechanism1(pBtCoexist, false, true, false, false);
+			halbtc8723b2ant_SwMechanism2(pBtCoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_RunCoexistMechanism(PBTC_COEXIST pBtCoexist)
+{
+	u8 algorithm = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism() ===>\n"));
+
+	if (pBtCoexist->bManualControl) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n"));
+		return;
+	}
+
+	if (pCoexSta->bUnderIps) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is under IPS !!!\n"));
+		return;
+	}
+
+	algorithm = halbtc8723b2ant_ActionAlgorithm(pBtCoexist);
+	if (pCoexSta->bC2hBtInquiryPage && (BT_8723B_2ANT_COEX_ALGO_PANHS != algorithm)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT is under inquiry/page scan !!\n"));
+		halbtc8723b2ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else {
+		if (pCoexDm->bNeedRecover0x948) {
+			pCoexDm->bNeedRecover0x948 = false;
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, pCoexDm->backup0x948);
+		}
+	}
+
+	pCoexDm->curAlgorithm = algorithm;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Algorithm = %d\n", pCoexDm->curAlgorithm));
+
+	if (halbtc8723b2ant_IsCommonAction(pBtCoexist)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant common.\n"));
+		pCoexDm->bAutoTdmaAdjust = false;
+	} else {
+		if (pCoexDm->curAlgorithm != pCoexDm->preAlgorithm) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE,
+				(
+					"[BTCoex], preAlgorithm =%d, curAlgorithm =%d\n",
+					pCoexDm->preAlgorithm,
+					pCoexDm->curAlgorithm
+				)
+			);
+			pCoexDm->bAutoTdmaAdjust = false;
+		}
+
+
+		switch (pCoexDm->curAlgorithm) {
+		case BT_8723B_2ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = SCO.\n"));
+			halbtc8723b2ant_ActionSco(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID.\n"));
+			halbtc8723b2ant_ActionHid(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = A2DP.\n"));
+			halbtc8723b2ant_ActionA2dp(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = A2DP+PAN(HS).\n"));
+			halbtc8723b2ant_ActionA2dpPanHs(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN(EDR).\n"));
+			halbtc8723b2ant_ActionPanEdr(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HS mode.\n"));
+			halbtc8723b2ant_ActionPanHs(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN+A2DP.\n"));
+			halbtc8723b2ant_ActionPanEdrA2dp(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN(EDR)+HID.\n"));
+			halbtc8723b2ant_ActionPanEdrHid(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID+A2DP+PAN.\n"));
+			halbtc8723b2ant_ActionHidA2dpPanEdr(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID+A2DP.\n"));
+			halbtc8723b2ant_ActionHidA2dp(pBtCoexist);
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = coexist All Off!!\n"));
+			halbtc8723b2ant_CoexAllOff(pBtCoexist);
+			break;
+		}
+		pCoexDm->preAlgorithm = pCoexDm->curAlgorithm;
+	}
+}
+
+static void halbtc8723b2ant_WifiOffHwCfg(PBTC_COEXIST pBtCoexist)
+{
+	bool bIsInMpMode = false;
+	u8 H2C_Parameter[2] = {0};
+	u32 fwVer = 0;
+
+	/*  set wlan_act to low */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0x4);
+
+	pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1, 0xfffff, 0x780); /* WiFi goto standby while GNT_BT 0-->1 */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+	if (fwVer >= 0x180000) {
+		/* Use H2C to set GNT_BT to HIGH */
+		H2C_Parameter[0] = 1;
+		pBtCoexist->fBtcFillH2c(pBtCoexist, 0x6E, 1, H2C_Parameter);
+	} else
+		pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x18);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_IS_IN_MP_MODE, &bIsInMpMode);
+	if (!bIsInMpMode)
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x0); /* BT select s0/s1 is controlled by BT */
+	else
+		pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x67, 0x20, 0x1); /* BT select s0/s1 is controlled by WiFi */
+}
+
+static void halbtc8723b2ant_InitHwConfig(PBTC_COEXIST pBtCoexist, bool bBackUp)
+{
+	u8 u1Tmp = 0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], 2Ant Init HW Config!!\n"));
+
+	/*  backup rf 0x1e value */
+	pCoexDm->btRf0x1eBackup =
+		pBtCoexist->fBtcGetRfReg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff);
+
+	/*  0x790[5:0]= 0x5 */
+	u1Tmp = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x790);
+	u1Tmp &= 0xc0;
+	u1Tmp |= 0x5;
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x790, u1Tmp);
+
+	/* Antenna config */
+	halbtc8723b2ant_SetAntPath(pBtCoexist, BTC_ANT_WIFI_AT_MAIN, true, false);
+
+	/*  PTA parameter */
+	halbtc8723b2ant_CoexTableWithType(pBtCoexist, FORCE_EXEC, 0);
+
+	/*  Enable counter statistics */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0xc); /* 0x76e[3] = 1, WLAN_Act control by PTA */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x778, 0x3);
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x40, 0x20, 0x1);
+}
+
+/*  */
+/*  work around function start with wa_halbtc8723b2ant_ */
+/*  */
+/*  */
+/*  extern function start with EXhalbtc8723b2ant_ */
+/*  */
+void EXhalbtc8723b2ant_PowerOnSetting(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BOARD_INFO pBoardInfo = &pBtCoexist->boardInfo;
+	u8 u1Tmp = 0x4; /* Set BIT2 by default since it's 2ant case */
+	u16 u2Tmp = 0x0;
+
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x67, 0x20);
+
+	/*  enable BB, REG_SYS_FUNC_EN such that we can write 0x948 correctly. */
+	u2Tmp = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0x2);
+	pBtCoexist->fBtcWrite2Byte(pBtCoexist, 0x2, u2Tmp|BIT0|BIT1);
+
+	/*  set GRAN_BT = 1 */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x765, 0x18);
+	/*  set WLAN_ACT = 0 */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0x4);
+
+	/*  */
+	/*  S0 or S1 setting and Local register setting(By the setting fw can get ant number, S0/S1, ... info) */
+	/*  Local setting bit define */
+	/* 	BIT0: "0" for no antenna inverse; "1" for antenna inverse */
+	/* 	BIT1: "0" for internal switch; "1" for external switch */
+	/* 	BIT2: "0" for one antenna; "1" for two antenna */
+	/*  NOTE: here default all internal switch and 1-antenna ==> BIT1 = 0 and BIT2 = 0 */
+	if (pBtCoexist->chipInterface == BTC_INTF_USB) {
+		/*  fixed at S0 for USB interface */
+		pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+
+		u1Tmp |= 0x1;	/*  antenna inverse */
+		pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0xfe08, u1Tmp);
+
+		pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_AUX_PORT;
+	} else {
+		/*  for PCIE and SDIO interface, we check efuse 0xc3[6] */
+		if (pBoardInfo->singleAntPath == 0) {
+			/*  set to S1 */
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x280);
+			pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_MAIN_PORT;
+		} else if (pBoardInfo->singleAntPath == 1) {
+			/*  set to S0 */
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, 0x0);
+			u1Tmp |= 0x1;	/*  antenna inverse */
+			pBoardInfo->btdmAntPos = BTC_ANTENNA_AT_AUX_PORT;
+		}
+
+		if (pBtCoexist->chipInterface == BTC_INTF_PCI)
+			pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0x384, u1Tmp);
+		else if (pBtCoexist->chipInterface == BTC_INTF_SDIO)
+			pBtCoexist->fBtcWriteLocalReg1Byte(pBtCoexist, 0x60, u1Tmp);
+	}
+}
+
+void EXhalbtc8723b2ant_InitHwConfig(PBTC_COEXIST pBtCoexist, bool bWifiOnly)
+{
+	halbtc8723b2ant_InitHwConfig(pBtCoexist, true);
+}
+
+void EXhalbtc8723b2ant_InitCoexDm(PBTC_COEXIST pBtCoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Coex Mechanism Init!!\n"));
+
+	halbtc8723b2ant_InitCoexDm(pBtCoexist);
+}
+
+void EXhalbtc8723b2ant_DisplayCoexInfo(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BOARD_INFO pBoardInfo = &pBtCoexist->boardInfo;
+	PBTC_STACK_INFO pStackInfo = &pBtCoexist->stackInfo;
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	u8 *cliBuf = pBtCoexist->cliBuf;
+	u8 u1Tmp[4], i, btInfoExt, psTdmaCase = 0;
+	u32 u4Tmp[4];
+	bool bRoam = false, bScan = false, bLink = false, bWifiUnder5G = false;
+	bool bBtHsOn = false, bWifiBusy = false;
+	s32 wifiRssi = 0, btHsRssi = 0;
+	u32 wifiBw, wifiTrafficDir, faOfdm, faCck;
+	u8 wifiDot11Chnl, wifiHsChnl;
+	u32 fwVer = 0, btPatchVer = 0;
+	u8 apNum = 0;
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
+	CL_PRINTF(cliBuf);
+
+	if (pBtCoexist->bManualControl) {
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[Under Manual Control]============");
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ==========================================");
+		CL_PRINTF(cliBuf);
+	}
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:", \
+		pBoardInfo->pgAntNum,
+		pBoardInfo->btdmAntNum
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
+		(pStackInfo->bProfileNotified ? "Yes" : "No"),
+		pStackInfo->hciVersion
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)", "CoexVer/ FwVer/ PatchVer", \
+		GLCoexVerDate8723b2Ant, GLCoexVer8723b2Ant, fwVer, btPatchVer, btPatchVer);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
+		wifiDot11Chnl,
+		wifiHsChnl,
+		bBtHsOn
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
+		pCoexDm->wifiChnlInfo[0],
+		pCoexDm->wifiChnlInfo[1],
+		pCoexDm->wifiChnlInfo[2]
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_AP_NUM, &apNum);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d", "Wifi rssi/ HS rssi/ AP#", \
+		wifiRssi,
+		btHsRssi,
+		apNum
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d ", "Wifi bLink/ bRoam/ bScan", \
+		bLink,
+		bRoam,
+		bScan
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_UNDER_5G, &bWifiUnder5G);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s / %s/ %s ", "Wifi status", \
+		(bWifiUnder5G ? "5G" : "2.4G"),
+		((BTC_WIFI_BW_LEGACY == wifiBw) ? "Legacy" : (((BTC_WIFI_BW_HT40 == wifiBw) ? "HT40" : "HT20"))),
+		((!bWifiBusy) ? "idle" : ((BTC_WIFI_TRAFFIC_TX == wifiTrafficDir) ? "uplink" : "downlink"))
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]", \
+		((pBtCoexist->btInfo.bBtDisabled) ? ("disabled") : ((pCoexSta->bC2hBtInquiryPage) ? ("inquiry/page scan") : ((BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus) ? "non-connected idle" :
+		((BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) ? "connected-idle" : "busy")))),
+		pCoexSta->btRssi,
+		pCoexSta->btRetryCnt
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
+		pBtLinkInfo->bScoExist,
+		pBtLinkInfo->bHidExist,
+		pBtLinkInfo->bPanExist,
+		pBtLinkInfo->bA2dpExist
+	);
+	CL_PRINTF(cliBuf);
+	pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_BT_LINK_INFO);
+
+	btInfoExt = pCoexSta->btInfoExt;
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s", "BT Info A2DP rate", \
+		(btInfoExt&BIT0) ? "Basic rate" : "EDR rate"
+	);
+	CL_PRINTF(cliBuf);
+
+	for (i = 0; i < BT_INFO_SRC_8723B_2ANT_MAX; i++) {
+		if (pCoexSta->btInfoC2hCnt[i]) {
+			CL_SPRINTF(
+				cliBuf,
+				BT_TMP_BUF_SIZE,
+				"\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8723b2Ant[i], \
+				pCoexSta->btInfoC2h[i][0],
+				pCoexSta->btInfoC2h[i][1],
+				pCoexSta->btInfoC2h[i][2],
+				pCoexSta->btInfoC2h[i][3],
+				pCoexSta->btInfoC2h[i][4],
+				pCoexSta->btInfoC2h[i][5],
+				pCoexSta->btInfoC2h[i][6],
+				pCoexSta->btInfoC2hCnt[i]
+			);
+			CL_PRINTF(cliBuf);
+		}
+	}
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s/%s", "PS state, IPS/LPS", \
+		((pCoexSta->bUnderIps ? "IPS ON" : "IPS OFF")),
+		((pCoexSta->bUnderLps ? "LPS ON" : "LPS OFF"))
+	);
+	CL_PRINTF(cliBuf);
+	pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	/*  Sw mechanism */
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s", "============[Sw mechanism]============"
+	);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d ", "SM1[ShRf/ LpRA/ LimDig]", \
+		pCoexDm->bCurRfRxLpfShrink,
+		pCoexDm->bCurLowPenaltyRa,
+		pCoexDm->bLimitedDig
+	);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d(0x%x) ",
+		"SM2[AgcT/ AdcB/ SwDacSwing(lvl)]", \
+		pCoexDm->bCurAgcTableEn,
+		pCoexDm->bCurAdcBackOff,
+		pCoexDm->bCurDacSwingOn,
+		pCoexDm->curDacSwingLvl
+	);
+	CL_PRINTF(cliBuf);
+
+	/*  Fw mechanism */
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
+	CL_PRINTF(cliBuf);
+
+	psTdmaCase = pCoexDm->curPsTdma;
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)", "PS TDMA", \
+		pCoexDm->psTdmaPara[0],
+		pCoexDm->psTdmaPara[1],
+		pCoexDm->psTdmaPara[2],
+		pCoexDm->psTdmaPara[3],
+		pCoexDm->psTdmaPara[4],
+		psTdmaCase, pCoexDm->bAutoTdmaAdjust
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d ", "DecBtPwr/ IgnWlanAct", \
+		pCoexDm->curBtDecPwrLvl,
+		pCoexDm->bCurIgnoreWlanAct
+	);
+	CL_PRINTF(cliBuf);
+
+	/*  Hw setting */
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x", "RF-A, 0x1e initVal", \
+		pCoexDm->btRf0x1eBackup
+	);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x778);
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x880);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x", "0x778/0x880[29:25]", \
+		u1Tmp[0],
+		(u4Tmp[0]&0x3e000000) >> 25
+	);
+	CL_PRINTF(cliBuf);
+
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x67);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x765);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x948/ 0x67[5] / 0x765", \
+		u4Tmp[0],
+		((u1Tmp[0]&0x20)>>5),
+		u1Tmp[1]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x92c);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x930);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x944);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]", \
+		u4Tmp[0]&0x3,
+		u4Tmp[1]&0xff,
+		u4Tmp[2]&0x3
+	);
+	CL_PRINTF(cliBuf);
+
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x39);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x40);
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+	u1Tmp[2] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x64);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x38[11]/0x40/0x4c[24:23]/0x64[0]", \
+		((u1Tmp[0] & 0x8)>>3),
+		u1Tmp[1],
+		((u4Tmp[0]&0x01800000)>>23),
+		u1Tmp[2]&0x1
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x550);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x522);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
+		u4Tmp[0],
+		u1Tmp[0]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xc50);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x49c);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x", "0xc50(dig)/0x49c(null-drop)", \
+		u4Tmp[0]&0xff,
+		u1Tmp[0]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda0);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda4);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda8);
+	u4Tmp[3] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xcf0);
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0xa5b);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0xa5c);
+
+	faOfdm =
+		((u4Tmp[0]&0xffff0000) >> 16) +
+		((u4Tmp[1]&0xffff0000) >> 16) +
+		(u4Tmp[1] & 0xffff) +  (u4Tmp[2] & 0xffff) + \
+		((u4Tmp[3]&0xffff0000) >> 16) +
+		(u4Tmp[3] & 0xffff);
+
+	faCck = (u1Tmp[0] << 8) + u1Tmp[1];
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "OFDM-CCA/OFDM-FA/CCK-FA", \
+		u4Tmp[0]&0xffff,
+		faOfdm,
+		faCck
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c0);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c4);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c8);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x6cc);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
+		u4Tmp[0],
+		u4Tmp[1],
+		u4Tmp[2],
+		u1Tmp[0]
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d", "0x770(high-pri rx/tx)", \
+		pCoexSta->highPriorityRx,
+		pCoexSta->highPriorityTx
+	);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d", "0x774(low-pri rx/tx)", \
+		pCoexSta->lowPriorityRx,
+		pCoexSta->lowPriorityTx
+	);
+	CL_PRINTF(cliBuf);
+
+	halbtc8723b2ant_MonitorBtCtr(pBtCoexist);
+	pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+
+void EXhalbtc8723b2ant_IpsNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n"));
+		pCoexSta->bUnderIps = true;
+		halbtc8723b2ant_WifiOffHwCfg(pBtCoexist);
+		halbtc8723b2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
+		halbtc8723b2ant_CoexAllOff(pBtCoexist);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n"));
+		pCoexSta->bUnderIps = false;
+		halbtc8723b2ant_InitHwConfig(pBtCoexist, false);
+		halbtc8723b2ant_InitCoexDm(pBtCoexist);
+		halbtc8723b2ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b2ant_LpsNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n"));
+		pCoexSta->bUnderLps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n"));
+		pCoexSta->bUnderLps = false;
+	}
+}
+
+void EXhalbtc8723b2ant_ScanNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (BTC_SCAN_START == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n"));
+	} else if (BTC_SCAN_FINISH == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n"));
+	}
+}
+
+void EXhalbtc8723b2ant_ConnectNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (BTC_ASSOCIATE_START == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
+	} else if (BTC_ASSOCIATE_FINISH == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
+	}
+}
+
+void EXhalbtc8723b2ant_MediaStatusNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	u8 H2C_Parameter[3] = {0};
+	u32 wifiBw;
+	u8 wifiCentralChnl;
+	u8 apNum = 0;
+
+	if (BTC_MEDIA_CONNECT == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA connect notify\n"));
+	} else {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA disconnect notify\n"));
+	}
+
+	/*  only 2.4G we need to inform bt the chnl mask */
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifiCentralChnl);
+	if ((BTC_MEDIA_CONNECT == type) && (wifiCentralChnl <= 14)) {
+		H2C_Parameter[0] = 0x1;
+		H2C_Parameter[1] = wifiCentralChnl;
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+		if (BTC_WIFI_BW_HT40 == wifiBw)
+			H2C_Parameter[2] = 0x30;
+		else {
+			pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_AP_NUM, &apNum);
+			if (apNum < 10)
+				H2C_Parameter[2] = 0x30;
+			else
+				H2C_Parameter[2] = 0x20;
+		}
+	}
+
+	pCoexDm->wifiChnlInfo[0] = H2C_Parameter[0];
+	pCoexDm->wifiChnlInfo[1] = H2C_Parameter[1];
+	pCoexDm->wifiChnlInfo[2] = H2C_Parameter[2];
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], FW write 0x66 = 0x%x\n",
+			H2C_Parameter[0]<<16|H2C_Parameter[1]<<8|H2C_Parameter[2]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x66, 3, H2C_Parameter);
+}
+
+void EXhalbtc8723b2ant_SpecialPacketNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (type == BTC_PACKET_DHCP) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], DHCP Packet notify\n"));
+	}
+}
+
+void EXhalbtc8723b2ant_BtInfoNotify(
+	PBTC_COEXIST pBtCoexist, u8 *tmpBuf, u8 length
+)
+{
+	u8 	btInfo = 0;
+	u8 	i, rspSource = 0;
+	bool bBtBusy = false, bLimitedDig = false;
+	bool bWifiConnected = false;
+
+	pCoexSta->bC2hBtInfoReqSent = false;
+
+	rspSource = tmpBuf[0]&0xf;
+	if (rspSource >= BT_INFO_SRC_8723B_2ANT_MAX)
+		rspSource = BT_INFO_SRC_8723B_2ANT_WIFI_FW;
+
+	pCoexSta->btInfoC2hCnt[rspSource]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Bt info[%d], length =%d, hex data =[", rspSource, length));
+	for (i = 0; i < length; i++) {
+		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
+		if (i == 1)
+			btInfo = tmpBuf[i];
+
+		if (i == length-1) {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x]\n", tmpBuf[i]));
+		} else {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x, ", tmpBuf[i]));
+		}
+	}
+
+	if (pBtCoexist->bManualControl) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for Manual CTRL<===\n"));
+		return;
+	}
+
+	if (BT_INFO_SRC_8723B_2ANT_WIFI_FW != rspSource) {
+		pCoexSta->btRetryCnt = pCoexSta->btInfoC2h[rspSource][2]&0xf; /* [3:0] */
+
+		pCoexSta->btRssi = pCoexSta->btInfoC2h[rspSource][3]*2+10;
+
+		pCoexSta->btInfoExt = pCoexSta->btInfoC2h[rspSource][4];
+
+		pCoexSta->bBtTxRxMask = (pCoexSta->btInfoC2h[rspSource][2]&0x40);
+		pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_TX_RX_MASK, &pCoexSta->bBtTxRxMask);
+		if (pCoexSta->bBtTxRxMask) {
+			/* BT into is responded by BT FW and BT RF REG 0x3C != 0x01 => Need to switch BT TRx Mask */
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Switch BT TRx Mask since BT RF REG 0x3C != 0x01\n"));
+			pBtCoexist->fBtcSetBtReg(pBtCoexist, BTC_BT_REG_RF, 0x3c, 0x01);
+		}
+
+		/*  Here we need to resend some wifi info to BT */
+		/*  because bt is reset and loss of the info. */
+		if ((pCoexSta->btInfoExt & BIT1)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n"));
+			pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+
+			if (bWifiConnected)
+				EXhalbtc8723b2ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_CONNECT);
+			else
+				EXhalbtc8723b2ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+		}
+
+		if ((pCoexSta->btInfoExt & BIT3)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT ext info bit3 check, set BT NOT to ignore Wlan active!!\n"));
+			halbtc8723b2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, false);
+		} else {
+			/*  BT already NOT ignore Wlan active, do nothing here. */
+		}
+	}
+
+	/*  check BIT2 first ==> check if bt is under inquiry or page scan */
+	if (btInfo & BT_INFO_8723B_2ANT_B_INQ_PAGE)
+		pCoexSta->bC2hBtInquiryPage = true;
+	else
+		pCoexSta->bC2hBtInquiryPage = false;
+
+	/*  set link exist status */
+	if (!(btInfo&BT_INFO_8723B_2ANT_B_CONNECTION)) {
+		pCoexSta->bBtLinkExist = false;
+		pCoexSta->bPanExist = false;
+		pCoexSta->bA2dpExist = false;
+		pCoexSta->bHidExist = false;
+		pCoexSta->bScoExist = false;
+	} else { /*  connection exists */
+		pCoexSta->bBtLinkExist = true;
+		if (btInfo & BT_INFO_8723B_2ANT_B_FTP)
+			pCoexSta->bPanExist = true;
+		else
+			pCoexSta->bPanExist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_A2DP)
+			pCoexSta->bA2dpExist = true;
+		else
+			pCoexSta->bA2dpExist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_HID)
+			pCoexSta->bHidExist = true;
+		else
+			pCoexSta->bHidExist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_SCO_ESCO)
+			pCoexSta->bScoExist = true;
+		else
+			pCoexSta->bScoExist = false;
+	}
+
+	halbtc8723b2ant_UpdateBtLinkInfo(pBtCoexist);
+
+	if (!(btInfo&BT_INFO_8723B_2ANT_B_CONNECTION)) {
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Non-Connected idle!!!\n"));
+	} else if (btInfo == BT_INFO_8723B_2ANT_B_CONNECTION)	{ /*  connection exists but no busy */
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n"));
+	} else if (
+		(btInfo&BT_INFO_8723B_2ANT_B_SCO_ESCO) ||
+		(btInfo&BT_INFO_8723B_2ANT_B_SCO_BUSY)
+	) {
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_SCO_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT SCO busy!!!\n"));
+	} else if (btInfo&BT_INFO_8723B_2ANT_B_ACL_BUSY) {
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT ACL busy!!!\n"));
+	} else {
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Non-Defined state!!!\n"));
+	}
+
+	if (
+		(BT_8723B_2ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_2ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+	) {
+		bBtBusy = true;
+		bLimitedDig = true;
+	} else {
+		bBtBusy = false;
+		bLimitedDig = false;
+	}
+
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
+
+	pCoexDm->bLimitedDig = bLimitedDig;
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_LIMITED_DIG, &bLimitedDig);
+
+	halbtc8723b2ant_RunCoexistMechanism(pBtCoexist);
+}
+
+void EXhalbtc8723b2ant_HaltNotify(PBTC_COEXIST pBtCoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Halt notify\n"));
+
+	halbtc8723b2ant_WifiOffHwCfg(pBtCoexist);
+	pBtCoexist->fBtcSetBtReg(pBtCoexist, BTC_BT_REG_RF, 0x3c, 0x15); /* BT goto standby while GNT_BT 1-->0 */
+	halbtc8723b2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
+
+	EXhalbtc8723b2ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void EXhalbtc8723b2ant_PnpNotify(PBTC_COEXIST pBtCoexist, u8 pnpState)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify\n"));
+
+	if (BTC_WIFI_PNP_SLEEP == pnpState) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify to SLEEP\n"));
+	} else if (BTC_WIFI_PNP_WAKE_UP == pnpState) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify to WAKE UP\n"));
+		halbtc8723b2ant_InitHwConfig(pBtCoexist, false);
+		halbtc8723b2ant_InitCoexDm(pBtCoexist);
+		halbtc8723b2ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b2ant_Periodical(PBTC_COEXIST pBtCoexist)
+{
+	static u8 disVerInfoCnt = 0;
+	u32 fwVer = 0, btPatchVer = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ==========================Periodical ===========================\n"));
+
+	if (disVerInfoCnt <= 5) {
+		disVerInfoCnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n", \
+			GLCoexVerDate8723b2Ant, GLCoexVer8723b2Ant, fwVer, btPatchVer, btPatchVer));
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
+	}
+
+	if (
+		halbtc8723b2ant_IsWifiStatusChanged(pBtCoexist) ||
+		pCoexDm->bAutoTdmaAdjust
+	)
+		halbtc8723b2ant_RunCoexistMechanism(pBtCoexist);
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalBtc8723b2Ant.h linux/3rdparty/rtl8723bs/hal/HalBtc8723b2Ant.h
--- linux.old/3rdparty/rtl8723bs/hal/HalBtc8723b2Ant.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalBtc8723b2Ant.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,155 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+/*  The following is for 8723B 2Ant BT Co-exist definition */
+#define	BT_INFO_8723B_2ANT_B_FTP		BIT7
+#define	BT_INFO_8723B_2ANT_B_A2DP		BIT6
+#define	BT_INFO_8723B_2ANT_B_HID		BIT5
+#define	BT_INFO_8723B_2ANT_B_SCO_BUSY		BIT4
+#define	BT_INFO_8723B_2ANT_B_ACL_BUSY		BIT3
+#define	BT_INFO_8723B_2ANT_B_INQ_PAGE		BIT2
+#define	BT_INFO_8723B_2ANT_B_SCO_ESCO		BIT1
+#define	BT_INFO_8723B_2ANT_B_CONNECTION		BIT0
+
+#define		BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT		2
+
+typedef enum _BT_INFO_SRC_8723B_2ANT {
+	BT_INFO_SRC_8723B_2ANT_WIFI_FW        = 0x0,
+	BT_INFO_SRC_8723B_2ANT_BT_RSP         = 0x1,
+	BT_INFO_SRC_8723B_2ANT_BT_ACTIVE_SEND = 0x2,
+	BT_INFO_SRC_8723B_2ANT_MAX
+} BT_INFO_SRC_8723B_2ANT, *PBT_INFO_SRC_8723B_2ANT;
+
+typedef enum _BT_8723B_2ANT_BT_STATUS {
+	BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE = 0x0,
+	BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE     = 0x1,
+	BT_8723B_2ANT_BT_STATUS_INQ_PAGE           = 0x2,
+	BT_8723B_2ANT_BT_STATUS_ACL_BUSY           = 0x3,
+	BT_8723B_2ANT_BT_STATUS_SCO_BUSY           = 0x4,
+	BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY       = 0x5,
+	BT_8723B_2ANT_BT_STATUS_MAX
+} BT_8723B_2ANT_BT_STATUS, *PBT_8723B_2ANT_BT_STATUS;
+
+typedef enum _BT_8723B_2ANT_COEX_ALGO {
+	BT_8723B_2ANT_COEX_ALGO_UNDEFINED       = 0x0,
+	BT_8723B_2ANT_COEX_ALGO_SCO             = 0x1,
+	BT_8723B_2ANT_COEX_ALGO_HID             = 0x2,
+	BT_8723B_2ANT_COEX_ALGO_A2DP            = 0x3,
+	BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS      = 0x4,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR          = 0x5,
+	BT_8723B_2ANT_COEX_ALGO_PANHS           = 0x6,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP     = 0x7,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR_HID      = 0x8,
+	BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x9,
+	BT_8723B_2ANT_COEX_ALGO_HID_A2DP        = 0xa,
+	BT_8723B_2ANT_COEX_ALGO_MAX             = 0xb,
+} BT_8723B_2ANT_COEX_ALGO, *PBT_8723B_2ANT_COEX_ALGO;
+
+typedef struct _COEX_DM_8723B_2ANT {
+	/*  fw mechanism */
+	u8 preBtDecPwrLvl;
+	u8 curBtDecPwrLvl;
+	u8 preFwDacSwingLvl;
+	u8 curFwDacSwingLvl;
+	bool bCurIgnoreWlanAct;
+	bool bPreIgnoreWlanAct;
+	u8 prePsTdma;
+	u8 curPsTdma;
+	u8 psTdmaPara[5];
+	u8 psTdmaDuAdjType;
+	bool bResetTdmaAdjust;
+	bool bAutoTdmaAdjust;
+	bool bPrePsTdmaOn;
+	bool bCurPsTdmaOn;
+	bool bPreBtAutoReport;
+	bool bCurBtAutoReport;
+
+	/*  sw mechanism */
+	bool bPreRfRxLpfShrink;
+	bool bCurRfRxLpfShrink;
+	u32 btRf0x1eBackup;
+	bool bPreLowPenaltyRa;
+	bool bCurLowPenaltyRa;
+	bool bPreDacSwingOn;
+	u32  preDacSwingLvl;
+	bool bCurDacSwingOn;
+	u32  curDacSwingLvl;
+	bool bPreAdcBackOff;
+	bool bCurAdcBackOff;
+	bool bPreAgcTableEn;
+	bool bCurAgcTableEn;
+	u32 preVal0x6c0;
+	u32 curVal0x6c0;
+	u32 preVal0x6c4;
+	u32 curVal0x6c4;
+	u32 preVal0x6c8;
+	u32 curVal0x6c8;
+	u8 preVal0x6cc;
+	u8 curVal0x6cc;
+	bool bLimitedDig;
+
+	/*  algorithm related */
+	u8 preAlgorithm;
+	u8 curAlgorithm;
+	u8 btStatus;
+	u8 wifiChnlInfo[3];
+
+	bool bNeedRecover0x948;
+	u32 backup0x948;
+} COEX_DM_8723B_2ANT, *PCOEX_DM_8723B_2ANT;
+
+typedef struct _COEX_STA_8723B_2ANT {
+	bool bBtLinkExist;
+	bool bScoExist;
+	bool bA2dpExist;
+	bool bHidExist;
+	bool bPanExist;
+
+	bool bUnderLps;
+	bool bUnderIps;
+	u32 highPriorityTx;
+	u32 highPriorityRx;
+	u32 lowPriorityTx;
+	u32 lowPriorityRx;
+	u8 btRssi;
+	bool bBtTxRxMask;
+	u8 preBtRssiState;
+	u8 preWifiRssiState[4];
+	bool bC2hBtInfoReqSent;
+	u8 btInfoC2h[BT_INFO_SRC_8723B_2ANT_MAX][10];
+	u32 btInfoC2hCnt[BT_INFO_SRC_8723B_2ANT_MAX];
+	bool bC2hBtInquiryPage;
+	u8 btRetryCnt;
+	u8 btInfoExt;
+} COEX_STA_8723B_2ANT, *PCOEX_STA_8723B_2ANT;
+
+/*  */
+/*  The following is interface which will notify coex module. */
+/*  */
+void EXhalbtc8723b2ant_PowerOnSetting(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b2ant_InitHwConfig(PBTC_COEXIST pBtCoexist, bool bWifiOnly);
+void EXhalbtc8723b2ant_InitCoexDm(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b2ant_IpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_LpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_ScanNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_ConnectNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_MediaStatusNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_SpecialPacketNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_BtInfoNotify(
+	PBTC_COEXIST pBtCoexist, u8 *tmpBuf, u8 length
+);
+void EXhalbtc8723b2ant_HaltNotify(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b2ant_PnpNotify(PBTC_COEXIST pBtCoexist, u8 pnpState);
+void EXhalbtc8723b2ant_Periodical(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b2ant_DisplayCoexInfo(PBTC_COEXIST pBtCoexist);
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/hal_btcoex.c linux/3rdparty/rtl8723bs/hal/hal_btcoex.c
--- linux.old/3rdparty/rtl8723bs/hal/hal_btcoex.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/hal_btcoex.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,1720 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 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.
+ *
+ ******************************************************************************/
+#define __HAL_BTCOEX_C__
+
+#include <hal_data.h>
+#include <rtw_debug.h>
+#include <hal_btcoex.h>
+#include <Mp_Precomp.h>
+
+/* 		Global variables */
+static const char *const BtProfileString[] = {
+	"NONE",
+	"A2DP",
+	"PAN",
+	"HID",
+	"SCO",
+};
+
+static const char *const BtSpecString[] = {
+	"1.0b",
+	"1.1",
+	"1.2",
+	"2.0+EDR",
+	"2.1+EDR",
+	"3.0+HS",
+	"4.0",
+};
+
+static const char *const BtLinkRoleString[] = {
+	"Master",
+	"Slave",
+};
+
+static const char *const h2cStaString[] = {
+	"successful",
+	"h2c busy",
+	"rf off",
+	"fw not read",
+};
+
+static const char *const ioStaString[] = {
+	"success",
+	"can not IO",
+	"rf off",
+	"fw not read",
+	"wait io timeout",
+	"invalid len",
+	"idle Q empty",
+	"insert waitQ fail",
+	"unknown fail",
+	"wrong level",
+	"h2c stopped",
+};
+
+BTC_COEXIST GLBtCoexist;
+static u8 GLBtcWiFiInScanState;
+static u8 GLBtcWiFiInIQKState;
+
+u32 GLBtcDbgType[BTC_MSG_MAX];
+static u8 GLBtcDbgBuf[BT_TMP_BUF_SIZE];
+
+typedef struct _btcoexdbginfo {
+	u8 *info;
+	u32 size; /*  buffer total size */
+	u32 len; /*  now used length */
+} BTCDBGINFO, *PBTCDBGINFO;
+
+static BTCDBGINFO GLBtcDbgInfo;
+
+#define	BT_Operation(Adapter)						false
+
+static void DBG_BT_INFO_INIT(PBTCDBGINFO pinfo, u8 *pbuf, u32 size)
+{
+	if (NULL == pinfo)
+		return;
+
+	memset(pinfo, 0, sizeof(BTCDBGINFO));
+
+	if (pbuf && size) {
+		pinfo->info = pbuf;
+		pinfo->size = size;
+	}
+}
+
+void DBG_BT_INFO(u8 *dbgmsg)
+{
+	PBTCDBGINFO pinfo;
+	u32 msglen;
+	u8 *pbuf;
+
+
+	pinfo = &GLBtcDbgInfo;
+
+	if (NULL == pinfo->info)
+		return;
+
+	msglen = strlen(dbgmsg);
+	if (pinfo->len + msglen > pinfo->size)
+		return;
+
+	pbuf = pinfo->info + pinfo->len;
+	memcpy(pbuf, dbgmsg, msglen);
+	pinfo->len += msglen;
+}
+
+/*  */
+/* 		Debug related function */
+/*  */
+static u8 halbtcoutsrc_IsBtCoexistAvailable(PBTC_COEXIST pBtCoexist)
+{
+	if (!pBtCoexist->bBinded ||
+		NULL == pBtCoexist->Adapter){
+		return false;
+	}
+	return true;
+}
+
+static void halbtcoutsrc_DbgInit(void)
+{
+	u8 i;
+
+	for (i = 0; i < BTC_MSG_MAX; i++)
+		GLBtcDbgType[i] = 0;
+
+	GLBtcDbgType[BTC_MSG_INTERFACE]			=	\
+/* 			INTF_INIT								| */
+/* 			INTF_NOTIFY							| */
+			0;
+
+	GLBtcDbgType[BTC_MSG_ALGORITHM]			=	\
+/* 			ALGO_BT_RSSI_STATE					| */
+/* 			ALGO_WIFI_RSSI_STATE					| */
+/* 			ALGO_BT_MONITOR						| */
+/* 			ALGO_TRACE							| */
+/* 			ALGO_TRACE_FW						| */
+/* 			ALGO_TRACE_FW_DETAIL				| */
+/* 			ALGO_TRACE_FW_EXEC					| */
+/* 			ALGO_TRACE_SW						| */
+/* 			ALGO_TRACE_SW_DETAIL				| */
+/* 			ALGO_TRACE_SW_EXEC					| */
+			0;
+}
+
+static void halbtcoutsrc_LeaveLps(PBTC_COEXIST pBtCoexist)
+{
+	struct adapter *padapter;
+
+
+	padapter = pBtCoexist->Adapter;
+
+	pBtCoexist->btInfo.bBtCtrlLps = true;
+	pBtCoexist->btInfo.bBtLpsOn = false;
+
+	rtw_btcoex_LPS_Leave(padapter);
+}
+
+static void halbtcoutsrc_EnterLps(PBTC_COEXIST pBtCoexist)
+{
+	struct adapter *padapter;
+
+
+	padapter = pBtCoexist->Adapter;
+
+	pBtCoexist->btInfo.bBtCtrlLps = true;
+	pBtCoexist->btInfo.bBtLpsOn = true;
+
+	rtw_btcoex_LPS_Enter(padapter);
+}
+
+static void halbtcoutsrc_NormalLps(PBTC_COEXIST pBtCoexist)
+{
+	struct adapter *padapter;
+
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Normal LPS behavior!!!\n"));
+
+	padapter = pBtCoexist->Adapter;
+
+	if (pBtCoexist->btInfo.bBtCtrlLps) {
+		pBtCoexist->btInfo.bBtLpsOn = false;
+		rtw_btcoex_LPS_Leave(padapter);
+		pBtCoexist->btInfo.bBtCtrlLps = false;
+
+		/*  recover the LPS state to the original */
+	}
+}
+
+/*
+ *  Constraint:
+ *   1. this function will request pwrctrl->lock
+ */
+static void halbtcoutsrc_LeaveLowPower(PBTC_COEXIST pBtCoexist)
+{
+	struct adapter *padapter;
+	struct hal_com_data *pHalData;
+	s32 ready;
+	unsigned long stime;
+	unsigned long utime;
+	u32 timeout; /*  unit: ms */
+
+
+	padapter = pBtCoexist->Adapter;
+	pHalData = GET_HAL_DATA(padapter);
+	ready = _FAIL;
+#ifdef LPS_RPWM_WAIT_MS
+	timeout = LPS_RPWM_WAIT_MS;
+#else /*  !LPS_RPWM_WAIT_MS */
+	timeout = 30;
+#endif /*  !LPS_RPWM_WAIT_MS */
+
+	stime = jiffies;
+	do {
+		ready = rtw_register_task_alive(padapter, BTCOEX_ALIVE);
+		if (_SUCCESS == ready)
+			break;
+
+		utime = jiffies_to_msecs(jiffies - stime);
+		if (utime > timeout)
+			break;
+
+		msleep(1);
+	} while (1);
+}
+
+/*
+ *  Constraint:
+ *   1. this function will request pwrctrl->lock
+ */
+static void halbtcoutsrc_NormalLowPower(PBTC_COEXIST pBtCoexist)
+{
+	struct adapter *padapter;
+
+
+	padapter = pBtCoexist->Adapter;
+	rtw_unregister_task_alive(padapter, BTCOEX_ALIVE);
+}
+
+static void halbtcoutsrc_DisableLowPower(PBTC_COEXIST pBtCoexist, u8 bLowPwrDisable)
+{
+	pBtCoexist->btInfo.bBtDisableLowPwr = bLowPwrDisable;
+	if (bLowPwrDisable)
+		halbtcoutsrc_LeaveLowPower(pBtCoexist);		/*  leave 32k low power. */
+	else
+		halbtcoutsrc_NormalLowPower(pBtCoexist);	/*  original 32k low power behavior. */
+}
+
+static void halbtcoutsrc_AggregationCheck(PBTC_COEXIST pBtCoexist)
+{
+	struct adapter *padapter;
+	bool bNeedToAct;
+
+
+	padapter = pBtCoexist->Adapter;
+	bNeedToAct = false;
+
+	if (pBtCoexist->btInfo.bRejectAggPkt)
+		rtw_btcoex_RejectApAggregatedPacket(padapter, true);
+	else{
+
+		if (pBtCoexist->btInfo.bPreBtCtrlAggBufSize !=
+			pBtCoexist->btInfo.bBtCtrlAggBufSize){
+
+			bNeedToAct = true;
+			pBtCoexist->btInfo.bPreBtCtrlAggBufSize = pBtCoexist->btInfo.bBtCtrlAggBufSize;
+		}
+
+		if (pBtCoexist->btInfo.bBtCtrlAggBufSize) {
+			if (pBtCoexist->btInfo.preAggBufSize !=
+				pBtCoexist->btInfo.aggBufSize){
+				bNeedToAct = true;
+			}
+			pBtCoexist->btInfo.preAggBufSize = pBtCoexist->btInfo.aggBufSize;
+		}
+
+		if (bNeedToAct) {
+			rtw_btcoex_RejectApAggregatedPacket(padapter, true);
+			rtw_btcoex_RejectApAggregatedPacket(padapter, false);
+		}
+	}
+}
+
+static u8 halbtcoutsrc_IsWifiBusy(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv;
+
+
+	pmlmepriv = &padapter->mlmepriv;
+
+	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == true) {
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+			return true;
+		if (true == pmlmepriv->LinkDetectInfo.bBusyTraffic)
+			return true;
+	}
+
+	return false;
+}
+
+static u32 _halbtcoutsrc_GetWifiLinkStatus(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv;
+	u8 bp2p;
+	u32 portConnectedStatus;
+
+
+	pmlmepriv = &padapter->mlmepriv;
+	bp2p = false;
+	portConnectedStatus = 0;
+
+	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE) == true) {
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+			if (true == bp2p)
+				portConnectedStatus |= WIFI_P2P_GO_CONNECTED;
+			else
+				portConnectedStatus |= WIFI_AP_CONNECTED;
+		} else {
+			if (true == bp2p)
+				portConnectedStatus |= WIFI_P2P_GC_CONNECTED;
+			else
+				portConnectedStatus |= WIFI_STA_CONNECTED;
+		}
+	}
+
+	return portConnectedStatus;
+}
+
+static u32 halbtcoutsrc_GetWifiLinkStatus(PBTC_COEXIST pBtCoexist)
+{
+	/*  */
+	/*  return value: */
+	/*  [31:16]=> connected port number */
+	/*  [15:0]=> port connected bit define */
+	/*  */
+
+	struct adapter *padapter;
+	u32 retVal;
+	u32 portConnectedStatus, numOfConnectedPort;
+
+
+	padapter = pBtCoexist->Adapter;
+	portConnectedStatus = 0;
+	numOfConnectedPort = 0;
+
+	retVal = _halbtcoutsrc_GetWifiLinkStatus(padapter);
+	if (retVal) {
+		portConnectedStatus |= retVal;
+		numOfConnectedPort++;
+	}
+
+	retVal = (numOfConnectedPort << 16) | portConnectedStatus;
+
+	return retVal;
+}
+
+static u32 halbtcoutsrc_GetBtPatchVer(PBTC_COEXIST pBtCoexist)
+{
+	return pBtCoexist->btInfo.btRealFwVer;
+}
+
+static s32 halbtcoutsrc_GetWifiRssi(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+	s32 UndecoratedSmoothedPWDB = 0;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	UndecoratedSmoothedPWDB = pHalData->dmpriv.EntryMinUndecoratedSmoothedPWDB;
+
+	return UndecoratedSmoothedPWDB;
+}
+
+static u8 halbtcoutsrc_GetWifiScanAPNum(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext;
+	static u8 scan_AP_num = 0;
+
+	pmlmeext = &padapter->mlmeextpriv;
+
+	if (GLBtcWiFiInScanState == false) {
+		if (pmlmeext->sitesurvey_res.bss_cnt > 0xFF)
+			scan_AP_num = 0xFF;
+		else
+			scan_AP_num = (u8)pmlmeext->sitesurvey_res.bss_cnt;
+	}
+
+	return scan_AP_num;
+}
+
+static u8 halbtcoutsrc_Get(void *pBtcContext, u8 getType, void *pOutBuf)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+	struct hal_com_data *pHalData;
+	struct mlme_ext_priv *mlmeext;
+	u8 *pu8;
+	s32 *pS4Tmp;
+	u32 *pU4Tmp;
+	u8 *pU1Tmp;
+	u8 ret;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return false;
+
+	padapter = pBtCoexist->Adapter;
+	pHalData = GET_HAL_DATA(padapter);
+	mlmeext = &padapter->mlmeextpriv;
+	pu8 = (u8 *)pOutBuf;
+	pS4Tmp = (s32 *)pOutBuf;
+	pU4Tmp = (u32 *)pOutBuf;
+	pU1Tmp = (u8 *)pOutBuf;
+	ret = true;
+
+	switch (getType) {
+	case BTC_GET_BL_HS_OPERATION:
+		*pu8 = false;
+		ret = false;
+		break;
+
+	case BTC_GET_BL_HS_CONNECTING:
+		*pu8 = false;
+		ret = false;
+		break;
+
+	case BTC_GET_BL_WIFI_CONNECTED:
+		*pu8 = check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE);
+		break;
+
+	case BTC_GET_BL_WIFI_BUSY:
+		*pu8 = halbtcoutsrc_IsWifiBusy(padapter);
+		break;
+
+	case BTC_GET_BL_WIFI_SCAN:
+		/* Use the value of the new variable GLBtcWiFiInScanState to judge whether WiFi is in scan state or not, since the originally used flag
+			WIFI_SITE_MONITOR in fwstate may not be cleared in time */
+		*pu8 = GLBtcWiFiInScanState;
+		break;
+
+	case BTC_GET_BL_WIFI_LINK:
+		*pu8 = check_fwstate(&padapter->mlmepriv, WIFI_UNDER_LINKING);
+		break;
+
+	case BTC_GET_BL_WIFI_ROAM:
+		*pu8 = check_fwstate(&padapter->mlmepriv, WIFI_UNDER_LINKING);
+		break;
+
+	case BTC_GET_BL_WIFI_4_WAY_PROGRESS:
+		*pu8 = false;
+		break;
+
+	case BTC_GET_BL_WIFI_UNDER_5G:
+		*pu8 = (pHalData->CurrentBandType == 1) ? true : false;
+		break;
+
+	case BTC_GET_BL_WIFI_AP_MODE_ENABLE:
+		*pu8 = check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE);
+		break;
+
+	case BTC_GET_BL_WIFI_ENABLE_ENCRYPTION:
+		*pu8 = padapter->securitypriv.dot11PrivacyAlgrthm == 0 ? false : true;
+		break;
+
+	case BTC_GET_BL_WIFI_UNDER_B_MODE:
+		if (mlmeext->cur_wireless_mode == WIRELESS_11B)
+			*pu8 = true;
+		else
+			*pu8 = false;
+		break;
+
+	case BTC_GET_BL_WIFI_IS_IN_MP_MODE:
+		*pu8 = false;
+		break;
+
+	case BTC_GET_BL_EXT_SWITCH:
+		*pu8 = false;
+		break;
+
+	case BTC_GET_S4_WIFI_RSSI:
+		*pS4Tmp = halbtcoutsrc_GetWifiRssi(padapter);
+		break;
+
+	case BTC_GET_S4_HS_RSSI:
+		*pS4Tmp = 0;
+		ret = false;
+		break;
+
+	case BTC_GET_U4_WIFI_BW:
+		if (IsLegacyOnly(mlmeext->cur_wireless_mode))
+			*pU4Tmp = BTC_WIFI_BW_LEGACY;
+		else if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_20)
+			*pU4Tmp = BTC_WIFI_BW_HT20;
+		else if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_40)
+			*pU4Tmp = BTC_WIFI_BW_HT40;
+		else
+			*pU4Tmp = BTC_WIFI_BW_HT40; /* todo */
+		break;
+
+	case BTC_GET_U4_WIFI_TRAFFIC_DIRECTION:
+		{
+			PRT_LINK_DETECT_T plinkinfo;
+			plinkinfo = &padapter->mlmepriv.LinkDetectInfo;
+
+			if (plinkinfo->NumTxOkInPeriod > plinkinfo->NumRxOkInPeriod)
+				*pU4Tmp = BTC_WIFI_TRAFFIC_TX;
+			else
+				*pU4Tmp = BTC_WIFI_TRAFFIC_RX;
+		}
+		break;
+
+	case BTC_GET_U4_WIFI_FW_VER:
+		*pU4Tmp = pHalData->FirmwareVersion << 16;
+		*pU4Tmp |= pHalData->FirmwareSubVersion;
+		break;
+
+	case BTC_GET_U4_WIFI_LINK_STATUS:
+		*pU4Tmp = halbtcoutsrc_GetWifiLinkStatus(pBtCoexist);
+		break;
+
+	case BTC_GET_U4_BT_PATCH_VER:
+		*pU4Tmp = halbtcoutsrc_GetBtPatchVer(pBtCoexist);
+		break;
+
+	case BTC_GET_U1_WIFI_DOT11_CHNL:
+		*pU1Tmp = padapter->mlmeextpriv.cur_channel;
+		break;
+
+	case BTC_GET_U1_WIFI_CENTRAL_CHNL:
+		*pU1Tmp = pHalData->CurrentChannel;
+		break;
+
+	case BTC_GET_U1_WIFI_HS_CHNL:
+		*pU1Tmp = 0;
+		ret = false;
+		break;
+
+	case BTC_GET_U1_MAC_PHY_MODE:
+		*pU1Tmp = BTC_SMSP;
+/* 			*pU1Tmp = BTC_DMSP; */
+/* 			*pU1Tmp = BTC_DMDP; */
+/* 			*pU1Tmp = BTC_MP_UNKNOWN; */
+		break;
+
+	case BTC_GET_U1_AP_NUM:
+		*pU1Tmp = halbtcoutsrc_GetWifiScanAPNum(padapter);
+		break;
+
+	/* 1Ant =========== */
+	case BTC_GET_U1_LPS_MODE:
+		*pU1Tmp = padapter->dvobj->pwrctl_priv.pwr_mode;
+		break;
+
+	default:
+		ret = false;
+		break;
+	}
+
+	return ret;
+}
+
+static u8 halbtcoutsrc_Set(void *pBtcContext, u8 setType, void *pInBuf)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+	struct hal_com_data *pHalData;
+	u8 *pu8;
+	u8 *pU1Tmp;
+	u32 *pU4Tmp;
+	u8 ret;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+	pHalData = GET_HAL_DATA(padapter);
+	pu8 = (u8 *)pInBuf;
+	pU1Tmp = (u8 *)pInBuf;
+	pU4Tmp = (u32 *)pInBuf;
+	ret = true;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return false;
+
+	switch (setType) {
+	/*  set some u8 type variables. */
+	case BTC_SET_BL_BT_DISABLE:
+		pBtCoexist->btInfo.bBtDisabled = *pu8;
+		break;
+
+	case BTC_SET_BL_BT_TRAFFIC_BUSY:
+		pBtCoexist->btInfo.bBtBusy = *pu8;
+		break;
+
+	case BTC_SET_BL_BT_LIMITED_DIG:
+		pBtCoexist->btInfo.bLimitedDig = *pu8;
+		break;
+
+	case BTC_SET_BL_FORCE_TO_ROAM:
+		pBtCoexist->btInfo.bForceToRoam = *pu8;
+		break;
+
+	case BTC_SET_BL_TO_REJ_AP_AGG_PKT:
+		pBtCoexist->btInfo.bRejectAggPkt = *pu8;
+		break;
+
+	case BTC_SET_BL_BT_CTRL_AGG_SIZE:
+		pBtCoexist->btInfo.bBtCtrlAggBufSize = *pu8;
+		break;
+
+	case BTC_SET_BL_INC_SCAN_DEV_NUM:
+		pBtCoexist->btInfo.bIncreaseScanDevNum = *pu8;
+		break;
+
+	case BTC_SET_BL_BT_TX_RX_MASK:
+		pBtCoexist->btInfo.bBtTxRxMask = *pu8;
+		break;
+
+	/*  set some u8 type variables. */
+	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON:
+		pBtCoexist->btInfo.rssiAdjustForAgcTableOn = *pU1Tmp;
+		break;
+
+	case BTC_SET_U1_AGG_BUF_SIZE:
+		pBtCoexist->btInfo.aggBufSize = *pU1Tmp;
+		break;
+
+	/*  the following are some action which will be triggered */
+	case BTC_SET_ACT_GET_BT_RSSI:
+		ret = false;
+		break;
+
+	case BTC_SET_ACT_AGGREGATE_CTRL:
+		halbtcoutsrc_AggregationCheck(pBtCoexist);
+		break;
+
+	/* 1Ant =========== */
+	/*  set some u8 type variables. */
+	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE:
+		pBtCoexist->btInfo.rssiAdjustFor1AntCoexType = *pU1Tmp;
+		break;
+
+	case BTC_SET_U1_LPS_VAL:
+		pBtCoexist->btInfo.lpsVal = *pU1Tmp;
+		break;
+
+	case BTC_SET_U1_RPWM_VAL:
+		pBtCoexist->btInfo.rpwmVal = *pU1Tmp;
+		break;
+
+	/*  the following are some action which will be triggered */
+	case BTC_SET_ACT_LEAVE_LPS:
+		halbtcoutsrc_LeaveLps(pBtCoexist);
+		break;
+
+	case BTC_SET_ACT_ENTER_LPS:
+		halbtcoutsrc_EnterLps(pBtCoexist);
+		break;
+
+	case BTC_SET_ACT_NORMAL_LPS:
+		halbtcoutsrc_NormalLps(pBtCoexist);
+		break;
+
+	case BTC_SET_ACT_DISABLE_LOW_POWER:
+		halbtcoutsrc_DisableLowPower(pBtCoexist, *pu8);
+		break;
+
+	case BTC_SET_ACT_UPDATE_RAMASK:
+		pBtCoexist->btInfo.raMask = *pU4Tmp;
+
+		if (check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE) == true) {
+			struct sta_info *psta;
+			struct wlan_bssid_ex *cur_network;
+
+			cur_network = &padapter->mlmeextpriv.mlmext_info.network;
+			psta = rtw_get_stainfo(&padapter->stapriv, cur_network->MacAddress);
+			rtw_hal_update_ra_mask(psta, 0);
+		}
+		break;
+
+	case BTC_SET_ACT_SEND_MIMO_PS:
+		ret = false;
+		break;
+
+	case BTC_SET_ACT_CTRL_BT_INFO:
+		ret = false;
+		break;
+
+	case BTC_SET_ACT_CTRL_BT_COEX:
+		ret = false;
+		break;
+	case BTC_SET_ACT_CTRL_8723B_ANT:
+		ret = false;
+		break;
+	/*  */
+	default:
+		ret = false;
+		break;
+	}
+
+	return ret;
+}
+
+static void halbtcoutsrc_DisplayFwPwrModeCmd(PBTC_COEXIST pBtCoexist)
+{
+	u8 *cliBuf = pBtCoexist->cliBuf;
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x %02x ", "Power mode cmd ", \
+		pBtCoexist->pwrModeVal[0], pBtCoexist->pwrModeVal[1],
+		pBtCoexist->pwrModeVal[2], pBtCoexist->pwrModeVal[3],
+		pBtCoexist->pwrModeVal[4], pBtCoexist->pwrModeVal[5]);
+	CL_PRINTF(cliBuf);
+}
+
+/*  */
+/* 		IO related function */
+/*  */
+static u8 halbtcoutsrc_Read1Byte(void *pBtcContext, u32 RegAddr)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return rtw_read8(padapter, RegAddr);
+}
+
+static u16 halbtcoutsrc_Read2Byte(void *pBtcContext, u32 RegAddr)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return	rtw_read16(padapter, RegAddr);
+}
+
+static u32 halbtcoutsrc_Read4Byte(void *pBtcContext, u32 RegAddr)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return	rtw_read32(padapter, RegAddr);
+}
+
+static void halbtcoutsrc_Write1Byte(void *pBtcContext, u32 RegAddr, u8 Data)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_write8(padapter, RegAddr, Data);
+}
+
+static void halbtcoutsrc_BitMaskWrite1Byte(void *pBtcContext, u32 regAddr, u8 bitMask, u8 data1b)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+	u8 originalValue, bitShift;
+	u8 i;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+	originalValue = 0;
+	bitShift = 0;
+
+	if (bitMask != 0xFF) {
+		originalValue = rtw_read8(padapter, regAddr);
+
+		for (i = 0; i <= 7; i++) {
+			if ((bitMask>>i)&0x1)
+				break;
+		}
+		bitShift = i;
+
+		data1b = (originalValue & ~bitMask) | ((data1b << bitShift) & bitMask);
+	}
+
+	rtw_write8(padapter, regAddr, data1b);
+}
+
+static void halbtcoutsrc_Write2Byte(void *pBtcContext, u32 RegAddr, u16 Data)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_write16(padapter, RegAddr, Data);
+}
+
+static void halbtcoutsrc_Write4Byte(void *pBtcContext, u32 RegAddr, u32 Data)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_write32(padapter, RegAddr, Data);
+}
+
+static void halbtcoutsrc_WriteLocalReg1Byte(void *pBtcContext, u32 RegAddr, u8 Data)
+{
+	PBTC_COEXIST		pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	struct adapter *Adapter = pBtCoexist->Adapter;
+
+	if (BTC_INTF_SDIO == pBtCoexist->chipInterface) {
+		rtw_write8(Adapter, SDIO_LOCAL_BASE | RegAddr, Data);
+	} else {
+		rtw_write8(Adapter, RegAddr, Data);
+	}
+}
+
+static void halbtcoutsrc_SetBbReg(void *pBtcContext, u32 RegAddr, u32 BitMask, u32 Data)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	PHY_SetBBReg(padapter, RegAddr, BitMask, Data);
+}
+
+
+static u32 halbtcoutsrc_GetBbReg(void *pBtcContext, u32 RegAddr, u32 BitMask)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return PHY_QueryBBReg(padapter, RegAddr, BitMask);
+}
+
+static void halbtcoutsrc_SetRfReg(void *pBtcContext, u8 eRFPath, u32 RegAddr, u32 BitMask, u32 Data)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	PHY_SetRFReg(padapter, eRFPath, RegAddr, BitMask, Data);
+}
+
+static u32 halbtcoutsrc_GetRfReg(void *pBtcContext, u8 eRFPath, u32 RegAddr, u32 BitMask)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return PHY_QueryRFReg(padapter, eRFPath, RegAddr, BitMask);
+}
+
+static void halbtcoutsrc_SetBtReg(void *pBtcContext, u8 RegType, u32 RegAddr, u32 Data)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+	u8 CmdBuffer1[4] = {0};
+	u8 CmdBuffer2[4] = {0};
+	u8 *AddrToSet = (u8 *)&RegAddr;
+	u8 *ValueToSet = (u8 *)&Data;
+	u8 OperVer = 0;
+	u8 ReqNum = 0;
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	CmdBuffer1[0] |= (OperVer & 0x0f);						/* Set OperVer */
+	CmdBuffer1[0] |= ((ReqNum << 4) & 0xf0);				/* Set ReqNum */
+	CmdBuffer1[1] = 0x0d;									/* Set OpCode to BT_LO_OP_WRITE_REG_VALUE */
+	CmdBuffer1[2] = ValueToSet[0];							/* Set WriteRegValue */
+	rtw_hal_fill_h2c_cmd(padapter, 0x67, 4, &(CmdBuffer1[0]));
+
+	msleep(200);
+	ReqNum++;
+
+	CmdBuffer2[0] |= (OperVer & 0x0f);						/* Set OperVer */
+	CmdBuffer2[0] |= ((ReqNum << 4) & 0xf0);				/* Set ReqNum */
+	CmdBuffer2[1] = 0x0c;									/* Set OpCode of BT_LO_OP_WRITE_REG_ADDR */
+	CmdBuffer2[3] = AddrToSet[0];							/* Set WriteRegAddr */
+	rtw_hal_fill_h2c_cmd(padapter, 0x67, 4, &(CmdBuffer2[0]));
+}
+
+static u32 halbtcoutsrc_GetBtReg(void *pBtcContext, u8 RegType, u32 RegAddr)
+{
+	/* To be implemented. Always return 0 temporarily */
+	return 0;
+}
+
+static void halbtcoutsrc_FillH2cCmd(void *pBtcContext, u8 elementId, u32 cmdLen, u8 *pCmdBuffer)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_hal_fill_h2c_cmd(padapter, elementId, cmdLen, pCmdBuffer);
+}
+
+static void halbtcoutsrc_DisplayDbgMsg(void *pBtcContext, u8 dispType)
+{
+	PBTC_COEXIST pBtCoexist;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	switch (dispType) {
+	case BTC_DBG_DISP_COEX_STATISTICS:
+		break;
+	case BTC_DBG_DISP_BT_LINK_INFO:
+		break;
+	case BTC_DBG_DISP_FW_PWR_MODE_CMD:
+		halbtcoutsrc_DisplayFwPwrModeCmd(pBtCoexist);
+		break;
+	default:
+		break;
+	}
+}
+
+/*  */
+/* 		Extern functions called by other module */
+/*  */
+static u8 EXhalbtcoutsrc_BindBtCoexWithAdapter(void *padapter)
+{
+	PBTC_COEXIST		pBtCoexist = &GLBtCoexist;
+
+	if (pBtCoexist->bBinded)
+		return false;
+	else
+		pBtCoexist->bBinded = true;
+
+	pBtCoexist->statistics.cntBind++;
+
+	pBtCoexist->Adapter = padapter;
+
+	pBtCoexist->stackInfo.bProfileNotified = false;
+
+	pBtCoexist->btInfo.bBtCtrlAggBufSize = false;
+	pBtCoexist->btInfo.aggBufSize = 5;
+
+	pBtCoexist->btInfo.bIncreaseScanDevNum = false;
+
+	/*  set default antenna position to main  port */
+	pBtCoexist->boardInfo.btdmAntPos = BTC_ANTENNA_AT_MAIN_PORT;
+
+	return true;
+}
+
+u8 EXhalbtcoutsrc_InitlizeVariables(void *padapter)
+{
+	PBTC_COEXIST pBtCoexist = &GLBtCoexist;
+
+	/* pBtCoexist->statistics.cntBind++; */
+
+	halbtcoutsrc_DbgInit();
+
+	pBtCoexist->chipInterface = BTC_INTF_SDIO;
+
+	EXhalbtcoutsrc_BindBtCoexWithAdapter(padapter);
+
+	pBtCoexist->fBtcRead1Byte = halbtcoutsrc_Read1Byte;
+	pBtCoexist->fBtcWrite1Byte = halbtcoutsrc_Write1Byte;
+	pBtCoexist->fBtcWrite1ByteBitMask = halbtcoutsrc_BitMaskWrite1Byte;
+	pBtCoexist->fBtcRead2Byte = halbtcoutsrc_Read2Byte;
+	pBtCoexist->fBtcWrite2Byte = halbtcoutsrc_Write2Byte;
+	pBtCoexist->fBtcRead4Byte = halbtcoutsrc_Read4Byte;
+	pBtCoexist->fBtcWrite4Byte = halbtcoutsrc_Write4Byte;
+	pBtCoexist->fBtcWriteLocalReg1Byte = halbtcoutsrc_WriteLocalReg1Byte;
+
+	pBtCoexist->fBtcSetBbReg = halbtcoutsrc_SetBbReg;
+	pBtCoexist->fBtcGetBbReg = halbtcoutsrc_GetBbReg;
+
+	pBtCoexist->fBtcSetRfReg = halbtcoutsrc_SetRfReg;
+	pBtCoexist->fBtcGetRfReg = halbtcoutsrc_GetRfReg;
+
+	pBtCoexist->fBtcFillH2c = halbtcoutsrc_FillH2cCmd;
+	pBtCoexist->fBtcDispDbgMsg = halbtcoutsrc_DisplayDbgMsg;
+
+	pBtCoexist->fBtcGet = halbtcoutsrc_Get;
+	pBtCoexist->fBtcSet = halbtcoutsrc_Set;
+	pBtCoexist->fBtcGetBtReg = halbtcoutsrc_GetBtReg;
+	pBtCoexist->fBtcSetBtReg = halbtcoutsrc_SetBtReg;
+
+	pBtCoexist->cliBuf = &GLBtcDbgBuf[0];
+
+	pBtCoexist->boardInfo.singleAntPath = 0;
+
+	GLBtcWiFiInScanState = false;
+
+	GLBtcWiFiInIQKState = false;
+
+	return true;
+}
+
+void EXhalbtcoutsrc_PowerOnSetting(PBTC_COEXIST pBtCoexist)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	/* Power on setting function is only added in 8723B currently */
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_PowerOnSetting(pBtCoexist);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_PowerOnSetting(pBtCoexist);
+}
+
+void EXhalbtcoutsrc_InitHwConfig(PBTC_COEXIST pBtCoexist, u8 bWifiOnly)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntInitHwConfig++;
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_InitHwConfig(pBtCoexist, bWifiOnly);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_InitHwConfig(pBtCoexist, bWifiOnly);
+}
+
+void EXhalbtcoutsrc_InitCoexDm(PBTC_COEXIST pBtCoexist)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntInitCoexDm++;
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_InitCoexDm(pBtCoexist);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_InitCoexDm(pBtCoexist);
+
+	pBtCoexist->bInitilized = true;
+}
+
+void EXhalbtcoutsrc_IpsNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	u8 ipsType;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntIpsNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (IPS_NONE == type)
+		ipsType = BTC_IPS_LEAVE;
+	else
+		ipsType = BTC_IPS_ENTER;
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_IpsNotify(pBtCoexist, ipsType);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_IpsNotify(pBtCoexist, ipsType);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_LpsNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	u8 lpsType;
+
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntLpsNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (PS_MODE_ACTIVE == type)
+		lpsType = BTC_LPS_DISABLE;
+	else
+		lpsType = BTC_LPS_ENABLE;
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_LpsNotify(pBtCoexist, lpsType);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_LpsNotify(pBtCoexist, lpsType);
+}
+
+void EXhalbtcoutsrc_ScanNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	u8 scanType;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+	pBtCoexist->statistics.cntScanNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (type) {
+		scanType = BTC_SCAN_START;
+		GLBtcWiFiInScanState = true;
+	} else {
+		scanType = BTC_SCAN_FINISH;
+		GLBtcWiFiInScanState = false;
+	}
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_ScanNotify(pBtCoexist, scanType);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_ScanNotify(pBtCoexist, scanType);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_ConnectNotify(PBTC_COEXIST pBtCoexist, u8 action)
+{
+	u8 assoType;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+	pBtCoexist->statistics.cntConnectNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (action)
+		assoType = BTC_ASSOCIATE_START;
+	else
+		assoType = BTC_ASSOCIATE_FINISH;
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_ConnectNotify(pBtCoexist, assoType);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_ConnectNotify(pBtCoexist, assoType);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_MediaStatusNotify(PBTC_COEXIST pBtCoexist, RT_MEDIA_STATUS mediaStatus)
+{
+	u8 mStatus;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntMediaStatusNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (RT_MEDIA_CONNECT == mediaStatus)
+		mStatus = BTC_MEDIA_CONNECT;
+	else
+		mStatus = BTC_MEDIA_DISCONNECT;
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_MediaStatusNotify(pBtCoexist, mStatus);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_MediaStatusNotify(pBtCoexist, mStatus);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_SpecialPacketNotify(PBTC_COEXIST pBtCoexist, u8 pktType)
+{
+	u8 packetType;
+
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+	pBtCoexist->statistics.cntSpecialPacketNotify++;
+	if (pBtCoexist->bManualControl)
+		return;
+
+	if (PACKET_DHCP == pktType)
+		packetType = BTC_PACKET_DHCP;
+	else if (PACKET_EAPOL == pktType)
+		packetType = BTC_PACKET_EAPOL;
+	else if (PACKET_ARP == pktType)
+		packetType = BTC_PACKET_ARP;
+	else{
+		packetType = BTC_PACKET_UNKNOWN;
+		return;
+	}
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_SpecialPacketNotify(pBtCoexist, packetType);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_SpecialPacketNotify(pBtCoexist, packetType);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_BtInfoNotify(PBTC_COEXIST pBtCoexist, u8 *tmpBuf, u8 length)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	pBtCoexist->statistics.cntBtInfoNotify++;
+
+	/*  All notify is called in cmd thread, don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_BtInfoNotify(pBtCoexist, tmpBuf, length);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_BtInfoNotify(pBtCoexist, tmpBuf, length);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_HaltNotify(PBTC_COEXIST pBtCoexist)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_HaltNotify(pBtCoexist);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_HaltNotify(pBtCoexist);
+
+	pBtCoexist->bBinded = false;
+}
+
+void EXhalbtcoutsrc_PnpNotify(PBTC_COEXIST pBtCoexist, u8 pnpState)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	/*  */
+	/*  currently only 1ant we have to do the notification, */
+	/*  once pnp is notified to sleep state, we have to leave LPS that we can sleep normally. */
+	/*  */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_PnpNotify(pBtCoexist, pnpState);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_PnpNotify(pBtCoexist, pnpState);
+}
+
+void EXhalbtcoutsrc_Periodical(PBTC_COEXIST pBtCoexist)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+	pBtCoexist->statistics.cntPeriodical++;
+
+	/*  Periodical should be called in cmd thread, */
+	/*  don't need to leave low power again */
+/* 	halbtcoutsrc_LeaveLowPower(pBtCoexist); */
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_Periodical(pBtCoexist);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_Periodical(pBtCoexist);
+
+/* 	halbtcoutsrc_NormalLowPower(pBtCoexist); */
+}
+
+void EXhalbtcoutsrc_SetChipType(u8 chipType)
+{
+	GLBtCoexist.boardInfo.btChipType = BTC_CHIP_RTL8723B;
+}
+
+void EXhalbtcoutsrc_SetAntNum(u8 type, u8 antNum)
+{
+	if (BT_COEX_ANT_TYPE_PG == type) {
+		GLBtCoexist.boardInfo.pgAntNum = antNum;
+		GLBtCoexist.boardInfo.btdmAntNum = antNum;
+	} else if (BT_COEX_ANT_TYPE_ANTDIV == type) {
+		GLBtCoexist.boardInfo.btdmAntNum = antNum;
+		/* GLBtCoexist.boardInfo.btdmAntPos = BTC_ANTENNA_AT_MAIN_PORT; */
+	} else if (BT_COEX_ANT_TYPE_DETECTED == type) {
+		GLBtCoexist.boardInfo.btdmAntNum = antNum;
+		/* GLBtCoexist.boardInfo.btdmAntPos = BTC_ANTENNA_AT_MAIN_PORT; */
+	}
+}
+
+/*  */
+/*  Currently used by 8723b only, S0 or S1 */
+/*  */
+void EXhalbtcoutsrc_SetSingleAntPath(u8 singleAntPath)
+{
+	GLBtCoexist.boardInfo.singleAntPath = singleAntPath;
+}
+
+void EXhalbtcoutsrc_DisplayBtCoexInfo(PBTC_COEXIST pBtCoexist)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	halbtcoutsrc_LeaveLowPower(pBtCoexist);
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_DisplayCoexInfo(pBtCoexist);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_DisplayCoexInfo(pBtCoexist);
+
+	halbtcoutsrc_NormalLowPower(pBtCoexist);
+}
+
+/*
+ * Description:
+ *Run BT-Coexist mechansim or not
+ *
+ */
+void hal_btcoex_SetBTCoexist(struct adapter *padapter, u8 bBtExist)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+	pHalData->bt_coexist.bBtExist = bBtExist;
+}
+
+/*
+ * Dewcription:
+ *Check is co-exist mechanism enabled or not
+ *
+ * Return:
+ *true	Enable BT co-exist mechanism
+ *false	Disable BT co-exist mechanism
+ */
+u8 hal_btcoex_IsBtExist(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+	return pHalData->bt_coexist.bBtExist;
+}
+
+u8 hal_btcoex_IsBtDisabled(struct adapter *padapter)
+{
+	if (!hal_btcoex_IsBtExist(padapter))
+		return true;
+
+	if (GLBtCoexist.btInfo.bBtDisabled)
+		return true;
+	else
+		return false;
+}
+
+void hal_btcoex_SetChipType(struct adapter *padapter, u8 chipType)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+	pHalData->bt_coexist.btChipType = chipType;
+
+	EXhalbtcoutsrc_SetChipType(chipType);
+}
+
+void hal_btcoex_SetPgAntNum(struct adapter *padapter, u8 antNum)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	pHalData->bt_coexist.btTotalAntNum = antNum;
+	EXhalbtcoutsrc_SetAntNum(BT_COEX_ANT_TYPE_PG, antNum);
+}
+
+void hal_btcoex_SetSingleAntPath(struct adapter *padapter, u8 singleAntPath)
+{
+	EXhalbtcoutsrc_SetSingleAntPath(singleAntPath);
+}
+
+u8 hal_btcoex_Initialize(struct adapter *padapter)
+{
+	u8 ret1;
+	u8 ret2;
+
+
+	memset(&GLBtCoexist, 0, sizeof(GLBtCoexist));
+	ret1 = EXhalbtcoutsrc_InitlizeVariables((void *)padapter);
+	ret2 = (ret1 == true) ? true : false;
+
+	return ret2;
+}
+
+void hal_btcoex_PowerOnSetting(struct adapter *padapter)
+{
+	EXhalbtcoutsrc_PowerOnSetting(&GLBtCoexist);
+}
+
+void hal_btcoex_InitHwConfig(struct adapter *padapter, u8 bWifiOnly)
+{
+	if (!hal_btcoex_IsBtExist(padapter))
+		return;
+
+	EXhalbtcoutsrc_InitHwConfig(&GLBtCoexist, bWifiOnly);
+	EXhalbtcoutsrc_InitCoexDm(&GLBtCoexist);
+}
+
+void hal_btcoex_IpsNotify(struct adapter *padapter, u8 type)
+{
+	EXhalbtcoutsrc_IpsNotify(&GLBtCoexist, type);
+}
+
+void hal_btcoex_LpsNotify(struct adapter *padapter, u8 type)
+{
+	EXhalbtcoutsrc_LpsNotify(&GLBtCoexist, type);
+}
+
+void hal_btcoex_ScanNotify(struct adapter *padapter, u8 type)
+{
+	EXhalbtcoutsrc_ScanNotify(&GLBtCoexist, type);
+}
+
+void hal_btcoex_ConnectNotify(struct adapter *padapter, u8 action)
+{
+	EXhalbtcoutsrc_ConnectNotify(&GLBtCoexist, action);
+}
+
+void hal_btcoex_MediaStatusNotify(struct adapter *padapter, u8 mediaStatus)
+{
+	EXhalbtcoutsrc_MediaStatusNotify(&GLBtCoexist, mediaStatus);
+}
+
+void hal_btcoex_SpecialPacketNotify(struct adapter *padapter, u8 pktType)
+{
+	EXhalbtcoutsrc_SpecialPacketNotify(&GLBtCoexist, pktType);
+}
+
+void hal_btcoex_IQKNotify(struct adapter *padapter, u8 state)
+{
+	GLBtcWiFiInIQKState = state;
+}
+
+void hal_btcoex_BtInfoNotify(struct adapter *padapter, u8 length, u8 *tmpBuf)
+{
+	if (GLBtcWiFiInIQKState == true)
+		return;
+
+	EXhalbtcoutsrc_BtInfoNotify(&GLBtCoexist, tmpBuf, length);
+}
+
+void hal_btcoex_SuspendNotify(struct adapter *padapter, u8 state)
+{
+	if (state == 1)
+		state = BTC_WIFI_PNP_SLEEP;
+	else
+		state = BTC_WIFI_PNP_WAKE_UP;
+
+	EXhalbtcoutsrc_PnpNotify(&GLBtCoexist, state);
+}
+
+void hal_btcoex_HaltNotify(struct adapter *padapter)
+{
+	EXhalbtcoutsrc_HaltNotify(&GLBtCoexist);
+}
+
+void hal_btcoex_Hanlder(struct adapter *padapter)
+{
+	EXhalbtcoutsrc_Periodical(&GLBtCoexist);
+}
+
+s32 hal_btcoex_IsBTCoexCtrlAMPDUSize(struct adapter *padapter)
+{
+	return (s32)GLBtCoexist.btInfo.bBtCtrlAggBufSize;
+}
+
+void hal_btcoex_SetManualControl(struct adapter *padapter, u8 bmanual)
+{
+	GLBtCoexist.bManualControl = bmanual;
+}
+
+u8 hal_btcoex_IsBtControlLps(struct adapter *padapter)
+{
+	if (hal_btcoex_IsBtExist(padapter) == false)
+		return false;
+
+	if (GLBtCoexist.btInfo.bBtDisabled)
+		return false;
+
+	if (GLBtCoexist.btInfo.bBtCtrlLps)
+		return true;
+
+	return false;
+}
+
+u8 hal_btcoex_IsLpsOn(struct adapter *padapter)
+{
+	if (hal_btcoex_IsBtExist(padapter) == false)
+		return false;
+
+	if (GLBtCoexist.btInfo.bBtDisabled)
+		return false;
+
+	if (GLBtCoexist.btInfo.bBtLpsOn)
+		return true;
+
+	return false;
+}
+
+u8 hal_btcoex_RpwmVal(struct adapter *padapter)
+{
+	return GLBtCoexist.btInfo.rpwmVal;
+}
+
+u8 hal_btcoex_LpsVal(struct adapter *padapter)
+{
+	return GLBtCoexist.btInfo.lpsVal;
+}
+
+u32 hal_btcoex_GetRaMask(struct adapter *padapter)
+{
+	if (!hal_btcoex_IsBtExist(padapter))
+		return 0;
+
+	if (GLBtCoexist.btInfo.bBtDisabled)
+		return 0;
+
+	if (GLBtCoexist.boardInfo.btdmAntNum != 1)
+		return 0;
+
+	return GLBtCoexist.btInfo.raMask;
+}
+
+void hal_btcoex_RecordPwrMode(struct adapter *padapter, u8 *pCmdBuf, u8 cmdLen)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write pwrModeCmd = 0x%04x%08x\n",
+		pCmdBuf[0]<<8|pCmdBuf[1],
+		pCmdBuf[2]<<24|pCmdBuf[3]<<16|pCmdBuf[4]<<8|pCmdBuf[5]));
+
+	memcpy(GLBtCoexist.pwrModeVal, pCmdBuf, cmdLen);
+}
+
+void hal_btcoex_DisplayBtCoexInfo(struct adapter *padapter, u8 *pbuf, u32 bufsize)
+{
+	PBTCDBGINFO pinfo;
+
+
+	pinfo = &GLBtcDbgInfo;
+	DBG_BT_INFO_INIT(pinfo, pbuf, bufsize);
+	EXhalbtcoutsrc_DisplayBtCoexInfo(&GLBtCoexist);
+	DBG_BT_INFO_INIT(pinfo, NULL, 0);
+}
+
+void hal_btcoex_SetDBG(struct adapter *padapter, u32 *pDbgModule)
+{
+	u32 i;
+
+
+	if (NULL == pDbgModule)
+		return;
+
+	for (i = 0; i < BTC_MSG_MAX; i++)
+		GLBtcDbgType[i] = pDbgModule[i];
+}
+
+u32 hal_btcoex_GetDBG(struct adapter *padapter, u8 *pStrBuf, u32 bufSize)
+{
+	s32 count;
+	u8 *pstr;
+	u32 leftSize;
+
+
+	if ((NULL == pStrBuf) || (0 == bufSize))
+		return 0;
+
+	pstr = pStrBuf;
+	leftSize = bufSize;
+/* 	DBG_871X(FUNC_ADPT_FMT ": bufsize =%d\n", FUNC_ADPT_ARG(padapter), bufSize); */
+
+	count = rtw_sprintf(pstr, leftSize, "#define DBG\t%d\n", DBG);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+
+	count = rtw_sprintf(pstr, leftSize, "BTCOEX Debug Setting:\n");
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+
+	count = rtw_sprintf(pstr, leftSize,
+		"INTERFACE / ALGORITHM: 0x%08X / 0x%08X\n\n",
+		GLBtcDbgType[BTC_MSG_INTERFACE],
+		GLBtcDbgType[BTC_MSG_ALGORITHM]);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+
+	count = rtw_sprintf(pstr, leftSize, "INTERFACE Debug Setting Definition:\n");
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[0]=%d for INTF_INIT\n",
+		(GLBtcDbgType[BTC_MSG_INTERFACE]&INTF_INIT)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[2]=%d for INTF_NOTIFY\n\n",
+		(GLBtcDbgType[BTC_MSG_INTERFACE]&INTF_NOTIFY)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+
+	count = rtw_sprintf(pstr, leftSize, "ALGORITHM Debug Setting Definition:\n");
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[0]=%d for BT_RSSI_STATE\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_BT_RSSI_STATE)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[1]=%d for WIFI_RSSI_STATE\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_WIFI_RSSI_STATE)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[2]=%d for BT_MONITOR\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_BT_MONITOR)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[3]=%d for TRACE\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[4]=%d for TRACE_FW\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_FW)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[5]=%d for TRACE_FW_DETAIL\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_FW_DETAIL)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[6]=%d for TRACE_FW_EXEC\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_FW_EXEC)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[7]=%d for TRACE_SW\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_SW)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[8]=%d for TRACE_SW_DETAIL\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_SW_DETAIL)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[9]=%d for TRACE_SW_EXEC\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_SW_EXEC)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+
+exit:
+	count = pstr - pStrBuf;
+/* 	DBG_871X(FUNC_ADPT_FMT ": usedsize =%d\n", FUNC_ADPT_ARG(padapter), count); */
+
+	return count;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalBtcOutSrc.h linux/3rdparty/rtl8723bs/hal/HalBtcOutSrc.h
--- linux.old/3rdparty/rtl8723bs/hal/HalBtcOutSrc.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalBtcOutSrc.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,565 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#ifndef	__HALBTC_OUT_SRC_H__
+#define __HALBTC_OUT_SRC_H__
+
+#define NORMAL_EXEC		false
+#define FORCE_EXEC		true
+
+#define BTC_RF_OFF		0x0
+#define BTC_RF_ON		0x1
+
+#define BTC_RF_A		0x0
+#define BTC_RF_B		0x1
+#define BTC_RF_C		0x2
+#define BTC_RF_D		0x3
+
+#define BTC_SMSP		SINGLEMAC_SINGLEPHY
+#define BTC_DMDP		DUALMAC_DUALPHY
+#define BTC_DMSP		DUALMAC_SINGLEPHY
+#define BTC_MP_UNKNOWN		0xff
+
+#define BT_COEX_ANT_TYPE_PG	0
+#define BT_COEX_ANT_TYPE_ANTDIV		1
+#define BT_COEX_ANT_TYPE_DETECTED	2
+
+#define BTC_MIMO_PS_STATIC	0	/*  1ss */
+#define BTC_MIMO_PS_DYNAMIC	1	/*  2ss */
+
+#define BTC_RATE_DISABLE	0
+#define BTC_RATE_ENABLE		1
+
+/*  single Antenna definition */
+#define BTC_ANT_PATH_WIFI	0
+#define BTC_ANT_PATH_BT		1
+#define BTC_ANT_PATH_PTA	2
+/*  dual Antenna definition */
+#define BTC_ANT_WIFI_AT_MAIN	0
+#define BTC_ANT_WIFI_AT_AUX	1
+/*  coupler Antenna definition */
+#define BTC_ANT_WIFI_AT_CPL_MAIN	0
+#define BTC_ANT_WIFI_AT_CPL_AUX		1
+
+typedef enum _BTC_POWERSAVE_TYPE{
+	BTC_PS_WIFI_NATIVE	= 0,	/*  wifi original power save behavior */
+	BTC_PS_LPS_ON		= 1,
+	BTC_PS_LPS_OFF		= 2,
+	BTC_PS_MAX
+} BTC_POWERSAVE_TYPE, *PBTC_POWERSAVE_TYPE;
+
+typedef enum _BTC_BT_REG_TYPE{
+	BTC_BT_REG_RF		= 0,
+	BTC_BT_REG_MODEM	= 1,
+	BTC_BT_REG_BLUEWIZE	= 2,
+	BTC_BT_REG_VENDOR	= 3,
+	BTC_BT_REG_LE		= 4,
+	BTC_BT_REG_MAX
+} BTC_BT_REG_TYPE, *PBTC_BT_REG_TYPE;
+
+typedef enum _BTC_CHIP_INTERFACE{
+	BTC_INTF_UNKNOWN	= 0,
+	BTC_INTF_PCI		= 1,
+	BTC_INTF_USB		= 2,
+	BTC_INTF_SDIO		= 3,
+	BTC_INTF_MAX
+} BTC_CHIP_INTERFACE, *PBTC_CHIP_INTERFACE;
+
+typedef enum _BTC_CHIP_TYPE {
+	BTC_CHIP_UNDEF		= 0,
+	BTC_CHIP_CSR_BC4	= 1,
+	BTC_CHIP_CSR_BC8	= 2,
+	BTC_CHIP_RTL8723A	= 3,
+	BTC_CHIP_RTL8821	= 4,
+	BTC_CHIP_RTL8723B	= 5,
+	BTC_CHIP_MAX
+} BTC_CHIP_TYPE, *PBTC_CHIP_TYPE;
+
+typedef enum _BTC_MSG_TYPE {
+	BTC_MSG_INTERFACE	= 0x0,
+	BTC_MSG_ALGORITHM	= 0x1,
+	BTC_MSG_MAX
+} BTC_MSG_TYPE;
+extern u32 		GLBtcDbgType[];
+
+/*  following is for BTC_MSG_INTERFACE */
+#define INTF_INIT	BIT0
+#define INTF_NOTIFY	BIT2
+
+/*  following is for BTC_ALGORITHM */
+#define ALGO_BT_RSSI_STATE				BIT0
+#define ALGO_WIFI_RSSI_STATE			BIT1
+#define ALGO_BT_MONITOR					BIT2
+#define ALGO_TRACE						BIT3
+#define ALGO_TRACE_FW					BIT4
+#define ALGO_TRACE_FW_DETAIL			BIT5
+#define ALGO_TRACE_FW_EXEC				BIT6
+#define ALGO_TRACE_SW					BIT7
+#define ALGO_TRACE_SW_DETAIL			BIT8
+#define ALGO_TRACE_SW_EXEC				BIT9
+
+/*  following is for wifi link status */
+#define WIFI_STA_CONNECTED				BIT0
+#define WIFI_AP_CONNECTED				BIT1
+#define WIFI_HS_CONNECTED				BIT2
+#define WIFI_P2P_GO_CONNECTED			BIT3
+#define WIFI_P2P_GC_CONNECTED			BIT4
+
+/*  following is for command line utility */
+#define CL_SPRINTF	snprintf
+#define CL_PRINTF	DCMD_Printf
+
+/*  The following is for dbgview print */
+#if DBG
+#define BTC_PRINT(dbgtype, dbgflag, printstr)\
+{\
+	if (GLBtcDbgType[dbgtype] & dbgflag)\
+		DbgPrint printstr;\
+}
+
+#define BTC_PRINT_F(dbgtype, dbgflag, printstr)\
+{\
+	if (GLBtcDbgType[dbgtype] & dbgflag) {\
+		DbgPrint("%s(): ", __func__);\
+		DbgPrint printstr;\
+	} \
+}
+
+#define BTC_PRINT_ADDR(dbgtype, dbgflag, printstr, _Ptr)\
+{\
+	if (GLBtcDbgType[dbgtype] & dbgflag) {\
+		int __i;\
+		u8 *ptr = (u8 *)_Ptr;\
+		DbgPrint printstr;\
+		DbgPrint(" ");\
+		for (__i = 0; __i < 6; __i++)\
+			DbgPrint("%02X%s", ptr[__i], (__i == 5) ? "" : "-");\
+		DbgPrint("\n");\
+	} \
+}
+
+#define BTC_PRINT_DATA(dbgtype, dbgflag, _TitleString, _HexData, _HexDataLen)\
+{\
+	if (GLBtcDbgType[dbgtype] & dbgflag) {\
+		int __i;\
+		u8 *ptr = (u8 *)_HexData;\
+		DbgPrint(_TitleString);\
+		for (__i = 0; __i < (int)_HexDataLen; __i++) {\
+			DbgPrint("%02X%s", ptr[__i], (((__i + 1) % 4) == 0) ? "  " : " ");\
+			if (((__i + 1) % 16) == 0)\
+				DbgPrint("\n");\
+		} \
+		DbgPrint("\n");\
+	} \
+}
+
+#else
+#define BTC_PRINT(dbgtype, dbgflag, printstr)
+#define BTC_PRINT_F(dbgtype, dbgflag, printstr)
+#define BTC_PRINT_ADDR(dbgtype, dbgflag, printstr, _Ptr)
+#define BTC_PRINT_DATA(dbgtype, dbgflag, _TitleString, _HexData, _HexDataLen)
+#endif
+
+typedef struct _BTC_BOARD_INFO {
+	/*  The following is some board information */
+	u8 btChipType;
+	u8 pgAntNum;	/*  pg ant number */
+	u8 btdmAntNum;	/*  ant number for btdm */
+	u8 btdmAntPos;		/* Bryant Add to indicate Antenna Position for (pgAntNum = 2) && (btdmAntNum = 1)  (DPDT+1Ant case) */
+	u8 singleAntPath;	/*  current used for 8723b only, 1 =>s0,  0 =>s1 */
+	/* bool				bBtExist; */
+} BTC_BOARD_INFO, *PBTC_BOARD_INFO;
+
+typedef enum _BTC_DBG_OPCODE {
+	BTC_DBG_SET_COEX_NORMAL			    = 0x0,
+	BTC_DBG_SET_COEX_WIFI_ONLY		    = 0x1,
+	BTC_DBG_SET_COEX_BT_ONLY		    = 0x2,
+	BTC_DBG_SET_COEX_DEC_BT_PWR		    = 0x3,
+	BTC_DBG_SET_COEX_BT_AFH_MAP		    = 0x4,
+	BTC_DBG_SET_COEX_BT_IGNORE_WLAN_ACT	= 0x5,
+	BTC_DBG_MAX
+} BTC_DBG_OPCODE, *PBTC_DBG_OPCODE;
+
+typedef enum _BTC_RSSI_STATE {
+	BTC_RSSI_STATE_HIGH			    = 0x0,
+	BTC_RSSI_STATE_MEDIUM			= 0x1,
+	BTC_RSSI_STATE_LOW			    = 0x2,
+	BTC_RSSI_STATE_STAY_HIGH		= 0x3,
+	BTC_RSSI_STATE_STAY_MEDIUM		= 0x4,
+	BTC_RSSI_STATE_STAY_LOW			= 0x5,
+	BTC_RSSI_MAX
+} BTC_RSSI_STATE, *PBTC_RSSI_STATE;
+#define BTC_RSSI_HIGH(_rssi_)	((_rssi_ == BTC_RSSI_STATE_HIGH || _rssi_ == BTC_RSSI_STATE_STAY_HIGH) ? true : false)
+#define BTC_RSSI_MEDIUM(_rssi_)	((_rssi_ == BTC_RSSI_STATE_MEDIUM || _rssi_ == BTC_RSSI_STATE_STAY_MEDIUM) ? true : false)
+#define BTC_RSSI_LOW(_rssi_)	((_rssi_ == BTC_RSSI_STATE_LOW || _rssi_ == BTC_RSSI_STATE_STAY_LOW) ? true : false)
+
+typedef enum _BTC_WIFI_ROLE {
+	BTC_ROLE_STATION			= 0x0,
+	BTC_ROLE_AP					= 0x1,
+	BTC_ROLE_IBSS				= 0x2,
+	BTC_ROLE_HS_MODE			= 0x3,
+	BTC_ROLE_MAX
+} BTC_WIFI_ROLE, *PBTC_WIFI_ROLE;
+
+typedef enum _BTC_WIFI_BW_MODE {
+	BTC_WIFI_BW_LEGACY			= 0x0,
+	BTC_WIFI_BW_HT20			= 0x1,
+	BTC_WIFI_BW_HT40			= 0x2,
+	BTC_WIFI_BW_MAX
+} BTC_WIFI_BW_MODE, *PBTC_WIFI_BW_MODE;
+
+typedef enum _BTC_WIFI_TRAFFIC_DIR {
+	BTC_WIFI_TRAFFIC_TX			= 0x0,
+	BTC_WIFI_TRAFFIC_RX			= 0x1,
+	BTC_WIFI_TRAFFIC_MAX
+} BTC_WIFI_TRAFFIC_DIR, *PBTC_WIFI_TRAFFIC_DIR;
+
+typedef enum _BTC_WIFI_PNP {
+	BTC_WIFI_PNP_WAKE_UP		= 0x0,
+	BTC_WIFI_PNP_SLEEP			= 0x1,
+	BTC_WIFI_PNP_MAX
+} BTC_WIFI_PNP, *PBTC_WIFI_PNP;
+
+/* for 8723b-d cut large current issue */
+typedef enum _BT_WIFI_COEX_STATE {
+	BTC_WIFI_STAT_INIT,
+	BTC_WIFI_STAT_IQK,
+	BTC_WIFI_STAT_NORMAL_OFF,
+	BTC_WIFI_STAT_MP_OFF,
+	BTC_WIFI_STAT_NORMAL,
+	BTC_WIFI_STAT_ANT_DIV,
+	BTC_WIFI_STAT_MAX
+} BT_WIFI_COEX_STATE, *PBT_WIFI_COEX_STATE;
+
+/*  defined for BFP_BTC_GET */
+typedef enum _BTC_GET_TYPE {
+	/*  type bool */
+	BTC_GET_BL_HS_OPERATION,
+	BTC_GET_BL_HS_CONNECTING,
+	BTC_GET_BL_WIFI_CONNECTED,
+	BTC_GET_BL_WIFI_BUSY,
+	BTC_GET_BL_WIFI_SCAN,
+	BTC_GET_BL_WIFI_LINK,
+	BTC_GET_BL_WIFI_ROAM,
+	BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+	BTC_GET_BL_WIFI_UNDER_5G,
+	BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+	BTC_GET_BL_WIFI_ENABLE_ENCRYPTION,
+	BTC_GET_BL_WIFI_UNDER_B_MODE,
+	BTC_GET_BL_EXT_SWITCH,
+	BTC_GET_BL_WIFI_IS_IN_MP_MODE,
+
+	/*  type s32 */
+	BTC_GET_S4_WIFI_RSSI,
+	BTC_GET_S4_HS_RSSI,
+
+	/*  type u32 */
+	BTC_GET_U4_WIFI_BW,
+	BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+	BTC_GET_U4_WIFI_FW_VER,
+	BTC_GET_U4_WIFI_LINK_STATUS,
+	BTC_GET_U4_BT_PATCH_VER,
+
+	/*  type u8 */
+	BTC_GET_U1_WIFI_DOT11_CHNL,
+	BTC_GET_U1_WIFI_CENTRAL_CHNL,
+	BTC_GET_U1_WIFI_HS_CHNL,
+	BTC_GET_U1_MAC_PHY_MODE,
+	BTC_GET_U1_AP_NUM,
+
+	/*  for 1Ant ====== */
+	BTC_GET_U1_LPS_MODE,
+
+	BTC_GET_MAX
+} BTC_GET_TYPE, *PBTC_GET_TYPE;
+
+/*  defined for BFP_BTC_SET */
+typedef enum _BTC_SET_TYPE {
+	/*  type bool */
+	BTC_SET_BL_BT_DISABLE,
+	BTC_SET_BL_BT_TRAFFIC_BUSY,
+	BTC_SET_BL_BT_LIMITED_DIG,
+	BTC_SET_BL_FORCE_TO_ROAM,
+	BTC_SET_BL_TO_REJ_AP_AGG_PKT,
+	BTC_SET_BL_BT_CTRL_AGG_SIZE,
+	BTC_SET_BL_INC_SCAN_DEV_NUM,
+	BTC_SET_BL_BT_TX_RX_MASK,
+
+	/*  type u8 */
+	BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON,
+	BTC_SET_U1_AGG_BUF_SIZE,
+
+	/*  type trigger some action */
+	BTC_SET_ACT_GET_BT_RSSI,
+	BTC_SET_ACT_AGGREGATE_CTRL,
+	/*  for 1Ant ====== */
+	/*  type bool */
+
+	/*  type u8 */
+	BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
+	BTC_SET_U1_LPS_VAL,
+	BTC_SET_U1_RPWM_VAL,
+	/*  type trigger some action */
+	BTC_SET_ACT_LEAVE_LPS,
+	BTC_SET_ACT_ENTER_LPS,
+	BTC_SET_ACT_NORMAL_LPS,
+	BTC_SET_ACT_DISABLE_LOW_POWER,
+	BTC_SET_ACT_UPDATE_RAMASK,
+	BTC_SET_ACT_SEND_MIMO_PS,
+	/*  BT Coex related */
+	BTC_SET_ACT_CTRL_BT_INFO,
+	BTC_SET_ACT_CTRL_BT_COEX,
+	BTC_SET_ACT_CTRL_8723B_ANT,
+	/*  */
+	BTC_SET_MAX
+} BTC_SET_TYPE, *PBTC_SET_TYPE;
+
+typedef enum _BTC_DBG_DISP_TYPE {
+	BTC_DBG_DISP_COEX_STATISTICS		= 0x0,
+	BTC_DBG_DISP_BT_LINK_INFO			= 0x1,
+	BTC_DBG_DISP_FW_PWR_MODE_CMD		= 0x2,
+	BTC_DBG_DISP_MAX
+} BTC_DBG_DISP_TYPE, *PBTC_DBG_DISP_TYPE;
+
+typedef enum _BTC_NOTIFY_TYPE_IPS {
+	BTC_IPS_LEAVE						= 0x0,
+	BTC_IPS_ENTER						= 0x1,
+	BTC_IPS_MAX
+} BTC_NOTIFY_TYPE_IPS, *PBTC_NOTIFY_TYPE_IPS;
+
+typedef enum _BTC_NOTIFY_TYPE_LPS {
+	BTC_LPS_DISABLE						= 0x0,
+	BTC_LPS_ENABLE						= 0x1,
+	BTC_LPS_MAX
+} BTC_NOTIFY_TYPE_LPS, *PBTC_NOTIFY_TYPE_LPS;
+
+typedef enum _BTC_NOTIFY_TYPE_SCAN {
+	BTC_SCAN_FINISH						= 0x0,
+	BTC_SCAN_START						= 0x1,
+	BTC_SCAN_MAX
+} BTC_NOTIFY_TYPE_SCAN, *PBTC_NOTIFY_TYPE_SCAN;
+
+typedef enum _BTC_NOTIFY_TYPE_ASSOCIATE {
+	BTC_ASSOCIATE_FINISH				= 0x0,
+	BTC_ASSOCIATE_START					= 0x1,
+	BTC_ASSOCIATE_MAX
+} BTC_NOTIFY_TYPE_ASSOCIATE, *PBTC_NOTIFY_TYPE_ASSOCIATE;
+
+typedef enum _BTC_NOTIFY_TYPE_MEDIA_STATUS {
+	BTC_MEDIA_DISCONNECT				= 0x0,
+	BTC_MEDIA_CONNECT					= 0x1,
+	BTC_MEDIA_MAX
+} BTC_NOTIFY_TYPE_MEDIA_STATUS, *PBTC_NOTIFY_TYPE_MEDIA_STATUS;
+
+typedef enum _BTC_NOTIFY_TYPE_SPECIAL_PACKET {
+	BTC_PACKET_UNKNOWN					= 0x0,
+	BTC_PACKET_DHCP						= 0x1,
+	BTC_PACKET_ARP						= 0x2,
+	BTC_PACKET_EAPOL					= 0x3,
+	BTC_PACKET_MAX
+} BTC_NOTIFY_TYPE_SPECIAL_PACKET, *PBTC_NOTIFY_TYPE_SPECIAL_PACKET;
+
+typedef enum _BTC_NOTIFY_TYPE_STACK_OPERATION {
+	BTC_STACK_OP_NONE					= 0x0,
+	BTC_STACK_OP_INQ_PAGE_PAIR_START	= 0x1,
+	BTC_STACK_OP_INQ_PAGE_PAIR_FINISH	= 0x2,
+	BTC_STACK_OP_MAX
+} BTC_NOTIFY_TYPE_STACK_OPERATION, *PBTC_NOTIFY_TYPE_STACK_OPERATION;
+
+/* Bryant Add */
+typedef enum _BTC_ANTENNA_POS {
+	BTC_ANTENNA_AT_MAIN_PORT = 0x1,
+	BTC_ANTENNA_AT_AUX_PORT  = 0x2,
+} BTC_ANTENNA_POS, *PBTC_ANTENNA_POS;
+
+typedef u8 (*BFP_BTC_R1)(void *pBtcContext, u32 RegAddr);
+typedef u16(*BFP_BTC_R2)(void *pBtcContext, u32 RegAddr);
+typedef u32 (*BFP_BTC_R4)(void *pBtcContext, u32 RegAddr);
+typedef void (*BFP_BTC_W1)(void *pBtcContext, u32 RegAddr, u8 Data);
+typedef void(*BFP_BTC_W1_BIT_MASK)(
+	void *pBtcContext, u32 regAddr, u8 bitMask, u8 data1b
+);
+typedef void (*BFP_BTC_W2)(void *pBtcContext, u32 RegAddr, u16 Data);
+typedef void (*BFP_BTC_W4)(void *pBtcContext, u32 RegAddr, u32 Data);
+typedef void (*BFP_BTC_LOCAL_REG_W1)(void *pBtcContext, u32 RegAddr, u8 Data);
+typedef void (*BFP_BTC_SET_BB_REG)(
+	void *pBtcContext, u32 RegAddr, u32 BitMask, u32 Data
+);
+typedef u32 (*BFP_BTC_GET_BB_REG)(void *pBtcContext, u32 RegAddr, u32 BitMask);
+typedef void (*BFP_BTC_SET_RF_REG)(
+	void *pBtcContext, u8 eRFPath, u32 RegAddr, u32 BitMask, u32 Data
+);
+typedef u32 (*BFP_BTC_GET_RF_REG)(
+	void *pBtcContext, u8 eRFPath, u32 RegAddr, u32 BitMask
+);
+typedef void (*BFP_BTC_FILL_H2C)(
+	void *pBtcContext, u8 elementId, u32 cmdLen, u8 *pCmdBuffer
+);
+
+typedef	u8 (*BFP_BTC_GET)(void *pBtCoexist, u8 getType, void *pOutBuf);
+
+typedef	u8 (*BFP_BTC_SET)(void *pBtCoexist, u8 setType, void *pInBuf);
+typedef void (*BFP_BTC_SET_BT_REG)(
+	void *pBtcContext, u8 regType, u32 offset, u32 value
+);
+typedef u32 (*BFP_BTC_GET_BT_REG)(void *pBtcContext, u8 regType, u32 offset);
+typedef void (*BFP_BTC_DISP_DBG_MSG)(void *pBtCoexist, u8 dispType);
+
+typedef struct _BTC_BT_INFO {
+	bool bBtDisabled;
+	u8 rssiAdjustForAgcTableOn;
+	u8 rssiAdjustFor1AntCoexType;
+	bool bPreBtCtrlAggBufSize;
+	bool bBtCtrlAggBufSize;
+	bool bRejectAggPkt;
+	bool bIncreaseScanDevNum;
+	bool bBtTxRxMask;
+	u8 preAggBufSize;
+	u8 aggBufSize;
+	bool bBtBusy;
+	bool bLimitedDig;
+	u16 btHciVer;
+	u16 btRealFwVer;
+	u8 btFwVer;
+	u32 getBtFwVerCnt;
+
+	bool bBtDisableLowPwr;
+
+	bool bBtCtrlLps;
+	bool bBtLpsOn;
+	bool bForceToRoam;	/*  for 1Ant solution */
+	u8 lpsVal;
+	u8 rpwmVal;
+	u32 raMask;
+} BTC_BT_INFO, *PBTC_BT_INFO;
+
+typedef struct _BTC_STACK_INFO {
+	bool bProfileNotified;
+	u16 hciVersion;	/*  stack hci version */
+	u8 numOfLink;
+	bool bBtLinkExist;
+	bool bScoExist;
+	bool bAclExist;
+	bool bA2dpExist;
+	bool bHidExist;
+	u8 numOfHid;
+	bool bPanExist;
+	bool bUnknownAclExist;
+	s8 minBtRssi;
+} BTC_STACK_INFO, *PBTC_STACK_INFO;
+
+typedef struct _BTC_BT_LINK_INFO {
+	bool bBtLinkExist;
+	bool bScoExist;
+	bool bScoOnly;
+	bool bA2dpExist;
+	bool bA2dpOnly;
+	bool bHidExist;
+	bool bHidOnly;
+	bool bPanExist;
+	bool bPanOnly;
+	bool bSlaveRole;
+} BTC_BT_LINK_INFO, *PBTC_BT_LINK_INFO;
+
+typedef struct _BTC_STATISTICS {
+	u32 cntBind;
+	u32 cntPowerOn;
+	u32 cntInitHwConfig;
+	u32 cntInitCoexDm;
+	u32 cntIpsNotify;
+	u32 cntLpsNotify;
+	u32 cntScanNotify;
+	u32 cntConnectNotify;
+	u32 cntMediaStatusNotify;
+	u32 cntSpecialPacketNotify;
+	u32 cntBtInfoNotify;
+	u32 cntRfStatusNotify;
+	u32 cntPeriodical;
+	u32 cntCoexDmSwitch;
+	u32 cntStackOperationNotify;
+	u32 cntDbgCtrl;
+} BTC_STATISTICS, *PBTC_STATISTICS;
+
+typedef struct _BTC_COEXIST {
+	bool bBinded;		/*  make sure only one adapter can bind the data context */
+	void *Adapter;		/*  default adapter */
+	BTC_BOARD_INFO boardInfo;
+	BTC_BT_INFO btInfo;		/*  some bt info referenced by non-bt module */
+	BTC_STACK_INFO stackInfo;
+	BTC_BT_LINK_INFO btLinkInfo;
+	BTC_CHIP_INTERFACE chipInterface;
+
+	bool bInitilized;
+	bool bStopCoexDm;
+	bool bManualControl;
+	u8 *cliBuf;
+	BTC_STATISTICS statistics;
+	u8 pwrModeVal[10];
+
+	/*  function pointers */
+	/*  io related */
+	BFP_BTC_R1 fBtcRead1Byte;
+	BFP_BTC_W1 fBtcWrite1Byte;
+	BFP_BTC_W1_BIT_MASK fBtcWrite1ByteBitMask;
+	BFP_BTC_R2 fBtcRead2Byte;
+	BFP_BTC_W2 fBtcWrite2Byte;
+	BFP_BTC_R4 fBtcRead4Byte;
+	BFP_BTC_W4 fBtcWrite4Byte;
+	BFP_BTC_LOCAL_REG_W1 fBtcWriteLocalReg1Byte;
+	/*  read/write bb related */
+	BFP_BTC_SET_BB_REG fBtcSetBbReg;
+	BFP_BTC_GET_BB_REG fBtcGetBbReg;
+
+	/*  read/write rf related */
+	BFP_BTC_SET_RF_REG fBtcSetRfReg;
+	BFP_BTC_GET_RF_REG fBtcGetRfReg;
+
+	/*  fill h2c related */
+	BFP_BTC_FILL_H2C fBtcFillH2c;
+	/*  other */
+	BFP_BTC_DISP_DBG_MSG fBtcDispDbgMsg;
+	/*  normal get/set related */
+	BFP_BTC_GET fBtcGet;
+	BFP_BTC_SET fBtcSet;
+
+	BFP_BTC_GET_BT_REG fBtcGetBtReg;
+	BFP_BTC_SET_BT_REG fBtcSetBtReg;
+} BTC_COEXIST, *PBTC_COEXIST;
+
+extern BTC_COEXIST GLBtCoexist;
+
+u8 EXhalbtcoutsrc_InitlizeVariables(void *Adapter);
+void EXhalbtcoutsrc_PowerOnSetting(PBTC_COEXIST pBtCoexist);
+void EXhalbtcoutsrc_InitHwConfig(PBTC_COEXIST pBtCoexist, u8 bWifiOnly);
+void EXhalbtcoutsrc_InitCoexDm(PBTC_COEXIST pBtCoexist);
+void EXhalbtcoutsrc_IpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtcoutsrc_LpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtcoutsrc_ScanNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtcoutsrc_ConnectNotify(PBTC_COEXIST pBtCoexist, u8 action);
+void EXhalbtcoutsrc_MediaStatusNotify(
+	PBTC_COEXIST pBtCoexist, RT_MEDIA_STATUS mediaStatus
+);
+void EXhalbtcoutsrc_SpecialPacketNotify(PBTC_COEXIST pBtCoexist, u8 pktType);
+void EXhalbtcoutsrc_BtInfoNotify(
+	PBTC_COEXIST pBtCoexist, u8 *tmpBuf, u8 length
+);
+void EXhalbtcoutsrc_HaltNotify(PBTC_COEXIST pBtCoexist);
+void EXhalbtcoutsrc_PnpNotify(PBTC_COEXIST pBtCoexist, u8 pnpState);
+void EXhalbtcoutsrc_Periodical(PBTC_COEXIST pBtCoexist);
+void EXhalbtcoutsrc_SetChipType(u8 chipType);
+void EXhalbtcoutsrc_SetAntNum(u8 type, u8 antNum);
+void EXhalbtcoutsrc_SetSingleAntPath(u8 singleAntPath);
+void EXhalbtcoutsrc_DisplayBtCoexInfo(PBTC_COEXIST pBtCoexist);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/hal_com.c linux/3rdparty/rtl8723bs/hal/hal_com.c
--- linux.old/3rdparty/rtl8723bs/hal/hal_com.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/hal_com.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,1753 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#define _HAL_COM_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include "hal_com_h2c.h"
+
+#include "odm_precomp.h"
+
+u8 rtw_hal_data_init(struct adapter *padapter)
+{
+	if (is_primary_adapter(padapter)) {	/* if (padapter->isprimary) */
+		padapter->hal_data_sz = sizeof(struct hal_com_data);
+		padapter->HalData = vzalloc(padapter->hal_data_sz);
+		if (padapter->HalData == NULL) {
+			DBG_8192C("cant not alloc memory for HAL DATA\n");
+			return _FAIL;
+		}
+	}
+	return _SUCCESS;
+}
+
+void rtw_hal_data_deinit(struct adapter *padapter)
+{
+	if (is_primary_adapter(padapter)) {	/* if (padapter->isprimary) */
+		if (padapter->HalData) {
+			#ifdef CONFIG_LOAD_PHY_PARA_FROM_FILE
+			phy_free_filebuf(padapter);
+			#endif
+			vfree(padapter->HalData);
+			padapter->HalData = NULL;
+			padapter->hal_data_sz = 0;
+		}
+	}
+}
+
+
+void dump_chip_info(HAL_VERSION	ChipVersion)
+{
+	int cnt = 0;
+	u8 buf[128];
+
+	cnt += sprintf((buf+cnt), "Chip Version Info: CHIP_8723B_");
+	cnt += sprintf((buf+cnt), "%s_", IS_NORMAL_CHIP(ChipVersion) ? "Normal_Chip" : "Test_Chip");
+	if (IS_CHIP_VENDOR_TSMC(ChipVersion))
+		cnt += sprintf((buf+cnt), "%s_", "TSMC");
+	else if (IS_CHIP_VENDOR_UMC(ChipVersion))
+		cnt += sprintf((buf+cnt), "%s_", "UMC");
+	else if (IS_CHIP_VENDOR_SMIC(ChipVersion))
+		cnt += sprintf((buf+cnt), "%s_", "SMIC");
+
+	if (IS_A_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "A_CUT_");
+	else if (IS_B_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "B_CUT_");
+	else if (IS_C_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "C_CUT_");
+	else if (IS_D_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "D_CUT_");
+	else if (IS_E_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "E_CUT_");
+	else if (IS_I_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "I_CUT_");
+	else if (IS_J_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "J_CUT_");
+	else if (IS_K_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "K_CUT_");
+	else
+		cnt += sprintf((buf+cnt), "UNKNOWN_CUT(%d)_", ChipVersion.CUTVersion);
+
+	if (IS_1T1R(ChipVersion))
+		cnt += sprintf((buf+cnt), "1T1R_");
+	else if (IS_1T2R(ChipVersion))
+		cnt += sprintf((buf+cnt), "1T2R_");
+	else if (IS_2T2R(ChipVersion))
+		cnt += sprintf((buf+cnt), "2T2R_");
+	else
+		cnt += sprintf((buf+cnt), "UNKNOWN_RFTYPE(%d)_", ChipVersion.RFType);
+
+	cnt += sprintf((buf+cnt), "RomVer(%d)\n", ChipVersion.ROMVer);
+
+	DBG_871X("%s", buf);
+}
+
+
+#define	EEPROM_CHANNEL_PLAN_BY_HW_MASK	0x80
+
+/*
+ * Description:
+ *Use hardware(efuse), driver parameter(registry) and default channel plan
+ *to decide which one should be used.
+ *
+ * Parameters:
+ *padapter			pointer of adapter
+ *hw_channel_plan		channel plan from HW (efuse/eeprom)
+ *					BIT[7] software configure mode; 0:Enable, 1:disable
+ *					BIT[6:0] Channel Plan
+ *sw_channel_plan		channel plan from SW (registry/module param)
+ *def_channel_plan	channel plan used when HW/SW both invalid
+ *AutoLoadFail		efuse autoload fail or not
+ *
+ * Return:
+ *Final channel plan decision
+ *
+ */
+u8 hal_com_config_channel_plan(
+	struct adapter *padapter,
+	u8 hw_channel_plan,
+	u8 sw_channel_plan,
+	u8 def_channel_plan,
+	bool AutoLoadFail
+)
+{
+	struct hal_com_data *pHalData;
+	u8 chnlPlan;
+
+	pHalData = GET_HAL_DATA(padapter);
+	pHalData->bDisableSWChannelPlan = false;
+	chnlPlan = def_channel_plan;
+
+	if (0xFF == hw_channel_plan)
+		AutoLoadFail = true;
+
+	if (false == AutoLoadFail) {
+		u8 hw_chnlPlan;
+
+		hw_chnlPlan = hw_channel_plan & (~EEPROM_CHANNEL_PLAN_BY_HW_MASK);
+		if (rtw_is_channel_plan_valid(hw_chnlPlan)) {
+#ifndef CONFIG_SW_CHANNEL_PLAN
+			if (hw_channel_plan & EEPROM_CHANNEL_PLAN_BY_HW_MASK)
+				pHalData->bDisableSWChannelPlan = true;
+#endif /*  !CONFIG_SW_CHANNEL_PLAN */
+
+			chnlPlan = hw_chnlPlan;
+		}
+	}
+
+	if (
+		(false == pHalData->bDisableSWChannelPlan) &&
+		rtw_is_channel_plan_valid(sw_channel_plan)
+	)
+		chnlPlan = sw_channel_plan;
+
+	return chnlPlan;
+}
+
+bool HAL_IsLegalChannel(struct adapter *Adapter, u32 Channel)
+{
+	bool bLegalChannel = true;
+
+	if (Channel > 14) {
+		bLegalChannel = false;
+		DBG_871X("Channel > 14 but wireless_mode do not support 5G\n");
+	} else if ((Channel <= 14) && (Channel >= 1)) {
+		if (IsSupported24G(Adapter->registrypriv.wireless_mode) == false) {
+			bLegalChannel = false;
+			DBG_871X("(Channel <= 14) && (Channel >= 1) but wireless_mode do not support 2.4G\n");
+		}
+	} else {
+		bLegalChannel = false;
+		DBG_871X("Channel is Invalid !!!\n");
+	}
+
+	return bLegalChannel;
+}
+
+u8 MRateToHwRate(u8 rate)
+{
+	u8 ret = DESC_RATE1M;
+
+	switch (rate) {
+	case MGN_1M:
+		ret = DESC_RATE1M;
+		break;
+	case MGN_2M:
+		ret = DESC_RATE2M;
+		break;
+	case MGN_5_5M:
+		ret = DESC_RATE5_5M;
+		break;
+	case MGN_11M:
+		ret = DESC_RATE11M;
+		break;
+	case MGN_6M:
+		ret = DESC_RATE6M;
+		break;
+	case MGN_9M:
+		ret = DESC_RATE9M;
+		break;
+	case MGN_12M:
+		ret = DESC_RATE12M;
+		break;
+	case MGN_18M:
+		ret = DESC_RATE18M;
+		break;
+	case MGN_24M:
+		ret = DESC_RATE24M;
+		break;
+	case MGN_36M:
+		ret = DESC_RATE36M;
+		break;
+	case MGN_48M:
+		ret = DESC_RATE48M;
+		break;
+	case MGN_54M:
+		ret = DESC_RATE54M;
+		break;
+	case MGN_MCS0:
+		ret = DESC_RATEMCS0;
+		break;
+	case MGN_MCS1:
+		ret = DESC_RATEMCS1;
+		break;
+	case MGN_MCS2:
+		ret = DESC_RATEMCS2;
+		break;
+	case MGN_MCS3:
+		ret = DESC_RATEMCS3;
+		break;
+	case MGN_MCS4:
+		ret = DESC_RATEMCS4;
+		break;
+	case MGN_MCS5:
+		ret = DESC_RATEMCS5;
+		break;
+	case MGN_MCS6:
+		ret = DESC_RATEMCS6;
+		break;
+	case MGN_MCS7:
+		ret = DESC_RATEMCS7;
+		break;
+	case MGN_MCS8:
+		ret = DESC_RATEMCS8;
+		break;
+	case MGN_MCS9:
+		ret = DESC_RATEMCS9;
+		break;
+	case MGN_MCS10:
+		ret = DESC_RATEMCS10;
+		break;
+	case MGN_MCS11:
+		ret = DESC_RATEMCS11;
+		break;
+	case MGN_MCS12:
+		ret = DESC_RATEMCS12;
+		break;
+	case MGN_MCS13:
+		ret = DESC_RATEMCS13;
+		break;
+	case MGN_MCS14:
+		ret = DESC_RATEMCS14;
+		break;
+	case MGN_MCS15:
+		ret = DESC_RATEMCS15;
+		break;
+	case MGN_MCS16:
+		ret = DESC_RATEMCS16;
+		break;
+	case MGN_MCS17:
+		ret = DESC_RATEMCS17;
+		break;
+	case MGN_MCS18:
+		ret = DESC_RATEMCS18;
+		break;
+	case MGN_MCS19:
+		ret = DESC_RATEMCS19;
+		break;
+	case MGN_MCS20:
+		ret = DESC_RATEMCS20;
+		break;
+	case MGN_MCS21:
+		ret = DESC_RATEMCS21;
+		break;
+	case MGN_MCS22:
+		ret = DESC_RATEMCS22;
+		break;
+	case MGN_MCS23:
+		ret = DESC_RATEMCS23;
+		break;
+	case MGN_MCS24:
+		ret = DESC_RATEMCS24;
+		break;
+	case MGN_MCS25:
+		ret = DESC_RATEMCS25;
+		break;
+	case MGN_MCS26:
+		ret = DESC_RATEMCS26;
+		break;
+	case MGN_MCS27:
+		ret = DESC_RATEMCS27;
+		break;
+	case MGN_MCS28:
+		ret = DESC_RATEMCS28;
+		break;
+	case MGN_MCS29:
+		ret = DESC_RATEMCS29;
+		break;
+	case MGN_MCS30:
+		ret = DESC_RATEMCS30;
+		break;
+	case MGN_MCS31:
+		ret = DESC_RATEMCS31;
+		break;
+	case MGN_VHT1SS_MCS0:
+		ret = DESC_RATEVHTSS1MCS0;
+		break;
+	case MGN_VHT1SS_MCS1:
+		ret = DESC_RATEVHTSS1MCS1;
+		break;
+	case MGN_VHT1SS_MCS2:
+		ret = DESC_RATEVHTSS1MCS2;
+		break;
+	case MGN_VHT1SS_MCS3:
+		ret = DESC_RATEVHTSS1MCS3;
+		break;
+	case MGN_VHT1SS_MCS4:
+		ret = DESC_RATEVHTSS1MCS4;
+		break;
+	case MGN_VHT1SS_MCS5:
+		ret = DESC_RATEVHTSS1MCS5;
+		break;
+	case MGN_VHT1SS_MCS6:
+		ret = DESC_RATEVHTSS1MCS6;
+		break;
+	case MGN_VHT1SS_MCS7:
+		ret = DESC_RATEVHTSS1MCS7;
+		break;
+	case MGN_VHT1SS_MCS8:
+		ret = DESC_RATEVHTSS1MCS8;
+		break;
+	case MGN_VHT1SS_MCS9:
+		ret = DESC_RATEVHTSS1MCS9;
+		break;
+	case MGN_VHT2SS_MCS0:
+		ret = DESC_RATEVHTSS2MCS0;
+		break;
+	case MGN_VHT2SS_MCS1:
+		ret = DESC_RATEVHTSS2MCS1;
+		break;
+	case MGN_VHT2SS_MCS2:
+		ret = DESC_RATEVHTSS2MCS2;
+		break;
+	case MGN_VHT2SS_MCS3:
+		ret = DESC_RATEVHTSS2MCS3;
+		break;
+	case MGN_VHT2SS_MCS4:
+		ret = DESC_RATEVHTSS2MCS4;
+		break;
+	case MGN_VHT2SS_MCS5:
+		ret = DESC_RATEVHTSS2MCS5;
+		break;
+	case MGN_VHT2SS_MCS6:
+		ret = DESC_RATEVHTSS2MCS6;
+		break;
+	case MGN_VHT2SS_MCS7:
+		ret = DESC_RATEVHTSS2MCS7;
+		break;
+	case MGN_VHT2SS_MCS8:
+		ret = DESC_RATEVHTSS2MCS8;
+		break;
+	case MGN_VHT2SS_MCS9:
+		ret = DESC_RATEVHTSS2MCS9;
+		break;
+	case MGN_VHT3SS_MCS0:
+		ret = DESC_RATEVHTSS3MCS0;
+		break;
+	case MGN_VHT3SS_MCS1:
+		ret = DESC_RATEVHTSS3MCS1;
+		break;
+	case MGN_VHT3SS_MCS2:
+		ret = DESC_RATEVHTSS3MCS2;
+		break;
+	case MGN_VHT3SS_MCS3:
+		ret = DESC_RATEVHTSS3MCS3;
+		break;
+	case MGN_VHT3SS_MCS4:
+		ret = DESC_RATEVHTSS3MCS4;
+		break;
+	case MGN_VHT3SS_MCS5:
+		ret = DESC_RATEVHTSS3MCS5;
+		break;
+	case MGN_VHT3SS_MCS6:
+		ret = DESC_RATEVHTSS3MCS6;
+		break;
+	case MGN_VHT3SS_MCS7:
+		ret = DESC_RATEVHTSS3MCS7;
+		break;
+	case MGN_VHT3SS_MCS8:
+		ret = DESC_RATEVHTSS3MCS8;
+		break;
+	case MGN_VHT3SS_MCS9:
+		ret = DESC_RATEVHTSS3MCS9;
+		break;
+	case MGN_VHT4SS_MCS0:
+		ret = DESC_RATEVHTSS4MCS0;
+		break;
+	case MGN_VHT4SS_MCS1:
+		ret = DESC_RATEVHTSS4MCS1;
+		break;
+	case MGN_VHT4SS_MCS2:
+		ret = DESC_RATEVHTSS4MCS2;
+		break;
+	case MGN_VHT4SS_MCS3:
+		ret = DESC_RATEVHTSS4MCS3;
+		break;
+	case MGN_VHT4SS_MCS4:
+		ret = DESC_RATEVHTSS4MCS4;
+		break;
+	case MGN_VHT4SS_MCS5:
+		ret = DESC_RATEVHTSS4MCS5;
+		break;
+	case MGN_VHT4SS_MCS6:
+		ret = DESC_RATEVHTSS4MCS6;
+		break;
+	case MGN_VHT4SS_MCS7:
+		ret = DESC_RATEVHTSS4MCS7;
+		break;
+	case MGN_VHT4SS_MCS8:
+		ret = DESC_RATEVHTSS4MCS8;
+		break;
+	case MGN_VHT4SS_MCS9:
+		ret = DESC_RATEVHTSS4MCS9;
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+u8 HwRateToMRate(u8 rate)
+{
+	u8 ret_rate = MGN_1M;
+
+	switch (rate) {
+	case DESC_RATE1M:
+		ret_rate = MGN_1M;
+		break;
+	case DESC_RATE2M:
+		ret_rate = MGN_2M;
+		break;
+	case DESC_RATE5_5M:
+		ret_rate = MGN_5_5M;
+		break;
+	case DESC_RATE11M:
+		ret_rate = MGN_11M;
+		break;
+	case DESC_RATE6M:
+		ret_rate = MGN_6M;
+		break;
+	case DESC_RATE9M:
+		ret_rate = MGN_9M;
+		break;
+	case DESC_RATE12M:
+		ret_rate = MGN_12M;
+		break;
+	case DESC_RATE18M:
+		ret_rate = MGN_18M;
+		break;
+	case DESC_RATE24M:
+		ret_rate = MGN_24M;
+		break;
+	case DESC_RATE36M:
+		ret_rate = MGN_36M;
+		break;
+	case DESC_RATE48M:
+		ret_rate = MGN_48M;
+		break;
+	case DESC_RATE54M:
+		ret_rate = MGN_54M;
+		break;
+	case DESC_RATEMCS0:
+		ret_rate = MGN_MCS0;
+		break;
+	case DESC_RATEMCS1:
+		ret_rate = MGN_MCS1;
+		break;
+	case DESC_RATEMCS2:
+		ret_rate = MGN_MCS2;
+		break;
+	case DESC_RATEMCS3:
+		ret_rate = MGN_MCS3;
+		break;
+	case DESC_RATEMCS4:
+		ret_rate = MGN_MCS4;
+		break;
+	case DESC_RATEMCS5:
+		ret_rate = MGN_MCS5;
+		break;
+	case DESC_RATEMCS6:
+		ret_rate = MGN_MCS6;
+		break;
+	case DESC_RATEMCS7:
+		ret_rate = MGN_MCS7;
+		break;
+	case DESC_RATEMCS8:
+		ret_rate = MGN_MCS8;
+		break;
+	case DESC_RATEMCS9:
+		ret_rate = MGN_MCS9;
+		break;
+	case DESC_RATEMCS10:
+		ret_rate = MGN_MCS10;
+		break;
+	case DESC_RATEMCS11:
+		ret_rate = MGN_MCS11;
+		break;
+	case DESC_RATEMCS12:
+		ret_rate = MGN_MCS12;
+		break;
+	case DESC_RATEMCS13:
+		ret_rate = MGN_MCS13;
+		break;
+	case DESC_RATEMCS14:
+		ret_rate = MGN_MCS14;
+		break;
+	case DESC_RATEMCS15:
+		ret_rate = MGN_MCS15;
+		break;
+	case DESC_RATEMCS16:
+		ret_rate = MGN_MCS16;
+		break;
+	case DESC_RATEMCS17:
+		ret_rate = MGN_MCS17;
+		break;
+	case DESC_RATEMCS18:
+		ret_rate = MGN_MCS18;
+		break;
+	case DESC_RATEMCS19:
+		ret_rate = MGN_MCS19;
+		break;
+	case DESC_RATEMCS20:
+		ret_rate = MGN_MCS20;
+		break;
+	case DESC_RATEMCS21:
+		ret_rate = MGN_MCS21;
+		break;
+	case DESC_RATEMCS22:
+		ret_rate = MGN_MCS22;
+		break;
+	case DESC_RATEMCS23:
+		ret_rate = MGN_MCS23;
+		break;
+	case DESC_RATEMCS24:
+		ret_rate = MGN_MCS24;
+		break;
+	case DESC_RATEMCS25:
+		ret_rate = MGN_MCS25;
+		break;
+	case DESC_RATEMCS26:
+		ret_rate = MGN_MCS26;
+		break;
+	case DESC_RATEMCS27:
+		ret_rate = MGN_MCS27;
+		break;
+	case DESC_RATEMCS28:
+		ret_rate = MGN_MCS28;
+		break;
+	case DESC_RATEMCS29:
+		ret_rate = MGN_MCS29;
+		break;
+	case DESC_RATEMCS30:
+		ret_rate = MGN_MCS30;
+		break;
+	case DESC_RATEMCS31:
+		ret_rate = MGN_MCS31;
+		break;
+	case DESC_RATEVHTSS1MCS0:
+		ret_rate = MGN_VHT1SS_MCS0;
+		break;
+	case DESC_RATEVHTSS1MCS1:
+		ret_rate = MGN_VHT1SS_MCS1;
+		break;
+	case DESC_RATEVHTSS1MCS2:
+		ret_rate = MGN_VHT1SS_MCS2;
+		break;
+	case DESC_RATEVHTSS1MCS3:
+		ret_rate = MGN_VHT1SS_MCS3;
+		break;
+	case DESC_RATEVHTSS1MCS4:
+		ret_rate = MGN_VHT1SS_MCS4;
+		break;
+	case DESC_RATEVHTSS1MCS5:
+		ret_rate = MGN_VHT1SS_MCS5;
+		break;
+	case DESC_RATEVHTSS1MCS6:
+		ret_rate = MGN_VHT1SS_MCS6;
+		break;
+	case DESC_RATEVHTSS1MCS7:
+		ret_rate = MGN_VHT1SS_MCS7;
+		break;
+	case DESC_RATEVHTSS1MCS8:
+		ret_rate = MGN_VHT1SS_MCS8;
+		break;
+	case DESC_RATEVHTSS1MCS9:
+		ret_rate = MGN_VHT1SS_MCS9;
+		break;
+	case DESC_RATEVHTSS2MCS0:
+		ret_rate = MGN_VHT2SS_MCS0;
+		break;
+	case DESC_RATEVHTSS2MCS1:
+		ret_rate = MGN_VHT2SS_MCS1;
+		break;
+	case DESC_RATEVHTSS2MCS2:
+		ret_rate = MGN_VHT2SS_MCS2;
+		break;
+	case DESC_RATEVHTSS2MCS3:
+		ret_rate = MGN_VHT2SS_MCS3;
+		break;
+	case DESC_RATEVHTSS2MCS4:
+		ret_rate = MGN_VHT2SS_MCS4;
+		break;
+	case DESC_RATEVHTSS2MCS5:
+		ret_rate = MGN_VHT2SS_MCS5;
+		break;
+	case DESC_RATEVHTSS2MCS6:
+		ret_rate = MGN_VHT2SS_MCS6;
+		break;
+	case DESC_RATEVHTSS2MCS7:
+		ret_rate = MGN_VHT2SS_MCS7;
+		break;
+	case DESC_RATEVHTSS2MCS8:
+		ret_rate = MGN_VHT2SS_MCS8;
+		break;
+	case DESC_RATEVHTSS2MCS9:
+		ret_rate = MGN_VHT2SS_MCS9;
+		break;
+	case DESC_RATEVHTSS3MCS0:
+		ret_rate = MGN_VHT3SS_MCS0;
+		break;
+	case DESC_RATEVHTSS3MCS1:
+		ret_rate = MGN_VHT3SS_MCS1;
+		break;
+	case DESC_RATEVHTSS3MCS2:
+		ret_rate = MGN_VHT3SS_MCS2;
+		break;
+	case DESC_RATEVHTSS3MCS3:
+		ret_rate = MGN_VHT3SS_MCS3;
+		break;
+	case DESC_RATEVHTSS3MCS4:
+		ret_rate = MGN_VHT3SS_MCS4;
+		break;
+	case DESC_RATEVHTSS3MCS5:
+		ret_rate = MGN_VHT3SS_MCS5;
+		break;
+	case DESC_RATEVHTSS3MCS6:
+		ret_rate = MGN_VHT3SS_MCS6;
+		break;
+	case DESC_RATEVHTSS3MCS7:
+		ret_rate = MGN_VHT3SS_MCS7;
+		break;
+	case DESC_RATEVHTSS3MCS8:
+		ret_rate = MGN_VHT3SS_MCS8;
+		break;
+	case DESC_RATEVHTSS3MCS9:
+		ret_rate = MGN_VHT3SS_MCS9;
+		break;
+	case DESC_RATEVHTSS4MCS0:
+		ret_rate = MGN_VHT4SS_MCS0;
+		break;
+	case DESC_RATEVHTSS4MCS1:
+		ret_rate = MGN_VHT4SS_MCS1;
+		break;
+	case DESC_RATEVHTSS4MCS2:
+		ret_rate = MGN_VHT4SS_MCS2;
+		break;
+	case DESC_RATEVHTSS4MCS3:
+		ret_rate = MGN_VHT4SS_MCS3;
+		break;
+	case DESC_RATEVHTSS4MCS4:
+		ret_rate = MGN_VHT4SS_MCS4;
+		break;
+	case DESC_RATEVHTSS4MCS5:
+		ret_rate = MGN_VHT4SS_MCS5;
+		break;
+	case DESC_RATEVHTSS4MCS6:
+		ret_rate = MGN_VHT4SS_MCS6;
+		break;
+	case DESC_RATEVHTSS4MCS7:
+		ret_rate = MGN_VHT4SS_MCS7;
+		break;
+	case DESC_RATEVHTSS4MCS8:
+		ret_rate = MGN_VHT4SS_MCS8;
+		break;
+	case DESC_RATEVHTSS4MCS9:
+		ret_rate = MGN_VHT4SS_MCS9;
+		break;
+
+	default:
+		DBG_871X("HwRateToMRate(): Non supported Rate [%x]!!!\n", rate);
+		break;
+	}
+
+	return ret_rate;
+}
+
+void HalSetBrateCfg(struct adapter *Adapter, u8 *mBratesOS, u16 *pBrateCfg)
+{
+	u8 i, is_brate, brate;
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+
+		is_brate = mBratesOS[i] & IEEE80211_BASIC_RATE_MASK;
+		brate = mBratesOS[i] & 0x7f;
+
+		if (is_brate) {
+			switch (brate) {
+			case IEEE80211_CCK_RATE_1MB:
+				*pBrateCfg |= RATE_1M;
+				break;
+			case IEEE80211_CCK_RATE_2MB:
+				*pBrateCfg |= RATE_2M;
+				break;
+			case IEEE80211_CCK_RATE_5MB:
+				*pBrateCfg |= RATE_5_5M;
+				break;
+			case IEEE80211_CCK_RATE_11MB:
+				*pBrateCfg |= RATE_11M;
+				break;
+			case IEEE80211_OFDM_RATE_6MB:
+				*pBrateCfg |= RATE_6M;
+				break;
+			case IEEE80211_OFDM_RATE_9MB:
+				*pBrateCfg |= RATE_9M;
+				break;
+			case IEEE80211_OFDM_RATE_12MB:
+				*pBrateCfg |= RATE_12M;
+				break;
+			case IEEE80211_OFDM_RATE_18MB:
+				*pBrateCfg |= RATE_18M;
+				break;
+			case IEEE80211_OFDM_RATE_24MB:
+				*pBrateCfg |= RATE_24M;
+				break;
+			case IEEE80211_OFDM_RATE_36MB:
+				*pBrateCfg |= RATE_36M;
+				break;
+			case IEEE80211_OFDM_RATE_48MB:
+				*pBrateCfg |= RATE_48M;
+				break;
+			case IEEE80211_OFDM_RATE_54MB:
+				*pBrateCfg |= RATE_54M;
+				break;
+			}
+		}
+	}
+}
+
+static void _OneOutPipeMapping(struct adapter *padapter)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+
+	pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[0];/* VO */
+	pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[0];/* VI */
+	pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[0];/* BE */
+	pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[0];/* BK */
+
+	pdvobjpriv->Queue2Pipe[4] = pdvobjpriv->RtOutPipe[0];/* BCN */
+	pdvobjpriv->Queue2Pipe[5] = pdvobjpriv->RtOutPipe[0];/* MGT */
+	pdvobjpriv->Queue2Pipe[6] = pdvobjpriv->RtOutPipe[0];/* HIGH */
+	pdvobjpriv->Queue2Pipe[7] = pdvobjpriv->RtOutPipe[0];/* TXCMD */
+}
+
+static void _TwoOutPipeMapping(struct adapter *padapter, bool bWIFICfg)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+
+	if (bWIFICfg) { /* WMM */
+
+		/* 	BK,	BE,	VI,	VO,	BCN,	CMD, MGT, HIGH, HCCA */
+		/*   0,		1,	0,	1,	0,	0,	0,	0,		0	}; */
+		/* 0:ep_0 num, 1:ep_1 num */
+
+		pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[1];/* VO */
+		pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[0];/* VI */
+		pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[1];/* BE */
+		pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[0];/* BK */
+
+		pdvobjpriv->Queue2Pipe[4] = pdvobjpriv->RtOutPipe[0];/* BCN */
+		pdvobjpriv->Queue2Pipe[5] = pdvobjpriv->RtOutPipe[0];/* MGT */
+		pdvobjpriv->Queue2Pipe[6] = pdvobjpriv->RtOutPipe[0];/* HIGH */
+		pdvobjpriv->Queue2Pipe[7] = pdvobjpriv->RtOutPipe[0];/* TXCMD */
+
+	} else { /* typical setting */
+
+
+		/* BK,	BE,	VI,	VO,	BCN,	CMD, MGT, HIGH, HCCA */
+		/*   1,		1,	0,	0,	0,	0,	0,	0,		0	}; */
+		/* 0:ep_0 num, 1:ep_1 num */
+
+		pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[0];/* VO */
+		pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[0];/* VI */
+		pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[1];/* BE */
+		pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[1];/* BK */
+
+		pdvobjpriv->Queue2Pipe[4] = pdvobjpriv->RtOutPipe[0];/* BCN */
+		pdvobjpriv->Queue2Pipe[5] = pdvobjpriv->RtOutPipe[0];/* MGT */
+		pdvobjpriv->Queue2Pipe[6] = pdvobjpriv->RtOutPipe[0];/* HIGH */
+		pdvobjpriv->Queue2Pipe[7] = pdvobjpriv->RtOutPipe[0];/* TXCMD */
+
+	}
+
+}
+
+static void _ThreeOutPipeMapping(struct adapter *padapter, bool bWIFICfg)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+
+	if (bWIFICfg) { /* for WMM */
+
+		/* 	BK,	BE,	VI,	VO,	BCN,	CMD, MGT, HIGH, HCCA */
+		/*   1,		2,	1,	0,	0,	0,	0,	0,		0	}; */
+		/* 0:H, 1:N, 2:L */
+
+		pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[0];/* VO */
+		pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[1];/* VI */
+		pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[2];/* BE */
+		pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[1];/* BK */
+
+		pdvobjpriv->Queue2Pipe[4] = pdvobjpriv->RtOutPipe[0];/* BCN */
+		pdvobjpriv->Queue2Pipe[5] = pdvobjpriv->RtOutPipe[0];/* MGT */
+		pdvobjpriv->Queue2Pipe[6] = pdvobjpriv->RtOutPipe[0];/* HIGH */
+		pdvobjpriv->Queue2Pipe[7] = pdvobjpriv->RtOutPipe[0];/* TXCMD */
+
+	} else { /* typical setting */
+
+
+		/* 	BK,	BE,	VI,	VO,	BCN,	CMD, MGT, HIGH, HCCA */
+		/*   2,		2,	1,	0,	0,	0,	0,	0,		0	}; */
+		/* 0:H, 1:N, 2:L */
+
+		pdvobjpriv->Queue2Pipe[0] = pdvobjpriv->RtOutPipe[0];/* VO */
+		pdvobjpriv->Queue2Pipe[1] = pdvobjpriv->RtOutPipe[1];/* VI */
+		pdvobjpriv->Queue2Pipe[2] = pdvobjpriv->RtOutPipe[2];/* BE */
+		pdvobjpriv->Queue2Pipe[3] = pdvobjpriv->RtOutPipe[2];/* BK */
+
+		pdvobjpriv->Queue2Pipe[4] = pdvobjpriv->RtOutPipe[0];/* BCN */
+		pdvobjpriv->Queue2Pipe[5] = pdvobjpriv->RtOutPipe[0];/* MGT */
+		pdvobjpriv->Queue2Pipe[6] = pdvobjpriv->RtOutPipe[0];/* HIGH */
+		pdvobjpriv->Queue2Pipe[7] = pdvobjpriv->RtOutPipe[0];/* TXCMD */
+	}
+
+}
+
+bool Hal_MappingOutPipe(struct adapter *padapter, u8 NumOutPipe)
+{
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+
+	bool bWIFICfg = (pregistrypriv->wifi_spec) ? true : false;
+
+	bool result = true;
+
+	switch (NumOutPipe) {
+	case 2:
+		_TwoOutPipeMapping(padapter, bWIFICfg);
+		break;
+	case 3:
+	case 4:
+		_ThreeOutPipeMapping(padapter, bWIFICfg);
+		break;
+	case 1:
+		_OneOutPipeMapping(padapter);
+		break;
+	default:
+		result = false;
+		break;
+	}
+
+	return result;
+
+}
+
+void hal_init_macaddr(struct adapter *adapter)
+{
+	rtw_hal_set_hwreg(adapter, HW_VAR_MAC_ADDR, adapter->eeprompriv.mac_addr);
+}
+
+void rtw_init_hal_com_default_value(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	pHalData->AntDetection = 1;
+}
+
+/*
+* C2H event format:
+* Field	 TRIGGER		CONTENT	   CMD_SEQ	CMD_LEN		 CMD_ID
+* BITS	 [127:120]	[119:16]      [15:8]		  [7:4]		   [3:0]
+*/
+
+void c2h_evt_clear(struct adapter *adapter)
+{
+	rtw_write8(adapter, REG_C2HEVT_CLEAR, C2H_EVT_HOST_CLOSE);
+}
+
+/*
+* C2H event format:
+* Field    TRIGGER    CMD_LEN    CONTENT    CMD_SEQ    CMD_ID
+* BITS    [127:120]   [119:112]    [111:16]	     [15:8]         [7:0]
+*/
+s32 c2h_evt_read_88xx(struct adapter *adapter, u8 *buf)
+{
+	s32 ret = _FAIL;
+	struct c2h_evt_hdr_88xx *c2h_evt;
+	int i;
+	u8 trigger;
+
+	if (buf == NULL)
+		goto exit;
+
+	trigger = rtw_read8(adapter, REG_C2HEVT_CLEAR);
+
+	if (trigger == C2H_EVT_HOST_CLOSE)
+		goto exit; /* Not ready */
+	else if (trigger != C2H_EVT_FW_CLOSE)
+		goto clear_evt; /* Not a valid value */
+
+	c2h_evt = (struct c2h_evt_hdr_88xx *)buf;
+
+	memset(c2h_evt, 0, 16);
+
+	c2h_evt->id = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL);
+	c2h_evt->seq = rtw_read8(adapter, REG_C2HEVT_CMD_SEQ_88XX);
+	c2h_evt->plen = rtw_read8(adapter, REG_C2HEVT_CMD_LEN_88XX);
+
+	RT_PRINT_DATA(
+		_module_hal_init_c_,
+		_drv_info_,
+		"c2h_evt_read(): ",
+		&c2h_evt,
+		sizeof(c2h_evt)
+	);
+
+	DBG_871X(
+		"%s id:%u, len:%u, seq:%u, trigger:0x%02x\n",
+		__func__,
+		c2h_evt->id,
+		c2h_evt->plen,
+		c2h_evt->seq,
+		trigger
+	);
+
+	/* Read the content */
+	for (i = 0; i < c2h_evt->plen; i++)
+		c2h_evt->payload[i] = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL + 2 + i);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_info_, "c2h_evt_read(): Command Content:\n",
+		c2h_evt->payload, c2h_evt->plen);
+
+	ret = _SUCCESS;
+
+clear_evt:
+	/*
+	* Clear event to notify FW we have read the command.
+	* If this field isn't clear, the FW won't update the next command message.
+	*/
+	c2h_evt_clear(adapter);
+exit:
+	return ret;
+}
+
+
+u8  rtw_hal_networktype_to_raid(struct adapter *adapter, struct sta_info *psta)
+{
+	return networktype_to_raid_ex(adapter, psta);
+}
+
+u8 rtw_get_mgntframe_raid(struct adapter *adapter, unsigned char network_type)
+{
+
+	u8 raid;
+	raid = (network_type & WIRELESS_11B) ? RATEID_IDX_B : RATEID_IDX_G;
+	return raid;
+}
+
+void rtw_hal_update_sta_rate_mask(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 i, rf_type, limit;
+	u32 tx_ra_bitmap;
+
+	if (psta == NULL)
+		return;
+
+	tx_ra_bitmap = 0;
+
+	/* b/g mode ra_bitmap */
+	for (i = 0; i < sizeof(psta->bssrateset); i++) {
+		if (psta->bssrateset[i])
+			tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value(psta->bssrateset[i]&0x7f);
+	}
+
+	/* n mode ra_bitmap */
+	if (psta->htpriv.ht_option) {
+		rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+		if (rf_type == RF_2T2R)
+			limit = 16; /*  2R */
+		else
+			limit = 8; /*   1R */
+
+		for (i = 0; i < limit; i++) {
+			if (psta->htpriv.ht_cap.supp_mcs_set[i/8] & BIT(i%8))
+				tx_ra_bitmap |= BIT(i+12);
+		}
+	}
+
+	psta->ra_mask = tx_ra_bitmap;
+	psta->init_rate = get_highest_rate_idx(tx_ra_bitmap)&0x3f;
+}
+
+void hw_var_port_switch(struct adapter *adapter)
+{
+}
+
+void SetHwReg(struct adapter *adapter, u8 variable, u8 *val)
+{
+	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &(hal_data->odmpriv);
+
+	switch (variable) {
+	case HW_VAR_PORT_SWITCH:
+		hw_var_port_switch(adapter);
+		break;
+	case HW_VAR_INIT_RTS_RATE:
+		rtw_warn_on(1);
+		break;
+	case HW_VAR_SEC_CFG:
+	{
+		u16 reg_scr;
+
+		reg_scr = rtw_read16(adapter, REG_SECCFG);
+		rtw_write16(adapter, REG_SECCFG, reg_scr|SCR_CHK_KEYID|SCR_RxDecEnable|SCR_TxEncEnable);
+	}
+		break;
+	case HW_VAR_SEC_DK_CFG:
+	{
+		struct security_priv *sec = &adapter->securitypriv;
+		u8 reg_scr = rtw_read8(adapter, REG_SECCFG);
+
+		if (val) { /* Enable default key related setting */
+			reg_scr |= SCR_TXBCUSEDK;
+			if (sec->dot11AuthAlgrthm != dot11AuthAlgrthm_8021X)
+				reg_scr |= (SCR_RxUseDK|SCR_TxUseDK);
+		} else /* Disable default key related setting */
+			reg_scr &= ~(SCR_RXBCUSEDK|SCR_TXBCUSEDK|SCR_RxUseDK|SCR_TxUseDK);
+
+		rtw_write8(adapter, REG_SECCFG, reg_scr);
+	}
+		break;
+	case HW_VAR_DM_FLAG:
+		odm->SupportAbility = *((u32 *)val);
+		break;
+	case HW_VAR_DM_FUNC_OP:
+		if (*((u8 *)val) == true) {
+			/* save dm flag */
+			odm->BK_SupportAbility = odm->SupportAbility;
+		} else {
+			/* restore dm flag */
+			odm->SupportAbility = odm->BK_SupportAbility;
+		}
+		break;
+	case HW_VAR_DM_FUNC_SET:
+		if (*((u32 *)val) == DYNAMIC_ALL_FUNC_ENABLE) {
+			struct dm_priv *dm = &hal_data->dmpriv;
+			dm->DMFlag = dm->InitDMFlag;
+			odm->SupportAbility = dm->InitODMFlag;
+		} else {
+			odm->SupportAbility |= *((u32 *)val);
+		}
+		break;
+	case HW_VAR_DM_FUNC_CLR:
+		/*
+		* input is already a mask to clear function
+		* don't invert it again! George, Lucas@20130513
+		*/
+		odm->SupportAbility &= *((u32 *)val);
+		break;
+	case HW_VAR_AMPDU_MIN_SPACE:
+		/* TODO - Is something needed here? */
+		break;
+	case HW_VAR_WIRELESS_MODE:
+		/* TODO - Is something needed here? */
+		break;
+	default:
+		DBG_871X_LEVEL(
+			_drv_always_,
+			FUNC_ADPT_FMT" variable(%d) not defined!\n",
+			FUNC_ADPT_ARG(adapter),
+			variable
+		);
+		break;
+	}
+}
+
+void GetHwReg(struct adapter *adapter, u8 variable, u8 *val)
+{
+	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &(hal_data->odmpriv);
+
+	switch (variable) {
+	case HW_VAR_BASIC_RATE:
+		*((u16 *)val) = hal_data->BasicRateSet;
+		break;
+	case HW_VAR_DM_FLAG:
+		*((u32 *)val) = odm->SupportAbility;
+		break;
+	case HW_VAR_RF_TYPE:
+		*((u8 *)val) = hal_data->rf_type;
+		break;
+	default:
+		DBG_871X_LEVEL(
+			_drv_always_,
+			FUNC_ADPT_FMT" variable(%d) not defined!\n",
+			FUNC_ADPT_ARG(adapter),
+			variable
+		);
+		break;
+	}
+}
+
+
+
+
+u8 SetHalDefVar(
+	struct adapter *adapter, enum HAL_DEF_VARIABLE variable, void *value
+)
+{
+	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &(hal_data->odmpriv);
+	u8 bResult = _SUCCESS;
+
+	switch (variable) {
+	case HW_DEF_FA_CNT_DUMP:
+		/* ODM_COMP_COMMON */
+		if (*((u8 *)value))
+			odm->DebugComponents |= (ODM_COMP_DIG | ODM_COMP_FA_CNT);
+		else
+			odm->DebugComponents &= ~(ODM_COMP_DIG | ODM_COMP_FA_CNT);
+		break;
+	case HAL_DEF_DBG_RX_INFO_DUMP:
+		DBG_871X("============ Rx Info dump ===================\n");
+		DBG_871X("bLinked = %d, RSSI_Min = %d(%%)\n",
+			odm->bLinked, odm->RSSI_Min);
+
+		if (odm->bLinked) {
+			DBG_871X("RxRate = %s, RSSI_A = %d(%%), RSSI_B = %d(%%)\n",
+				HDATA_RATE(odm->RxRate), odm->RSSI_A, odm->RSSI_B);
+
+			#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+			rtw_dump_raw_rssi_info(adapter);
+			#endif
+		}
+		break;
+	case HW_DEF_ODM_DBG_FLAG:
+		ODM_CmnInfoUpdate(odm, ODM_CMNINFO_DBG_COMP, *((u64 *)value));
+		break;
+	case HW_DEF_ODM_DBG_LEVEL:
+		ODM_CmnInfoUpdate(odm, ODM_CMNINFO_DBG_LEVEL, *((u32 *)value));
+		break;
+	case HAL_DEF_DBG_DM_FUNC:
+	{
+		u8 dm_func = *((u8 *)value);
+		struct dm_priv *dm = &hal_data->dmpriv;
+
+		if (dm_func == 0) { /* disable all dynamic func */
+			odm->SupportAbility = DYNAMIC_FUNC_DISABLE;
+			DBG_8192C("==> Disable all dynamic function...\n");
+		} else if (dm_func == 1) {/* disable DIG */
+			odm->SupportAbility  &= (~DYNAMIC_BB_DIG);
+			DBG_8192C("==> Disable DIG...\n");
+		} else if (dm_func == 2) {/* disable High power */
+			odm->SupportAbility  &= (~DYNAMIC_BB_DYNAMIC_TXPWR);
+		} else if (dm_func == 3) {/* disable tx power tracking */
+			odm->SupportAbility  &= (~DYNAMIC_RF_CALIBRATION);
+			DBG_8192C("==> Disable tx power tracking...\n");
+		} else if (dm_func == 4) {/* disable BT coexistence */
+			dm->DMFlag &= (~DYNAMIC_FUNC_BT);
+		} else if (dm_func == 5) {/* disable antenna diversity */
+			odm->SupportAbility  &= (~DYNAMIC_BB_ANT_DIV);
+		} else if (dm_func == 6) {/* turn on all dynamic func */
+			if (!(odm->SupportAbility  & DYNAMIC_BB_DIG)) {
+				DIG_T	*pDigTable = &odm->DM_DigTable;
+				pDigTable->CurIGValue = rtw_read8(adapter, 0xc50);
+			}
+			dm->DMFlag |= DYNAMIC_FUNC_BT;
+			odm->SupportAbility = DYNAMIC_ALL_FUNC_ENABLE;
+			DBG_8192C("==> Turn on all dynamic function...\n");
+		}
+	}
+		break;
+	case HAL_DEF_DBG_DUMP_RXPKT:
+		hal_data->bDumpRxPkt = *((u8 *)value);
+		break;
+	case HAL_DEF_DBG_DUMP_TXPKT:
+		hal_data->bDumpTxPkt = *((u8 *)value);
+		break;
+	case HAL_DEF_ANT_DETECT:
+		hal_data->AntDetection = *((u8 *)value);
+		break;
+	default:
+		DBG_871X_LEVEL(_drv_always_, "%s: [WARNING] HAL_DEF_VARIABLE(%d) not defined!\n", __func__, variable);
+		bResult = _FAIL;
+		break;
+	}
+
+	return bResult;
+}
+
+u8 GetHalDefVar(
+	struct adapter *adapter, enum HAL_DEF_VARIABLE variable, void *value
+)
+{
+	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &(hal_data->odmpriv);
+	u8 bResult = _SUCCESS;
+
+	switch (variable) {
+	case HAL_DEF_UNDERCORATEDSMOOTHEDPWDB:
+		{
+			struct mlme_priv *pmlmepriv;
+			struct sta_priv *pstapriv;
+			struct sta_info *psta;
+
+			pmlmepriv = &adapter->mlmepriv;
+			pstapriv = &adapter->stapriv;
+			psta = rtw_get_stainfo(pstapriv, pmlmepriv->cur_network.network.MacAddress);
+			if (psta)
+				*((int *)value) = psta->rssi_stat.UndecoratedSmoothedPWDB;
+		}
+		break;
+	case HW_DEF_ODM_DBG_FLAG:
+		*((u64 *)value) = odm->DebugComponents;
+		break;
+	case HW_DEF_ODM_DBG_LEVEL:
+		*((u32 *)value) = odm->DebugLevel;
+		break;
+	case HAL_DEF_DBG_DM_FUNC:
+		*((u32 *)value) = hal_data->odmpriv.SupportAbility;
+		break;
+	case HAL_DEF_DBG_DUMP_RXPKT:
+		*((u8 *)value) = hal_data->bDumpRxPkt;
+		break;
+	case HAL_DEF_DBG_DUMP_TXPKT:
+		*((u8 *)value) = hal_data->bDumpTxPkt;
+		break;
+	case HAL_DEF_ANT_DETECT:
+		*((u8 *)value) = hal_data->AntDetection;
+		break;
+	case HAL_DEF_MACID_SLEEP:
+		*(u8 *)value = false;
+		break;
+	case HAL_DEF_TX_PAGE_SIZE:
+		*((u32 *)value) = PAGE_SIZE_128;
+		break;
+	default:
+		DBG_871X_LEVEL(_drv_always_, "%s: [WARNING] HAL_DEF_VARIABLE(%d) not defined!\n", __func__, variable);
+		bResult = _FAIL;
+		break;
+	}
+
+	return bResult;
+}
+
+void GetHalODMVar(
+	struct adapter *Adapter,
+	enum HAL_ODM_VARIABLE eVariable,
+	void *pValue1,
+	void *pValue2
+)
+{
+	switch (eVariable) {
+#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+	case HAL_ODM_NOISE_MONITOR:
+		{
+			struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+			u8 chan = *(u8 *)pValue1;
+			*(s16 *)pValue2 = pHalData->noise[chan];
+			#ifdef DBG_NOISE_MONITOR
+			DBG_8192C("### Noise monitor chan(%d)-noise:%d (dBm) ###\n",
+				chan, pHalData->noise[chan]);
+			#endif
+
+		}
+		break;
+#endif/* ifdef CONFIG_BACKGROUND_NOISE_MONITOR */
+	default:
+		break;
+	}
+}
+
+void SetHalODMVar(
+	struct adapter *Adapter,
+	enum HAL_ODM_VARIABLE eVariable,
+	void *pValue1,
+	bool bSet
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	PDM_ODM_T podmpriv = &pHalData->odmpriv;
+	/* _irqL irqL; */
+	switch (eVariable) {
+	case HAL_ODM_STA_INFO:
+		{
+			struct sta_info *psta = (struct sta_info *)pValue1;
+			if (bSet) {
+				DBG_8192C("### Set STA_(%d) info ###\n", psta->mac_id);
+				ODM_CmnInfoPtrArrayHook(podmpriv, ODM_CMNINFO_STA_STATUS, psta->mac_id, psta);
+			} else {
+				DBG_8192C("### Clean STA_(%d) info ###\n", psta->mac_id);
+				/* spin_lock_bh(&pHalData->odm_stainfo_lock); */
+				ODM_CmnInfoPtrArrayHook(podmpriv, ODM_CMNINFO_STA_STATUS, psta->mac_id, NULL);
+
+				/* spin_unlock_bh(&pHalData->odm_stainfo_lock); */
+		    }
+		}
+		break;
+	case HAL_ODM_P2P_STATE:
+			ODM_CmnInfoUpdate(podmpriv, ODM_CMNINFO_WIFI_DIRECT, bSet);
+		break;
+	case HAL_ODM_WIFI_DISPLAY_STATE:
+			ODM_CmnInfoUpdate(podmpriv, ODM_CMNINFO_WIFI_DISPLAY, bSet);
+		break;
+	#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+	case HAL_ODM_NOISE_MONITOR:
+		{
+			struct noise_info *pinfo = (struct noise_info *)pValue1;
+
+			#ifdef DBG_NOISE_MONITOR
+			DBG_8192C("### Noise monitor chan(%d)-bPauseDIG:%d, IGIValue:0x%02x, max_time:%d (ms) ###\n",
+				pinfo->chan, pinfo->bPauseDIG, pinfo->IGIValue, pinfo->max_time);
+			#endif
+
+			pHalData->noise[pinfo->chan] = ODM_InbandNoise_Monitor(podmpriv, pinfo->bPauseDIG, pinfo->IGIValue, pinfo->max_time);
+			DBG_871X("chan_%d, noise = %d (dBm)\n", pinfo->chan, pHalData->noise[pinfo->chan]);
+			#ifdef DBG_NOISE_MONITOR
+			DBG_871X("noise_a = %d, noise_b = %d  noise_all:%d\n",
+				podmpriv->noise_level.noise[ODM_RF_PATH_A],
+				podmpriv->noise_level.noise[ODM_RF_PATH_B],
+				podmpriv->noise_level.noise_all);
+			#endif
+		}
+		break;
+	#endif/* ifdef CONFIG_BACKGROUND_NOISE_MONITOR */
+
+	default:
+		break;
+	}
+}
+
+
+bool eqNByte(u8 *str1, u8 *str2, u32 num)
+{
+	if (num == 0)
+		return false;
+	while (num > 0) {
+		num--;
+		if (str1[num] != str2[num])
+			return false;
+	}
+	return true;
+}
+
+/*  */
+/* 	Description: */
+/* 		Return true if chTmp is represent for hex digit and */
+/* 		false otherwise. */
+/*  */
+/*  */
+bool IsHexDigit(char chTmp)
+{
+	if (
+		(chTmp >= '0' && chTmp <= '9') ||
+		(chTmp >= 'a' && chTmp <= 'f') ||
+		(chTmp >= 'A' && chTmp <= 'F')
+	)
+		return true;
+	else
+		return false;
+}
+
+
+/*  */
+/* 	Description: */
+/* 		Translate a character to hex digit. */
+/*  */
+u32 MapCharToHexDigit(char chTmp)
+{
+	if (chTmp >= '0' && chTmp <= '9')
+		return (chTmp - '0');
+	else if (chTmp >= 'a' && chTmp <= 'f')
+		return (10 + (chTmp - 'a'));
+	else if (chTmp >= 'A' && chTmp <= 'F')
+		return (10 + (chTmp - 'A'));
+	else
+		return 0;
+}
+
+
+
+/* 	Description: */
+/* 		Parse hex number from the string pucStr. */
+bool GetHexValueFromString(char *szStr, u32 *pu4bVal, u32 *pu4bMove)
+{
+	char *szScan = szStr;
+
+	/*  Check input parameter. */
+	if (szStr == NULL || pu4bVal == NULL || pu4bMove == NULL) {
+		DBG_871X("GetHexValueFromString(): Invalid inpur argumetns! szStr: %p, pu4bVal: %p, pu4bMove: %p\n", szStr, pu4bVal, pu4bMove);
+		return false;
+	}
+
+	/*  Initialize output. */
+	*pu4bMove = 0;
+	*pu4bVal = 0;
+
+	/*  Skip leading space. */
+	while (*szScan != '\0' && (*szScan == ' ' || *szScan == '\t')) {
+		szScan++;
+		(*pu4bMove)++;
+	}
+
+	/*  Skip leading '0x' or '0X'. */
+	if (*szScan == '0' && (*(szScan+1) == 'x' || *(szScan+1) == 'X')) {
+		szScan += 2;
+		(*pu4bMove) += 2;
+	}
+
+	/*  Check if szScan is now pointer to a character for hex digit, */
+	/*  if not, it means this is not a valid hex number. */
+	if (!IsHexDigit(*szScan))
+		return false;
+
+	/*  Parse each digit. */
+	do {
+		(*pu4bVal) <<= 4;
+		*pu4bVal += MapCharToHexDigit(*szScan);
+
+		szScan++;
+		(*pu4bMove)++;
+	} while (IsHexDigit(*szScan));
+
+	return true;
+}
+
+bool GetFractionValueFromString(
+	char *szStr, u8 *pInteger, u8 *pFraction, u32 *pu4bMove
+)
+{
+	char *szScan = szStr;
+
+	/*  Initialize output. */
+	*pu4bMove = 0;
+	*pInteger = 0;
+	*pFraction = 0;
+
+	/*  Skip leading space. */
+	while (*szScan != '\0' &&	(*szScan == ' ' || *szScan == '\t')) {
+		++szScan;
+		++(*pu4bMove);
+	}
+
+	/*  Parse each digit. */
+	do {
+		(*pInteger) *= 10;
+		*pInteger += (*szScan - '0');
+
+		++szScan;
+		++(*pu4bMove);
+
+		if (*szScan == '.') {
+			++szScan;
+			++(*pu4bMove);
+
+			if (*szScan < '0' || *szScan > '9')
+				return false;
+			else {
+				*pFraction = *szScan - '0';
+				++szScan;
+				++(*pu4bMove);
+				return true;
+			}
+		}
+	} while (*szScan >= '0' && *szScan <= '9');
+
+	return true;
+}
+
+/*  */
+/* 	Description: */
+/* 		Return true if szStr is comment out with leading "//". */
+/*  */
+bool IsCommentString(char *szStr)
+{
+	if (*szStr == '/' && *(szStr+1) == '/')
+		return true;
+	else
+		return false;
+}
+
+bool GetU1ByteIntegerFromStringInDecimal(char *Str, u8 *pInt)
+{
+	u16 i = 0;
+	*pInt = 0;
+
+	while (Str[i] != '\0') {
+		if (Str[i] >= '0' && Str[i] <= '9') {
+			*pInt *= 10;
+			*pInt += (Str[i] - '0');
+		} else
+			return false;
+
+		++i;
+	}
+
+	return true;
+}
+
+/*  <20121004, Kordan> For example,
+ *  ParseQualifiedString(inString, 0, outString, '[', ']') gets "Kordan" from
+ *  a string "Hello [Kordan]".
+ *  If RightQualifier does not exist, it will hang in the while loop
+ */
+bool ParseQualifiedString(
+	char *In, u32 *Start, char *Out, char LeftQualifier, char RightQualifier
+)
+{
+	u32 i = 0, j = 0;
+	char c = In[(*Start)++];
+
+	if (c != LeftQualifier)
+		return false;
+
+	i = (*Start);
+	while ((c = In[(*Start)++]) != RightQualifier)
+		; /*  find ']' */
+	j = (*Start) - 2;
+	strncpy((char *)Out, (const char *)(In+i), j-i+1);
+
+	return true;
+}
+
+bool isAllSpaceOrTab(u8 *data, u8 size)
+{
+	u8 cnt = 0, NumOfSpaceAndTab = 0;
+
+	while (size > cnt) {
+		if (data[cnt] == ' ' || data[cnt] == '\t' || data[cnt] == '\0')
+			++NumOfSpaceAndTab;
+
+		++cnt;
+	}
+
+	return size == NumOfSpaceAndTab;
+}
+
+
+void rtw_hal_check_rxfifo_full(struct adapter *adapter)
+{
+	struct dvobj_priv *psdpriv = adapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	int save_cnt = false;
+
+	/* switch counter to RX fifo */
+	/* printk("8723b or 8192e , MAC_667 set 0xf0\n"); */
+	rtw_write8(adapter, REG_RXERR_RPT+3, rtw_read8(adapter, REG_RXERR_RPT+3)|0xf0);
+	save_cnt = true;
+	/* todo: other chips */
+
+	if (save_cnt) {
+		/* rtw_write8(adapter, REG_RXERR_RPT+3, rtw_read8(adapter, REG_RXERR_RPT+3)|0xa0); */
+		pdbgpriv->dbg_rx_fifo_last_overflow = pdbgpriv->dbg_rx_fifo_curr_overflow;
+		pdbgpriv->dbg_rx_fifo_curr_overflow = rtw_read16(adapter, REG_RXERR_RPT);
+		pdbgpriv->dbg_rx_fifo_diff_overflow = pdbgpriv->dbg_rx_fifo_curr_overflow-pdbgpriv->dbg_rx_fifo_last_overflow;
+	}
+}
+
+void linked_info_dump(struct adapter *padapter, u8 benable)
+{
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	if (padapter->bLinkInfoDump == benable)
+		return;
+
+	DBG_871X("%s %s\n", __func__, (benable) ? "enable" : "disable");
+
+	if (benable) {
+		pwrctrlpriv->org_power_mgnt = pwrctrlpriv->power_mgnt;/* keep org value */
+		rtw_pm_set_lps(padapter, PS_MODE_ACTIVE);
+
+		pwrctrlpriv->ips_org_mode = pwrctrlpriv->ips_mode;/* keep org value */
+		rtw_pm_set_ips(padapter, IPS_NONE);
+	} else {
+		rtw_pm_set_ips(padapter, pwrctrlpriv->ips_org_mode);
+
+		rtw_pm_set_lps(padapter, pwrctrlpriv->ips_org_mode);
+	}
+	padapter->bLinkInfoDump = benable;
+}
+
+#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+void rtw_get_raw_rssi_info(void *sel, struct adapter *padapter)
+{
+	u8 isCCKrate, rf_path;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct rx_raw_rssi *psample_pkt_rssi = &padapter->recvpriv.raw_rssi_info;
+
+	DBG_871X_SEL_NL(
+		sel,
+		"RxRate = %s, PWDBALL = %d(%%), rx_pwr_all = %d(dBm)\n",
+		HDATA_RATE(psample_pkt_rssi->data_rate),
+		psample_pkt_rssi->pwdball, psample_pkt_rssi->pwr_all
+	);
+
+	isCCKrate = (psample_pkt_rssi->data_rate <= DESC_RATE11M) ? true : false;
+
+	if (isCCKrate)
+		psample_pkt_rssi->mimo_singal_strength[0] = psample_pkt_rssi->pwdball;
+
+	for (rf_path = 0; rf_path < pHalData->NumTotalRFPath; rf_path++) {
+		DBG_871X_SEL_NL(
+			sel,
+			"RF_PATH_%d =>singal_strength:%d(%%), singal_quality:%d(%%)\n",
+			rf_path, psample_pkt_rssi->mimo_singal_strength[rf_path],
+			psample_pkt_rssi->mimo_singal_quality[rf_path]
+		);
+
+		if (!isCCKrate) {
+			DBG_871X_SEL_NL(
+				sel,
+				"\trx_ofdm_pwr:%d(dBm), rx_ofdm_snr:%d(dB)\n",
+				psample_pkt_rssi->ofdm_pwr[rf_path],
+				psample_pkt_rssi->ofdm_snr[rf_path]
+			);
+		}
+	}
+}
+
+void rtw_dump_raw_rssi_info(struct adapter *padapter)
+{
+	u8 isCCKrate, rf_path;
+	struct hal_com_data *pHalData =  GET_HAL_DATA(padapter);
+	struct rx_raw_rssi *psample_pkt_rssi = &padapter->recvpriv.raw_rssi_info;
+	DBG_871X("============ RAW Rx Info dump ===================\n");
+	DBG_871X("RxRate = %s, PWDBALL = %d(%%), rx_pwr_all = %d(dBm)\n",
+			HDATA_RATE(psample_pkt_rssi->data_rate), psample_pkt_rssi->pwdball, psample_pkt_rssi->pwr_all);
+
+	isCCKrate = (psample_pkt_rssi->data_rate <= DESC_RATE11M) ? true : false;
+
+	if (isCCKrate)
+		psample_pkt_rssi->mimo_singal_strength[0] = psample_pkt_rssi->pwdball;
+
+	for (rf_path = 0; rf_path < pHalData->NumTotalRFPath; rf_path++) {
+		DBG_871X("RF_PATH_%d =>singal_strength:%d(%%), singal_quality:%d(%%)"
+			, rf_path, psample_pkt_rssi->mimo_singal_strength[rf_path], psample_pkt_rssi->mimo_singal_quality[rf_path]);
+
+		if (!isCCKrate) {
+			printk(", rx_ofdm_pwr:%d(dBm), rx_ofdm_snr:%d(dB)\n",
+			psample_pkt_rssi->ofdm_pwr[rf_path], psample_pkt_rssi->ofdm_snr[rf_path]);
+		} else {
+			printk("\n");
+		}
+	}
+}
+
+void rtw_store_phy_info(struct adapter *padapter, union recv_frame *prframe)
+{
+	u8 isCCKrate, rf_path;
+	struct hal_com_data *pHalData =  GET_HAL_DATA(padapter);
+	struct rx_pkt_attrib *pattrib = &prframe->u.hdr.attrib;
+
+	PODM_PHY_INFO_T pPhyInfo  = (PODM_PHY_INFO_T)(&pattrib->phy_info);
+	struct rx_raw_rssi *psample_pkt_rssi = &padapter->recvpriv.raw_rssi_info;
+
+	psample_pkt_rssi->data_rate = pattrib->data_rate;
+	isCCKrate = (pattrib->data_rate <= DESC_RATE11M) ? true : false;
+
+	psample_pkt_rssi->pwdball = pPhyInfo->RxPWDBAll;
+	psample_pkt_rssi->pwr_all = pPhyInfo->RecvSignalPower;
+
+	for (rf_path = 0; rf_path < pHalData->NumTotalRFPath; rf_path++) {
+		psample_pkt_rssi->mimo_singal_strength[rf_path] = pPhyInfo->RxMIMOSignalStrength[rf_path];
+		psample_pkt_rssi->mimo_singal_quality[rf_path] = pPhyInfo->RxMIMOSignalQuality[rf_path];
+		if (!isCCKrate) {
+			psample_pkt_rssi->ofdm_pwr[rf_path] = pPhyInfo->RxPwr[rf_path];
+			psample_pkt_rssi->ofdm_snr[rf_path] = pPhyInfo->RxSNR[rf_path];
+		}
+	}
+}
+#endif
+
+static u32 Array_kfreemap[] = {
+	0xf8, 0xe,
+	0xf6, 0xc,
+	0xf4, 0xa,
+	0xf2, 0x8,
+	0xf0, 0x6,
+	0xf3, 0x4,
+	0xf5, 0x2,
+	0xf7, 0x0,
+	0xf9, 0x0,
+	0xfc, 0x0,
+};
+
+void rtw_bb_rf_gain_offset(struct adapter *padapter)
+{
+	u8 value = padapter->eeprompriv.EEPROMRFGainOffset;
+	u32 res, i = 0;
+	u32 ArrayLen = sizeof(Array_kfreemap)/sizeof(u32);
+	u32 *Array = Array_kfreemap;
+	u32 v1 = 0, v2 = 0, target = 0;
+	/* DBG_871X("+%s value: 0x%02x+\n", __func__, value); */
+
+	if (value & BIT4) {
+		DBG_871X("Offset RF Gain.\n");
+		DBG_871X("Offset RF Gain.  padapter->eeprompriv.EEPROMRFGainVal = 0x%x\n", padapter->eeprompriv.EEPROMRFGainVal);
+		if (padapter->eeprompriv.EEPROMRFGainVal != 0xff) {
+			res = rtw_hal_read_rfreg(padapter, RF_PATH_A, 0x7f, 0xffffffff);
+			res &= 0xfff87fff;
+			DBG_871X("Offset RF Gain. before reg 0x7f = 0x%08x\n", res);
+			/* res &= 0xfff87fff; */
+			for (i = 0; i < ArrayLen; i += 2) {
+				v1 = Array[i];
+				v2 = Array[i+1];
+				 if (v1 == padapter->eeprompriv.EEPROMRFGainVal) {
+						DBG_871X("Offset RF Gain. got v1 = 0x%x , v2 = 0x%x\n", v1, v2);
+						target = v2;
+						break;
+				 }
+			}
+			DBG_871X("padapter->eeprompriv.EEPROMRFGainVal = 0x%x , Gain offset Target Value = 0x%x\n", padapter->eeprompriv.EEPROMRFGainVal, target);
+			PHY_SetRFReg(padapter, RF_PATH_A, REG_RF_BB_GAIN_OFFSET, BIT18|BIT17|BIT16|BIT15, target);
+
+			/* res |= (padapter->eeprompriv.EEPROMRFGainVal & 0x0f)<< 15; */
+			/* rtw_hal_write_rfreg(padapter, RF_PATH_A, REG_RF_BB_GAIN_OFFSET, RF_GAIN_OFFSET_MASK, res); */
+			res = rtw_hal_read_rfreg(padapter, RF_PATH_A, 0x7f, 0xffffffff);
+			DBG_871X("Offset RF Gain. After reg 0x7f = 0x%08x\n", res);
+		} else
+			DBG_871X("Offset RF Gain.  padapter->eeprompriv.EEPROMRFGainVal = 0x%x	!= 0xff, didn't run Kfree\n", padapter->eeprompriv.EEPROMRFGainVal);
+	} else
+		DBG_871X("Using the default RF gain.\n");
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/hal_com_phycfg.c linux/3rdparty/rtl8723bs/hal/hal_com_phycfg.c
--- linux.old/3rdparty/rtl8723bs/hal/hal_com_phycfg.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/hal_com_phycfg.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,3288 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#define _HAL_COM_PHYCFG_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+
+u8 PHY_GetTxPowerByRateBase(struct adapter *Adapter, u8 Band, u8 RfPath,
+			    u8 TxNum, enum RATE_SECTION RateSection)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	u8	value = 0;
+
+	if (RfPath > ODM_RF_PATH_D) {
+		DBG_871X("Invalid Rf Path %d in PHY_GetTxPowerByRateBase()\n", RfPath);
+		return 0;
+	}
+
+	if (Band == BAND_ON_2_4G) {
+		switch (RateSection) {
+		case CCK:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][0];
+			break;
+		case OFDM:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][1];
+			break;
+		case HT_MCS0_MCS7:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][2];
+			break;
+		case HT_MCS8_MCS15:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][3];
+			break;
+		case HT_MCS16_MCS23:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][4];
+			break;
+		case HT_MCS24_MCS31:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][5];
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][6];
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][7];
+			break;
+		case VHT_3SSMCS0_3SSMCS9:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][8];
+			break;
+		case VHT_4SSMCS0_4SSMCS9:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][9];
+			break;
+		default:
+			DBG_871X("Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in PHY_GetTxPowerByRateBase()\n",
+					 RateSection, RfPath, TxNum);
+			break;
+
+		};
+	} else if (Band == BAND_ON_5G) {
+		switch (RateSection) {
+		case OFDM:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][0];
+			break;
+		case HT_MCS0_MCS7:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][1];
+			break;
+		case HT_MCS8_MCS15:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][2];
+			break;
+		case HT_MCS16_MCS23:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][3];
+			break;
+		case HT_MCS24_MCS31:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][4];
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][5];
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][6];
+			break;
+		case VHT_3SSMCS0_3SSMCS9:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][7];
+			break;
+		case VHT_4SSMCS0_4SSMCS9:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][8];
+			break;
+		default:
+			DBG_871X("Invalid RateSection %d in Band 5G, Rf Path %d, %dTx in PHY_GetTxPowerByRateBase()\n",
+					 RateSection, RfPath, TxNum);
+			break;
+		};
+	} else
+		DBG_871X("Invalid Band %d in PHY_GetTxPowerByRateBase()\n", Band);
+
+	return value;
+}
+
+static void
+phy_SetTxPowerByRateBase(
+	struct adapter *Adapter,
+	u8 Band,
+	u8 RfPath,
+	enum RATE_SECTION	RateSection,
+	u8 TxNum,
+	u8 Value
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+
+	if (RfPath > ODM_RF_PATH_D) {
+		DBG_871X("Invalid Rf Path %d in phy_SetTxPowerByRatBase()\n", RfPath);
+		return;
+	}
+
+	if (Band == BAND_ON_2_4G) {
+		switch (RateSection) {
+		case CCK:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][0] = Value;
+			break;
+		case OFDM:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][1] = Value;
+			break;
+		case HT_MCS0_MCS7:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][2] = Value;
+			break;
+		case HT_MCS8_MCS15:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][3] = Value;
+			break;
+		case HT_MCS16_MCS23:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][4] = Value;
+			break;
+		case HT_MCS24_MCS31:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][5] = Value;
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][6] = Value;
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][7] = Value;
+			break;
+		case VHT_3SSMCS0_3SSMCS9:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][8] = Value;
+			break;
+		case VHT_4SSMCS0_4SSMCS9:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][9] = Value;
+			break;
+		default:
+			DBG_871X("Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in phy_SetTxPowerByRateBase()\n",
+					 RateSection, RfPath, TxNum);
+			break;
+		};
+	} else if (Band == BAND_ON_5G) {
+		switch (RateSection) {
+		case OFDM:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][0] = Value;
+			break;
+		case HT_MCS0_MCS7:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][1] = Value;
+			break;
+		case HT_MCS8_MCS15:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][2] = Value;
+			break;
+		case HT_MCS16_MCS23:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][3] = Value;
+			break;
+		case HT_MCS24_MCS31:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][4] = Value;
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][5] = Value;
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][6] = Value;
+			break;
+		case VHT_3SSMCS0_3SSMCS9:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][7] = Value;
+			break;
+		case VHT_4SSMCS0_4SSMCS9:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][8] = Value;
+			break;
+		default:
+			DBG_871X("Invalid RateSection %d in Band 5G, Rf Path %d, %dTx in phy_SetTxPowerByRateBase()\n",
+					 RateSection, RfPath, TxNum);
+			break;
+		};
+	} else
+		DBG_871X("Invalid Band %d in phy_SetTxPowerByRateBase()\n", Band);
+}
+
+static void
+phy_StoreTxPowerByRateBase(
+struct adapter *padapter
+	)
+{
+	u8 path, base;
+
+	/* DBG_871X("===>%s\n", __func__); */
+
+	for (path = ODM_RF_PATH_A; path <= ODM_RF_PATH_B; ++path) {
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_1TX, MGN_11M);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, CCK, RF_1TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 1Tx CCK = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_1TX, MGN_54M);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, OFDM, RF_1TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 1Tx OFDM = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_1TX, MGN_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, HT_MCS0_MCS7, RF_1TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 1Tx MCS0-7 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_2TX, MGN_MCS15);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, HT_MCS8_MCS15, RF_2TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 2Tx MCS8-15 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_3TX, MGN_MCS23);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, HT_MCS16_MCS23, RF_3TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 3Tx MCS16-23 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_1TX, MGN_VHT1SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, VHT_1SSMCS0_1SSMCS9, RF_1TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 1Tx VHT1SS = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_2TX, MGN_VHT2SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, VHT_2SSMCS0_2SSMCS9, RF_2TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 2Tx VHT2SS = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_3TX, MGN_VHT3SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, VHT_3SSMCS0_3SSMCS9, RF_3TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 3Tx VHT3SS = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_1TX, MGN_54M);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, OFDM, RF_1TX, base);
+		/* DBG_871X("Power index base of 5G path %d 1Tx OFDM = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_1TX, MGN_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, HT_MCS0_MCS7, RF_1TX, base);
+		/* DBG_871X("Power index base of 5G path %d 1Tx MCS0~7 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_2TX, MGN_MCS15);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, HT_MCS8_MCS15, RF_2TX, base);
+		/* DBG_871X("Power index base of 5G path %d 2Tx MCS8~15 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_3TX, MGN_MCS23);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, HT_MCS16_MCS23, RF_3TX, base);
+		/* DBG_871X("Power index base of 5G path %d 3Tx MCS16~23 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_1TX, MGN_VHT1SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, VHT_1SSMCS0_1SSMCS9, RF_1TX, base);
+		/* DBG_871X("Power index base of 5G path %d 1Tx VHT1SS = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_2TX, MGN_VHT2SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, VHT_2SSMCS0_2SSMCS9, RF_2TX, base);
+		/* DBG_871X("Power index base of 5G path %d 2Tx VHT2SS = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_3TX, MGN_VHT2SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, VHT_3SSMCS0_3SSMCS9, RF_3TX, base);
+		/* DBG_871X("Power index base of 5G path %d 3Tx VHT3SS = > 0x%x\n", path, base); */
+	}
+
+	/* DBG_871X("<===%s\n", __func__); */
+}
+
+u8 PHY_GetRateSectionIndexOfTxPowerByRate(
+	struct adapter *padapter, u32 RegAddr, u32 BitMask
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+	u8	index = 0;
+
+	if (pDM_Odm->PhyRegPgVersion == 0) {
+		switch (RegAddr) {
+		case rTxAGC_A_Rate18_06:
+			index = 0;
+			break;
+		case rTxAGC_A_Rate54_24:
+			index = 1;
+			break;
+		case rTxAGC_A_CCK1_Mcs32:
+			index = 6;
+			break;
+		case rTxAGC_B_CCK11_A_CCK2_11:
+			if (BitMask == bMaskH3Bytes)
+				index = 7;
+			else if (BitMask == 0x000000ff)
+				index = 15;
+			break;
+
+		case rTxAGC_A_Mcs03_Mcs00:
+			index = 2;
+			break;
+		case rTxAGC_A_Mcs07_Mcs04:
+			index = 3;
+			break;
+		case rTxAGC_A_Mcs11_Mcs08:
+			index = 4;
+			break;
+		case rTxAGC_A_Mcs15_Mcs12:
+			index = 5;
+			break;
+		case rTxAGC_B_Rate18_06:
+			index = 8;
+			break;
+		case rTxAGC_B_Rate54_24:
+			index = 9;
+			break;
+		case rTxAGC_B_CCK1_55_Mcs32:
+			index = 14;
+			break;
+		case rTxAGC_B_Mcs03_Mcs00:
+			index = 10;
+			break;
+		case rTxAGC_B_Mcs07_Mcs04:
+			index = 11;
+			break;
+		case rTxAGC_B_Mcs11_Mcs08:
+			index = 12;
+			break;
+		case rTxAGC_B_Mcs15_Mcs12:
+			index = 13;
+			break;
+		default:
+			DBG_871X("Invalid RegAddr 0x3%x in PHY_GetRateSectionIndexOfTxPowerByRate()", RegAddr);
+			break;
+		};
+	}
+
+	return index;
+}
+
+void
+PHY_GetRateValuesOfTxPowerByRate(
+	struct adapter *padapter,
+	u32	RegAddr,
+	u32	BitMask,
+	u32	Value,
+	u8 *RateIndex,
+	s8 *PwrByRateVal,
+	u8 *RateNum
+)
+{
+	u8 i = 0;
+
+	switch (RegAddr) {
+	case rTxAGC_A_Rate18_06:
+	case rTxAGC_B_Rate18_06:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_6M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_9M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_12M);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_18M);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case rTxAGC_A_Rate54_24:
+	case rTxAGC_B_Rate54_24:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_24M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_36M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_48M);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_54M);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case rTxAGC_A_CCK1_Mcs32:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_1M);
+		PwrByRateVal[0] = (s8) ((((Value >> (8 + 4)) & 0xF)) * 10 +
+										((Value >> 8) & 0xF));
+		*RateNum = 1;
+		break;
+
+	case rTxAGC_B_CCK11_A_CCK2_11:
+		if (BitMask == 0xffffff00) {
+			RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_2M);
+			RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_5_5M);
+			RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_11M);
+			for (i = 1; i < 4; ++i) {
+				PwrByRateVal[i - 1] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+												((Value >> (i * 8)) & 0xF));
+			}
+			*RateNum = 3;
+		} else if (BitMask == 0x000000ff) {
+			RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_11M);
+			PwrByRateVal[0] = (s8) ((((Value >> 4) & 0xF)) * 10 + (Value & 0xF));
+			*RateNum = 1;
+		}
+		break;
+
+	case rTxAGC_A_Mcs03_Mcs00:
+	case rTxAGC_B_Mcs03_Mcs00:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS0);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS1);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS2);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS3);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case rTxAGC_A_Mcs07_Mcs04:
+	case rTxAGC_B_Mcs07_Mcs04:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS4);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS5);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS6);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS7);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case rTxAGC_A_Mcs11_Mcs08:
+	case rTxAGC_B_Mcs11_Mcs08:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS8);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS9);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS10);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS11);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case rTxAGC_A_Mcs15_Mcs12:
+	case rTxAGC_B_Mcs15_Mcs12:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS12);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS13);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS14);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS15);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+
+		break;
+
+	case rTxAGC_B_CCK1_55_Mcs32:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_1M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_2M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_5_5M);
+		for (i = 1; i < 4; ++i) {
+			PwrByRateVal[i - 1] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 3;
+		break;
+
+	case 0xC20:
+	case 0xE20:
+	case 0x1820:
+	case 0x1a20:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_1M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_2M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_5_5M);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_11M);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC24:
+	case 0xE24:
+	case 0x1824:
+	case 0x1a24:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_6M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_9M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_12M);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_18M);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC28:
+	case 0xE28:
+	case 0x1828:
+	case 0x1a28:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_24M);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_36M);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_48M);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_54M);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC2C:
+	case 0xE2C:
+	case 0x182C:
+	case 0x1a2C:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS0);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS1);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS2);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS3);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC30:
+	case 0xE30:
+	case 0x1830:
+	case 0x1a30:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS4);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS5);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS6);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS7);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC34:
+	case 0xE34:
+	case 0x1834:
+	case 0x1a34:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS8);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS9);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS10);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS11);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC38:
+	case 0xE38:
+	case 0x1838:
+	case 0x1a38:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS12);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS13);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS14);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS15);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC3C:
+	case 0xE3C:
+	case 0x183C:
+	case 0x1a3C:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS0);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS1);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS2);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS3);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC40:
+	case 0xE40:
+	case 0x1840:
+	case 0x1a40:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS4);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS5);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS6);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS7);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC44:
+	case 0xE44:
+	case 0x1844:
+	case 0x1a44:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS8);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS9);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS0);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS1);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC48:
+	case 0xE48:
+	case 0x1848:
+	case 0x1a48:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS2);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS3);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS4);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS5);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC4C:
+	case 0xE4C:
+	case 0x184C:
+	case 0x1a4C:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS6);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS7);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS8);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS9);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xCD8:
+	case 0xED8:
+	case 0x18D8:
+	case 0x1aD8:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS16);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS17);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS18);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS19);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xCDC:
+	case 0xEDC:
+	case 0x18DC:
+	case 0x1aDC:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS20);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS21);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS22);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS23);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xCE0:
+	case 0xEE0:
+	case 0x18E0:
+	case 0x1aE0:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS0);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS1);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS2);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS3);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xCE4:
+	case 0xEE4:
+	case 0x18E4:
+	case 0x1aE4:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS4);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS5);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS6);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS7);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xCE8:
+	case 0xEE8:
+	case 0x18E8:
+	case 0x1aE8:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS8);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS9);
+		for (i = 0; i < 2; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	default:
+		DBG_871X("Invalid RegAddr 0x%x in %s()\n", RegAddr, __func__);
+		break;
+	};
+}
+
+static void PHY_StoreTxPowerByRateNew(
+	struct adapter *padapter,
+	u32	Band,
+	u32	RfPath,
+	u32	TxNum,
+	u32	RegAddr,
+	u32	BitMask,
+	u32	Data
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 i = 0, rateIndex[4] = {0}, rateNum = 0;
+	s8	PwrByRateVal[4] = {0};
+
+	PHY_GetRateValuesOfTxPowerByRate(padapter, RegAddr, BitMask, Data, rateIndex, PwrByRateVal, &rateNum);
+
+	if (Band != BAND_ON_2_4G && Band != BAND_ON_5G) {
+		DBG_871X("Invalid Band %d\n", Band);
+		return;
+	}
+
+	if (RfPath > ODM_RF_PATH_D) {
+		DBG_871X("Invalid RfPath %d\n", RfPath);
+		return;
+	}
+
+	if (TxNum > ODM_RF_PATH_D) {
+		DBG_871X("Invalid TxNum %d\n", TxNum);
+		return;
+	}
+
+	for (i = 0; i < rateNum; ++i) {
+		if (rateIndex[i] == PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS0) ||
+			 rateIndex[i] == PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS1))
+			TxNum = RF_2TX;
+
+		pHalData->TxPwrByRateOffset[Band][RfPath][TxNum][rateIndex[i]] = PwrByRateVal[i];
+	}
+}
+
+static void PHY_StoreTxPowerByRateOld(
+	struct adapter *padapter, u32	RegAddr, u32 BitMask, u32 Data
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8	index = PHY_GetRateSectionIndexOfTxPowerByRate(padapter, RegAddr, BitMask);
+
+	pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][index] = Data;
+	/* DBG_871X("MCSTxPowerLevelOriginalOffset[%d][0] = 0x%x\n", pHalData->pwrGroupCnt, */
+	/*	pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][0]); */
+}
+
+void PHY_InitTxPowerByRate(struct adapter *padapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 band, rfPath, TxNum, rate;
+
+	for (band = BAND_ON_2_4G; band <= BAND_ON_5G; ++band)
+			for (rfPath = 0; rfPath < TX_PWR_BY_RATE_NUM_RF; ++rfPath)
+				for (TxNum = 0; TxNum < TX_PWR_BY_RATE_NUM_RF; ++TxNum)
+					for (rate = 0; rate < TX_PWR_BY_RATE_NUM_RATE; ++rate)
+						pHalData->TxPwrByRateOffset[band][rfPath][TxNum][rate] = 0;
+}
+
+void PHY_StoreTxPowerByRate(
+	struct adapter *padapter,
+	u32	Band,
+	u32	RfPath,
+	u32	TxNum,
+	u32	RegAddr,
+	u32	BitMask,
+	u32	Data
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T		pDM_Odm = &pHalData->odmpriv;
+
+	if (pDM_Odm->PhyRegPgVersion > 0)
+		PHY_StoreTxPowerByRateNew(padapter, Band, RfPath, TxNum, RegAddr, BitMask, Data);
+	else if (pDM_Odm->PhyRegPgVersion == 0) {
+		PHY_StoreTxPowerByRateOld(padapter, RegAddr, BitMask, Data);
+
+		if (RegAddr == rTxAGC_A_Mcs15_Mcs12 && pHalData->rf_type == RF_1T1R)
+			pHalData->pwrGroupCnt++;
+		else if (RegAddr == rTxAGC_B_Mcs15_Mcs12 && pHalData->rf_type != RF_1T1R)
+			pHalData->pwrGroupCnt++;
+	} else
+		DBG_871X("Invalid PHY_REG_PG.txt version %d\n",  pDM_Odm->PhyRegPgVersion);
+
+}
+
+static void
+phy_ConvertTxPowerByRateInDbmToRelativeValues(
+struct adapter *padapter
+	)
+{
+	u8	base = 0, i = 0, value = 0, band = 0, path = 0, txNum = 0;
+	u8	cckRates[4] = {
+		MGN_1M, MGN_2M, MGN_5_5M, MGN_11M
+	};
+	u8	ofdmRates[8] = {
+		MGN_6M, MGN_9M, MGN_12M, MGN_18M, MGN_24M, MGN_36M, MGN_48M, MGN_54M
+	};
+	u8 mcs0_7Rates[8] = {
+		MGN_MCS0, MGN_MCS1, MGN_MCS2, MGN_MCS3, MGN_MCS4, MGN_MCS5, MGN_MCS6, MGN_MCS7
+	};
+	u8 mcs8_15Rates[8] = {
+		MGN_MCS8, MGN_MCS9, MGN_MCS10, MGN_MCS11, MGN_MCS12, MGN_MCS13, MGN_MCS14, MGN_MCS15
+	};
+	u8 mcs16_23Rates[8] = {
+		MGN_MCS16, MGN_MCS17, MGN_MCS18, MGN_MCS19, MGN_MCS20, MGN_MCS21, MGN_MCS22, MGN_MCS23
+	};
+	u8 vht1ssRates[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 vht2ssRates[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 vht3ssRates[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
+	};
+
+	/* DBG_871X("===>PHY_ConvertTxPowerByRateInDbmToRelativeValues()\n"); */
+
+	for (band = BAND_ON_2_4G; band <= BAND_ON_5G; ++band) {
+		for (path = ODM_RF_PATH_A; path <= ODM_RF_PATH_D; ++path) {
+			for (txNum = RF_1TX; txNum < RF_MAX_TX_NUM; ++txNum) {
+				/*  CCK */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_11M);
+				for (i = 0; i < sizeof(cckRates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, cckRates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, cckRates[i], value - base);
+				}
+
+				/*  OFDM */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_54M);
+				for (i = 0; i < sizeof(ofdmRates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, ofdmRates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, ofdmRates[i], value - base);
+				}
+
+				/*  HT MCS0~7 */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_MCS7);
+				for (i = 0; i < sizeof(mcs0_7Rates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, mcs0_7Rates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, mcs0_7Rates[i], value - base);
+				}
+
+				/*  HT MCS8~15 */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_MCS15);
+				for (i = 0; i < sizeof(mcs8_15Rates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, mcs8_15Rates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, mcs8_15Rates[i], value - base);
+				}
+
+				/*  HT MCS16~23 */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_MCS23);
+				for (i = 0; i < sizeof(mcs16_23Rates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, mcs16_23Rates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, mcs16_23Rates[i], value - base);
+				}
+
+				/*  VHT 1SS */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_VHT1SS_MCS7);
+				for (i = 0; i < sizeof(vht1ssRates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, vht1ssRates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, vht1ssRates[i], value - base);
+				}
+
+				/*  VHT 2SS */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_VHT2SS_MCS7);
+				for (i = 0; i < sizeof(vht2ssRates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, vht2ssRates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, vht2ssRates[i], value - base);
+				}
+
+				/*  VHT 3SS */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_VHT3SS_MCS7);
+				for (i = 0; i < sizeof(vht3ssRates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, vht3ssRates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, vht3ssRates[i], value - base);
+				}
+			}
+		}
+	}
+
+	/* DBG_871X("<===PHY_ConvertTxPowerByRateInDbmToRelativeValues()\n"); */
+}
+
+/*
+  * This function must be called if the value in the PHY_REG_PG.txt(or header)
+  * is exact dBm values
+  */
+void PHY_TxPowerByRateConfiguration(struct adapter *padapter)
+{
+	phy_StoreTxPowerByRateBase(padapter);
+	phy_ConvertTxPowerByRateInDbmToRelativeValues(padapter);
+}
+
+void PHY_SetTxPowerIndexByRateSection(
+	struct adapter *padapter, u8 RFPath, u8 Channel, u8 RateSection
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	if (RateSection == CCK) {
+		u8 cckRates[]   = {MGN_1M, MGN_2M, MGN_5_5M, MGN_11M};
+		if (pHalData->CurrentBandType == BAND_ON_2_4G)
+			PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									  cckRates, sizeof(cckRates)/sizeof(u8));
+
+	} else if (RateSection == OFDM) {
+		u8 ofdmRates[]  = {MGN_6M, MGN_9M, MGN_12M, MGN_18M, MGN_24M, MGN_36M, MGN_48M, MGN_54M};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									 ofdmRates, sizeof(ofdmRates)/sizeof(u8));
+
+	} else if (RateSection == HT_MCS0_MCS7) {
+		u8 htRates1T[]  = {MGN_MCS0, MGN_MCS1, MGN_MCS2, MGN_MCS3, MGN_MCS4, MGN_MCS5, MGN_MCS6, MGN_MCS7};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									 htRates1T, sizeof(htRates1T)/sizeof(u8));
+
+	} else if (RateSection == HT_MCS8_MCS15) {
+		u8 htRates2T[]  = {MGN_MCS8, MGN_MCS9, MGN_MCS10, MGN_MCS11, MGN_MCS12, MGN_MCS13, MGN_MCS14, MGN_MCS15};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									 htRates2T, sizeof(htRates2T)/sizeof(u8));
+
+	} else if (RateSection == HT_MCS16_MCS23) {
+		u8 htRates3T[]  = {MGN_MCS16, MGN_MCS17, MGN_MCS18, MGN_MCS19, MGN_MCS20, MGN_MCS21, MGN_MCS22, MGN_MCS23};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									 htRates3T, sizeof(htRates3T)/sizeof(u8));
+
+	} else if (RateSection == HT_MCS24_MCS31) {
+		u8 htRates4T[]  = {MGN_MCS24, MGN_MCS25, MGN_MCS26, MGN_MCS27, MGN_MCS28, MGN_MCS29, MGN_MCS30, MGN_MCS31};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									 htRates4T, sizeof(htRates4T)/sizeof(u8));
+
+	} else if (RateSection == VHT_1SSMCS0_1SSMCS9) {
+		u8 vhtRates1T[] = {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};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									vhtRates1T, sizeof(vhtRates1T)/sizeof(u8));
+
+	} else if (RateSection == VHT_2SSMCS0_2SSMCS9) {
+		u8 vhtRates2T[] = {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};
+
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+								  vhtRates2T, sizeof(vhtRates2T)/sizeof(u8));
+	} else if (RateSection == VHT_3SSMCS0_3SSMCS9) {
+		u8 vhtRates3T[] = {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};
+
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+								  vhtRates3T, sizeof(vhtRates3T)/sizeof(u8));
+	} else if (RateSection == VHT_4SSMCS0_4SSMCS9) {
+		u8 vhtRates4T[] = {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};
+
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+								  vhtRates4T, sizeof(vhtRates4T)/sizeof(u8));
+	} else
+		DBG_871X("Invalid RateSection %d in %s", RateSection, __func__);
+}
+
+static bool phy_GetChnlIndex(u8 Channel, u8 *ChannelIdx)
+{
+	u8 channel5G[CHANNEL_MAX_NUMBER_5G] = {
+		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, 142, 144, 149, 151, 153, 155, 157, 159, 161,
+		163, 165, 167, 168, 169, 171, 173, 175, 177
+	};
+	u8  i = 0;
+	bool bIn24G = true;
+
+	if (Channel <= 14) {
+		bIn24G = true;
+		*ChannelIdx = Channel-1;
+	} else {
+		bIn24G = false;
+
+		for (i = 0; i < sizeof(channel5G)/sizeof(u8); ++i) {
+			if (channel5G[i] == Channel) {
+				*ChannelIdx = i;
+				return bIn24G;
+			}
+		}
+	}
+
+	return bIn24G;
+}
+
+u8 PHY_GetTxPowerIndexBase(
+	struct adapter *padapter,
+	u8 RFPath,
+	u8 Rate,
+	enum CHANNEL_WIDTH	BandWidth,
+	u8 Channel,
+	bool *bIn24G
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	u8 i = 0;	/* default set to 1S */
+	u8 txPower = 0;
+	u8 chnlIdx = (Channel-1);
+
+	if (HAL_IsLegalChannel(padapter, Channel) == false) {
+		chnlIdx = 0;
+		DBG_871X("Illegal channel!!\n");
+	}
+
+	*bIn24G = phy_GetChnlIndex(Channel, &chnlIdx);
+
+	/* DBG_871X("[%s] Channel Index: %d\n", (*bIn24G?"2.4G":"5G"), chnlIdx); */
+
+	if (*bIn24G) { /* 3 ============================== 2.4 G ============================== */
+		if (IS_CCK_RATE(Rate))
+			txPower = pHalData->Index24G_CCK_Base[RFPath][chnlIdx];
+		else if (MGN_6M <= Rate)
+			txPower = pHalData->Index24G_BW40_Base[RFPath][chnlIdx];
+		else
+			DBG_871X("PHY_GetTxPowerIndexBase: INVALID Rate.\n");
+
+		/* DBG_871X("Base Tx power(RF-%c, Rate #%d, Channel Index %d) = 0x%X\n", */
+		/*		((RFPath == 0)?'A':'B'), Rate, chnlIdx, txPower); */
+
+		/*  OFDM-1T */
+		if ((MGN_6M <= Rate && Rate <= MGN_54M) && !IS_CCK_RATE(Rate)) {
+			txPower += pHalData->OFDM_24G_Diff[RFPath][TX_1S];
+			/* DBG_871X("+PowerDiff 2.4G (RF-%c): (OFDM-1T) = (%d)\n", ((RFPath == 0)?'A':'B'), pHalData->OFDM_24G_Diff[RFPath][TX_1S]); */
+		}
+		if (BandWidth == CHANNEL_WIDTH_20) { /*  BW20-1S, BW20-2S */
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_24G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_24G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_24G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS24 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_24G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 2.4G (RF-%c): (BW20-1S, BW20-2S, BW20-3S, BW20-4S) = (%d, %d, %d, %d)\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW20_24G_Diff[RFPath][TX_1S], pHalData->BW20_24G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW20_24G_Diff[RFPath][TX_3S], pHalData->BW20_24G_Diff[RFPath][TX_4S]); */
+		} else if (BandWidth == CHANNEL_WIDTH_40) { /*  BW40-1S, BW40-2S */
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS24 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 2.4G (RF-%c): (BW40-1S, BW40-2S, BW40-3S, BW40-4S) = (%d, %d, %d, %d)\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW40_24G_Diff[RFPath][TX_1S], pHalData->BW40_24G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW40_24G_Diff[RFPath][TX_3S], pHalData->BW40_24G_Diff[RFPath][TX_4S]); */
+		}
+		/*  Willis suggest adopt BW 40M power index while in BW 80 mode */
+		else if (BandWidth == CHANNEL_WIDTH_80) {
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS24 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 2.4G (RF-%c): (BW40-1S, BW40-2S, BW40-3S, BW40-4T) = (%d, %d, %d, %d) P.S. Current is in BW 80MHz\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW40_24G_Diff[RFPath][TX_1S], pHalData->BW40_24G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW40_24G_Diff[RFPath][TX_3S], pHalData->BW40_24G_Diff[RFPath][TX_4S]); */
+		}
+	} else {/* 3 ============================== 5 G ============================== */
+		if (MGN_6M <= Rate)
+			txPower = pHalData->Index5G_BW40_Base[RFPath][chnlIdx];
+		else
+			DBG_871X("===> mpt_ProQueryCalTxPower_Jaguar: INVALID Rate.\n");
+
+		/* DBG_871X("Base Tx power(RF-%c, Rate #%d, Channel Index %d) = 0x%X\n", */
+		/*	((RFPath == 0)?'A':'B'), Rate, chnlIdx, txPower); */
+
+		/*  OFDM-1T */
+		if ((MGN_6M <= Rate && Rate <= MGN_54M) && !IS_CCK_RATE(Rate)) {
+			txPower += pHalData->OFDM_5G_Diff[RFPath][TX_1S];
+			/* DBG_871X("+PowerDiff 5G (RF-%c): (OFDM-1T) = (%d)\n", ((RFPath == 0)?'A':'B'), pHalData->OFDM_5G_Diff[RFPath][TX_1S]); */
+		}
+
+		/*  BW20-1S, BW20-2S */
+		if (BandWidth == CHANNEL_WIDTH_20) {
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31)  || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_5G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_5G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_5G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS24 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_5G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 5G (RF-%c): (BW20-1S, BW20-2S, BW20-3S, BW20-4S) = (%d, %d, %d, %d)\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW20_5G_Diff[RFPath][TX_1S], pHalData->BW20_5G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW20_5G_Diff[RFPath][TX_3S], pHalData->BW20_5G_Diff[RFPath][TX_4S]); */
+		} else if (BandWidth == CHANNEL_WIDTH_40) { /*  BW40-1S, BW40-2S */
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31)  || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_5G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_5G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_5G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS24 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_5G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 5G(RF-%c): (BW40-1S, BW40-2S) = (%d, %d, %d, %d)\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW40_5G_Diff[RFPath][TX_1S], pHalData->BW40_5G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW40_5G_Diff[RFPath][TX_3S], pHalData->BW40_5G_Diff[RFPath][TX_4S]); */
+		} else if (BandWidth == CHANNEL_WIDTH_80) { /*  BW80-1S, BW80-2S */
+			/*  <20121220, Kordan> Get the index of array "Index5G_BW80_Base". */
+			u8 channel5G_80M[CHANNEL_MAX_NUMBER_5G_80M] = {42, 58, 106, 122, 138, 155, 171};
+			for (i = 0; i < sizeof(channel5G_80M)/sizeof(u8); ++i)
+				if (channel5G_80M[i] == Channel)
+					chnlIdx = i;
+
+			txPower = pHalData->Index5G_BW80_Base[RFPath][chnlIdx];
+
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31)  || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += + pHalData->BW80_5G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW80_5G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW80_5G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS23 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW80_5G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 5G(RF-%c): (BW80-1S, BW80-2S, BW80-3S, BW80-4S) = (%d, %d, %d, %d)\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW80_5G_Diff[RFPath][TX_1S], pHalData->BW80_5G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW80_5G_Diff[RFPath][TX_3S], pHalData->BW80_5G_Diff[RFPath][TX_4S]); */
+		}
+	}
+
+	return txPower;
+}
+
+s8 PHY_GetTxPowerTrackingOffset(struct adapter *padapter, u8 RFPath, u8 Rate)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+	s8 offset = 0;
+
+	if (pDM_Odm->RFCalibrateInfo.TxPowerTrackControl  == false)
+		return offset;
+
+	if ((Rate == MGN_1M) || (Rate == MGN_2M) || (Rate == MGN_5_5M) || (Rate == MGN_11M)) {
+		offset = pDM_Odm->Remnant_CCKSwingIdx;
+		/* DBG_871X("+Remnant_CCKSwingIdx = 0x%x\n", RFPath, Rate, pDM_Odm->Remnant_CCKSwingIdx); */
+	} else {
+		offset = pDM_Odm->Remnant_OFDMSwingIdx[RFPath];
+		/* DBG_871X("+Remanant_OFDMSwingIdx[RFPath %u][Rate 0x%x] = 0x%x\n", RFPath, Rate, pDM_Odm->Remnant_OFDMSwingIdx[RFPath]); */
+
+	}
+
+	return offset;
+}
+
+u8 PHY_GetRateIndexOfTxPowerByRate(u8 Rate)
+{
+	u8 index = 0;
+	switch (Rate) {
+	case MGN_1M:
+		index = 0;
+		break;
+	case MGN_2M:
+		index = 1;
+		break;
+	case MGN_5_5M:
+		index = 2;
+		break;
+	case MGN_11M:
+		index = 3;
+		break;
+	case MGN_6M:
+		index = 4;
+		break;
+	case MGN_9M:
+		index = 5;
+		break;
+	case MGN_12M:
+		index = 6;
+		break;
+	case MGN_18M:
+		index = 7;
+		break;
+	case MGN_24M:
+		index = 8;
+		break;
+	case MGN_36M:
+		index = 9;
+		break;
+	case MGN_48M:
+		index = 10;
+		break;
+	case MGN_54M:
+		index = 11;
+		break;
+	case MGN_MCS0:
+		index = 12;
+		break;
+	case MGN_MCS1:
+		index = 13;
+		break;
+	case MGN_MCS2:
+		index = 14;
+		break;
+	case MGN_MCS3:
+		index = 15;
+		break;
+	case MGN_MCS4:
+		index = 16;
+		break;
+	case MGN_MCS5:
+		index = 17;
+		break;
+	case MGN_MCS6:
+		index = 18;
+		break;
+	case MGN_MCS7:
+		index = 19;
+		break;
+	case MGN_MCS8:
+		index = 20;
+		break;
+	case MGN_MCS9:
+		index = 21;
+		break;
+	case MGN_MCS10:
+		index = 22;
+		break;
+	case MGN_MCS11:
+		index = 23;
+		break;
+	case MGN_MCS12:
+		index = 24;
+		break;
+	case MGN_MCS13:
+		index = 25;
+		break;
+	case MGN_MCS14:
+		index = 26;
+		break;
+	case MGN_MCS15:
+		index = 27;
+		break;
+	case MGN_MCS16:
+		index = 28;
+		break;
+	case MGN_MCS17:
+		index = 29;
+		break;
+	case MGN_MCS18:
+		index = 30;
+		break;
+	case MGN_MCS19:
+		index = 31;
+		break;
+	case MGN_MCS20:
+		index = 32;
+		break;
+	case MGN_MCS21:
+		index = 33;
+		break;
+	case MGN_MCS22:
+		index = 34;
+		break;
+	case MGN_MCS23:
+		index = 35;
+		break;
+	case MGN_MCS24:
+		index = 36;
+		break;
+	case MGN_MCS25:
+		index = 37;
+		break;
+	case MGN_MCS26:
+		index = 38;
+		break;
+	case MGN_MCS27:
+		index = 39;
+		break;
+	case MGN_MCS28:
+		index = 40;
+		break;
+	case MGN_MCS29:
+		index = 41;
+		break;
+	case MGN_MCS30:
+		index = 42;
+		break;
+	case MGN_MCS31:
+		index = 43;
+		break;
+	case MGN_VHT1SS_MCS0:
+		index = 44;
+		break;
+	case MGN_VHT1SS_MCS1:
+		index = 45;
+		break;
+	case MGN_VHT1SS_MCS2:
+		index = 46;
+		break;
+	case MGN_VHT1SS_MCS3:
+		index = 47;
+		break;
+	case MGN_VHT1SS_MCS4:
+		index = 48;
+		break;
+	case MGN_VHT1SS_MCS5:
+		index = 49;
+		break;
+	case MGN_VHT1SS_MCS6:
+		index = 50;
+		break;
+	case MGN_VHT1SS_MCS7:
+		index = 51;
+		break;
+	case MGN_VHT1SS_MCS8:
+		index = 52;
+		break;
+	case MGN_VHT1SS_MCS9:
+		index = 53;
+		break;
+	case MGN_VHT2SS_MCS0:
+		index = 54;
+		break;
+	case MGN_VHT2SS_MCS1:
+		index = 55;
+		break;
+	case MGN_VHT2SS_MCS2:
+		index = 56;
+		break;
+	case MGN_VHT2SS_MCS3:
+		index = 57;
+		break;
+	case MGN_VHT2SS_MCS4:
+		index = 58;
+		break;
+	case MGN_VHT2SS_MCS5:
+		index = 59;
+		break;
+	case MGN_VHT2SS_MCS6:
+		index = 60;
+		break;
+	case MGN_VHT2SS_MCS7:
+		index = 61;
+		break;
+	case MGN_VHT2SS_MCS8:
+		index = 62;
+		break;
+	case MGN_VHT2SS_MCS9:
+		index = 63;
+		break;
+	case MGN_VHT3SS_MCS0:
+		index = 64;
+		break;
+	case MGN_VHT3SS_MCS1:
+		index = 65;
+		break;
+	case MGN_VHT3SS_MCS2:
+		index = 66;
+		break;
+	case MGN_VHT3SS_MCS3:
+		index = 67;
+		break;
+	case MGN_VHT3SS_MCS4:
+		index = 68;
+		break;
+	case MGN_VHT3SS_MCS5:
+		index = 69;
+		break;
+	case MGN_VHT3SS_MCS6:
+		index = 70;
+		break;
+	case MGN_VHT3SS_MCS7:
+		index = 71;
+		break;
+	case MGN_VHT3SS_MCS8:
+		index = 72;
+		break;
+	case MGN_VHT3SS_MCS9:
+		index = 73;
+		break;
+	case MGN_VHT4SS_MCS0:
+		index = 74;
+		break;
+	case MGN_VHT4SS_MCS1:
+		index = 75;
+		break;
+	case MGN_VHT4SS_MCS2:
+		index = 76;
+		break;
+	case MGN_VHT4SS_MCS3:
+		index = 77;
+		break;
+	case MGN_VHT4SS_MCS4:
+		index = 78;
+		break;
+	case MGN_VHT4SS_MCS5:
+		index = 79;
+		break;
+	case MGN_VHT4SS_MCS6:
+		index = 80;
+		break;
+	case MGN_VHT4SS_MCS7:
+		index = 81;
+		break;
+	case MGN_VHT4SS_MCS8:
+		index = 82;
+		break;
+	case MGN_VHT4SS_MCS9:
+		index = 83;
+		break;
+	default:
+		DBG_871X("Invalid rate 0x%x in %s\n", Rate, __func__);
+		break;
+	};
+
+	return index;
+}
+
+s8 PHY_GetTxPowerByRate(
+	struct adapter *padapter, u8 Band, u8 RFPath, u8 TxNum, u8 Rate
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	s8 value = 0;
+	u8 rateIndex = PHY_GetRateIndexOfTxPowerByRate(Rate);
+
+	if ((padapter->registrypriv.RegEnableTxPowerByRate == 2 && pHalData->EEPROMRegulatory == 2) ||
+		   padapter->registrypriv.RegEnableTxPowerByRate == 0)
+		return 0;
+
+	if (Band != BAND_ON_2_4G && Band != BAND_ON_5G) {
+		DBG_871X("Invalid band %d in %s\n", Band, __func__);
+		return value;
+	}
+	if (RFPath > ODM_RF_PATH_D) {
+		DBG_871X("Invalid RfPath %d in %s\n", RFPath, __func__);
+		return value;
+	}
+	if (TxNum >= RF_MAX_TX_NUM) {
+		DBG_871X("Invalid TxNum %d in %s\n", TxNum, __func__);
+		return value;
+	}
+	if (rateIndex >= TX_PWR_BY_RATE_NUM_RATE) {
+		DBG_871X("Invalid RateIndex %d in %s\n", rateIndex, __func__);
+		return value;
+	}
+
+	value = pHalData->TxPwrByRateOffset[Band][RFPath][TxNum][rateIndex];
+
+	return value;
+
+}
+
+void PHY_SetTxPowerByRate(
+	struct adapter *padapter,
+	u8 Band,
+	u8 RFPath,
+	u8 TxNum,
+	u8 Rate,
+	s8 Value
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 rateIndex = PHY_GetRateIndexOfTxPowerByRate(Rate);
+
+	if (Band != BAND_ON_2_4G && Band != BAND_ON_5G) {
+		DBG_871X("Invalid band %d in %s\n", Band, __func__);
+		return;
+	}
+	if (RFPath > ODM_RF_PATH_D) {
+		DBG_871X("Invalid RfPath %d in %s\n", RFPath, __func__);
+		return;
+	}
+	if (TxNum >= RF_MAX_TX_NUM) {
+		DBG_871X("Invalid TxNum %d in %s\n", TxNum, __func__);
+		return;
+	}
+	if (rateIndex >= TX_PWR_BY_RATE_NUM_RATE) {
+		DBG_871X("Invalid RateIndex %d in %s\n", rateIndex, __func__);
+		return;
+	}
+
+	pHalData->TxPwrByRateOffset[Band][RFPath][TxNum][rateIndex] = Value;
+}
+
+void PHY_SetTxPowerLevelByPath(struct adapter *Adapter, u8 channel, u8 path)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	bool bIsIn24G = (pHalData->CurrentBandType == BAND_ON_2_4G);
+
+	/* if (pMgntInfo->RegNByteAccess == 0) */
+	{
+		if (bIsIn24G)
+			PHY_SetTxPowerIndexByRateSection(Adapter, path, channel, CCK);
+
+		PHY_SetTxPowerIndexByRateSection(Adapter, path, channel, OFDM);
+		PHY_SetTxPowerIndexByRateSection(Adapter, path, channel, HT_MCS0_MCS7);
+
+		if (pHalData->NumTotalRFPath >= 2)
+			PHY_SetTxPowerIndexByRateSection(Adapter, path, channel, HT_MCS8_MCS15);
+
+	}
+}
+
+void PHY_SetTxPowerIndexByRateArray(
+	struct adapter *padapter,
+	u8 RFPath,
+	enum CHANNEL_WIDTH BandWidth,
+	u8 Channel,
+	u8 *Rates,
+	u8 RateArraySize
+)
+{
+	u32 powerIndex = 0;
+	int	i = 0;
+
+	for (i = 0; i < RateArraySize; ++i) {
+		powerIndex = PHY_GetTxPowerIndex(padapter, RFPath, Rates[i], BandWidth, Channel);
+		PHY_SetTxPowerIndex(padapter, powerIndex, RFPath, Rates[i]);
+	}
+}
+
+static s8 phy_GetWorldWideLimit(s8 *LimitTable)
+{
+	s8	min = LimitTable[0];
+	u8 i = 0;
+
+	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
+		if (LimitTable[i] < min)
+			min = LimitTable[i];
+	}
+
+	return min;
+}
+
+static s8 phy_GetChannelIndexOfTxPowerLimit(u8 Band, u8 Channel)
+{
+	s8	channelIndex = -1;
+	u8 channel5G[CHANNEL_MAX_NUMBER_5G] = {
+		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, 142, 144, 149, 151, 153, 155, 157, 159, 161,
+		163, 165, 167, 168, 169, 171, 173, 175, 177
+	};
+	u8 i = 0;
+	if (Band == BAND_ON_2_4G)
+		channelIndex = Channel - 1;
+	else if (Band == BAND_ON_5G) {
+		for (i = 0; i < sizeof(channel5G)/sizeof(u8); ++i) {
+			if (channel5G[i] == Channel)
+				channelIndex = i;
+		}
+	} else
+		DBG_871X("Invalid Band %d in %s", Band, __func__);
+
+	if (channelIndex == -1)
+		DBG_871X("Invalid Channel %d of Band %d in %s", Channel, Band, __func__);
+
+	return channelIndex;
+}
+
+s8 PHY_GetTxPowerLimit(
+	struct adapter *Adapter,
+	u32 RegPwrTblSel,
+	enum BAND_TYPE Band,
+	enum CHANNEL_WIDTH Bandwidth,
+	u8 RfPath,
+	u8 DataRate,
+	u8 Channel
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	s16	band = -1, regulation = -1, bandwidth = -1, rateSection = -1, channel = -1;
+	s8 powerLimit = MAX_POWER_INDEX;
+
+	if ((Adapter->registrypriv.RegEnableTxPowerLimit == 2 && pHalData->EEPROMRegulatory != 1) ||
+		   Adapter->registrypriv.RegEnableTxPowerLimit == 0)
+		return MAX_POWER_INDEX;
+
+	switch (Adapter->registrypriv.RegPwrTblSel) {
+	case 1:
+			regulation = TXPWR_LMT_ETSI;
+			break;
+	case 2:
+			regulation = TXPWR_LMT_MKK;
+			break;
+	case 3:
+			regulation = TXPWR_LMT_FCC;
+			break;
+
+	case 4:
+			regulation = TXPWR_LMT_WW;
+			break;
+
+	default:
+			regulation = (Band == BAND_ON_2_4G) ? pHalData->Regulation2_4G : pHalData->Regulation5G;
+			break;
+	}
+
+	/* DBG_871X("pMgntInfo->RegPwrTblSel %d, final regulation %d\n", Adapter->registrypriv.RegPwrTblSel, regulation); */
+
+
+	if (Band == BAND_ON_2_4G)
+		band = 0;
+	else if (Band == BAND_ON_5G)
+		band = 1;
+
+	if (Bandwidth == CHANNEL_WIDTH_20)
+		bandwidth = 0;
+	else if (Bandwidth == CHANNEL_WIDTH_40)
+		bandwidth = 1;
+	else if (Bandwidth == CHANNEL_WIDTH_80)
+		bandwidth = 2;
+	else if (Bandwidth == CHANNEL_WIDTH_160)
+		bandwidth = 3;
+
+	switch (DataRate) {
+	case MGN_1M: case MGN_2M: case MGN_5_5M: case MGN_11M:
+		rateSection = 0;
+		break;
+
+	case MGN_6M: case MGN_9M: case MGN_12M: case MGN_18M:
+	case MGN_24M: case MGN_36M: case MGN_48M: case MGN_54M:
+		rateSection = 1;
+		break;
+
+	case MGN_MCS0: case MGN_MCS1: case MGN_MCS2: case MGN_MCS3:
+	case MGN_MCS4: case MGN_MCS5: case MGN_MCS6: case MGN_MCS7:
+		rateSection = 2;
+		break;
+
+	case MGN_MCS8: case MGN_MCS9: case MGN_MCS10: case MGN_MCS11:
+	case MGN_MCS12: case MGN_MCS13: case MGN_MCS14: case MGN_MCS15:
+		rateSection = 3;
+		break;
+
+	case MGN_MCS16: case MGN_MCS17: case MGN_MCS18: case MGN_MCS19:
+	case MGN_MCS20: case MGN_MCS21: case MGN_MCS22: case MGN_MCS23:
+		rateSection = 4;
+		break;
+
+	case MGN_MCS24: case MGN_MCS25: case MGN_MCS26: case MGN_MCS27:
+	case MGN_MCS28: case MGN_MCS29: case MGN_MCS30: case MGN_MCS31:
+		rateSection = 5;
+		break;
+
+	case MGN_VHT1SS_MCS0: case MGN_VHT1SS_MCS1: case MGN_VHT1SS_MCS2:
+	case MGN_VHT1SS_MCS3: case MGN_VHT1SS_MCS4: case MGN_VHT1SS_MCS5:
+	case MGN_VHT1SS_MCS6: case MGN_VHT1SS_MCS7: case MGN_VHT1SS_MCS8:
+	case MGN_VHT1SS_MCS9:
+		rateSection = 6;
+		break;
+
+	case MGN_VHT2SS_MCS0: case MGN_VHT2SS_MCS1: case MGN_VHT2SS_MCS2:
+	case MGN_VHT2SS_MCS3: case MGN_VHT2SS_MCS4: case MGN_VHT2SS_MCS5:
+	case MGN_VHT2SS_MCS6: case MGN_VHT2SS_MCS7: case MGN_VHT2SS_MCS8:
+	case MGN_VHT2SS_MCS9:
+		rateSection = 7;
+		break;
+
+	case MGN_VHT3SS_MCS0: case MGN_VHT3SS_MCS1: case MGN_VHT3SS_MCS2:
+	case MGN_VHT3SS_MCS3: case MGN_VHT3SS_MCS4: case MGN_VHT3SS_MCS5:
+	case MGN_VHT3SS_MCS6: case MGN_VHT3SS_MCS7: case MGN_VHT3SS_MCS8:
+	case MGN_VHT3SS_MCS9:
+		rateSection = 8;
+		break;
+
+	case MGN_VHT4SS_MCS0: case MGN_VHT4SS_MCS1: case MGN_VHT4SS_MCS2:
+	case MGN_VHT4SS_MCS3: case MGN_VHT4SS_MCS4: case MGN_VHT4SS_MCS5:
+	case MGN_VHT4SS_MCS6: case MGN_VHT4SS_MCS7: case MGN_VHT4SS_MCS8:
+	case MGN_VHT4SS_MCS9:
+		rateSection = 9;
+		break;
+
+	default:
+		DBG_871X("Wrong rate 0x%x\n", DataRate);
+		break;
+	}
+
+	if (Band == BAND_ON_5G  && rateSection == 0)
+			DBG_871X("Wrong rate 0x%x: No CCK in 5G Band\n", DataRate);
+
+	/*  workaround for wrong index combination to obtain tx power limit, */
+	/*  OFDM only exists in BW 20M */
+	if (rateSection == 1)
+		bandwidth = 0;
+
+	/*  workaround for wrong index combination to obtain tx power limit, */
+	/*  CCK table will only be given in BW 20M */
+	if (rateSection == 0)
+		bandwidth = 0;
+
+	/*  workaround for wrong indxe combination to obtain tx power limit, */
+	/*  HT on 80M will reference to HT on 40M */
+	if ((rateSection == 2 || rateSection == 3) && Band == BAND_ON_5G && bandwidth == 2) {
+		bandwidth = 1;
+	}
+
+	if (Band == BAND_ON_2_4G)
+		channel = phy_GetChannelIndexOfTxPowerLimit(BAND_ON_2_4G, Channel);
+	else if (Band == BAND_ON_5G)
+		channel = phy_GetChannelIndexOfTxPowerLimit(BAND_ON_5G, Channel);
+	else if (Band == BAND_ON_BOTH) {
+		/*  BAND_ON_BOTH don't care temporarily */
+	}
+
+	if (band == -1 || regulation == -1 || bandwidth == -1 ||
+	     rateSection == -1 || channel == -1) {
+		/* DBG_871X("Wrong index value to access power limit table [band %d][regulation %d][bandwidth %d][rf_path %d][rate_section %d][chnlGroup %d]\n", */
+		/*	  band, regulation, bandwidth, RfPath, rateSection, channelGroup); */
+
+		return MAX_POWER_INDEX;
+	}
+
+	if (Band == BAND_ON_2_4G) {
+		s8 limits[10] = {0}; u8 i = 0;
+		for (i = 0; i < MAX_REGULATION_NUM; i++)
+			limits[i] = pHalData->TxPwrLimit_2_4G[i][bandwidth][rateSection][channel][RfPath];
+
+		powerLimit = (regulation == TXPWR_LMT_WW) ? phy_GetWorldWideLimit(limits) :
+			pHalData->TxPwrLimit_2_4G[regulation][bandwidth][rateSection][channel][RfPath];
+
+	} else if (Band == BAND_ON_5G) {
+		s8 limits[10] = {0}; u8 i = 0;
+		for (i = 0; i < MAX_REGULATION_NUM; ++i)
+			limits[i] = pHalData->TxPwrLimit_5G[i][bandwidth][rateSection][channel][RfPath];
+
+		powerLimit = (regulation == TXPWR_LMT_WW) ? phy_GetWorldWideLimit(limits) :
+			pHalData->TxPwrLimit_5G[regulation][bandwidth][rateSection][channel][RfPath];
+	} else
+		DBG_871X("No power limit table of the specified band\n");
+
+	/*  combine 5G VHT & HT rate */
+	/*  5G 20M and 40M HT and VHT can cross reference */
+	/*
+	if (Band == BAND_ON_5G && powerLimit == MAX_POWER_INDEX) {
+		if (bandwidth == 0 || bandwidth == 1) {
+			RT_TRACE(COMP_INIT, DBG_LOUD, ("No power limit table of the specified band %d, bandwidth %d, ratesection %d, rf path %d\n",
+					  band, bandwidth, rateSection, RfPath));
+			if (rateSection == 2)
+				powerLimit = pHalData->TxPwrLimit_5G[regulation]
+										[bandwidth][4][channelGroup][RfPath];
+			else if (rateSection == 4)
+				powerLimit = pHalData->TxPwrLimit_5G[regulation]
+										[bandwidth][2][channelGroup][RfPath];
+			else if (rateSection == 3)
+				powerLimit = pHalData->TxPwrLimit_5G[regulation]
+										[bandwidth][5][channelGroup][RfPath];
+			else if (rateSection == 5)
+				powerLimit = pHalData->TxPwrLimit_5G[regulation]
+										[bandwidth][3][channelGroup][RfPath];
+		}
+	}
+	*/
+	/* DBG_871X("TxPwrLmt[Regulation %d][Band %d][BW %d][RFPath %d][Rate 0x%x][Chnl %d] = %d\n", */
+	/*		regulation, pHalData->CurrentBandType, Bandwidth, RfPath, DataRate, Channel, powerLimit); */
+	return powerLimit;
+}
+
+static void phy_CrossReferenceHTAndVHTTxPowerLimit(struct adapter *padapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 regulation, bw, channel, rateSection;
+	s8 tempPwrLmt = 0;
+
+	for (regulation = 0; regulation < MAX_REGULATION_NUM; ++regulation) {
+		for (bw = 0; bw < MAX_5G_BANDWITH_NUM; ++bw) {
+			for (channel = 0; channel < CHANNEL_MAX_NUMBER_5G; ++channel) {
+				for (rateSection = 0; rateSection < MAX_RATE_SECTION_NUM; ++rateSection) {
+					tempPwrLmt = pHalData->TxPwrLimit_5G[regulation][bw][rateSection][channel][ODM_RF_PATH_A];
+					if (tempPwrLmt == MAX_POWER_INDEX) {
+						u8 baseSection = 2, refSection = 6;
+						if (bw == 0 || bw == 1) { /*  5G 20M 40M VHT and HT can cross reference */
+							/* DBG_871X("No power limit table of the specified band %d, bandwidth %d, ratesection %d, channel %d, rf path %d\n", */
+							/*			1, bw, rateSection, channel, ODM_RF_PATH_A); */
+							if (rateSection >= 2 && rateSection <= 9) {
+								if (rateSection == 2) {
+									baseSection = 2;
+									refSection = 6;
+								} else if (rateSection == 3) {
+									baseSection = 3;
+									refSection = 7;
+								} else if (rateSection == 4) {
+									baseSection = 4;
+									refSection = 8;
+								} else if (rateSection == 5) {
+									baseSection = 5;
+									refSection = 9;
+								} else if (rateSection == 6) {
+									baseSection = 6;
+									refSection = 2;
+								} else if (rateSection == 7) {
+									baseSection = 7;
+									refSection = 3;
+								} else if (rateSection == 8) {
+									baseSection = 8;
+									refSection = 4;
+								} else if (rateSection == 9) {
+									baseSection = 9;
+									refSection = 5;
+								}
+								pHalData->TxPwrLimit_5G[regulation][bw][baseSection][channel][ODM_RF_PATH_A] =
+									pHalData->TxPwrLimit_5G[regulation][bw][refSection][channel][ODM_RF_PATH_A];
+							}
+
+							/* DBG_871X("use other value %d", tempPwrLmt); */
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
+void PHY_ConvertTxPowerLimitToPowerIndex(struct adapter *Adapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	u8 BW40PwrBasedBm2_4G = 0x2E;
+	u8 regulation, bw, channel, rateSection;
+	s8 tempValue = 0, tempPwrLmt = 0;
+	u8 rfPath = 0;
+
+	/* DBG_871X("=====> PHY_ConvertTxPowerLimitToPowerIndex()\n"); */
+
+	phy_CrossReferenceHTAndVHTTxPowerLimit(Adapter);
+
+	for (regulation = 0; regulation < MAX_REGULATION_NUM; ++regulation) {
+		for (bw = 0; bw < MAX_2_4G_BANDWITH_NUM; ++bw) {
+			for (channel = 0; channel < CHANNEL_MAX_NUMBER_2G; ++channel) {
+				for (rateSection = 0; rateSection < MAX_RATE_SECTION_NUM; ++rateSection) {
+					tempPwrLmt = pHalData->TxPwrLimit_2_4G[regulation][bw][rateSection][channel][ODM_RF_PATH_A];
+
+					for (rfPath = ODM_RF_PATH_A; rfPath < MAX_RF_PATH_NUM; ++rfPath) {
+						if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_EXACT_VALUE) {
+							if (rateSection == 5) /*  HT 4T */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_4TX, HT_MCS24_MCS31);
+							else if (rateSection == 4) /*  HT 3T */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_3TX, HT_MCS16_MCS23);
+							else if (rateSection == 3) /*  HT 2T */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_2TX, HT_MCS8_MCS15);
+							else if (rateSection == 2) /*  HT 1T */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_1TX, HT_MCS0_MCS7);
+							else if (rateSection == 1) /*  OFDM */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_1TX, OFDM);
+							else if (rateSection == 0) /*  CCK */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_1TX, CCK);
+						} else
+							BW40PwrBasedBm2_4G = Adapter->registrypriv.RegPowerBase * 2;
+
+						if (tempPwrLmt != MAX_POWER_INDEX) {
+							tempValue = tempPwrLmt - BW40PwrBasedBm2_4G;
+							pHalData->TxPwrLimit_2_4G[regulation][bw][rateSection][channel][rfPath] = tempValue;
+						}
+					}
+				}
+			}
+		}
+	}
+
+	/* DBG_871X("<===== PHY_ConvertTxPowerLimitToPowerIndex()\n"); */
+}
+
+void PHY_InitTxPowerLimit(struct adapter *Adapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	u8 i, j, k, l, m;
+
+	/* DBG_871X("=====> PHY_InitTxPowerLimit()!\n"); */
+
+	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
+		for (j = 0; j < MAX_2_4G_BANDWITH_NUM; ++j)
+			for (k = 0; k < MAX_RATE_SECTION_NUM; ++k)
+				for (m = 0; m < CHANNEL_MAX_NUMBER_2G; ++m)
+					for (l = 0; l < MAX_RF_PATH_NUM; ++l)
+						pHalData->TxPwrLimit_2_4G[i][j][k][m][l] = MAX_POWER_INDEX;
+	}
+
+	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
+		for (j = 0; j < MAX_5G_BANDWITH_NUM; ++j)
+			for (k = 0; k < MAX_RATE_SECTION_NUM; ++k)
+				for (m = 0; m < CHANNEL_MAX_NUMBER_5G; ++m)
+					for (l = 0; l < MAX_RF_PATH_NUM; ++l)
+						pHalData->TxPwrLimit_5G[i][j][k][m][l] = MAX_POWER_INDEX;
+	}
+
+	/* DBG_871X("<===== PHY_InitTxPowerLimit()!\n"); */
+}
+
+void PHY_SetTxPowerLimit(
+	struct adapter *Adapter,
+	u8 *Regulation,
+	u8 *Band,
+	u8 *Bandwidth,
+	u8 *RateSection,
+	u8 *RfPath,
+	u8 *Channel,
+	u8 *PowerLimit
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	u8 regulation = 0, bandwidth = 0, rateSection = 0, channel;
+	s8 powerLimit = 0, prevPowerLimit, channelIndex;
+
+	/* DBG_871X("Index of power limit table [band %s][regulation %s][bw %s][rate section %s][rf path %s][chnl %s][val %s]\n", */
+	/*	  Band, Regulation, Bandwidth, RateSection, RfPath, Channel, PowerLimit); */
+
+	if (!GetU1ByteIntegerFromStringInDecimal((s8 *)Channel, &channel) ||
+		 !GetU1ByteIntegerFromStringInDecimal((s8 *)PowerLimit, &powerLimit))
+		DBG_871X("Illegal index of power limit table [chnl %s][val %s]\n", Channel, PowerLimit);
+
+	powerLimit = powerLimit > MAX_POWER_INDEX ? MAX_POWER_INDEX : powerLimit;
+
+	if (eqNByte(Regulation, (u8 *)("FCC"), 3))
+		regulation = 0;
+	else if (eqNByte(Regulation, (u8 *)("MKK"), 3))
+		regulation = 1;
+	else if (eqNByte(Regulation, (u8 *)("ETSI"), 4))
+		regulation = 2;
+	else if (eqNByte(Regulation, (u8 *)("WW13"), 4))
+		regulation = 3;
+
+	if (eqNByte(RateSection, (u8 *)("CCK"), 3) && eqNByte(RfPath, (u8 *)("1T"), 2))
+		rateSection = 0;
+	else if (eqNByte(RateSection, (u8 *)("OFDM"), 4) && eqNByte(RfPath, (u8 *)("1T"), 2))
+		rateSection = 1;
+	else if (eqNByte(RateSection, (u8 *)("HT"), 2) && eqNByte(RfPath, (u8 *)("1T"), 2))
+		rateSection = 2;
+	else if (eqNByte(RateSection, (u8 *)("HT"), 2) && eqNByte(RfPath, (u8 *)("2T"), 2))
+		rateSection = 3;
+	else if (eqNByte(RateSection, (u8 *)("HT"), 2) && eqNByte(RfPath, (u8 *)("3T"), 2))
+		rateSection = 4;
+	else if (eqNByte(RateSection, (u8 *)("HT"), 2) && eqNByte(RfPath, (u8 *)("4T"), 2))
+		rateSection = 5;
+	else if (eqNByte(RateSection, (u8 *)("VHT"), 3) && eqNByte(RfPath, (u8 *)("1T"), 2))
+		rateSection = 6;
+	else if (eqNByte(RateSection, (u8 *)("VHT"), 3) && eqNByte(RfPath, (u8 *)("2T"), 2))
+		rateSection = 7;
+	else if (eqNByte(RateSection, (u8 *)("VHT"), 3) && eqNByte(RfPath, (u8 *)("3T"), 2))
+		rateSection = 8;
+	else if (eqNByte(RateSection, (u8 *)("VHT"), 3) && eqNByte(RfPath, (u8 *)("4T"), 2))
+		rateSection = 9;
+	else {
+		DBG_871X("Wrong rate section!\n");
+		return;
+	}
+
+
+	if (eqNByte(Bandwidth, (u8 *)("20M"), 3))
+		bandwidth = 0;
+	else if (eqNByte(Bandwidth, (u8 *)("40M"), 3))
+		bandwidth = 1;
+	else if (eqNByte(Bandwidth, (u8 *)("80M"), 3))
+		bandwidth = 2;
+	else if (eqNByte(Bandwidth, (u8 *)("160M"), 4))
+		bandwidth = 3;
+
+	if (eqNByte(Band, (u8 *)("2.4G"), 4)) {
+		channelIndex = phy_GetChannelIndexOfTxPowerLimit(BAND_ON_2_4G, channel);
+
+		if (channelIndex == -1)
+			return;
+
+		prevPowerLimit = pHalData->TxPwrLimit_2_4G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A];
+
+		if (powerLimit < prevPowerLimit)
+			pHalData->TxPwrLimit_2_4G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A] = powerLimit;
+
+		/* DBG_871X("2.4G Band value : [regulation %d][bw %d][rate_section %d][chnl %d][val %d]\n", */
+		/*	  regulation, bandwidth, rateSection, channelIndex, pHalData->TxPwrLimit_2_4G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A]); */
+	} else if (eqNByte(Band, (u8 *)("5G"), 2)) {
+		channelIndex = phy_GetChannelIndexOfTxPowerLimit(BAND_ON_5G, channel);
+
+		if (channelIndex == -1)
+			return;
+
+		prevPowerLimit = pHalData->TxPwrLimit_5G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A];
+
+		if (powerLimit < prevPowerLimit)
+			pHalData->TxPwrLimit_5G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A] = powerLimit;
+
+		/* DBG_871X("5G Band value : [regulation %d][bw %d][rate_section %d][chnl %d][val %d]\n", */
+		/*	  regulation, bandwidth, rateSection, channel, pHalData->TxPwrLimit_5G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A]); */
+	} else {
+		DBG_871X("Cannot recognize the band info in %s\n", Band);
+		return;
+	}
+}
+
+u8 PHY_GetTxPowerIndex(
+	struct adapter *padapter,
+	u8 RFPath,
+	u8 Rate,
+	enum CHANNEL_WIDTH BandWidth,
+	u8 Channel
+)
+{
+	return PHY_GetTxPowerIndex_8723B(padapter, RFPath, Rate, BandWidth, Channel);
+}
+
+void PHY_SetTxPowerIndex(
+	struct adapter *padapter, u32 PowerIndex, u8 RFPath, u8 Rate
+)
+{
+	PHY_SetTxPowerIndex_8723B(padapter, PowerIndex, RFPath, Rate);
+}
+
+void Hal_ChannelPlanToRegulation(struct adapter *Adapter, u16 ChannelPlan)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	pHalData->Regulation2_4G = TXPWR_LMT_WW;
+	pHalData->Regulation5G = TXPWR_LMT_WW;
+
+	switch (ChannelPlan) {
+	case RT_CHANNEL_DOMAIN_WORLD_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_WW;
+		break;
+	case RT_CHANNEL_DOMAIN_ETSI1_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_MKK1_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_MKK;
+		break;
+	case RT_CHANNEL_DOMAIN_ETSI2_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC1:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI1:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_MKK1_MKK1:
+		pHalData->Regulation2_4G = TXPWR_LMT_MKK;
+		pHalData->Regulation5G = TXPWR_LMT_MKK;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_KCC1:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_MKK;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+				break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC3:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC4:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC5:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC6:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC7:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI3:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_MKK1_MKK2:
+		pHalData->Regulation2_4G = TXPWR_LMT_MKK;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_MKK1_MKK3:
+		pHalData->Regulation2_4G = TXPWR_LMT_MKK;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC1:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_GLOBAL_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_WW;
+		pHalData->Regulation5G = TXPWR_LMT_WW;
+		break;
+	case RT_CHANNEL_DOMAIN_ETSI1_ETSI4:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC3:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI5:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC8:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI6:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI7:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI8:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI9:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI10:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI11:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC4:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI12:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC9:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI13:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC10:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_REALTEK_DEFINE: /* Realtek Reserve */
+		pHalData->Regulation2_4G = TXPWR_LMT_WW;
+		pHalData->Regulation5G = TXPWR_LMT_WW;
+		break;
+	default:
+		break;
+	}
+}
+
+#ifdef CONFIG_LOAD_PHY_PARA_FROM_FILE
+
+static char file_path_bs[PATH_MAX];
+
+#define GetLineFromBuffer(buffer)	 strsep(&buffer, "\n")
+
+int phy_ConfigMACWithParaFile(struct adapter *Adapter, char *pFileName)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+	char *szLine, *ptmp;
+	u32 u4bRegOffset, u4bRegValue, u4bMove;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_MAC_PARA_FILE))
+		return rtStatus;
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pHalData->mac_reg_len == 0) && (pHalData->mac_reg == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pHalData->mac_reg = vzalloc(rlen);
+				if (pHalData->mac_reg) {
+					memcpy(pHalData->mac_reg, pHalData->para_file_buf, rlen);
+					pHalData->mac_reg_len = rlen;
+				} else
+					DBG_871X("%s mac_reg alloc fail !\n", __func__);
+			}
+		}
+	} else {
+		if ((pHalData->mac_reg_len != 0) && (pHalData->mac_reg != NULL)) {
+			memcpy(pHalData->para_file_buf, pHalData->mac_reg, pHalData->mac_reg_len);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		ptmp = pHalData->para_file_buf;
+		for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+			if (!IsCommentString(szLine)) {
+				/*  Get 1st hex value as register offset */
+				if (GetHexValueFromString(szLine, &u4bRegOffset, &u4bMove)) {
+					if (u4bRegOffset == 0xffff) /*  Ending. */
+						break;
+
+					/*  Get 2nd hex value as register value. */
+					szLine += u4bMove;
+					if (GetHexValueFromString(szLine, &u4bRegValue, &u4bMove))
+						rtw_write8(Adapter, u4bRegOffset, (u8)u4bRegValue);
+				}
+			}
+		}
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+int phy_ConfigBBWithParaFile(
+	struct adapter *Adapter, char *pFileName, u32 ConfigType
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+	char *szLine, *ptmp;
+	u32 u4bRegOffset, u4bRegValue, u4bMove;
+	char *pBuf = NULL;
+	u32 *pBufLen = NULL;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_BB_PARA_FILE))
+		return rtStatus;
+
+	switch (ConfigType) {
+	case CONFIG_BB_PHY_REG:
+		pBuf = pHalData->bb_phy_reg;
+		pBufLen = &pHalData->bb_phy_reg_len;
+		break;
+	case CONFIG_BB_AGC_TAB:
+		pBuf = pHalData->bb_agc_tab;
+		pBufLen = &pHalData->bb_agc_tab_len;
+		break;
+	default:
+		DBG_871X("Unknown ConfigType!! %d\r\n", ConfigType);
+		break;
+	}
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pBufLen != NULL) && (*pBufLen == 0) && (pBuf == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pBuf = vzalloc(rlen);
+				if (pBuf) {
+					memcpy(pBuf, pHalData->para_file_buf, rlen);
+					*pBufLen = rlen;
+
+					switch (ConfigType) {
+					case CONFIG_BB_PHY_REG:
+						pHalData->bb_phy_reg = pBuf;
+						break;
+					case CONFIG_BB_AGC_TAB:
+						pHalData->bb_agc_tab = pBuf;
+						break;
+					}
+				} else
+					DBG_871X("%s(): ConfigType %d  alloc fail !\n", __func__, ConfigType);
+			}
+		}
+	} else {
+		if ((pBufLen != NULL) && (*pBufLen == 0) && (pBuf == NULL)) {
+			memcpy(pHalData->para_file_buf, pBuf, *pBufLen);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		ptmp = pHalData->para_file_buf;
+		for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+			if (!IsCommentString(szLine)) {
+				/*  Get 1st hex value as register offset. */
+				if (GetHexValueFromString(szLine, &u4bRegOffset, &u4bMove)) {
+					if (u4bRegOffset == 0xffff) /*  Ending. */
+						break;
+					else if (u4bRegOffset == 0xfe || u4bRegOffset == 0xffe)
+						msleep(50);
+					else if (u4bRegOffset == 0xfd)
+						mdelay(5);
+					else if (u4bRegOffset == 0xfc)
+						mdelay(1);
+					else if (u4bRegOffset == 0xfb)
+						udelay(50);
+					else if (u4bRegOffset == 0xfa)
+						udelay(5);
+					else if (u4bRegOffset == 0xf9)
+						udelay(1);
+
+					/*  Get 2nd hex value as register value. */
+					szLine += u4bMove;
+					if (GetHexValueFromString(szLine, &u4bRegValue, &u4bMove)) {
+						/* DBG_871X("[BB-ADDR]%03lX =%08lX\n", u4bRegOffset, u4bRegValue); */
+						PHY_SetBBReg(Adapter, u4bRegOffset, bMaskDWord, u4bRegValue);
+
+						if (u4bRegOffset == 0xa24)
+							pHalData->odmpriv.RFCalibrateInfo.RegA24 = u4bRegValue;
+
+						/*  Add 1us delay between BB/RF register setting. */
+						udelay(1);
+					}
+				}
+			}
+		}
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+static void phy_DecryptBBPgParaFile(struct adapter *Adapter, char *buffer)
+{
+	u32 i = 0, j = 0;
+	u8 map[95] = {0};
+	u8 currentChar;
+	char *BufOfLines, *ptmp;
+
+	/* DBG_871X("=====>phy_DecryptBBPgParaFile()\n"); */
+	/*  32 the ascii code of the first visable char, 126 the last one */
+	for (i = 0; i < 95; ++i)
+		map[i] = (u8) (94 - i);
+
+	ptmp = buffer;
+	i = 0;
+	for (BufOfLines = GetLineFromBuffer(ptmp); BufOfLines != NULL; BufOfLines = GetLineFromBuffer(ptmp)) {
+		/* DBG_871X("Encrypted Line: %s\n", BufOfLines); */
+
+		for (j = 0; j < strlen(BufOfLines); ++j) {
+			currentChar = BufOfLines[j];
+
+			if (currentChar == '\0')
+				break;
+
+			currentChar -=  (u8) ((((i + j) * 3) % 128));
+
+			BufOfLines[j] = map[currentChar - 32] + 32;
+		}
+		/* DBG_871X("Decrypted Line: %s\n", BufOfLines); */
+		if (strlen(BufOfLines) != 0)
+			i++;
+		BufOfLines[strlen(BufOfLines)] = '\n';
+	}
+}
+
+static int phy_ParseBBPgParaFile(struct adapter *Adapter, char *buffer)
+{
+	int	rtStatus = _SUCCESS;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	char *szLine, *ptmp;
+	u32 u4bRegOffset, u4bRegMask, u4bRegValue;
+	u32 u4bMove;
+	bool firstLine = true;
+	u8 tx_num = 0;
+	u8 band = 0, rf_path = 0;
+
+	/* DBG_871X("=====>phy_ParseBBPgParaFile()\n"); */
+
+	if (Adapter->registrypriv.RegDecryptCustomFile == 1)
+		phy_DecryptBBPgParaFile(Adapter, buffer);
+
+	ptmp = buffer;
+	for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+		if (!IsCommentString(szLine)) {
+			if (isAllSpaceOrTab(szLine, sizeof(*szLine)))
+				continue;
+
+			/*  Get header info (relative value or exact value) */
+			if (firstLine) {
+				if (eqNByte(szLine, (u8 *)("#[v1]"), 5)) {
+
+					pHalData->odmpriv.PhyRegPgVersion = szLine[3] - '0';
+					/* DBG_871X("This is a new format PHY_REG_PG.txt\n"); */
+				} else if (eqNByte(szLine, (u8 *)("#[v0]"), 5)) {
+					pHalData->odmpriv.PhyRegPgVersion = szLine[3] - '0';
+					/* DBG_871X("This is a old format PHY_REG_PG.txt ok\n"); */
+				} else {
+					DBG_871X("The format in PHY_REG_PG are invalid %s\n", szLine);
+					return _FAIL;
+				}
+
+				if (eqNByte(szLine + 5, (u8 *)("[Exact]#"), 8)) {
+					pHalData->odmpriv.PhyRegPgValueType = PHY_REG_PG_EXACT_VALUE;
+					/* DBG_871X("The values in PHY_REG_PG are exact values ok\n"); */
+					firstLine = false;
+					continue;
+				} else if (eqNByte(szLine + 5, (u8 *)("[Relative]#"), 11)) {
+					pHalData->odmpriv.PhyRegPgValueType = PHY_REG_PG_RELATIVE_VALUE;
+					/* DBG_871X("The values in PHY_REG_PG are relative values ok\n"); */
+					firstLine = false;
+					continue;
+				} else {
+					DBG_871X("The values in PHY_REG_PG are invalid %s\n", szLine);
+					return _FAIL;
+				}
+			}
+
+			if (pHalData->odmpriv.PhyRegPgVersion == 0) {
+				/*  Get 1st hex value as register offset. */
+				if (GetHexValueFromString(szLine, &u4bRegOffset, &u4bMove)) {
+					szLine += u4bMove;
+					if (u4bRegOffset == 0xffff) /*  Ending. */
+						break;
+
+					/*  Get 2nd hex value as register mask. */
+					if (GetHexValueFromString(szLine, &u4bRegMask, &u4bMove))
+						szLine += u4bMove;
+					else
+						return _FAIL;
+
+					if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_RELATIVE_VALUE) {
+						/*  Get 3rd hex value as register value. */
+						if (GetHexValueFromString(szLine, &u4bRegValue, &u4bMove)) {
+							PHY_StoreTxPowerByRate(Adapter, 0, 0, 1, u4bRegOffset, u4bRegMask, u4bRegValue);
+							/* DBG_871X("[ADDR] %03X =%08X Mask =%08x\n", u4bRegOffset, u4bRegValue, u4bRegMask); */
+						} else
+							return _FAIL;
+					} else if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_EXACT_VALUE) {
+						u32 combineValue = 0;
+						u8 integer = 0, fraction = 0;
+
+						if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						integer *= 2;
+						if (fraction == 5)
+							integer += 1;
+						combineValue |= (((integer / 10) << 4) + (integer % 10));
+						/* DBG_871X(" %d", integer); */
+
+						if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						integer *= 2;
+						if (fraction == 5)
+							integer += 1;
+						combineValue <<= 8;
+						combineValue |= (((integer / 10) << 4) + (integer % 10));
+						/* DBG_871X(" %d", integer); */
+
+						if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						integer *= 2;
+						if (fraction == 5)
+							integer += 1;
+						combineValue <<= 8;
+						combineValue |= (((integer / 10) << 4) + (integer % 10));
+						/* DBG_871X(" %d", integer); */
+
+						if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						integer *= 2;
+						if (fraction == 5)
+							integer += 1;
+						combineValue <<= 8;
+						combineValue |= (((integer / 10) << 4) + (integer % 10));
+						/* DBG_871X(" %d", integer); */
+						PHY_StoreTxPowerByRate(Adapter, 0, 0, 1, u4bRegOffset, u4bRegMask, combineValue);
+
+						/* DBG_871X("[ADDR] 0x%3x = 0x%4x\n", u4bRegOffset, combineValue); */
+					}
+				}
+			} else if (pHalData->odmpriv.PhyRegPgVersion > 0) {
+				u32 index = 0;
+
+				if (eqNByte(szLine, "0xffff", 6))
+					break;
+
+				if (!eqNByte("#[END]#", szLine, 7)) {
+					/*  load the table label info */
+					if (szLine[0] == '#') {
+						index = 0;
+						if (eqNByte(szLine, "#[2.4G]", 7)) {
+							band = BAND_ON_2_4G;
+							index += 8;
+						} else if (eqNByte(szLine, "#[5G]", 5)) {
+							band = BAND_ON_5G;
+							index += 6;
+						} else {
+							DBG_871X("Invalid band %s in PHY_REG_PG.txt\n", szLine);
+							return _FAIL;
+						}
+
+						rf_path = szLine[index] - 'A';
+						/* DBG_871X(" Table label Band %d, RfPath %d\n", band, rf_path); */
+					} else { /*  load rows of tables */
+						if (szLine[1] == '1')
+							tx_num = RF_1TX;
+						else if (szLine[1] == '2')
+							tx_num = RF_2TX;
+						else if (szLine[1] == '3')
+							tx_num = RF_3TX;
+						else if (szLine[1] == '4')
+							tx_num = RF_4TX;
+						else {
+							DBG_871X("Invalid row in PHY_REG_PG.txt %c\n", szLine[1]);
+							return _FAIL;
+						}
+
+						while (szLine[index] != ']')
+							++index;
+						++index;/*  skip ] */
+
+						/*  Get 2nd hex value as register offset. */
+						szLine += index;
+						if (GetHexValueFromString(szLine, &u4bRegOffset, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						/*  Get 2nd hex value as register mask. */
+						if (GetHexValueFromString(szLine, &u4bRegMask, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_RELATIVE_VALUE) {
+							/*  Get 3rd hex value as register value. */
+							if (GetHexValueFromString(szLine, &u4bRegValue, &u4bMove)) {
+								PHY_StoreTxPowerByRate(Adapter, band, rf_path, tx_num, u4bRegOffset, u4bRegMask, u4bRegValue);
+								/* DBG_871X("[ADDR] %03X (tx_num %d) =%08X Mask =%08x\n", u4bRegOffset, tx_num, u4bRegValue, u4bRegMask); */
+							} else
+								return _FAIL;
+						} else if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_EXACT_VALUE) {
+							u32 combineValue = 0;
+							u8 integer = 0, fraction = 0;
+
+							if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+								szLine += u4bMove;
+							else
+								return _FAIL;
+
+							integer *= 2;
+							if (fraction == 5)
+								integer += 1;
+							combineValue |= (((integer / 10) << 4) + (integer % 10));
+							/* DBG_871X(" %d", integer); */
+
+							if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+								szLine += u4bMove;
+							else
+								return _FAIL;
+
+							integer *= 2;
+							if (fraction == 5)
+								integer += 1;
+							combineValue <<= 8;
+							combineValue |= (((integer / 10) << 4) + (integer % 10));
+							/* DBG_871X(" %d", integer); */
+
+							if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+								szLine += u4bMove;
+							else
+								return _FAIL;
+
+							integer *= 2;
+							if (fraction == 5)
+								integer += 1;
+							combineValue <<= 8;
+							combineValue |= (((integer / 10) << 4) + (integer % 10));
+							/* DBG_871X(" %d", integer); */
+
+							if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+								szLine += u4bMove;
+							else
+								return _FAIL;
+
+							integer *= 2;
+							if (fraction == 5)
+								integer += 1;
+							combineValue <<= 8;
+							combineValue |= (((integer / 10) << 4) + (integer % 10));
+							/* DBG_871X(" %d", integer); */
+							PHY_StoreTxPowerByRate(Adapter, band, rf_path, tx_num, u4bRegOffset, u4bRegMask, combineValue);
+
+							/* DBG_871X("[ADDR] 0x%3x (tx_num %d) = 0x%4x\n", u4bRegOffset, tx_num, combineValue); */
+						}
+					}
+				}
+			}
+		}
+	}
+	/* DBG_871X("<=====phy_ParseBBPgParaFile()\n"); */
+	return rtStatus;
+}
+
+int phy_ConfigBBWithPgParaFile(struct adapter *Adapter, char *pFileName)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_BB_PG_PARA_FILE))
+		return rtStatus;
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pHalData->bb_phy_reg_pg_len == 0) && (pHalData->bb_phy_reg_pg == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pHalData->bb_phy_reg_pg = vzalloc(rlen);
+				if (pHalData->bb_phy_reg_pg) {
+					memcpy(pHalData->bb_phy_reg_pg, pHalData->para_file_buf, rlen);
+					pHalData->bb_phy_reg_pg_len = rlen;
+				} else
+					DBG_871X("%s bb_phy_reg_pg alloc fail !\n", __func__);
+			}
+		}
+	} else {
+		if ((pHalData->bb_phy_reg_pg_len != 0) && (pHalData->bb_phy_reg_pg != NULL)) {
+			memcpy(pHalData->para_file_buf, pHalData->bb_phy_reg_pg, pHalData->bb_phy_reg_pg_len);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		/* DBG_871X("phy_ConfigBBWithPgParaFile(): read %s ok\n", pFileName); */
+		phy_ParseBBPgParaFile(Adapter, pHalData->para_file_buf);
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+int PHY_ConfigRFWithParaFile(
+	struct adapter *Adapter, char *pFileName, u8 eRFPath
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+	char *szLine, *ptmp;
+	u32 u4bRegOffset, u4bRegValue, u4bMove;
+	u16 i;
+	char *pBuf = NULL;
+	u32 *pBufLen = NULL;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_RF_PARA_FILE))
+		return rtStatus;
+
+	switch (eRFPath) {
+	case ODM_RF_PATH_A:
+		pBuf = pHalData->rf_radio_a;
+		pBufLen = &pHalData->rf_radio_a_len;
+		break;
+	case ODM_RF_PATH_B:
+		pBuf = pHalData->rf_radio_b;
+		pBufLen = &pHalData->rf_radio_b_len;
+		break;
+	default:
+		DBG_871X("Unknown RF path!! %d\r\n", eRFPath);
+		break;
+	}
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pBufLen != NULL) && (*pBufLen == 0) && (pBuf == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pBuf = vzalloc(rlen);
+				if (pBuf) {
+					memcpy(pBuf, pHalData->para_file_buf, rlen);
+					*pBufLen = rlen;
+
+					switch (eRFPath) {
+					case ODM_RF_PATH_A:
+						pHalData->rf_radio_a = pBuf;
+						break;
+					case ODM_RF_PATH_B:
+						pHalData->rf_radio_b = pBuf;
+						break;
+					}
+				} else
+					DBG_871X("%s(): eRFPath =%d  alloc fail !\n", __func__, eRFPath);
+			}
+		}
+	} else {
+		if ((pBufLen != NULL) && (*pBufLen == 0) && (pBuf == NULL)) {
+			memcpy(pHalData->para_file_buf, pBuf, *pBufLen);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		/* DBG_871X("%s(): read %s successfully\n", __func__, pFileName); */
+
+		ptmp = pHalData->para_file_buf;
+		for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+			if (!IsCommentString(szLine)) {
+				/*  Get 1st hex value as register offset. */
+				if (GetHexValueFromString(szLine, &u4bRegOffset, &u4bMove)) {
+					if (u4bRegOffset == 0xfe || u4bRegOffset == 0xffe) /*  Deay specific ms. Only RF configuration require delay. */
+						msleep(50);
+					else if (u4bRegOffset == 0xfd) {
+						/* mdelay(5); */
+						for (i = 0; i < 100; i++)
+							udelay(MAX_STALL_TIME);
+					} else if (u4bRegOffset == 0xfc) {
+						/* mdelay(1); */
+						for (i = 0; i < 20; i++)
+							udelay(MAX_STALL_TIME);
+					} else if (u4bRegOffset == 0xfb)
+						udelay(50);
+					else if (u4bRegOffset == 0xfa)
+						udelay(5);
+					else if (u4bRegOffset == 0xf9)
+						udelay(1);
+					else if (u4bRegOffset == 0xffff)
+						break;
+
+					/*  Get 2nd hex value as register value. */
+					szLine += u4bMove;
+					if (GetHexValueFromString(szLine, &u4bRegValue, &u4bMove)) {
+						PHY_SetRFReg(Adapter, eRFPath, u4bRegOffset, bRFRegOffsetMask, u4bRegValue);
+
+						/*  Temp add, for frequency lock, if no delay, that may cause */
+						/*  frequency shift, ex: 2412MHz => 2417MHz */
+						/*  If frequency shift, the following action may works. */
+						/*  Fractional-N table in radio_a.txt */
+						/* 0x2a 0x00001		channel 1 */
+						/* 0x2b 0x00808		frequency divider. */
+						/* 0x2b 0x53333 */
+						/* 0x2c 0x0000c */
+						udelay(1);
+					}
+				}
+			}
+		}
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+static void initDeltaSwingIndexTables(
+	struct adapter *Adapter,
+	char *Band,
+	char *Path,
+	char *Sign,
+	char *Channel,
+	char *Rate,
+	char *Data
+)
+{
+	#define STR_EQUAL_5G(_band, _path, _sign, _rate, _chnl) \
+		((strcmp(Band, _band) == 0) && (strcmp(Path, _path) == 0) && (strcmp(Sign, _sign) == 0) &&\
+		(strcmp(Rate, _rate) == 0) && (strcmp(Channel, _chnl) == 0)\
+	)
+	#define STR_EQUAL_2G(_band, _path, _sign, _rate) \
+		((strcmp(Band, _band) == 0) && (strcmp(Path, _path) == 0) && (strcmp(Sign, _sign) == 0) &&\
+		(strcmp(Rate, _rate) == 0)\
+	)
+
+	#define STORE_SWING_TABLE(_array, _iteratedIdx) \
+		for (token = strsep(&Data, delim); token != NULL; token = strsep(&Data, delim)) {\
+			sscanf(token, "%d", &idx);\
+			_array[_iteratedIdx++] = (u8)idx;\
+		} \
+
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+	PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+	u32 j = 0;
+	char *token;
+	char delim[] = ",";
+	u32 idx = 0;
+
+	/* DBG_871X("===>initDeltaSwingIndexTables(): Band: %s;\nPath: %s;\nSign: %s;\nChannel: %s;\nRate: %s;\n, Data: %s;\n", */
+	/*	Band, Path, Sign, Channel, Rate, Data); */
+
+	if (STR_EQUAL_2G("2G", "A", "+", "CCK")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_P, j);
+	} else if (STR_EQUAL_2G("2G", "A", "-", "CCK")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_N, j);
+	} else if (STR_EQUAL_2G("2G", "B", "+", "CCK")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_P, j);
+	} else if (STR_EQUAL_2G("2G", "B", "-", "CCK")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_N, j);
+	} else if (STR_EQUAL_2G("2G", "A", "+", "ALL")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GA_P, j);
+	} else if (STR_EQUAL_2G("2G", "A", "-", "ALL")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GA_N, j);
+	} else if (STR_EQUAL_2G("2G", "B", "+", "ALL")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GB_P, j);
+	} else if (STR_EQUAL_2G("2G", "B", "-", "ALL")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GB_N, j);
+	} else if (STR_EQUAL_5G("5G", "A", "+", "ALL", "0")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[0], j);
+	} else if (STR_EQUAL_5G("5G", "A", "-", "ALL", "0")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[0], j);
+	} else if (STR_EQUAL_5G("5G", "B", "+", "ALL", "0")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[0], j);
+	} else if (STR_EQUAL_5G("5G", "B", "-", "ALL", "0")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[0], j);
+	} else if (STR_EQUAL_5G("5G", "A", "+", "ALL", "1")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[1], j);
+	} else if (STR_EQUAL_5G("5G", "A", "-", "ALL", "1")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[1], j);
+	} else if (STR_EQUAL_5G("5G", "B", "+", "ALL", "1")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[1], j);
+	} else if (STR_EQUAL_5G("5G", "B", "-", "ALL", "1")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[1], j);
+	} else if (STR_EQUAL_5G("5G", "A", "+", "ALL", "2")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[2], j);
+	} else if (STR_EQUAL_5G("5G", "A", "-", "ALL", "2")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[2], j);
+	} else if (STR_EQUAL_5G("5G", "B", "+", "ALL", "2")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[2], j);
+	} else if (STR_EQUAL_5G("5G", "B", "-", "ALL", "2")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[2], j);
+	} else if (STR_EQUAL_5G("5G", "A", "+", "ALL", "3")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[3], j);
+	} else if (STR_EQUAL_5G("5G", "A", "-", "ALL", "3")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[3], j);
+	} else if (STR_EQUAL_5G("5G", "B", "+", "ALL", "3")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[3], j);
+	} else if (STR_EQUAL_5G("5G", "B", "-", "ALL", "3")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[3], j);
+	} else
+		DBG_871X("===>initDeltaSwingIndexTables(): The input is invalid!!\n");
+}
+
+int PHY_ConfigRFWithTxPwrTrackParaFile(struct adapter *Adapter, char *pFileName)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+	char *szLine, *ptmp;
+	u32 i = 0;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_RF_TXPWR_TRACK_PARA_FILE))
+		return rtStatus;
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pHalData->rf_tx_pwr_track_len == 0) && (pHalData->rf_tx_pwr_track == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pHalData->rf_tx_pwr_track = vzalloc(rlen);
+				if (pHalData->rf_tx_pwr_track) {
+					memcpy(pHalData->rf_tx_pwr_track, pHalData->para_file_buf, rlen);
+					pHalData->rf_tx_pwr_track_len = rlen;
+				} else
+					DBG_871X("%s rf_tx_pwr_track alloc fail !\n", __func__);
+			}
+		}
+	} else {
+		if ((pHalData->rf_tx_pwr_track_len != 0) && (pHalData->rf_tx_pwr_track != NULL)) {
+			memcpy(pHalData->para_file_buf, pHalData->rf_tx_pwr_track, pHalData->rf_tx_pwr_track_len);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		/* DBG_871X("%s(): read %s successfully\n", __func__, pFileName); */
+
+		ptmp = pHalData->para_file_buf;
+		for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+			if (!IsCommentString(szLine)) {
+				char band[5] = "", path[5] = "", sign[5] = "";
+				char chnl[5] = "", rate[10] = "";
+				char data[300] = ""; /*  100 is too small */
+
+				if (strlen(szLine) < 10 || szLine[0] != '[')
+					continue;
+
+				strncpy(band, szLine+1, 2);
+				strncpy(path, szLine+5, 1);
+				strncpy(sign, szLine+8, 1);
+
+				i = 10; /*  szLine+10 */
+				if (!ParseQualifiedString(szLine, &i, rate, '[', ']')) {
+					/* DBG_871X("Fail to parse rate!\n"); */
+				}
+				if (!ParseQualifiedString(szLine, &i, chnl, '[', ']')) {
+					/* DBG_871X("Fail to parse channel group!\n"); */
+				}
+				while (szLine[i] != '{' && i < strlen(szLine))
+					i++;
+				if (!ParseQualifiedString(szLine, &i, data, '{', '}')) {
+					/* DBG_871X("Fail to parse data!\n"); */
+				}
+
+				initDeltaSwingIndexTables(Adapter, band, path, sign, chnl, rate, data);
+			}
+		}
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+static int phy_ParsePowerLimitTableFile(struct adapter *Adapter, char *buffer)
+{
+	u32 i = 0, forCnt = 0;
+	u8 loadingStage = 0, limitValue = 0, fraction = 0;
+	char *szLine, *ptmp;
+	int	rtStatus = _SUCCESS;
+	char band[10], bandwidth[10], rateSection[10],
+		regulation[TXPWR_LMT_MAX_REGULATION_NUM][10], rfPath[10], colNumBuf[10];
+	u8 colNum = 0;
+
+	DBG_871X("===>phy_ParsePowerLimitTableFile()\n");
+
+	if (Adapter->registrypriv.RegDecryptCustomFile == 1)
+		phy_DecryptBBPgParaFile(Adapter, buffer);
+
+	ptmp = buffer;
+	for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+		/*  skip comment */
+		if (IsCommentString(szLine)) {
+			continue;
+		}
+
+		if (loadingStage == 0) {
+			for (forCnt = 0; forCnt < TXPWR_LMT_MAX_REGULATION_NUM; ++forCnt)
+				memset((void *) regulation[forCnt], 0, 10);
+
+			memset((void *) band, 0, 10);
+			memset((void *) bandwidth, 0, 10);
+			memset((void *) rateSection, 0, 10);
+			memset((void *) rfPath, 0, 10);
+			memset((void *) colNumBuf, 0, 10);
+
+			if (szLine[0] != '#' || szLine[1] != '#')
+				continue;
+
+			/*  skip the space */
+			i = 2;
+			while (szLine[i] == ' ' || szLine[i] == '\t')
+				++i;
+
+			szLine[--i] = ' '; /*  return the space in front of the regulation info */
+
+			/*  Parse the label of the table */
+			if (!ParseQualifiedString(szLine, &i, band, ' ', ',')) {
+				DBG_871X("Fail to parse band!\n");
+				return _FAIL;
+			}
+			if (!ParseQualifiedString(szLine, &i, bandwidth, ' ', ',')) {
+				DBG_871X("Fail to parse bandwidth!\n");
+				return _FAIL;
+			}
+			if (!ParseQualifiedString(szLine, &i, rfPath, ' ', ',')) {
+				DBG_871X("Fail to parse rf path!\n");
+				return _FAIL;
+			}
+			if (!ParseQualifiedString(szLine, &i, rateSection, ' ', ',')) {
+				DBG_871X("Fail to parse rate!\n");
+				return _FAIL;
+			}
+
+			loadingStage = 1;
+		} else if (loadingStage == 1) {
+			if (szLine[0] != '#' || szLine[1] != '#')
+				continue;
+
+			/*  skip the space */
+			i = 2;
+			while (szLine[i] == ' ' || szLine[i] == '\t')
+				++i;
+
+			if (!eqNByte((u8 *)(szLine + i), (u8 *)("START"), 5)) {
+				DBG_871X("Lost \"##   START\" label\n");
+				return _FAIL;
+			}
+
+			loadingStage = 2;
+		} else if (loadingStage == 2) {
+			if (szLine[0] != '#' || szLine[1] != '#')
+				continue;
+
+			/*  skip the space */
+			i = 2;
+			while (szLine[i] == ' ' || szLine[i] == '\t')
+				++i;
+
+			if (!ParseQualifiedString(szLine, &i, colNumBuf, '#', '#')) {
+				DBG_871X("Fail to parse column number!\n");
+				return _FAIL;
+			}
+
+			if (!GetU1ByteIntegerFromStringInDecimal(colNumBuf, &colNum))
+				return _FAIL;
+
+			if (colNum > TXPWR_LMT_MAX_REGULATION_NUM) {
+				DBG_871X(
+					"unvalid col number %d (greater than max %d)\n",
+					colNum, TXPWR_LMT_MAX_REGULATION_NUM
+				);
+				return _FAIL;
+			}
+
+			for (forCnt = 0; forCnt < colNum; ++forCnt) {
+				u8 regulation_name_cnt = 0;
+
+				/*  skip the space */
+				while (szLine[i] == ' ' || szLine[i] == '\t')
+					++i;
+
+				while (szLine[i] != ' ' && szLine[i] != '\t' && szLine[i] != '\0')
+					regulation[forCnt][regulation_name_cnt++] = szLine[i++];
+				/* DBG_871X("regulation %s!\n", regulation[forCnt]); */
+
+				if (regulation_name_cnt == 0) {
+					DBG_871X("unvalid number of regulation!\n");
+					return _FAIL;
+				}
+			}
+
+			loadingStage = 3;
+		} else if (loadingStage == 3) {
+			char channel[10] = {0}, powerLimit[10] = {0};
+			u8 cnt = 0;
+
+			/*  the table ends */
+			if (szLine[0] == '#' && szLine[1] == '#') {
+				i = 2;
+				while (szLine[i] == ' ' || szLine[i] == '\t')
+					++i;
+
+				if (eqNByte((u8 *)(szLine + i), (u8 *)("END"), 3)) {
+					loadingStage = 0;
+					continue;
+				} else {
+					DBG_871X("Wrong format\n");
+					DBG_871X("<===== phy_ParsePowerLimitTableFile()\n");
+					return _FAIL;
+				}
+			}
+
+			if ((szLine[0] != 'c' && szLine[0] != 'C') ||
+				 (szLine[1] != 'h' && szLine[1] != 'H')) {
+				DBG_871X("Meet wrong channel => power limt pair\n");
+				continue;
+			}
+			i = 2;/*  move to the  location behind 'h' */
+
+			/*  load the channel number */
+			cnt = 0;
+			while (szLine[i] >= '0' && szLine[i] <= '9') {
+				channel[cnt] = szLine[i];
+				++cnt;
+				++i;
+			}
+			/* DBG_871X("chnl %s!\n", channel); */
+
+			for (forCnt = 0; forCnt < colNum; ++forCnt) {
+				/*  skip the space between channel number and the power limit value */
+				while (szLine[i] == ' ' || szLine[i] == '\t')
+					++i;
+
+				/*  load the power limit value */
+				cnt = 0;
+				fraction = 0;
+				memset((void *) powerLimit, 0, 10);
+				while ((szLine[i] >= '0' && szLine[i] <= '9') || szLine[i] == '.') {
+					if (szLine[i] == '.') {
+						if ((szLine[i+1] >= '0' && szLine[i+1] <= '9')) {
+							fraction = szLine[i+1];
+							i += 2;
+						} else {
+							DBG_871X("Wrong fraction in TXPWR_LMT.txt\n");
+							return _FAIL;
+						}
+
+						break;
+					}
+
+					powerLimit[cnt] = szLine[i];
+					++cnt;
+					++i;
+				}
+
+				if (powerLimit[0] == '\0') {
+					powerLimit[0] = '6';
+					powerLimit[1] = '3';
+					i += 2;
+				} else {
+					if (!GetU1ByteIntegerFromStringInDecimal(powerLimit, &limitValue))
+						return _FAIL;
+
+					limitValue *= 2;
+					cnt = 0;
+					if (fraction == '5')
+						++limitValue;
+
+					/*  the value is greater or equal to 100 */
+					if (limitValue >= 100) {
+						powerLimit[cnt++] = limitValue/100 + '0';
+						limitValue %= 100;
+
+						if (limitValue >= 10) {
+							powerLimit[cnt++] = limitValue/10 + '0';
+							limitValue %= 10;
+						} else
+							powerLimit[cnt++] = '0';
+
+						powerLimit[cnt++] = limitValue + '0';
+					} else if (limitValue >= 10) { /*  the value is greater or equal to 10 */
+						powerLimit[cnt++] = limitValue/10 + '0';
+						limitValue %= 10;
+						powerLimit[cnt++] = limitValue + '0';
+					}
+					/*  the value is less than 10 */
+					else
+						powerLimit[cnt++] = limitValue + '0';
+
+					powerLimit[cnt] = '\0';
+				}
+
+				/* DBG_871X("ch%s => %s\n", channel, powerLimit); */
+
+				/*  store the power limit value */
+				PHY_SetTxPowerLimit(Adapter, (u8 *)regulation[forCnt], (u8 *)band,
+					(u8 *)bandwidth, (u8 *)rateSection, (u8 *)rfPath, (u8 *)channel, (u8 *)powerLimit);
+
+			}
+		} else {
+			DBG_871X("Abnormal loading stage in phy_ParsePowerLimitTableFile()!\n");
+			rtStatus = _FAIL;
+			break;
+		}
+	}
+
+	DBG_871X("<===phy_ParsePowerLimitTableFile()\n");
+	return rtStatus;
+}
+
+int PHY_ConfigRFWithPowerLimitTableParaFile(
+	struct adapter *Adapter, char *pFileName
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_RF_TXPWR_LMT_PARA_FILE))
+		return rtStatus;
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pHalData->rf_tx_pwr_lmt_len == 0) && (pHalData->rf_tx_pwr_lmt == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pHalData->rf_tx_pwr_lmt = vzalloc(rlen);
+				if (pHalData->rf_tx_pwr_lmt) {
+					memcpy(pHalData->rf_tx_pwr_lmt, pHalData->para_file_buf, rlen);
+					pHalData->rf_tx_pwr_lmt_len = rlen;
+				} else
+					DBG_871X("%s rf_tx_pwr_lmt alloc fail !\n", __func__);
+			}
+		}
+	} else {
+		if ((pHalData->rf_tx_pwr_lmt_len != 0) && (pHalData->rf_tx_pwr_lmt != NULL)) {
+			memcpy(pHalData->para_file_buf, pHalData->rf_tx_pwr_lmt, pHalData->rf_tx_pwr_lmt_len);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		/* DBG_871X("%s(): read %s ok\n", __func__, pFileName); */
+		rtStatus = phy_ParsePowerLimitTableFile(Adapter, pHalData->para_file_buf);
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+void phy_free_filebuf(struct adapter *padapter)
+{
+	struct hal_com_data		*pHalData = GET_HAL_DATA(padapter);
+
+	if (pHalData->mac_reg)
+		vfree(pHalData->mac_reg);
+	if (pHalData->bb_phy_reg)
+		vfree(pHalData->bb_phy_reg);
+	if (pHalData->bb_agc_tab)
+		vfree(pHalData->bb_agc_tab);
+	if (pHalData->bb_phy_reg_pg)
+		vfree(pHalData->bb_phy_reg_pg);
+	if (pHalData->bb_phy_reg_mp)
+		vfree(pHalData->bb_phy_reg_mp);
+	if (pHalData->rf_radio_a)
+		vfree(pHalData->rf_radio_a);
+	if (pHalData->rf_radio_b)
+		vfree(pHalData->rf_radio_b);
+	if (pHalData->rf_tx_pwr_track)
+		vfree(pHalData->rf_tx_pwr_track);
+	if (pHalData->rf_tx_pwr_lmt)
+		vfree(pHalData->rf_tx_pwr_lmt);
+
+}
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_BB.c linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_BB.c
--- linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_BB.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_BB.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,643 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License as
+* published by the Free Software Foundation.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+******************************************************************************/
+
+
+#include "odm_precomp.h"
+
+static bool CheckPositive(
+	PDM_ODM_T pDM_Odm, const u32 Condition1, const u32 Condition2
+)
+{
+	u8 _BoardType =
+		((pDM_Odm->BoardType & BIT4) >> 4) << 0 | /*  _GLNA */
+		((pDM_Odm->BoardType & BIT3) >> 3) << 1 | /*  _GPA */
+		((pDM_Odm->BoardType & BIT7) >> 7) << 2 | /*  _ALNA */
+		((pDM_Odm->BoardType & BIT6) >> 6) << 3 | /*  _APA */
+		((pDM_Odm->BoardType & BIT2) >> 2) << 4;  /*  _BT */
+
+	u32 cond1 = Condition1, cond2 = Condition2;
+	u32 driver1 =
+		pDM_Odm->CutVersion << 24 |
+		pDM_Odm->SupportPlatform << 16 |
+		pDM_Odm->PackageType << 12 |
+		pDM_Odm->SupportInterface << 8  |
+		_BoardType;
+
+	u32 driver2 =
+		pDM_Odm->TypeGLNA << 0 |
+		pDM_Odm->TypeGPA << 8 |
+		pDM_Odm->TypeALNA << 16 |
+		pDM_Odm->TypeAPA << 24;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (cond1, cond2) = (0x%X 0x%X)\n",
+			cond1,
+			cond2
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (driver1, driver2) = (0x%X 0x%X)\n",
+			driver1,
+			driver2
+		)
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		("	(Platform, Interface) = (0x%X, 0x%X)\n",
+			pDM_Odm->SupportPlatform,
+			pDM_Odm->SupportInterface
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"	(Board, Package) = (0x%X, 0x%X)\n",
+			pDM_Odm->BoardType,
+			pDM_Odm->PackageType
+		)
+	);
+
+
+	/*  Value Defined Check =============== */
+	/* QFN Type [15:12] and Cut Version [27:24] need to do value check */
+
+	if (((cond1 & 0x0000F000) != 0) && ((cond1 & 0x0000F000) != (driver1 & 0x0000F000)))
+		return false;
+	if (((cond1 & 0x0F000000) != 0) && ((cond1 & 0x0F000000) != (driver1 & 0x0F000000)))
+		return false;
+
+	/*  Bit Defined Check ================ */
+	/*  We don't care [31:28] and [23:20] */
+	/*  */
+	cond1   &= 0x000F0FFF;
+	driver1 &= 0x000F0FFF;
+
+	if ((cond1 & driver1) == cond1) {
+		u32 bitMask = 0;
+
+		if ((cond1 & 0x0F) == 0) /*  BoardType is DONTCARE */
+			return true;
+
+		if ((cond1 & BIT0) != 0) /* GLNA */
+			bitMask |= 0x000000FF;
+		if ((cond1 & BIT1) != 0) /* GPA */
+			bitMask |= 0x0000FF00;
+		if ((cond1 & BIT2) != 0) /* ALNA */
+			bitMask |= 0x00FF0000;
+		if ((cond1 & BIT3) != 0) /* APA */
+			bitMask |= 0xFF000000;
+
+		/*  BoardType of each RF path is matched */
+		if ((cond2 & bitMask) == (driver2 & bitMask))
+			return true;
+	}
+	return false;
+}
+
+static bool CheckNegative(
+	PDM_ODM_T pDM_Odm, const u32  Condition1, const u32 Condition2
+)
+{
+	return true;
+}
+
+/******************************************************************************
+*                           AGC_TAB.TXT
+******************************************************************************/
+
+static u32 Array_MP_8723B_AGC_TAB[] = {
+		0xC78, 0xFD000001,
+		0xC78, 0xFC010001,
+		0xC78, 0xFB020001,
+		0xC78, 0xFA030001,
+		0xC78, 0xF9040001,
+		0xC78, 0xF8050001,
+		0xC78, 0xF7060001,
+		0xC78, 0xF6070001,
+		0xC78, 0xF5080001,
+		0xC78, 0xF4090001,
+		0xC78, 0xF30A0001,
+		0xC78, 0xF20B0001,
+		0xC78, 0xF10C0001,
+		0xC78, 0xF00D0001,
+		0xC78, 0xEF0E0001,
+		0xC78, 0xEE0F0001,
+		0xC78, 0xED100001,
+		0xC78, 0xEC110001,
+		0xC78, 0xEB120001,
+		0xC78, 0xEA130001,
+		0xC78, 0xE9140001,
+		0xC78, 0xE8150001,
+		0xC78, 0xE7160001,
+		0xC78, 0xE6170001,
+		0xC78, 0xE5180001,
+		0xC78, 0xE4190001,
+		0xC78, 0xE31A0001,
+		0xC78, 0xA51B0001,
+		0xC78, 0xA41C0001,
+		0xC78, 0xA31D0001,
+		0xC78, 0x671E0001,
+		0xC78, 0x661F0001,
+		0xC78, 0x65200001,
+		0xC78, 0x64210001,
+		0xC78, 0x63220001,
+		0xC78, 0x4A230001,
+		0xC78, 0x49240001,
+		0xC78, 0x48250001,
+		0xC78, 0x47260001,
+		0xC78, 0x46270001,
+		0xC78, 0x45280001,
+		0xC78, 0x44290001,
+		0xC78, 0x432A0001,
+		0xC78, 0x422B0001,
+		0xC78, 0x292C0001,
+		0xC78, 0x282D0001,
+		0xC78, 0x272E0001,
+		0xC78, 0x262F0001,
+		0xC78, 0x0A300001,
+		0xC78, 0x09310001,
+		0xC78, 0x08320001,
+		0xC78, 0x07330001,
+		0xC78, 0x06340001,
+		0xC78, 0x05350001,
+		0xC78, 0x04360001,
+		0xC78, 0x03370001,
+		0xC78, 0x02380001,
+		0xC78, 0x01390001,
+		0xC78, 0x013A0001,
+		0xC78, 0x013B0001,
+		0xC78, 0x013C0001,
+		0xC78, 0x013D0001,
+		0xC78, 0x013E0001,
+		0xC78, 0x013F0001,
+		0xC78, 0xFC400001,
+		0xC78, 0xFB410001,
+		0xC78, 0xFA420001,
+		0xC78, 0xF9430001,
+		0xC78, 0xF8440001,
+		0xC78, 0xF7450001,
+		0xC78, 0xF6460001,
+		0xC78, 0xF5470001,
+		0xC78, 0xF4480001,
+		0xC78, 0xF3490001,
+		0xC78, 0xF24A0001,
+		0xC78, 0xF14B0001,
+		0xC78, 0xF04C0001,
+		0xC78, 0xEF4D0001,
+		0xC78, 0xEE4E0001,
+		0xC78, 0xED4F0001,
+		0xC78, 0xEC500001,
+		0xC78, 0xEB510001,
+		0xC78, 0xEA520001,
+		0xC78, 0xE9530001,
+		0xC78, 0xE8540001,
+		0xC78, 0xE7550001,
+		0xC78, 0xE6560001,
+		0xC78, 0xE5570001,
+		0xC78, 0xE4580001,
+		0xC78, 0xE3590001,
+		0xC78, 0xA65A0001,
+		0xC78, 0xA55B0001,
+		0xC78, 0xA45C0001,
+		0xC78, 0xA35D0001,
+		0xC78, 0x675E0001,
+		0xC78, 0x665F0001,
+		0xC78, 0x65600001,
+		0xC78, 0x64610001,
+		0xC78, 0x63620001,
+		0xC78, 0x62630001,
+		0xC78, 0x61640001,
+		0xC78, 0x48650001,
+		0xC78, 0x47660001,
+		0xC78, 0x46670001,
+		0xC78, 0x45680001,
+		0xC78, 0x44690001,
+		0xC78, 0x436A0001,
+		0xC78, 0x426B0001,
+		0xC78, 0x286C0001,
+		0xC78, 0x276D0001,
+		0xC78, 0x266E0001,
+		0xC78, 0x256F0001,
+		0xC78, 0x24700001,
+		0xC78, 0x09710001,
+		0xC78, 0x08720001,
+		0xC78, 0x07730001,
+		0xC78, 0x06740001,
+		0xC78, 0x05750001,
+		0xC78, 0x04760001,
+		0xC78, 0x03770001,
+		0xC78, 0x02780001,
+		0xC78, 0x01790001,
+		0xC78, 0x017A0001,
+		0xC78, 0x017B0001,
+		0xC78, 0x017C0001,
+		0xC78, 0x017D0001,
+		0xC78, 0x017E0001,
+		0xC78, 0x017F0001,
+		0xC50, 0x69553422,
+		0xC50, 0x69553420,
+		0x824, 0x00390204,
+
+};
+
+void ODM_ReadAndConfig_MP_8723B_AGC_TAB(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_AGC_TAB)/sizeof(u32);
+	u32 *Array = Array_MP_8723B_AGC_TAB;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_AGC_TAB\n")
+	);
+
+	for (i = 0; i < ArrayLen; i += 2) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i+1];
+
+		/*  This (offset, data) pair doesn't care the condition. */
+		if (v1 < 0x40000000) {
+			odm_ConfigBB_AGC_8723B(pDM_Odm, v1, bMaskDWord, v2);
+			continue;
+		} else {
+			/*  This line is the beginning of branch. */
+			bool bMatched = true;
+			u8  cCond  = (u8)((v1 & (BIT29|BIT28)) >> 28);
+
+			if (cCond == COND_ELSE) { /*  ELSE, ENDIF */
+				bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			} else if (!CheckPositive(pDM_Odm, v1, v2)) {
+				bMatched = false;
+				READ_NEXT_PAIR(v1, v2, i);
+				READ_NEXT_PAIR(v1, v2, i);
+			} else {
+				READ_NEXT_PAIR(v1, v2, i);
+				if (!CheckNegative(pDM_Odm, v1, v2))
+					bMatched = false;
+				else
+					bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			}
+
+			if (bMatched == false) {
+				/*  Condition isn't matched.
+				*   Discard the following (offset, data) pairs.
+				*/
+				while (v1 < 0x40000000 && i < ArrayLen-2)
+				READ_NEXT_PAIR(v1, v2, i);
+
+				i -= 2; /*  prevent from for-loop += 2 */
+			} else {
+				/*  Configure matched pairs and skip to end of if-else. */
+				while (v1 < 0x40000000 && i < ArrayLen-2) {
+					odm_ConfigBB_AGC_8723B(pDM_Odm, v1, bMaskDWord, v2);
+					READ_NEXT_PAIR(v1, v2, i);
+				}
+
+				/*  Keeps reading until ENDIF. */
+				cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				while (cCond != COND_ENDIF && i < ArrayLen-2) {
+					READ_NEXT_PAIR(v1, v2, i);
+					cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				}
+			}
+		}
+	}
+}
+
+/******************************************************************************
+*                           PHY_REG.TXT
+******************************************************************************/
+
+static u32 Array_MP_8723B_PHY_REG[] = {
+		0x800, 0x80040000,
+		0x804, 0x00000003,
+		0x808, 0x0000FC00,
+		0x80C, 0x0000000A,
+		0x810, 0x10001331,
+		0x814, 0x020C3D10,
+		0x818, 0x02200385,
+		0x81C, 0x00000000,
+		0x820, 0x01000100,
+		0x824, 0x00190204,
+		0x828, 0x00000000,
+		0x82C, 0x00000000,
+		0x830, 0x00000000,
+		0x834, 0x00000000,
+		0x838, 0x00000000,
+		0x83C, 0x00000000,
+		0x840, 0x00010000,
+		0x844, 0x00000000,
+		0x848, 0x00000000,
+		0x84C, 0x00000000,
+		0x850, 0x00000000,
+		0x854, 0x00000000,
+		0x858, 0x569A11A9,
+		0x85C, 0x01000014,
+		0x860, 0x66F60110,
+		0x864, 0x061F0649,
+		0x868, 0x00000000,
+		0x86C, 0x27272700,
+		0x870, 0x07000760,
+		0x874, 0x25004000,
+		0x878, 0x00000808,
+		0x87C, 0x00000000,
+		0x880, 0xB0000C1C,
+		0x884, 0x00000001,
+		0x888, 0x00000000,
+		0x88C, 0xCCC000C0,
+		0x890, 0x00000800,
+		0x894, 0xFFFFFFFE,
+		0x898, 0x40302010,
+		0x89C, 0x00706050,
+		0x900, 0x00000000,
+		0x904, 0x00000023,
+		0x908, 0x00000000,
+		0x90C, 0x81121111,
+		0x910, 0x00000002,
+		0x914, 0x00000201,
+		0xA00, 0x00D047C8,
+		0xA04, 0x80FF800C,
+		0xA08, 0x8C838300,
+		0xA0C, 0x2E7F120F,
+		0xA10, 0x9500BB78,
+		0xA14, 0x1114D028,
+		0xA18, 0x00881117,
+		0xA1C, 0x89140F00,
+		0xA20, 0x1A1B0000,
+		0xA24, 0x090E1317,
+		0xA28, 0x00000204,
+		0xA2C, 0x00D30000,
+		0xA70, 0x101FBF00,
+		0xA74, 0x00000007,
+		0xA78, 0x00000900,
+		0xA7C, 0x225B0606,
+		0xA80, 0x21806490,
+		0xB2C, 0x00000000,
+		0xC00, 0x48071D40,
+		0xC04, 0x03A05611,
+		0xC08, 0x000000E4,
+		0xC0C, 0x6C6C6C6C,
+		0xC10, 0x08800000,
+		0xC14, 0x40000100,
+		0xC18, 0x08800000,
+		0xC1C, 0x40000100,
+		0xC20, 0x00000000,
+		0xC24, 0x00000000,
+		0xC28, 0x00000000,
+		0xC2C, 0x00000000,
+		0xC30, 0x69E9AC44,
+		0xC34, 0x469652AF,
+		0xC38, 0x49795994,
+		0xC3C, 0x0A97971C,
+		0xC40, 0x1F7C403F,
+		0xC44, 0x000100B7,
+		0xC48, 0xEC020107,
+		0xC4C, 0x007F037F,
+		0xC50, 0x69553420,
+		0xC54, 0x43BC0094,
+		0xC58, 0x00013149,
+		0xC5C, 0x00250492,
+		0xC60, 0x00000000,
+		0xC64, 0x7112848B,
+		0xC68, 0x47C00BFF,
+		0xC6C, 0x00000036,
+		0xC70, 0x2C7F000D,
+		0xC74, 0x020610DB,
+		0xC78, 0x0000001F,
+		0xC7C, 0x00B91612,
+		0xC80, 0x390000E4,
+		0xC84, 0x20F60000,
+		0xC88, 0x40000100,
+		0xC8C, 0x20200000,
+		0xC90, 0x00020E1A,
+		0xC94, 0x00000000,
+		0xC98, 0x00020E1A,
+		0xC9C, 0x00007F7F,
+		0xCA0, 0x00000000,
+		0xCA4, 0x000300A0,
+		0xCA8, 0x00000000,
+		0xCAC, 0x00000000,
+		0xCB0, 0x00000000,
+		0xCB4, 0x00000000,
+		0xCB8, 0x00000000,
+		0xCBC, 0x28000000,
+		0xCC0, 0x00000000,
+		0xCC4, 0x00000000,
+		0xCC8, 0x00000000,
+		0xCCC, 0x00000000,
+		0xCD0, 0x00000000,
+		0xCD4, 0x00000000,
+		0xCD8, 0x64B22427,
+		0xCDC, 0x00766932,
+		0xCE0, 0x00222222,
+		0xCE4, 0x00000000,
+		0xCE8, 0x37644302,
+		0xCEC, 0x2F97D40C,
+		0xD00, 0x00000740,
+		0xD04, 0x40020401,
+		0xD08, 0x0000907F,
+		0xD0C, 0x20010201,
+		0xD10, 0xA0633333,
+		0xD14, 0x3333BC53,
+		0xD18, 0x7A8F5B6F,
+		0xD2C, 0xCC979975,
+		0xD30, 0x00000000,
+		0xD34, 0x80608000,
+		0xD38, 0x00000000,
+		0xD3C, 0x00127353,
+		0xD40, 0x00000000,
+		0xD44, 0x00000000,
+		0xD48, 0x00000000,
+		0xD4C, 0x00000000,
+		0xD50, 0x6437140A,
+		0xD54, 0x00000000,
+		0xD58, 0x00000282,
+		0xD5C, 0x30032064,
+		0xD60, 0x4653DE68,
+		0xD64, 0x04518A3C,
+		0xD68, 0x00002101,
+		0xD6C, 0x2A201C16,
+		0xD70, 0x1812362E,
+		0xD74, 0x322C2220,
+		0xD78, 0x000E3C24,
+		0xE00, 0x2D2D2D2D,
+		0xE04, 0x2D2D2D2D,
+		0xE08, 0x0390272D,
+		0xE10, 0x2D2D2D2D,
+		0xE14, 0x2D2D2D2D,
+		0xE18, 0x2D2D2D2D,
+		0xE1C, 0x2D2D2D2D,
+		0xE28, 0x00000000,
+		0xE30, 0x1000DC1F,
+		0xE34, 0x10008C1F,
+		0xE38, 0x02140102,
+		0xE3C, 0x681604C2,
+		0xE40, 0x01007C00,
+		0xE44, 0x01004800,
+		0xE48, 0xFB000000,
+		0xE4C, 0x000028D1,
+		0xE50, 0x1000DC1F,
+		0xE54, 0x10008C1F,
+		0xE58, 0x02140102,
+		0xE5C, 0x28160D05,
+		0xE60, 0x00000008,
+		0xE68, 0x001B2556,
+		0xE6C, 0x00C00096,
+		0xE70, 0x00C00096,
+		0xE74, 0x01000056,
+		0xE78, 0x01000014,
+		0xE7C, 0x01000056,
+		0xE80, 0x01000014,
+		0xE84, 0x00C00096,
+		0xE88, 0x01000056,
+		0xE8C, 0x00C00096,
+		0xED0, 0x00C00096,
+		0xED4, 0x00C00096,
+		0xED8, 0x00C00096,
+		0xEDC, 0x000000D6,
+		0xEE0, 0x000000D6,
+		0xEEC, 0x01C00016,
+		0xF14, 0x00000003,
+		0xF4C, 0x00000000,
+		0xF00, 0x00000300,
+		0x820, 0x01000100,
+		0x800, 0x83040000,
+
+};
+
+void ODM_ReadAndConfig_MP_8723B_PHY_REG(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_PHY_REG)/sizeof(u32);
+	u32 *Array = Array_MP_8723B_PHY_REG;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_PHY_REG\n")
+	);
+
+	for (i = 0; i < ArrayLen; i += 2) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i+1];
+
+		/*  This (offset, data) pair doesn't care the condition. */
+		if (v1 < 0x40000000) {
+			odm_ConfigBB_PHY_8723B(pDM_Odm, v1, bMaskDWord, v2);
+			continue;
+		} else {
+			/*  This line is the beginning of branch. */
+			bool bMatched = true;
+			u8  cCond  = (u8)((v1 & (BIT29|BIT28)) >> 28);
+
+			if (cCond == COND_ELSE) { /*  ELSE, ENDIF */
+				bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			} else if (!CheckPositive(pDM_Odm, v1, v2)) {
+				bMatched = false;
+				READ_NEXT_PAIR(v1, v2, i);
+				READ_NEXT_PAIR(v1, v2, i);
+			} else {
+				READ_NEXT_PAIR(v1, v2, i);
+				if (!CheckNegative(pDM_Odm, v1, v2))
+					bMatched = false;
+				else
+					bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			}
+
+			if (bMatched == false) {
+				/*  Condition isn't matched.
+				*   Discard the following (offset, data) pairs.
+				*/
+				while (v1 < 0x40000000 && i < ArrayLen-2)
+				READ_NEXT_PAIR(v1, v2, i);
+
+					i -= 2; /*  prevent from for-loop += 2 */
+			} else { /*  Configure matched pairs and skip to end of if-else. */
+				while (v1 < 0x40000000 && i < ArrayLen-2) {
+					odm_ConfigBB_PHY_8723B(pDM_Odm, v1, bMaskDWord, v2);
+					READ_NEXT_PAIR(v1, v2, i);
+				}
+
+				/*  Keeps reading until ENDIF. */
+				cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				while (cCond != COND_ENDIF && i < ArrayLen-2) {
+					READ_NEXT_PAIR(v1, v2, i);
+					cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				}
+			}
+		}
+	}
+}
+
+/******************************************************************************
+*                           PHY_REG_PG.TXT
+******************************************************************************/
+
+static u32 Array_MP_8723B_PHY_REG_PG[] = {
+	0, 0, 0, 0x00000e08, 0x0000ff00, 0x00003800,
+	0, 0, 0, 0x0000086c, 0xffffff00, 0x32343600,
+	0, 0, 0, 0x00000e00, 0xffffffff, 0x40424444,
+	0, 0, 0, 0x00000e04, 0xffffffff, 0x28323638,
+	0, 0, 0, 0x00000e10, 0xffffffff, 0x38404244,
+	0, 0, 0, 0x00000e14, 0xffffffff, 0x26303436
+};
+
+void ODM_ReadAndConfig_MP_8723B_PHY_REG_PG(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_PHY_REG_PG)/sizeof(u32);
+	u32 *Array = Array_MP_8723B_PHY_REG_PG;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_PHY_REG_PG\n")
+	);
+
+	pDM_Odm->PhyRegPgVersion = 1;
+	pDM_Odm->PhyRegPgValueType = PHY_REG_PG_EXACT_VALUE;
+
+	for (i = 0; i < ArrayLen; i += 6) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i+1];
+		u32 v3 = Array[i+2];
+		u32 v4 = Array[i+3];
+		u32 v5 = Array[i+4];
+		u32 v6 = Array[i+5];
+
+		odm_ConfigBB_PHY_REG_PG_8723B(pDM_Odm, v1, v2, v3, v4, v5, v6);
+	}
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_BB.h linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_BB.h
--- linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_BB.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_BB.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,48 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License as
+* published by the Free Software Foundation.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+******************************************************************************/
+
+#ifndef __INC_MP_BB_HW_IMG_8723B_H
+#define __INC_MP_BB_HW_IMG_8723B_H
+
+
+/******************************************************************************
+*                           AGC_TAB.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_AGC_TAB(/*  TC: Test Chip, MP: MP Chip */
+	PDM_ODM_T  pDM_Odm
+);
+
+/******************************************************************************
+*                           PHY_REG.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_PHY_REG(/*  TC: Test Chip, MP: MP Chip */
+	PDM_ODM_T  pDM_Odm
+);
+
+/******************************************************************************
+*                           PHY_REG_PG.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_PHY_REG_PG(/*  TC: Test Chip, MP: MP Chip */
+	PDM_ODM_T  pDM_Odm
+);
+u32 ODM_GetVersion_MP_8723B_PHY_REG_PG(void);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_MAC.c linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_MAC.c
--- linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_MAC.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_MAC.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,302 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License as
+* published by the Free Software Foundation.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+******************************************************************************/
+
+
+#include "odm_precomp.h"
+
+static bool CheckPositive(
+	PDM_ODM_T pDM_Odm, const u32 Condition1, const u32 Condition2
+)
+{
+	u8 _BoardType =
+		((pDM_Odm->BoardType & BIT4) >> 4) << 0 | /*  _GLNA */
+		((pDM_Odm->BoardType & BIT3) >> 3) << 1 | /*  _GPA */
+		((pDM_Odm->BoardType & BIT7) >> 7) << 2 | /*  _ALNA */
+		((pDM_Odm->BoardType & BIT6) >> 6) << 3 | /*  _APA */
+		((pDM_Odm->BoardType & BIT2) >> 2) << 4;  /*  _BT */
+
+	u32   cond1   = Condition1, cond2 = Condition2;
+	u32    driver1 =
+		pDM_Odm->CutVersion       << 24 |
+		pDM_Odm->SupportPlatform  << 16 |
+		pDM_Odm->PackageType      << 12 |
+		pDM_Odm->SupportInterface << 8  |
+		_BoardType;
+
+	u32 driver2 =
+		pDM_Odm->TypeGLNA <<  0 |
+		pDM_Odm->TypeGPA  <<  8 |
+		pDM_Odm->TypeALNA << 16 |
+		pDM_Odm->TypeAPA  << 24;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (cond1, cond2) = (0x%X 0x%X)\n",
+			cond1,
+			cond2
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (driver1, driver2) = (0x%X 0x%X)\n",
+			driver1,
+			driver2
+		)
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"	(Platform, Interface) = (0x%X, 0x%X)\n",
+			pDM_Odm->SupportPlatform,
+			pDM_Odm->SupportInterface
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"	(Board, Package) = (0x%X, 0x%X)\n",
+			pDM_Odm->BoardType,
+			pDM_Odm->PackageType
+		)
+	);
+
+
+	/*  Value Defined Check =============== */
+	/* QFN Type [15:12] and Cut Version [27:24] need to do value check */
+
+	if (((cond1 & 0x0000F000) != 0) && ((cond1 & 0x0000F000) != (driver1 & 0x0000F000)))
+		return false;
+	if (((cond1 & 0x0F000000) != 0) && ((cond1 & 0x0F000000) != (driver1 & 0x0F000000)))
+		return false;
+
+	/*  Bit Defined Check ================ */
+	/*  We don't care [31:28] and [23:20] */
+	/*  */
+	cond1   &= 0x000F0FFF;
+	driver1 &= 0x000F0FFF;
+
+	if ((cond1 & driver1) == cond1) {
+		u32 bitMask = 0;
+		if ((cond1 & 0x0F) == 0) /*  BoardType is DONTCARE */
+			return true;
+
+		if ((cond1 & BIT0) != 0) /* GLNA */
+			bitMask |= 0x000000FF;
+		if ((cond1 & BIT1) != 0) /* GPA */
+			bitMask |= 0x0000FF00;
+		if ((cond1 & BIT2) != 0) /* ALNA */
+			bitMask |= 0x00FF0000;
+		if ((cond1 & BIT3) != 0) /* APA */
+			bitMask |= 0xFF000000;
+
+		if ((cond2 & bitMask) == (driver2 & bitMask)) /*  BoardType of each RF path is matched */
+			return true;
+	}
+	return false;
+}
+
+static bool CheckNegative(
+	PDM_ODM_T pDM_Odm, const u32 Condition1, const u32 Condition2
+)
+{
+	return true;
+}
+
+/******************************************************************************
+*                           MAC_REG.TXT
+******************************************************************************/
+
+static u32 Array_MP_8723B_MAC_REG[] = {
+		0x02F, 0x00000030,
+		0x035, 0x00000000,
+		0x039, 0x00000008,
+		0x04E, 0x000000E0,
+		0x064, 0x00000000,
+		0x067, 0x00000020,
+		0x428, 0x0000000A,
+		0x429, 0x00000010,
+		0x430, 0x00000000,
+		0x431, 0x00000000,
+		0x432, 0x00000000,
+		0x433, 0x00000001,
+		0x434, 0x00000004,
+		0x435, 0x00000005,
+		0x436, 0x00000007,
+		0x437, 0x00000008,
+		0x43C, 0x00000004,
+		0x43D, 0x00000005,
+		0x43E, 0x00000007,
+		0x43F, 0x00000008,
+		0x440, 0x0000005D,
+		0x441, 0x00000001,
+		0x442, 0x00000000,
+		0x444, 0x00000010,
+		0x445, 0x00000000,
+		0x446, 0x00000000,
+		0x447, 0x00000000,
+		0x448, 0x00000000,
+		0x449, 0x000000F0,
+		0x44A, 0x0000000F,
+		0x44B, 0x0000003E,
+		0x44C, 0x00000010,
+		0x44D, 0x00000000,
+		0x44E, 0x00000000,
+		0x44F, 0x00000000,
+		0x450, 0x00000000,
+		0x451, 0x000000F0,
+		0x452, 0x0000000F,
+		0x453, 0x00000000,
+		0x456, 0x0000005E,
+		0x460, 0x00000066,
+		0x461, 0x00000066,
+		0x4C8, 0x000000FF,
+		0x4C9, 0x00000008,
+		0x4CC, 0x000000FF,
+		0x4CD, 0x000000FF,
+		0x4CE, 0x00000001,
+		0x500, 0x00000026,
+		0x501, 0x000000A2,
+		0x502, 0x0000002F,
+		0x503, 0x00000000,
+		0x504, 0x00000028,
+		0x505, 0x000000A3,
+		0x506, 0x0000005E,
+		0x507, 0x00000000,
+		0x508, 0x0000002B,
+		0x509, 0x000000A4,
+		0x50A, 0x0000005E,
+		0x50B, 0x00000000,
+		0x50C, 0x0000004F,
+		0x50D, 0x000000A4,
+		0x50E, 0x00000000,
+		0x50F, 0x00000000,
+		0x512, 0x0000001C,
+		0x514, 0x0000000A,
+		0x516, 0x0000000A,
+		0x525, 0x0000004F,
+		0x550, 0x00000010,
+		0x551, 0x00000010,
+		0x559, 0x00000002,
+		0x55C, 0x00000050,
+		0x55D, 0x000000FF,
+		0x605, 0x00000030,
+		0x608, 0x0000000E,
+		0x609, 0x0000002A,
+		0x620, 0x000000FF,
+		0x621, 0x000000FF,
+		0x622, 0x000000FF,
+		0x623, 0x000000FF,
+		0x624, 0x000000FF,
+		0x625, 0x000000FF,
+		0x626, 0x000000FF,
+		0x627, 0x000000FF,
+		0x638, 0x00000050,
+		0x63C, 0x0000000A,
+		0x63D, 0x0000000A,
+		0x63E, 0x0000000E,
+		0x63F, 0x0000000E,
+		0x640, 0x00000040,
+		0x642, 0x00000040,
+		0x643, 0x00000000,
+		0x652, 0x000000C8,
+		0x66E, 0x00000005,
+		0x700, 0x00000021,
+		0x701, 0x00000043,
+		0x702, 0x00000065,
+		0x703, 0x00000087,
+		0x708, 0x00000021,
+		0x709, 0x00000043,
+		0x70A, 0x00000065,
+		0x70B, 0x00000087,
+		0x765, 0x00000018,
+		0x76E, 0x00000004,
+
+};
+
+void ODM_ReadAndConfig_MP_8723B_MAC_REG(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_MAC_REG)/sizeof(u32);
+	u32 *Array = Array_MP_8723B_MAC_REG;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_MAC_REG\n")
+	);
+
+	for (i = 0; i < ArrayLen; i += 2) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i+1];
+
+		/*  This (offset, data) pair doesn't care the condition. */
+		if (v1 < 0x40000000) {
+			odm_ConfigMAC_8723B(pDM_Odm, v1, (u8)v2);
+			continue;
+		} else {
+			/*  This line is the beginning of branch. */
+			bool bMatched = true;
+			u8  cCond  = (u8)((v1 & (BIT29|BIT28)) >> 28);
+
+			if (cCond == COND_ELSE) { /*  ELSE, ENDIF */
+				bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			} else if (!CheckPositive(pDM_Odm, v1, v2)) {
+				bMatched = false;
+				READ_NEXT_PAIR(v1, v2, i);
+				READ_NEXT_PAIR(v1, v2, i);
+			} else {
+				READ_NEXT_PAIR(v1, v2, i);
+				if (!CheckNegative(pDM_Odm, v1, v2))
+					bMatched = false;
+				else
+					bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			}
+
+			if (bMatched == false) {
+				/*  Condition isn't matched. Discard the following (offset, data) pairs. */
+				while (v1 < 0x40000000 && i < ArrayLen-2)
+					READ_NEXT_PAIR(v1, v2, i);
+
+				i -= 2; /*  prevent from for-loop += 2 */
+			} else { /*  Configure matched pairs and skip to end of if-else. */
+				while (v1 < 0x40000000 && i < ArrayLen-2) {
+					odm_ConfigMAC_8723B(pDM_Odm, v1, (u8)v2);
+					READ_NEXT_PAIR(v1, v2, i);
+				}
+
+				/*  Keeps reading until ENDIF. */
+				cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				while (cCond != COND_ENDIF && i < ArrayLen-2) {
+					READ_NEXT_PAIR(v1, v2, i);
+					cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				}
+			}
+		}
+	}
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_MAC.h linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_MAC.h
--- linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_MAC.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_MAC.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,28 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License as
+* published by the Free Software Foundation.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+******************************************************************************/
+
+#ifndef __INC_MP_MAC_HW_IMG_8723B_H
+#define __INC_MP_MAC_HW_IMG_8723B_H
+
+
+/******************************************************************************
+*                           MAC_REG.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_MAC_REG(/*  TC: Test Chip, MP: MP Chip */
+	PDM_ODM_T  pDM_Odm
+);
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_RF.c linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_RF.c
--- linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_RF.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_RF.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,799 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License as
+* published by the Free Software Foundation.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+******************************************************************************/
+
+
+#include "odm_precomp.h"
+
+static bool CheckPositive(
+	PDM_ODM_T pDM_Odm, const u32 Condition1, const u32 Condition2
+)
+{
+	u8 _BoardType =
+			((pDM_Odm->BoardType & BIT4) >> 4) << 0 | /*  _GLNA */
+			((pDM_Odm->BoardType & BIT3) >> 3) << 1 | /*  _GPA */
+			((pDM_Odm->BoardType & BIT7) >> 7) << 2 | /*  _ALNA */
+			((pDM_Odm->BoardType & BIT6) >> 6) << 3 | /*  _APA */
+			((pDM_Odm->BoardType & BIT2) >> 2) << 4;  /*  _BT */
+
+	u32 cond1 = Condition1, cond2 = Condition2;
+	u32 driver1 =
+		pDM_Odm->CutVersion << 24 |
+		pDM_Odm->SupportPlatform << 16 |
+		pDM_Odm->PackageType << 12 |
+		pDM_Odm->SupportInterface << 8 |
+		_BoardType;
+
+	u32 driver2 =
+		pDM_Odm->TypeGLNA <<  0 |
+		pDM_Odm->TypeGPA  <<  8 |
+		pDM_Odm->TypeALNA << 16 |
+		pDM_Odm->TypeAPA  << 24;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (cond1, cond2) = (0x%X 0x%X)\n",
+			cond1,
+			cond2
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (driver1, driver2) = (0x%X 0x%X)\n",
+			driver1,
+			driver2
+		)
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"	(Platform, Interface) = (0x%X, 0x%X)\n",
+			pDM_Odm->SupportPlatform,
+			pDM_Odm->SupportInterface
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"	(Board, Package) = (0x%X, 0x%X)\n",
+			pDM_Odm->BoardType,
+			pDM_Odm->PackageType
+		)
+	);
+
+	/*  Value Defined Check =============== */
+	/* QFN Type [15:12] and Cut Version [27:24] need to do value check */
+
+	if (
+		((cond1 & 0x0000F000) != 0) &&
+		((cond1 & 0x0000F000) != (driver1 & 0x0000F000))
+	)
+		return false;
+
+	if (
+		((cond1 & 0x0F000000) != 0) &&
+		((cond1 & 0x0F000000) != (driver1 & 0x0F000000))
+	)
+		return false;
+
+	/*  Bit Defined Check ================ */
+	/*  We don't care [31:28] and [23:20] */
+	cond1   &= 0x000F0FFF;
+	driver1 &= 0x000F0FFF;
+
+	if ((cond1 & driver1) == cond1) {
+		u32 bitMask = 0;
+
+		if ((cond1 & 0x0F) == 0) /*  BoardType is DONTCARE */
+			return true;
+
+		if ((cond1 & BIT0) != 0) /* GLNA */
+			bitMask |= 0x000000FF;
+		if ((cond1 & BIT1) != 0) /* GPA */
+			bitMask |= 0x0000FF00;
+		if ((cond1 & BIT2) != 0) /* ALNA */
+			bitMask |= 0x00FF0000;
+		if ((cond1 & BIT3) != 0) /* APA */
+			bitMask |= 0xFF000000;
+
+		/*  BoardType of each RF path is matched */
+		if ((cond2 & bitMask) == (driver2 & bitMask))
+			return true;
+
+		return false;
+	}
+
+	return false;
+}
+
+static bool CheckNegative(
+	PDM_ODM_T pDM_Odm, const u32  Condition1, const u32 Condition2
+)
+{
+	return true;
+}
+
+/******************************************************************************
+*                           RadioA.TXT
+******************************************************************************/
+
+static u32 Array_MP_8723B_RadioA[] = {
+		0x000, 0x00010000,
+		0x0B0, 0x000DFFE0,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0B1, 0x00000018,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0B2, 0x00084C00,
+		0x0B5, 0x0000D2CC,
+		0x0B6, 0x000925AA,
+		0x0B7, 0x00000010,
+		0x0B8, 0x0000907F,
+		0x05C, 0x00000002,
+		0x07C, 0x00000002,
+		0x07E, 0x00000005,
+		0x08B, 0x0006FC00,
+		0x0B0, 0x000FF9F0,
+		0x01C, 0x000739D2,
+		0x01E, 0x00000000,
+		0x0DF, 0x00000780,
+		0x050, 0x00067435,
+	0x80002000, 0x00000000, 0x40000000, 0x00000000,
+		0x051, 0x0006B10E,
+	0x90003000, 0x00000000, 0x40000000, 0x00000000,
+		0x051, 0x0006B10E,
+	0x90004000, 0x00000000, 0x40000000, 0x00000000,
+		0x051, 0x0006B10E,
+	0xA0000000, 0x00000000,
+		0x051, 0x0006B04E,
+	0xB0000000, 0x00000000,
+		0x052, 0x000007D2,
+		0x053, 0x00000000,
+		0x054, 0x00050400,
+		0x055, 0x0004026E,
+		0x0DD, 0x0000004C,
+		0x070, 0x00067435,
+	0x80002000, 0x00000000, 0x40000000, 0x00000000,
+		0x071, 0x0006B10E,
+	0x90003000, 0x00000000, 0x40000000, 0x00000000,
+		0x071, 0x0006B10E,
+	0x90004000, 0x00000000, 0x40000000, 0x00000000,
+		0x071, 0x0006B10E,
+	0xA0000000, 0x00000000,
+		0x071, 0x0006B04E,
+	0xB0000000, 0x00000000,
+		0x072, 0x000007D2,
+		0x073, 0x00000000,
+		0x074, 0x00050400,
+		0x075, 0x0004026E,
+		0x0EF, 0x00000100,
+		0x034, 0x0000ADD7,
+		0x035, 0x00005C00,
+		0x034, 0x00009DD4,
+		0x035, 0x00005000,
+		0x034, 0x00008DD1,
+		0x035, 0x00004400,
+		0x034, 0x00007DCE,
+		0x035, 0x00003800,
+		0x034, 0x00006CD1,
+		0x035, 0x00004400,
+		0x034, 0x00005CCE,
+		0x035, 0x00003800,
+		0x034, 0x000048CE,
+		0x035, 0x00004400,
+		0x034, 0x000034CE,
+		0x035, 0x00003800,
+		0x034, 0x00002451,
+		0x035, 0x00004400,
+		0x034, 0x0000144E,
+		0x035, 0x00003800,
+		0x034, 0x00000051,
+		0x035, 0x00004400,
+		0x0EF, 0x00000000,
+		0x0EF, 0x00000100,
+		0x0ED, 0x00000010,
+		0x044, 0x0000ADD7,
+		0x044, 0x00009DD4,
+		0x044, 0x00008DD1,
+		0x044, 0x00007DCE,
+		0x044, 0x00006CC1,
+		0x044, 0x00005CCE,
+		0x044, 0x000044D1,
+		0x044, 0x000034CE,
+		0x044, 0x00002451,
+		0x044, 0x0000144E,
+		0x044, 0x00000051,
+		0x0EF, 0x00000000,
+		0x0ED, 0x00000000,
+		0x07F, 0x00020080,
+		0x0EF, 0x00002000,
+		0x03B, 0x000380EF,
+		0x03B, 0x000302FE,
+		0x03B, 0x00028CE6,
+		0x03B, 0x000200BC,
+		0x03B, 0x000188A5,
+		0x03B, 0x00010FBC,
+		0x03B, 0x00008F71,
+		0x03B, 0x00000900,
+		0x0EF, 0x00000000,
+		0x0ED, 0x00000001,
+		0x040, 0x000380EF,
+		0x040, 0x000302FE,
+		0x040, 0x00028CE6,
+		0x040, 0x000200BC,
+		0x040, 0x000188A5,
+		0x040, 0x00010FBC,
+		0x040, 0x00008F71,
+		0x040, 0x00000900,
+		0x0ED, 0x00000000,
+		0x082, 0x00080000,
+		0x083, 0x00008000,
+		0x084, 0x00048D80,
+		0x085, 0x00068000,
+		0x0A2, 0x00080000,
+		0x0A3, 0x00008000,
+		0x0A4, 0x00048D80,
+		0x0A5, 0x00068000,
+		0x0ED, 0x00000002,
+		0x0EF, 0x00000002,
+		0x056, 0x00000032,
+		0x076, 0x00000032,
+		0x001, 0x00000780,
+
+};
+
+void ODM_ReadAndConfig_MP_8723B_RadioA(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_RadioA)/sizeof(u32);
+	u32 *Array = Array_MP_8723B_RadioA;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_RadioA\n")
+	);
+
+	for (i = 0; i < ArrayLen; i += 2) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i+1];
+
+		/*  This (offset, data) pair doesn't care the condition. */
+		if (v1 < 0x40000000) {
+			odm_ConfigRF_RadioA_8723B(pDM_Odm, v1, v2);
+			continue;
+		} else {
+			/*  This line is the beginning of branch. */
+			bool bMatched = true;
+			u8  cCond  = (u8)((v1 & (BIT29|BIT28)) >> 28);
+
+			if (cCond == COND_ELSE) { /*  ELSE, ENDIF */
+				bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			} else if (!CheckPositive(pDM_Odm, v1, v2)) {
+				bMatched = false;
+				READ_NEXT_PAIR(v1, v2, i);
+				READ_NEXT_PAIR(v1, v2, i);
+			} else {
+				READ_NEXT_PAIR(v1, v2, i);
+				if (!CheckNegative(pDM_Odm, v1, v2))
+					bMatched = false;
+				else
+					bMatched = true;
+				READ_NEXT_PAIR(v1, v2, i);
+			}
+
+			if (bMatched == false) {
+				/*  Condition isn't matched.
+				*   Discard the following (offset, data) pairs.
+				*/
+				while (v1 < 0x40000000 && i < ArrayLen-2)
+					READ_NEXT_PAIR(v1, v2, i);
+
+				i -= 2; /*  prevent from for-loop += 2 */
+			} else {
+				/*  Configure matched pairs and skip to end of if-else. */
+				while (v1 < 0x40000000 && i < ArrayLen-2) {
+					odm_ConfigRF_RadioA_8723B(pDM_Odm, v1, v2);
+					READ_NEXT_PAIR(v1, v2, i);
+				}
+
+				/*  Keeps reading until ENDIF. */
+				cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				while (cCond != COND_ENDIF && i < ArrayLen-2) {
+					READ_NEXT_PAIR(v1, v2, i);
+					cCond = (u8)((v1 & (BIT29|BIT28)) >> 28);
+				}
+			}
+		}
+	}
+}
+
+/******************************************************************************
+*                           TxPowerTrack_SDIO.TXT
+******************************************************************************/
+
+static u8 gDeltaSwingTableIdx_MP_5GB_N_TxPowerTrack_SDIO_8723B[][DELTA_SWINGIDX_SIZE] = {
+	{
+		0, 1, 1, 2, 2, 3, 4, 5, 5, 6,  6,  7,  7,  8,  8,  9,
+		9, 10, 11, 12, 12, 13, 13, 14, 14, 14, 14, 14, 14, 14
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  7,  8,  8,  9,  9, 10,
+		10, 11, 11, 12, 12, 13, 13, 14, 14, 14, 14, 14, 14, 14
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  7,  8,  8,  9,  9, 10,
+		10, 11, 11, 12, 12, 13, 13, 14, 14, 14, 14, 14, 14, 14
+	},
+};
+static u8 gDeltaSwingTableIdx_MP_5GB_P_TxPowerTrack_SDIO_8723B[][DELTA_SWINGIDX_SIZE] = {
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 11, 12,
+		12, 13, 14, 15, 15, 16, 16, 17, 17, 18, 19, 20, 20, 20
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 11, 12,
+		12, 13, 14, 15, 15, 16, 17, 18, 18, 19, 19, 20, 20, 20
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 11, 12,
+		12, 13, 14, 15, 15, 16, 17, 18, 18, 19, 20, 21, 21, 21
+	},
+};
+static u8 gDeltaSwingTableIdx_MP_5GA_N_TxPowerTrack_SDIO_8723B[][DELTA_SWINGIDX_SIZE] = {
+	{
+		0, 1, 2, 3, 3, 4, 4, 5, 5, 6,  7,  8,  8,  9,  9, 10,
+		10, 11, 11, 12, 12, 13, 13, 14, 14, 14, 14, 14, 14, 14
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 6,  7,  7,  8,  8,  9, 10,
+		11, 11, 12, 13, 13, 14, 15, 16, 16, 16, 16, 16, 16, 16
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 10, 11,
+		11, 12, 13, 14, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16
+	},
+};
+static u8 gDeltaSwingTableIdx_MP_5GA_P_TxPowerTrack_SDIO_8723B[][DELTA_SWINGIDX_SIZE] = {
+	{
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 11, 12,
+		12, 13, 14, 15, 15, 16, 17, 18, 18, 19, 20, 21, 21, 21
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 11, 12,
+		12, 13, 14, 15, 15, 16, 17, 18, 18, 19, 20, 21, 21, 21
+	},
+};
+static u8 gDeltaSwingTableIdx_MP_2GB_N_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,  5,  6,  6, 6,  6,
+	7,  7,  7, 8,  8,  9,  9, 10, 10, 11, 12, 13, 14, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GB_P_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 3, 3, 4, 5, 5,  6,  6,  7,  7,  8,  8,
+	9,  9, 10, 10, 10, 11, 11, 12, 12, 13, 13, 14, 15, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GA_N_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,  5,  6,  6,  6,  6,
+	7,  7,  7,  8,  8,  9,  9, 10, 10, 11, 12, 13, 14, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GA_P_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 3, 3, 4, 5, 5,  6,  6,  7,  7,  8,  8,
+	9,  9, 10, 10, 10, 11, 11, 12, 12, 13, 13, 14, 15, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GCCKB_N_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 3, 3, 4, 4, 5,  6,  6,  7,  7,  7,  8,
+	8,  8,  9,  9,  9, 10, 10, 11, 11, 12, 12, 13, 14, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GCCKB_P_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,  5,  6,  6,  7,  7,
+	8,  8,  9,  9,  9, 10, 10, 11, 11, 12, 12, 13, 14, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GCCKA_N_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 3, 3, 4, 4, 5,  6,  6,  7,  7,  7,  8,
+	8,  8,  9,  9,  9, 10, 10, 11, 11, 12, 12, 13, 14, 15
+};
+static u8 gDeltaSwingTableIdx_MP_2GCCKA_P_TxPowerTrack_SDIO_8723B[] = {
+	0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,  5,  6,  6,  7,  7,
+	8,  8,  9,  9,  9, 10, 10, 11, 11, 12, 12, 13, 14, 15
+};
+
+void ODM_ReadAndConfig_MP_8723B_TxPowerTrack_SDIO(PDM_ODM_T pDM_Odm)
+{
+	PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_MP_8723B\n")
+	);
+
+
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GA_P,
+		gDeltaSwingTableIdx_MP_2GA_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GA_N,
+		gDeltaSwingTableIdx_MP_2GA_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GB_P,
+		gDeltaSwingTableIdx_MP_2GB_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GB_N,
+		gDeltaSwingTableIdx_MP_2GB_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_P,
+		gDeltaSwingTableIdx_MP_2GCCKA_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_N,
+		gDeltaSwingTableIdx_MP_2GCCKA_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_P,
+		gDeltaSwingTableIdx_MP_2GCCKB_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_N,
+		gDeltaSwingTableIdx_MP_2GCCKB_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE
+	);
+
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P,
+		gDeltaSwingTableIdx_MP_5GA_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE*3
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N,
+		gDeltaSwingTableIdx_MP_5GA_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE*3
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P,
+		gDeltaSwingTableIdx_MP_5GB_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE*3
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N,
+		gDeltaSwingTableIdx_MP_5GB_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE*3
+	);
+}
+
+/******************************************************************************
+*                           TXPWR_LMT.TXT
+******************************************************************************/
+
+static u8 *Array_MP_8723B_TXPWR_LMT[] = {
+	"FCC", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "CCK", "1T", "14", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "01", "28",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "02", "28",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "10", "28",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "11", "28",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"FCC", "2.4G", "20M", "HT", "1T", "01", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "02", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "03", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "04", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "05", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "06", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "07", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "08", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "09", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "10", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "11", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "HT", "1T", "14", "63",
+	"FCC", "2.4G", "20M", "HT", "2T", "01", "30",
+	"ETSI", "2.4G", "20M", "HT", "2T", "01", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "01", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "02", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "02", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "02", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "03", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "03", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "03", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "04", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "04", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "04", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "05", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "05", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "05", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "06", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "06", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "06", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "07", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "07", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "07", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "08", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "08", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "08", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "09", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "09", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "09", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "10", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "10", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "10", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "11", "30",
+	"ETSI", "2.4G", "20M", "HT", "2T", "11", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "11", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "12", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "12", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "12", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "13", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "13", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "13", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "14", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "14", "63",
+	"MKK", "2.4G", "20M", "HT", "2T", "14", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "01", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "01", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "01", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "02", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "02", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "02", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "03", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "03", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "03", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "04", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "04", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "04", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "05", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "05", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "05", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "06", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "06", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "06", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "07", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "07", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "07", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "08", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "08", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "08", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "09", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "09", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "09", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "10", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "10", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "10", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "11", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "11", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "11", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "12", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "12", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "12", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "13", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "13", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "13", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "14", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "14", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "14", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "01", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "01", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "01", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "02", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "02", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "02", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "03", "30",
+	"ETSI", "2.4G", "40M", "HT", "2T", "03", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "03", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "04", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "04", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "04", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "05", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "05", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "05", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "06", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "06", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "06", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "07", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "07", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "07", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "08", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "08", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "08", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "09", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "09", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "09", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "10", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "10", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "10", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "11", "30",
+	"ETSI", "2.4G", "40M", "HT", "2T", "11", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "11", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "12", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "12", "32",
+	"MKK", "2.4G", "40M", "HT", "2T", "12", "32",
+	"FCC", "2.4G", "40M", "HT", "2T", "13", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "13", "32",
+	"MKK", "2.4G", "40M", "HT", "2T", "13", "32",
+	"FCC", "2.4G", "40M", "HT", "2T", "14", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "14", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "14", "63"
+};
+
+void ODM_ReadAndConfig_MP_8723B_TXPWR_LMT(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_TXPWR_LMT)/sizeof(u8 *);
+	u8 **Array = Array_MP_8723B_TXPWR_LMT;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_TXPWR_LMT\n")
+	);
+
+	for (i = 0; i < ArrayLen; i += 7) {
+		u8 *regulation = Array[i];
+		u8 *band = Array[i+1];
+		u8 *bandwidth = Array[i+2];
+		u8 *rate = Array[i+3];
+		u8 *rfPath = Array[i+4];
+		u8 *chnl = Array[i+5];
+		u8 *val = Array[i+6];
+
+		odm_ConfigBB_TXPWR_LMT_8723B(
+			pDM_Odm,
+			regulation,
+			band,
+			bandwidth,
+			rate,
+			rfPath,
+			chnl,
+			val
+		);
+	}
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_RF.h linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_RF.h
--- linux.old/3rdparty/rtl8723bs/hal/HalHWImg8723B_RF.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalHWImg8723B_RF.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,49 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License as
+* published by the Free Software Foundation.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+******************************************************************************/
+
+#ifndef __INC_MP_RF_HW_IMG_8723B_H
+#define __INC_MP_RF_HW_IMG_8723B_H
+
+
+/******************************************************************************
+*                           RadioA.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_RadioA(/*  TC: Test Chip, MP: MP Chip */
+	PDM_ODM_T  pDM_Odm
+);
+
+/******************************************************************************
+*                           TxPowerTrack_SDIO.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_TxPowerTrack_SDIO(/*  TC: Test Chip, MP: MP Chip */
+	PDM_ODM_T  pDM_Odm
+);
+u32 ODM_GetVersion_MP_8723B_TxPowerTrack_SDIO(void);
+
+/******************************************************************************
+*                           TXPWR_LMT.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_TXPWR_LMT(/*  TC: Test Chip, MP: MP Chip */
+	PDM_ODM_T  pDM_Odm
+);
+u32 ODM_GetVersion_MP_8723B_TXPWR_LMT(void);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/hal_intf.c linux/3rdparty/rtl8723bs/hal/hal_intf.c
--- linux.old/3rdparty/rtl8723bs/hal/hal_intf.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/hal_intf.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,474 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+
+#define _HAL_INTF_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+
+void rtw_hal_chip_configure(struct adapter *padapter)
+{
+	if (padapter->HalFunc.intf_chip_configure)
+		padapter->HalFunc.intf_chip_configure(padapter);
+}
+
+void rtw_hal_read_chip_info(struct adapter *padapter)
+{
+	if (padapter->HalFunc.read_adapter_info)
+		padapter->HalFunc.read_adapter_info(padapter);
+}
+
+void rtw_hal_read_chip_version(struct adapter *padapter)
+{
+	if (padapter->HalFunc.read_chip_version)
+		padapter->HalFunc.read_chip_version(padapter);
+}
+
+void rtw_hal_def_value_init(struct adapter *padapter)
+{
+	if (is_primary_adapter(padapter))
+		if (padapter->HalFunc.init_default_value)
+			padapter->HalFunc.init_default_value(padapter);
+}
+
+void rtw_hal_free_data(struct adapter *padapter)
+{
+	/* free HAL Data */
+	rtw_hal_data_deinit(padapter);
+
+	if (is_primary_adapter(padapter))
+		if (padapter->HalFunc.free_hal_data)
+			padapter->HalFunc.free_hal_data(padapter);
+}
+
+void rtw_hal_dm_init(struct adapter *padapter)
+{
+	if (is_primary_adapter(padapter))
+		if (padapter->HalFunc.dm_init)
+			padapter->HalFunc.dm_init(padapter);
+}
+
+void rtw_hal_dm_deinit(struct adapter *padapter)
+{
+	/*  cancel dm  timer */
+	if (is_primary_adapter(padapter))
+		if (padapter->HalFunc.dm_deinit)
+			padapter->HalFunc.dm_deinit(padapter);
+}
+
+static void rtw_hal_init_opmode(struct adapter *padapter)
+{
+	enum NDIS_802_11_NETWORK_INFRASTRUCTURE networkType = Ndis802_11InfrastructureMax;
+	struct  mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	sint fw_state;
+
+	fw_state = get_fwstate(pmlmepriv);
+
+	if (fw_state & WIFI_ADHOC_STATE)
+		networkType = Ndis802_11IBSS;
+	else if (fw_state & WIFI_STATION_STATE)
+		networkType = Ndis802_11Infrastructure;
+	else if (fw_state & WIFI_AP_STATE)
+		networkType = Ndis802_11APMode;
+	else
+		return;
+
+	rtw_setopmode_cmd(padapter, networkType, false);
+}
+
+uint rtw_hal_init(struct adapter *padapter)
+{
+	uint status;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+	status = padapter->HalFunc.hal_init(padapter);
+
+	if (status == _SUCCESS) {
+		rtw_hal_init_opmode(padapter);
+
+		dvobj->padapters->hw_init_completed = true;
+
+		if (padapter->registrypriv.notch_filter == 1)
+			rtw_hal_notch_filter(padapter, 1);
+
+		rtw_hal_reset_security_engine(padapter);
+
+		rtw_sec_restore_wep_key(dvobj->padapters);
+
+		init_hw_mlme_ext(padapter);
+
+		rtw_bb_rf_gain_offset(padapter);
+	} else {
+		dvobj->padapters->hw_init_completed = false;
+		DBG_871X("rtw_hal_init: hal__init fail\n");
+	}
+
+	RT_TRACE(_module_hal_init_c_, _drv_err_, ("-rtl871x_hal_init:status = 0x%x\n", status));
+
+	return status;
+
+}
+
+uint rtw_hal_deinit(struct adapter *padapter)
+{
+	uint	status = _SUCCESS;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+
+	status = padapter->HalFunc.hal_deinit(padapter);
+
+	if (status == _SUCCESS) {
+		padapter = dvobj->padapters;
+		padapter->hw_init_completed = false;
+	} else {
+		DBG_871X("\n rtw_hal_deinit: hal_init fail\n");
+	}
+	return status;
+}
+
+void rtw_hal_set_hwreg(struct adapter *padapter, u8 variable, u8 *val)
+{
+	if (padapter->HalFunc.SetHwRegHandler)
+		padapter->HalFunc.SetHwRegHandler(padapter, variable, val);
+}
+
+void rtw_hal_get_hwreg(struct adapter *padapter, u8 variable, u8 *val)
+{
+	if (padapter->HalFunc.GetHwRegHandler)
+		padapter->HalFunc.GetHwRegHandler(padapter, variable, val);
+}
+
+void rtw_hal_set_hwreg_with_buf(struct adapter *padapter, u8 variable, u8 *pbuf, int len)
+{
+	if (padapter->HalFunc.SetHwRegHandlerWithBuf)
+		padapter->HalFunc.SetHwRegHandlerWithBuf(padapter, variable, pbuf, len);
+}
+
+u8 rtw_hal_set_def_var(struct adapter *padapter, enum HAL_DEF_VARIABLE eVariable, void *pValue)
+{
+	if (padapter->HalFunc.SetHalDefVarHandler)
+		return padapter->HalFunc.SetHalDefVarHandler(padapter, eVariable, pValue);
+	return _FAIL;
+}
+
+u8 rtw_hal_get_def_var(struct adapter *padapter, enum HAL_DEF_VARIABLE eVariable, void *pValue)
+{
+	if (padapter->HalFunc.GetHalDefVarHandler)
+		return padapter->HalFunc.GetHalDefVarHandler(padapter, eVariable, pValue);
+	return _FAIL;
+}
+
+void rtw_hal_set_odm_var(struct adapter *padapter, enum HAL_ODM_VARIABLE eVariable, void *pValue1, bool bSet)
+{
+	if (padapter->HalFunc.SetHalODMVarHandler)
+		padapter->HalFunc.SetHalODMVarHandler(padapter, eVariable, pValue1, bSet);
+}
+
+void rtw_hal_get_odm_var(struct adapter *padapter, enum HAL_ODM_VARIABLE eVariable, void *pValue1, void *pValue2)
+{
+	if (padapter->HalFunc.GetHalODMVarHandler)
+		padapter->HalFunc.GetHalODMVarHandler(padapter, eVariable, pValue1, pValue2);
+}
+
+void rtw_hal_enable_interrupt(struct adapter *padapter)
+{
+	if (padapter->HalFunc.enable_interrupt)
+		padapter->HalFunc.enable_interrupt(padapter);
+	else
+		DBG_871X("%s: HalFunc.enable_interrupt is NULL!\n", __func__);
+
+}
+
+void rtw_hal_disable_interrupt(struct adapter *padapter)
+{
+	if (padapter->HalFunc.disable_interrupt)
+		padapter->HalFunc.disable_interrupt(padapter);
+	else
+		DBG_871X("%s: HalFunc.disable_interrupt is NULL!\n", __func__);
+
+}
+
+u8 rtw_hal_check_ips_status(struct adapter *padapter)
+{
+	u8 val = false;
+	if (padapter->HalFunc.check_ips_status)
+		val = padapter->HalFunc.check_ips_status(padapter);
+	else
+		DBG_871X("%s: HalFunc.check_ips_status is NULL!\n", __func__);
+
+	return val;
+}
+
+s32	rtw_hal_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	if (padapter->HalFunc.hal_xmitframe_enqueue)
+		return padapter->HalFunc.hal_xmitframe_enqueue(padapter, pxmitframe);
+
+	return false;
+}
+
+s32	rtw_hal_xmit(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	if (padapter->HalFunc.hal_xmit)
+		return padapter->HalFunc.hal_xmit(padapter, pxmitframe);
+
+	return false;
+}
+
+/*
+ * [IMPORTANT] This function would be run in interrupt context.
+ */
+s32	rtw_hal_mgnt_xmit(struct adapter *padapter, struct xmit_frame *pmgntframe)
+{
+	s32 ret = _FAIL;
+	update_mgntframe_attrib_addr(padapter, pmgntframe);
+	/* pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; */
+	/* pwlanhdr = (struct rtw_ieee80211_hdr *)pframe; */
+	/* memcpy(pmgntframe->attrib.ra, pwlanhdr->addr1, ETH_ALEN); */
+
+	if (padapter->securitypriv.binstallBIPkey == true) {
+		if (IS_MCAST(pmgntframe->attrib.ra)) {
+			pmgntframe->attrib.encrypt = _BIP_;
+			/* pmgntframe->attrib.bswenc = true; */
+		} else {
+			pmgntframe->attrib.encrypt = _AES_;
+			pmgntframe->attrib.bswenc = true;
+		}
+		rtw_mgmt_xmitframe_coalesce(padapter, pmgntframe->pkt, pmgntframe);
+	}
+
+	if (padapter->HalFunc.mgnt_xmit)
+		ret = padapter->HalFunc.mgnt_xmit(padapter, pmgntframe);
+	return ret;
+}
+
+s32	rtw_hal_init_xmit_priv(struct adapter *padapter)
+{
+	if (padapter->HalFunc.init_xmit_priv != NULL)
+		return padapter->HalFunc.init_xmit_priv(padapter);
+	return _FAIL;
+}
+
+void rtw_hal_free_xmit_priv(struct adapter *padapter)
+{
+	if (padapter->HalFunc.free_xmit_priv != NULL)
+		padapter->HalFunc.free_xmit_priv(padapter);
+}
+
+s32	rtw_hal_init_recv_priv(struct adapter *padapter)
+{
+	if (padapter->HalFunc.init_recv_priv)
+		return padapter->HalFunc.init_recv_priv(padapter);
+
+	return _FAIL;
+}
+
+void rtw_hal_free_recv_priv(struct adapter *padapter)
+{
+
+	if (padapter->HalFunc.free_recv_priv)
+		padapter->HalFunc.free_recv_priv(padapter);
+}
+
+void rtw_hal_update_ra_mask(struct sta_info *psta, u8 rssi_level)
+{
+	struct adapter *padapter;
+	struct mlme_priv *pmlmepriv;
+
+	if (!psta)
+		return;
+
+	padapter = psta->padapter;
+
+	pmlmepriv = &(padapter->mlmepriv);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+		add_RATid(padapter, psta, rssi_level);
+	else {
+		if (padapter->HalFunc.UpdateRAMaskHandler)
+			padapter->HalFunc.UpdateRAMaskHandler(padapter, psta->mac_id, rssi_level);
+	}
+}
+
+void rtw_hal_add_ra_tid(struct adapter *padapter, u32 bitmap, u8 *arg, u8 rssi_level)
+{
+	if (padapter->HalFunc.Add_RateATid)
+		padapter->HalFunc.Add_RateATid(padapter, bitmap, arg, rssi_level);
+}
+
+/*Start specifical interface thread		*/
+void rtw_hal_start_thread(struct adapter *padapter)
+{
+	if (padapter->HalFunc.run_thread)
+		padapter->HalFunc.run_thread(padapter);
+}
+/*Start specifical interface thread		*/
+void rtw_hal_stop_thread(struct adapter *padapter)
+{
+	if (padapter->HalFunc.cancel_thread)
+		padapter->HalFunc.cancel_thread(padapter);
+}
+
+u32 rtw_hal_read_bbreg(struct adapter *padapter, u32 RegAddr, u32 BitMask)
+{
+	u32 data = 0;
+	if (padapter->HalFunc.read_bbreg)
+		 data = padapter->HalFunc.read_bbreg(padapter, RegAddr, BitMask);
+	return data;
+}
+void rtw_hal_write_bbreg(struct adapter *padapter, u32 RegAddr, u32 BitMask, u32 Data)
+{
+	if (padapter->HalFunc.write_bbreg)
+		padapter->HalFunc.write_bbreg(padapter, RegAddr, BitMask, Data);
+}
+
+u32 rtw_hal_read_rfreg(struct adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask)
+{
+	u32 data = 0;
+	if (padapter->HalFunc.read_rfreg)
+		data = padapter->HalFunc.read_rfreg(padapter, eRFPath, RegAddr, BitMask);
+	return data;
+}
+void rtw_hal_write_rfreg(struct adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask, u32 Data)
+{
+	if (padapter->HalFunc.write_rfreg)
+		padapter->HalFunc.write_rfreg(padapter, eRFPath, RegAddr, BitMask, Data);
+}
+
+void rtw_hal_set_chan(struct adapter *padapter, u8 channel)
+{
+	if (padapter->HalFunc.set_channel_handler)
+		padapter->HalFunc.set_channel_handler(padapter, channel);
+}
+
+void rtw_hal_set_chnl_bw(struct adapter *padapter, u8 channel,
+			 enum CHANNEL_WIDTH Bandwidth, u8 Offset40, u8 Offset80)
+{
+	if (padapter->HalFunc.set_chnl_bw_handler)
+		padapter->HalFunc.set_chnl_bw_handler(padapter, channel,
+						      Bandwidth, Offset40,
+						      Offset80);
+}
+
+void rtw_hal_dm_watchdog(struct adapter *padapter)
+{
+	if (padapter->HalFunc.hal_dm_watchdog)
+		padapter->HalFunc.hal_dm_watchdog(padapter);
+
+}
+
+void rtw_hal_dm_watchdog_in_lps(struct adapter *padapter)
+{
+	if (adapter_to_pwrctl(padapter)->bFwCurrentInPSMode == true) {
+		if (padapter->HalFunc.hal_dm_watchdog_in_lps)
+			padapter->HalFunc.hal_dm_watchdog_in_lps(padapter); /* this fuction caller is in interrupt context */
+	}
+}
+
+void rtw_hal_bcn_related_reg_setting(struct adapter *padapter)
+{
+	if (padapter->HalFunc.SetBeaconRelatedRegistersHandler)
+		padapter->HalFunc.SetBeaconRelatedRegistersHandler(padapter);
+}
+
+
+s32 rtw_hal_xmit_thread_handler(struct adapter *padapter)
+{
+	if (padapter->HalFunc.xmit_thread_handler)
+		return padapter->HalFunc.xmit_thread_handler(padapter);
+	return _FAIL;
+}
+
+void rtw_hal_notch_filter(struct adapter *adapter, bool enable)
+{
+	if (adapter->HalFunc.hal_notch_filter)
+		adapter->HalFunc.hal_notch_filter(adapter, enable);
+}
+
+void rtw_hal_reset_security_engine(struct adapter *adapter)
+{
+	if (adapter->HalFunc.hal_reset_security_engine)
+		adapter->HalFunc.hal_reset_security_engine(adapter);
+}
+
+bool rtw_hal_c2h_valid(struct adapter *adapter, u8 *buf)
+{
+	return c2h_evt_valid((struct c2h_evt_hdr_88xx *)buf);
+}
+
+s32 rtw_hal_c2h_evt_read(struct adapter *adapter, u8 *buf)
+{
+	return c2h_evt_read_88xx(adapter, buf);
+}
+
+s32 rtw_hal_c2h_handler(struct adapter *adapter, u8 *c2h_evt)
+{
+	s32 ret = _FAIL;
+	if (adapter->HalFunc.c2h_handler)
+		ret = adapter->HalFunc.c2h_handler(adapter, c2h_evt);
+	return ret;
+}
+
+c2h_id_filter rtw_hal_c2h_id_filter_ccx(struct adapter *adapter)
+{
+	return adapter->HalFunc.c2h_id_filter_ccx;
+}
+
+s32 rtw_hal_is_disable_sw_channel_plan(struct adapter *padapter)
+{
+	return GET_HAL_DATA(padapter)->bDisableSWChannelPlan;
+}
+
+s32 rtw_hal_macid_sleep(struct adapter *padapter, u32 macid)
+{
+	u8 support;
+
+
+	support = false;
+	rtw_hal_get_def_var(padapter, HAL_DEF_MACID_SLEEP, &support);
+	if (false == support)
+		return _FAIL;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_MACID_SLEEP, (u8 *)&macid);
+
+	return _SUCCESS;
+}
+
+s32 rtw_hal_macid_wakeup(struct adapter *padapter, u32 macid)
+{
+	u8 support;
+
+
+	support = false;
+	rtw_hal_get_def_var(padapter, HAL_DEF_MACID_SLEEP, &support);
+	if (false == support)
+		return _FAIL;
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_MACID_WAKEUP, (u8 *)&macid);
+
+	return _SUCCESS;
+}
+
+s32 rtw_hal_fill_h2c_cmd(struct adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer)
+{
+	s32 ret = _FAIL;
+
+	if (padapter->HalFunc.fill_h2c_cmd)
+		ret = padapter->HalFunc.fill_h2c_cmd(padapter, ElementID, CmdLen, pCmdBuffer);
+	else
+		DBG_871X("%s:  func[fill_h2c_cmd] not defined!\n", __func__);
+
+	return ret;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/hal_phy.c linux/3rdparty/rtl8723bs/hal/hal_phy.c
--- linux.old/3rdparty/rtl8723bs/hal/hal_phy.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/hal_phy.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,224 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#define _HAL_PHY_C_
+
+#include <drv_types.h>
+
+/**
+* Function:	PHY_CalculateBitShift
+*
+* OverView:	Get shifted position of the BitMask
+*
+* Input:
+*		u32 	BitMask,
+*
+* Output:	none
+* Return:		u32 	Return the shift bit bit position of the mask
+*/
+u32 PHY_CalculateBitShift(u32 BitMask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++) {
+		if (((BitMask>>i) &  0x1) == 1)
+			break;
+	}
+
+	return i;
+}
+
+
+/*  */
+/*  ==> RF shadow Operation API Code Section!!! */
+/*  */
+/*-----------------------------------------------------------------------------
+ * Function:	PHY_RFShadowRead
+ *			PHY_RFShadowWrite
+ *			PHY_RFShadowCompare
+ *			PHY_RFShadowRecorver
+ *			PHY_RFShadowCompareAll
+ *			PHY_RFShadowRecorverAll
+ *			PHY_RFShadowCompareFlagSet
+ *			PHY_RFShadowRecorverFlagSet
+ *
+ * Overview:	When we set RF register, we must write shadow at first.
+ *		When we are running, we must compare shadow abd locate error addr.
+ *		Decide to recorver or not.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/20/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+u32 PHY_RFShadowRead(IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset)
+{
+	return	RF_Shadow[eRFPath][Offset].Value;
+
+}	/* PHY_RFShadowRead */
+
+
+void PHY_RFShadowWrite(
+	IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset, IN u32 Data
+)
+{
+	RF_Shadow[eRFPath][Offset].Value = (Data & bRFRegOffsetMask);
+	RF_Shadow[eRFPath][Offset].Driver_Write = true;
+
+}	/* PHY_RFShadowWrite */
+
+
+bool PHY_RFShadowCompare(IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset)
+{
+	u32 reg;
+	/*  Check if we need to check the register */
+	if (RF_Shadow[eRFPath][Offset].Compare == true) {
+		reg = rtw_hal_read_rfreg(Adapter, eRFPath, Offset, bRFRegOffsetMask);
+		/*  Compare shadow and real rf register for 20bits!! */
+		if (RF_Shadow[eRFPath][Offset].Value != reg) {
+			/*  Locate error position. */
+			RF_Shadow[eRFPath][Offset].ErrorOrNot = true;
+			/* RT_TRACE(COMP_INIT, DBG_LOUD, */
+			/* PHY_RFShadowCompare RF-%d Addr%02lx Err = %05lx\n", */
+			/* eRFPath, Offset, reg)); */
+		}
+		return RF_Shadow[eRFPath][Offset].ErrorOrNot;
+	}
+	return false;
+}	/* PHY_RFShadowCompare */
+
+
+void PHY_RFShadowRecorver(IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset)
+{
+	/*  Check if the address is error */
+	if (RF_Shadow[eRFPath][Offset].ErrorOrNot == true) {
+		/*  Check if we need to recorver the register. */
+		if (RF_Shadow[eRFPath][Offset].Recorver == true) {
+			rtw_hal_write_rfreg(Adapter, eRFPath, Offset, bRFRegOffsetMask,
+							RF_Shadow[eRFPath][Offset].Value);
+			/* RT_TRACE(COMP_INIT, DBG_LOUD, */
+			/* PHY_RFShadowRecorver RF-%d Addr%02lx=%05lx", */
+			/* eRFPath, Offset, RF_Shadow[eRFPath][Offset].Value)); */
+		}
+	}
+
+}	/* PHY_RFShadowRecorver */
+
+
+void PHY_RFShadowCompareAll(IN PADAPTER Adapter)
+{
+	u8 eRFPath = 0;
+	u32 Offset = 0, maxReg = GET_RF6052_REAL_MAX_REG(Adapter);
+
+	for (eRFPath = 0; eRFPath < RF6052_MAX_PATH; eRFPath++) {
+		for (Offset = 0; Offset < maxReg; Offset++) {
+			PHY_RFShadowCompare(Adapter, eRFPath, Offset);
+		}
+	}
+
+}	/* PHY_RFShadowCompareAll */
+
+
+void PHY_RFShadowRecorverAll(IN PADAPTER Adapter)
+{
+	u8 eRFPath = 0;
+	u32 Offset = 0, maxReg = GET_RF6052_REAL_MAX_REG(Adapter);
+
+	for (eRFPath = 0; eRFPath < RF6052_MAX_PATH; eRFPath++) {
+		for (Offset = 0; Offset < maxReg; Offset++) {
+			PHY_RFShadowRecorver(Adapter, eRFPath, Offset);
+		}
+	}
+
+}	/* PHY_RFShadowRecorverAll */
+
+
+void
+PHY_RFShadowCompareFlagSet(
+	IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset, IN u8 Type
+)
+{
+	/*  Set True or False!!! */
+	RF_Shadow[eRFPath][Offset].Compare = Type;
+
+}	/* PHY_RFShadowCompareFlagSet */
+
+
+void PHY_RFShadowRecorverFlagSet(
+	IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset, IN u8 Type
+)
+{
+	/*  Set True or False!!! */
+	RF_Shadow[eRFPath][Offset].Recorver = Type;
+
+}	/* PHY_RFShadowRecorverFlagSet */
+
+
+void PHY_RFShadowCompareFlagSetAll(IN PADAPTER Adapter)
+{
+	u8 eRFPath = 0;
+	u32 Offset = 0, maxReg = GET_RF6052_REAL_MAX_REG(Adapter);
+
+	for (eRFPath = 0; eRFPath < RF6052_MAX_PATH; eRFPath++) {
+		for (Offset = 0; Offset < maxReg; Offset++) {
+			/*  2008/11/20 MH For S3S4 test, we only check reg 26/27 now!!!! */
+			if (Offset != 0x26 && Offset != 0x27)
+				PHY_RFShadowCompareFlagSet(Adapter, eRFPath, Offset, false);
+			else
+				PHY_RFShadowCompareFlagSet(Adapter, eRFPath, Offset, true);
+		}
+	}
+
+}	/* PHY_RFShadowCompareFlagSetAll */
+
+
+void PHY_RFShadowRecorverFlagSetAll(IN PADAPTER Adapter)
+{
+	u8 eRFPath = 0;
+	u32 Offset = 0, maxReg = GET_RF6052_REAL_MAX_REG(Adapter);
+
+	for (eRFPath = 0; eRFPath < RF6052_MAX_PATH; eRFPath++) {
+		for (Offset = 0; Offset < maxReg; Offset++) {
+			/*  2008/11/20 MH For S3S4 test, we only check reg 26/27 now!!!! */
+			if (Offset != 0x26 && Offset != 0x27)
+				PHY_RFShadowRecorverFlagSet(Adapter, eRFPath, Offset, false);
+			else
+				PHY_RFShadowRecorverFlagSet(Adapter, eRFPath, Offset, true);
+		}
+	}
+
+}	/* PHY_RFShadowCompareFlagSetAll */
+
+void PHY_RFShadowRefresh(IN PADAPTER Adapter)
+{
+	u8 eRFPath = 0;
+	u32 Offset = 0, maxReg = GET_RF6052_REAL_MAX_REG(Adapter);
+
+	for (eRFPath = 0; eRFPath < RF6052_MAX_PATH; eRFPath++) {
+		for (Offset = 0; Offset < maxReg; Offset++) {
+			RF_Shadow[eRFPath][Offset].Value = 0;
+			RF_Shadow[eRFPath][Offset].Compare = false;
+			RF_Shadow[eRFPath][Offset].Recorver  = false;
+			RF_Shadow[eRFPath][Offset].ErrorOrNot = false;
+			RF_Shadow[eRFPath][Offset].Driver_Write = false;
+		}
+	}
+
+}	/* PHY_RFShadowRead */
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalPhyRf_8723B.c linux/3rdparty/rtl8723bs/hal/HalPhyRf_8723B.c
--- linux.old/3rdparty/rtl8723bs/hal/HalPhyRf_8723B.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalPhyRf_8723B.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,2091 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include "odm_precomp.h"
+
+
+
+/*---------------------------Define Local Constant---------------------------*/
+/*  2010/04/25 MH Define the max tx power tracking tx agc power. */
+#define		ODM_TXPWRTRACK_MAX_IDX8723B	6
+
+/*  MACRO definition for pRFCalibrateInfo->TxIQC_8723B[0] */
+#define		PATH_S0							1 /*  RF_PATH_B */
+#define		IDX_0xC94						0
+#define		IDX_0xC80						1
+#define		IDX_0xC4C						2
+#define		IDX_0xC14						0
+#define		IDX_0xCA0						1
+#define		KEY							0
+#define		VAL							1
+
+/*  MACRO definition for pRFCalibrateInfo->TxIQC_8723B[1] */
+#define		PATH_S1							0 /*  RF_PATH_A */
+#define		IDX_0xC9C						0
+#define		IDX_0xC88						1
+#define		IDX_0xC4C						2
+#define		IDX_0xC1C						0
+#define		IDX_0xC78						1
+
+
+/*---------------------------Define Local Constant---------------------------*/
+
+/* In the case that we fail to read TxPowerTrack.txt, we use the table for
+ * 88E as the default table.
+ */
+static u8 DeltaSwingTableIdx_2GA_N_8188E[] = {
+	0, 0, 0, 2, 2, 3, 3, 4,  4,  4,  4,  5,  5,  6,  6,
+	7, 7, 7, 7, 8, 8, 9, 9, 10, 10, 10, 11, 11, 11, 11
+};
+static u8 DeltaSwingTableIdx_2GA_P_8188E[] = {
+	0, 0, 0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 4, 4, 4,
+	4, 4, 4, 5, 5, 7, 7, 8, 8, 8, 9, 9, 9, 9, 9
+};
+
+/* 3 ============================================================ */
+/* 3 Tx Power Tracking */
+/* 3 ============================================================ */
+
+
+static void setIqkMatrix_8723B(
+	PDM_ODM_T pDM_Odm,
+	u8 OFDM_index,
+	u8 RFPath,
+	s32 IqkResult_X,
+	s32 IqkResult_Y
+)
+{
+	s32 ele_A = 0, ele_D, ele_C = 0, value32;
+
+	if (OFDM_index >= OFDM_TABLE_SIZE)
+		OFDM_index = OFDM_TABLE_SIZE-1;
+
+	ele_D = (OFDMSwingTable_New[OFDM_index] & 0xFFC00000)>>22;
+
+	/* new element A = element D x X */
+	if ((IqkResult_X != 0) && (*(pDM_Odm->pBandType) == ODM_BAND_2_4G)) {
+		if ((IqkResult_X & 0x00000200) != 0)	/* consider minus */
+			IqkResult_X = IqkResult_X | 0xFFFFFC00;
+		ele_A = ((IqkResult_X * ele_D)>>8)&0x000003FF;
+
+		/* new element C = element D x Y */
+		if ((IqkResult_Y & 0x00000200) != 0)
+			IqkResult_Y = IqkResult_Y | 0xFFFFFC00;
+		ele_C = ((IqkResult_Y * ele_D)>>8)&0x000003FF;
+
+		/* if (RFPath == ODM_RF_PATH_A) */
+		switch (RFPath) {
+		case ODM_RF_PATH_A:
+			/* wirte new elements A, C, D to regC80 and regC94, element B is always 0 */
+			value32 = (ele_D<<22)|((ele_C&0x3F)<<16)|ele_A;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, bMaskDWord, value32);
+
+			value32 = (ele_C&0x000003C0)>>6;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XCTxAFE, bMaskH4Bits, value32);
+
+			value32 = ((IqkResult_X * ele_D)>>7)&0x01;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT24, value32);
+			break;
+		case ODM_RF_PATH_B:
+			/* wirte new elements A, C, D to regC88 and regC9C, element B is always 0 */
+			value32 = (ele_D<<22)|((ele_C&0x3F)<<16)|ele_A;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord, value32);
+
+			value32 = (ele_C&0x000003C0)>>6;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XDTxAFE, bMaskH4Bits, value32);
+
+			value32 = ((IqkResult_X * ele_D)>>7)&0x01;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT28, value32);
+
+			break;
+		default:
+			break;
+		}
+	} else {
+		switch (RFPath) {
+		case ODM_RF_PATH_A:
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, bMaskDWord, OFDMSwingTable_New[OFDM_index]);
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XCTxAFE, bMaskH4Bits, 0x00);
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT24, 0x00);
+			break;
+
+		case ODM_RF_PATH_B:
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord, OFDMSwingTable_New[OFDM_index]);
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XDTxAFE, bMaskH4Bits, 0x00);
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT28, 0x00);
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD, ("TxPwrTracking path B: X = 0x%x, Y = 0x%x ele_A = 0x%x ele_C = 0x%x ele_D = 0x%x 0xeb4 = 0x%x 0xebc = 0x%x\n",
+	(u32)IqkResult_X, (u32)IqkResult_Y, (u32)ele_A, (u32)ele_C, (u32)ele_D, (u32)IqkResult_X, (u32)IqkResult_Y));
+}
+
+
+static void setCCKFilterCoefficient(PDM_ODM_T pDM_Odm, u8 CCKSwingIndex)
+{
+	if (!pDM_Odm->RFCalibrateInfo.bCCKinCH14) {
+		rtw_write8(pDM_Odm->Adapter, 0xa22, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][0]);
+		rtw_write8(pDM_Odm->Adapter, 0xa23, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][1]);
+		rtw_write8(pDM_Odm->Adapter, 0xa24, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][2]);
+		rtw_write8(pDM_Odm->Adapter, 0xa25, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][3]);
+		rtw_write8(pDM_Odm->Adapter, 0xa26, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][4]);
+		rtw_write8(pDM_Odm->Adapter, 0xa27, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][5]);
+		rtw_write8(pDM_Odm->Adapter, 0xa28, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][6]);
+		rtw_write8(pDM_Odm->Adapter, 0xa29, CCKSwingTable_Ch1_Ch13_New[CCKSwingIndex][7]);
+	} else {
+		rtw_write8(pDM_Odm->Adapter, 0xa22, CCKSwingTable_Ch14_New[CCKSwingIndex][0]);
+		rtw_write8(pDM_Odm->Adapter, 0xa23, CCKSwingTable_Ch14_New[CCKSwingIndex][1]);
+		rtw_write8(pDM_Odm->Adapter, 0xa24, CCKSwingTable_Ch14_New[CCKSwingIndex][2]);
+		rtw_write8(pDM_Odm->Adapter, 0xa25, CCKSwingTable_Ch14_New[CCKSwingIndex][3]);
+		rtw_write8(pDM_Odm->Adapter, 0xa26, CCKSwingTable_Ch14_New[CCKSwingIndex][4]);
+		rtw_write8(pDM_Odm->Adapter, 0xa27, CCKSwingTable_Ch14_New[CCKSwingIndex][5]);
+		rtw_write8(pDM_Odm->Adapter, 0xa28, CCKSwingTable_Ch14_New[CCKSwingIndex][6]);
+		rtw_write8(pDM_Odm->Adapter, 0xa29, CCKSwingTable_Ch14_New[CCKSwingIndex][7]);
+	}
+}
+
+void DoIQK_8723B(
+	PDM_ODM_T pDM_Odm,
+	u8 DeltaThermalIndex,
+	u8 ThermalValue,
+	u8 Threshold
+)
+{
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	odm_TxPwrTrackSetPwr88E()
+ *
+ * Overview:	88E change all channel tx power accordign to flag.
+ *			OFDM & CCK are all different.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *When		Who	Remark
+ *04/23/2012	MHC	Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+void ODM_TxPwrTrackSetPwr_8723B(
+	PDM_ODM_T pDM_Odm,
+	PWRTRACK_METHOD Method,
+	u8 RFPath,
+	u8 ChannelMappedIndex
+)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	u8 PwrTrackingLimit_OFDM = 34; /* 0dB */
+	u8 PwrTrackingLimit_CCK = 28; /* 2dB */
+	u8 TxRate = 0xFF;
+	u8 Final_OFDM_Swing_Index = 0;
+	u8 Final_CCK_Swing_Index = 0;
+
+	{
+		u16 rate = *(pDM_Odm->pForcedDataRate);
+
+		if (!rate) { /* auto rate */
+			if (pDM_Odm->TxRate != 0xFF)
+				TxRate = HwRateToMRate(pDM_Odm->TxRate);
+		} else /* force rate */
+			TxRate = (u8)rate;
+
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD, ("===>ODM_TxPwrTrackSetPwr8723B\n"));
+
+	if (TxRate != 0xFF) {
+		/* 2 CCK */
+		if ((TxRate >= MGN_1M) && (TxRate <= MGN_11M))
+			PwrTrackingLimit_CCK = 28;	/* 2dB */
+		/* 2 OFDM */
+		else if ((TxRate >= MGN_6M) && (TxRate <= MGN_48M))
+			PwrTrackingLimit_OFDM = 36; /* 3dB */
+		else if (TxRate == MGN_54M)
+			PwrTrackingLimit_OFDM = 34; /* 2dB */
+
+		/* 2 HT */
+		else if ((TxRate >= MGN_MCS0) && (TxRate <= MGN_MCS2)) /* QPSK/BPSK */
+			PwrTrackingLimit_OFDM = 38; /* 4dB */
+		else if ((TxRate >= MGN_MCS3) && (TxRate <= MGN_MCS4)) /* 16QAM */
+			PwrTrackingLimit_OFDM = 36; /* 3dB */
+		else if ((TxRate >= MGN_MCS5) && (TxRate <= MGN_MCS7)) /* 64QAM */
+			PwrTrackingLimit_OFDM = 34; /* 2dB */
+
+		else
+			PwrTrackingLimit_OFDM =  pDM_Odm->DefaultOfdmIndex;   /* Default OFDM index = 30 */
+	}
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD, ("TxRate = 0x%x, PwrTrackingLimit =%d\n", TxRate, PwrTrackingLimit_OFDM));
+
+	if (Method == TXAGC) {
+		struct adapter *Adapter = pDM_Odm->Adapter;
+
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD, ("odm_TxPwrTrackSetPwr8723B CH =%d\n", *(pDM_Odm->pChannel)));
+
+		pDM_Odm->Remnant_OFDMSwingIdx[RFPath] = pDM_Odm->Absolute_OFDMSwingIdx[RFPath];
+
+		pDM_Odm->Modify_TxAGC_Flag_PathA = true;
+		pDM_Odm->Modify_TxAGC_Flag_PathA_CCK = true;
+
+		PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, CCK);
+		PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, OFDM);
+		PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, HT_MCS0_MCS7);
+	} else if (Method == BBSWING) {
+		Final_OFDM_Swing_Index = pDM_Odm->DefaultOfdmIndex + pDM_Odm->Absolute_OFDMSwingIdx[RFPath];
+		Final_CCK_Swing_Index = pDM_Odm->DefaultCckIndex + pDM_Odm->Absolute_OFDMSwingIdx[RFPath];
+
+		/*  Adjust BB swing by OFDM IQ matrix */
+		if (Final_OFDM_Swing_Index >= PwrTrackingLimit_OFDM)
+			Final_OFDM_Swing_Index = PwrTrackingLimit_OFDM;
+		else if (Final_OFDM_Swing_Index <= 0)
+			Final_OFDM_Swing_Index = 0;
+
+		if (Final_CCK_Swing_Index >= CCK_TABLE_SIZE)
+			Final_CCK_Swing_Index = CCK_TABLE_SIZE-1;
+		else if (pDM_Odm->BbSwingIdxCck <= 0)
+			Final_CCK_Swing_Index = 0;
+
+		setIqkMatrix_8723B(pDM_Odm, Final_OFDM_Swing_Index, RFPath,
+			pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][0],
+			pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][1]);
+
+		setCCKFilterCoefficient(pDM_Odm, Final_CCK_Swing_Index);
+
+	} else if (Method == MIX_MODE) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+			("pDM_Odm->DefaultOfdmIndex =%d,  pDM_Odm->DefaultCCKIndex =%d, pDM_Odm->Absolute_OFDMSwingIdx[RFPath]=%d, RF_Path = %d\n",
+			pDM_Odm->DefaultOfdmIndex, pDM_Odm->DefaultCckIndex, pDM_Odm->Absolute_OFDMSwingIdx[RFPath], RFPath));
+
+		Final_OFDM_Swing_Index = pDM_Odm->DefaultOfdmIndex + pDM_Odm->Absolute_OFDMSwingIdx[RFPath];
+		Final_CCK_Swing_Index = pDM_Odm->DefaultCckIndex + pDM_Odm->Absolute_OFDMSwingIdx[RFPath];
+
+		if (Final_OFDM_Swing_Index > PwrTrackingLimit_OFDM) { /* BBSwing higher then Limit */
+			pDM_Odm->Remnant_OFDMSwingIdx[RFPath] = Final_OFDM_Swing_Index - PwrTrackingLimit_OFDM;
+
+			setIqkMatrix_8723B(pDM_Odm, PwrTrackingLimit_OFDM, RFPath,
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][0],
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][1]);
+
+			pDM_Odm->Modify_TxAGC_Flag_PathA = true;
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, OFDM);
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, HT_MCS0_MCS7);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A Over BBSwing Limit , PwrTrackingLimit = %d , Remnant TxAGC Value = %d\n",
+				PwrTrackingLimit_OFDM, pDM_Odm->Remnant_OFDMSwingIdx[RFPath]));
+		} else if (Final_OFDM_Swing_Index <= 0) {
+			pDM_Odm->Remnant_OFDMSwingIdx[RFPath] = Final_OFDM_Swing_Index;
+
+			setIqkMatrix_8723B(pDM_Odm, 0, RFPath,
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][0],
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][1]);
+
+			pDM_Odm->Modify_TxAGC_Flag_PathA = true;
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, OFDM);
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, HT_MCS0_MCS7);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A Lower then BBSwing lower bound  0 , Remnant TxAGC Value = %d\n",
+				pDM_Odm->Remnant_OFDMSwingIdx[RFPath]));
+		} else {
+			setIqkMatrix_8723B(pDM_Odm, Final_OFDM_Swing_Index, RFPath,
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][0],
+				pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[ChannelMappedIndex].Value[0][1]);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A Compensate with BBSwing , Final_OFDM_Swing_Index = %d\n", Final_OFDM_Swing_Index));
+
+			if (pDM_Odm->Modify_TxAGC_Flag_PathA) { /* If TxAGC has changed, reset TxAGC again */
+				pDM_Odm->Remnant_OFDMSwingIdx[RFPath] = 0;
+				PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, OFDM);
+				PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, HT_MCS0_MCS7);
+				pDM_Odm->Modify_TxAGC_Flag_PathA = false;
+
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+					("******Path_A pDM_Odm->Modify_TxAGC_Flag = false\n"));
+			}
+		}
+
+		if (Final_CCK_Swing_Index > PwrTrackingLimit_CCK) {
+			pDM_Odm->Remnant_CCKSwingIdx = Final_CCK_Swing_Index - PwrTrackingLimit_CCK;
+			setCCKFilterCoefficient(pDM_Odm, PwrTrackingLimit_CCK);
+			pDM_Odm->Modify_TxAGC_Flag_PathA_CCK = true;
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, CCK);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A CCK Over Limit , PwrTrackingLimit_CCK = %d , pDM_Odm->Remnant_CCKSwingIdx  = %d\n", PwrTrackingLimit_CCK, pDM_Odm->Remnant_CCKSwingIdx));
+		} else if (Final_CCK_Swing_Index <= 0) { /*  Lowest CCK Index = 0 */
+			pDM_Odm->Remnant_CCKSwingIdx = Final_CCK_Swing_Index;
+			setCCKFilterCoefficient(pDM_Odm, 0);
+			pDM_Odm->Modify_TxAGC_Flag_PathA_CCK = true;
+			PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, CCK);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A CCK Under Limit , PwrTrackingLimit_CCK = %d , pDM_Odm->Remnant_CCKSwingIdx  = %d\n", 0, pDM_Odm->Remnant_CCKSwingIdx));
+		} else {
+			setCCKFilterCoefficient(pDM_Odm, Final_CCK_Swing_Index);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A CCK Compensate with BBSwing , Final_CCK_Swing_Index = %d\n", Final_CCK_Swing_Index));
+
+			if (pDM_Odm->Modify_TxAGC_Flag_PathA_CCK) { /* If TxAGC has changed, reset TxAGC again */
+				pDM_Odm->Remnant_CCKSwingIdx = 0;
+				PHY_SetTxPowerIndexByRateSection(Adapter, RFPath, pHalData->CurrentChannel, CCK);
+				pDM_Odm->Modify_TxAGC_Flag_PathA_CCK = false;
+
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+					("******Path_A pDM_Odm->Modify_TxAGC_Flag_CCK = false\n"));
+			}
+		}
+	} else
+		return; /*  This method is not supported. */
+}
+
+static void GetDeltaSwingTable_8723B(
+	PDM_ODM_T pDM_Odm,
+	u8 **TemperatureUP_A,
+	u8 **TemperatureDOWN_A,
+	u8 **TemperatureUP_B,
+	u8 **TemperatureDOWN_B
+)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	u16 rate = *(pDM_Odm->pForcedDataRate);
+	u8 channel = pHalData->CurrentChannel;
+
+	if (1 <= channel && channel <= 14) {
+		if (IS_CCK_RATE(rate)) {
+			*TemperatureUP_A   = pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_P;
+			*TemperatureDOWN_A = pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_N;
+			*TemperatureUP_B   = pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_P;
+			*TemperatureDOWN_B = pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_N;
+		} else {
+			*TemperatureUP_A   = pRFCalibrateInfo->DeltaSwingTableIdx_2GA_P;
+			*TemperatureDOWN_A = pRFCalibrateInfo->DeltaSwingTableIdx_2GA_N;
+			*TemperatureUP_B   = pRFCalibrateInfo->DeltaSwingTableIdx_2GB_P;
+			*TemperatureDOWN_B = pRFCalibrateInfo->DeltaSwingTableIdx_2GB_N;
+		}
+	} /*else if (36 <= channel && channel <= 64) {
+		*TemperatureUP_A   = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[0];
+		*TemperatureDOWN_A = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[0];
+		*TemperatureUP_B   = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[0];
+		*TemperatureDOWN_B = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[0];
+	} else if (100 <= channel && channel <= 140) {
+		*TemperatureUP_A   = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[1];
+		*TemperatureDOWN_A = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[1];
+		*TemperatureUP_B   = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[1];
+		*TemperatureDOWN_B = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[1];
+	} else if (149 <= channel && channel <= 173) {
+		*TemperatureUP_A   = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[2];
+		*TemperatureDOWN_A = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[2];
+		*TemperatureUP_B   = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[2];
+		*TemperatureDOWN_B = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[2];
+	}*/else {
+		*TemperatureUP_A   = (u8 *)DeltaSwingTableIdx_2GA_P_8188E;
+		*TemperatureDOWN_A = (u8 *)DeltaSwingTableIdx_2GA_N_8188E;
+		*TemperatureUP_B   = (u8 *)DeltaSwingTableIdx_2GA_P_8188E;
+		*TemperatureDOWN_B = (u8 *)DeltaSwingTableIdx_2GA_N_8188E;
+	}
+
+	return;
+}
+
+
+void ConfigureTxpowerTrack_8723B(PTXPWRTRACK_CFG pConfig)
+{
+	pConfig->SwingTableSize_CCK = CCK_TABLE_SIZE;
+	pConfig->SwingTableSize_OFDM = OFDM_TABLE_SIZE;
+	pConfig->Threshold_IQK = IQK_THRESHOLD;
+	pConfig->AverageThermalNum = AVG_THERMAL_NUM_8723B;
+	pConfig->RfPathCount = MAX_PATH_NUM_8723B;
+	pConfig->ThermalRegAddr = RF_T_METER_8723B;
+
+	pConfig->ODM_TxPwrTrackSetPwr = ODM_TxPwrTrackSetPwr_8723B;
+	pConfig->DoIQK = DoIQK_8723B;
+	pConfig->PHY_LCCalibrate = PHY_LCCalibrate_8723B;
+	pConfig->GetDeltaSwingTable = GetDeltaSwingTable_8723B;
+}
+
+/* 1 7. IQK */
+#define MAX_TOLERANCE		5
+#define IQK_DELAY_TIME		1		/* ms */
+
+/* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+static u8 phy_PathA_IQK_8723B(
+	struct adapter *padapter, bool configPathB, u8 RF_Path
+)
+{
+	u32 regEAC, regE94, regE9C, tmp, Path_SEL_BB /*, regEA4*/;
+	u8 result = 0x00;
+
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T		pDM_Odm = &pHalData->odmpriv;
+
+	/*  Save RF Path */
+	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord);
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A IQK!\n"));
+
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/* 	enable path A PA in TXIQK mode */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0003f);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xc7f87);
+	/* 	disable path B PA in TXIQK mode */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, bRFRegOffsetMask, 0x00020); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0x43, bRFRegOffsetMask, 0x40ec1); */
+
+	/* 1 Tx IQK */
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, 0x01007c00);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+	/* path-A IQK setting */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-A IQK setting!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x8214010a); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x821303ea);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x28110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x00462911);
+
+	/* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* Ant switch */
+	if (configPathB || (RF_Path == 0))
+		/*  wifi switch to S1 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000000);
+	else
+		/*  wifi switch to S0 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path A LOK & IQK */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path A LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_8723B)); */
+	/* PlatformStallExecution(IQK_DELAY_TIME_8723B*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regE94 = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord);
+	regE9C = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe94 = 0x%x, 0xe9c = 0x%x\n", regE94, regE9C));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe90(before IQK) = 0x%x, 0xe98(afer IQK) = 0x%x\n",
+	PHY_QueryBBReg(pDM_Odm->Adapter, 0xe90, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xe98, bMaskDWord)));
+
+
+	/* Allen 20131125 */
+	tmp = (regE9C & 0x03FF0000)>>16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (
+		!(regEAC & BIT28) &&
+		(((regE94 & 0x03FF0000)>>16) != 0x142) &&
+		(((regE9C & 0x03FF0000)>>16) != 0x42) &&
+		(((regE94 & 0x03FF0000)>>16) < 0x110) &&
+		(((regE94 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+		result |= 0x01;
+	else					/* if Tx not OK, ignore Rx */
+		return result;
+
+	return result;
+}
+
+/* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+static u8 phy_PathA_RxIQK8723B(
+	struct adapter *padapter, bool configPathB, u8 RF_Path
+)
+{
+	u32 regEAC, regE94, regE9C, regEA4, u4tmp, tmp, Path_SEL_BB;
+	u8 result = 0x00;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A Rx IQK!\n")); */
+
+	/*  Save RF Path */
+	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord);
+
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A RX IQK:Get TXIMR setting\n"));
+	/* 1 Get TXIMR setting */
+	/* modify RXIQK mode table */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-A Rx IQK modify RXIQK mode table!\n")); */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	/* LNA2 off, PA on for Dcut */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7fb7);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x0); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, 0x01007c00);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+
+	/* path-A IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82160c1f); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82130ff0);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x28110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x0046a911);
+
+	/* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* Ant switch */
+	if (configPathB || (RF_Path == 0))
+		/*  wifi switch to S1 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000000);
+	else
+		/*  wifi switch to S0 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path A LOK & IQK */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path A LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_8723B)); */
+	/* PlatformStallExecution(IQK_DELAY_TIME_8723B*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regE94 = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord);
+	regE9C = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe94 = 0x%x, 0xe9c = 0x%x\n", regE94, regE9C));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe90(before IQK) = 0x%x, 0xe98(afer IQK) = 0x%x\n",
+	PHY_QueryBBReg(pDM_Odm->Adapter, 0xe90, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xe98, bMaskDWord)));
+
+	/* Allen 20131125 */
+	tmp = (regE9C & 0x03FF0000)>>16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (
+		!(regEAC & BIT28) &&
+		(((regE94 & 0x03FF0000)>>16) != 0x142) &&
+		(((regE9C & 0x03FF0000)>>16) != 0x42) &&
+		(((regE94 & 0x03FF0000)>>16) < 0x110) &&
+		(((regE94 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+		result |= 0x01;
+	else				/* if Tx not OK, ignore Rx */
+		return result;
+
+	u4tmp = 0x80007C00 | (regE94&0x3FF0000) | ((regE9C&0x3FF0000) >> 16);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, u4tmp);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe40 = 0x%x u4tmp = 0x%x\n", PHY_QueryBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord), u4tmp));
+
+
+	/* 1 RX IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A RX IQK\n"));
+
+	/* modify RXIQK mode table */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-A Rx IQK modify RXIQK mode table 2!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	/* LAN2 on, PA off for Dcut */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7d77);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x0); */
+
+	/* PA, PAD setting */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xdf, bRFRegOffsetMask, 0xf80);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0x55, bRFRegOffsetMask, 0x4021f);
+
+
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+
+	/* path-A IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82110000);
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x281604c2); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x2813001f);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x0046a8d1);
+
+	/* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* Ant switch */
+	if (configPathB || (RF_Path == 0))
+		/*  wifi switch to S1 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000000);
+	else
+		/*  wifi switch to S0 */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path A LOK & IQK */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path A LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_88E)); */
+	/* PlatformStallExecution(IQK_DELAY_TIME_8723B*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regEA4 = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_Before_IQK_A_2, bMaskDWord);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xea4 = 0x%x, 0xeac = 0x%x\n", regEA4, regEAC));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xea0(before IQK) = 0x%x, 0xea8(afer IQK) = 0x%x\n",
+	PHY_QueryBBReg(pDM_Odm->Adapter, 0xea0, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xea8, bMaskDWord)));
+
+	/* 	PA/PAD controlled by 0x0 */
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xdf, bRFRegOffsetMask, 0x780);
+
+	/* Allen 20131125 */
+	tmp = (regEAC & 0x03FF0000)>>16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (
+		!(regEAC & BIT27) && /* if Tx is OK, check whether Rx is OK */
+		(((regEA4 & 0x03FF0000)>>16) != 0x132) &&
+		(((regEAC & 0x03FF0000)>>16) != 0x36) &&
+		(((regEA4 & 0x03FF0000)>>16) < 0x110) &&
+		(((regEA4 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+		result |= 0x02;
+	else							/* if Tx not OK, ignore Rx */
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path A Rx IQK fail!!\n"));
+	return result;
+}
+
+/* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+static u8 phy_PathB_IQK_8723B(struct adapter *padapter)
+{
+	u32 regEAC, regE94, regE9C, tmp, Path_SEL_BB/*, regEC4, regECC, Path_SEL_BB*/;
+	u8 result = 0x00;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path B IQK!\n"));
+
+	/*  Save RF Path */
+	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord);
+
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/* 	in TXIQK mode */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x20000); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0003f); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xc7f87); */
+	/* 	enable path B PA in TXIQK mode */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0x43, bRFRegOffsetMask, 0x30fc1);
+
+
+
+	/* 1 Tx IQK */
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, 0x01007c00);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+	/* path-A IQK setting */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-B IQK setting!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82140114); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x821303ea);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x28110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x00462911);
+
+	/* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* switch to path B */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xb0, bRFRegOffsetMask, 0xeffe0); */
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path B LOK & IQK */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path B LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path B LOK & IQK.\n", IQK_DELAY_TIME_88E)); */
+	/* PlatformStallExecution(IQK_DELAY_TIME_88E*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0x948 = 0x%x\n", PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord))); */
+
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regE94 = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord);
+	regE9C = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe94 = 0x%x, 0xe9c = 0x%x\n", regE94, regE9C));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe90(before IQK) = 0x%x, 0xe98(afer IQK) = 0x%x\n",
+	PHY_QueryBBReg(pDM_Odm->Adapter, 0xe90, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xe98, bMaskDWord)));
+
+	/* Allen 20131125 */
+	tmp = (regE9C & 0x03FF0000)>>16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (
+		!(regEAC & BIT28) &&
+		(((regE94 & 0x03FF0000)>>16) != 0x142) &&
+		(((regE9C & 0x03FF0000)>>16) != 0x42) &&
+		(((regE94 & 0x03FF0000)>>16) < 0x110) &&
+		(((regE94 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+		result |= 0x01;
+
+	return result;
+}
+
+/* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+static u8 phy_PathB_RxIQK8723B(struct adapter *padapter, bool configPathB)
+{
+	u32 regE94, regE9C, regEA4, regEAC, u4tmp, tmp, Path_SEL_BB;
+	u8 result = 0x00;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B Rx IQK!\n")); */
+
+	/*  Save RF Path */
+	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord);
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/* switch to path B */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+
+	/* 1 Get TXIMR setting */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B RX IQK:Get TXIMR setting!\n"));
+	/* modify RXIQK mode table */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-A Rx IQK modify RXIQK mode table!\n")); */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7fb7);
+	/* open PA S1 & SMIXER */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0x43, bRFRegOffsetMask, 0x30fcd);
+
+
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, 0x01007c00);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+
+
+	/* path-B IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82160c1f); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82130ff0);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x28110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x0046a911);
+
+    /* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* switch to path B */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xb0, bRFRegOffsetMask, 0xeffe0); */
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path B TXIQK @ RXIQK */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path B LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_88E)); */
+	/* PlatformStallExecution(IQK_DELAY_TIME_88E*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regE94 = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord);
+	regE9C = PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe94 = 0x%x, 0xe9c = 0x%x\n", regE94, regE9C));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe90(before IQK) = 0x%x, 0xe98(afer IQK) = 0x%x\n",
+		PHY_QueryBBReg(pDM_Odm->Adapter, 0xe90, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xe98, bMaskDWord)));
+
+	/* Allen 20131125 */
+	tmp = (regE9C & 0x03FF0000)>>16;
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("tmp1 = 0x%x\n", tmp)); */
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("tmp2 = 0x%x\n", tmp)); */
+
+	if (
+		!(regEAC & BIT28) &&
+		(((regE94 & 0x03FF0000)>>16) != 0x142) &&
+		(((regE9C & 0x03FF0000)>>16) != 0x42)  &&
+		(((regE94 & 0x03FF0000)>>16) < 0x110) &&
+		(((regE94 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+			result |= 0x01;
+	else	/* if Tx not OK, ignore Rx */
+		return result;
+
+	u4tmp = 0x80007C00 | (regE94&0x3FF0000)  | ((regE9C&0x3FF0000) >> 16);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord, u4tmp);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe40 = 0x%x u4tmp = 0x%x\n", PHY_QueryBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord), u4tmp));
+
+	/* 1 RX IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B RX IQK\n"));
+
+	/* modify RXIQK mode table */
+	/* 20121009, Kordan> RF Mode = 3 */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7d77);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x0); */
+
+	/* open PA S1 & close SMIXER */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0x43, bRFRegOffsetMask, 0x30ebd);
+
+	/* PA, PAD setting */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xdf, bRFRegOffsetMask, 0xf80); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0x56, bRFRegOffsetMask, 0x51000); */
+
+	/* IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK, bMaskDWord, 0x01004800);
+
+	/* path-B IQK setting */
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_B, bMaskDWord, 0x38008c1c);
+
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_A, bMaskDWord, 0x82110000);
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x281604c2); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_A, bMaskDWord, 0x2813001f);
+	PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_PI_B, bMaskDWord, 0x82110000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_PI_B, bMaskDWord, 0x28110000);
+
+	/* LO calibration setting */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Rsp, bMaskDWord, 0x0046a8d1);
+
+    /* enter IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x808000);
+
+	/* switch to path B */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xb0, bRFRegOffsetMask, 0xeffe0); */
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path B LOK & IQK */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path B LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_88E)); */
+	/* PlatformStallExecution(IQK_DELAY_TIME_88E*1000); */
+	mdelay(IQK_DELAY_TIME_8723B);
+
+	/* restore Ant Path */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB);
+	/* GNT_BT = 1 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00001800);
+
+    /* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/*  Check failed */
+	regEAC = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regEA4 = PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_Before_IQK_A_2, bMaskDWord);;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xea4 = 0x%x, 0xeac = 0x%x\n", regEA4, regEAC));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xea0(before IQK) = 0x%x, 0xea8(afer IQK) = 0x%x\n",
+		PHY_QueryBBReg(pDM_Odm->Adapter, 0xea0, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xea8, bMaskDWord)));
+
+	/* 	PA/PAD controlled by 0x0 */
+	/* leave IQK mode */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, 0xffffff00, 0x00000000); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_B, 0xdf, bRFRegOffsetMask, 0x180); */
+
+
+
+	/* Allen 20131125 */
+	tmp = (regEAC & 0x03FF0000)>>16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (
+		!(regEAC & BIT27) && /* if Tx is OK, check whether Rx is OK */
+		(((regEA4 & 0x03FF0000)>>16) != 0x132) &&
+		(((regEAC & 0x03FF0000)>>16) != 0x36) &&
+		(((regEA4 & 0x03FF0000)>>16) < 0x110) &&
+		(((regEA4 & 0x03FF0000)>>16) > 0xf0) &&
+		(tmp < 0xf)
+	)
+		result |= 0x02;
+	else
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path B Rx IQK fail!!\n"));
+
+	return result;
+}
+
+static void _PHY_PathAFillIQKMatrix8723B(
+	struct adapter *padapter,
+	bool bIQKOK,
+	s32 result[][8],
+	u8 final_candidate,
+	bool bTxOnly
+)
+{
+	u32 Oldval_0, X, TX0_A, reg;
+	s32 Y, TX0_C;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path A IQ Calibration %s !\n", (bIQKOK)?"Success":"Failed"));
+
+	if (final_candidate == 0xFF)
+		return;
+
+	else if (bIQKOK) {
+		Oldval_0 = (PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, bMaskDWord) >> 22) & 0x3FF;
+
+		X = result[final_candidate][0];
+		if ((X & 0x00000200) != 0)
+			X = X | 0xFFFFFC00;
+		TX0_A = (X * Oldval_0) >> 8;
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("X = 0x%x, TX0_A = 0x%x, Oldval_0 0x%x\n", X, TX0_A, Oldval_0));
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, 0x3FF, TX0_A);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT(31), ((X*Oldval_0>>7) & 0x1));
+
+		Y = result[final_candidate][1];
+		if ((Y & 0x00000200) != 0)
+			Y = Y | 0xFFFFFC00;
+
+		/* 2 Tx IQC */
+		TX0_C = (Y * Oldval_0) >> 8;
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Y = 0x%x, TX = 0x%x\n", Y, TX0_C));
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XCTxAFE, 0xF0000000, ((TX0_C&0x3C0)>>6));
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC94][KEY] = rOFDM0_XCTxAFE;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC94][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XCTxAFE, bMaskDWord);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, 0x003F0000, (TX0_C&0x3F));
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC80][KEY] = rOFDM0_XATxIQImbalance;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC80][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XATxIQImbalance, bMaskDWord);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT(29), ((Y*Oldval_0>>7) & 0x1));
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC4C][KEY] = rOFDM0_ECCAThreshold;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC4C][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskDWord);
+
+		if (bTxOnly) {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("_PHY_PathAFillIQKMatrix8723B only Tx OK\n"));
+
+			/*  <20130226, Kordan> Saving RxIQC, otherwise not initialized. */
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][KEY] = rOFDM0_RxIQExtAnta;
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][VAL] = 0xfffffff & PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_RxIQExtAnta, bMaskDWord);
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][KEY] = rOFDM0_XARxIQImbalance;
+/* 			pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XARxIQImbalance, bMaskDWord); */
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][VAL] = 0x40000100;
+			return;
+		}
+
+		reg = result[final_candidate][2];
+
+		/* 2 Rx IQC */
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XARxIQImbalance, 0x3FF, reg);
+		reg = result[final_candidate][3] & 0x3F;
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XARxIQImbalance, 0xFC00, reg);
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][KEY] = rOFDM0_XARxIQImbalance;
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XARxIQImbalance, bMaskDWord);
+
+		reg = (result[final_candidate][3] >> 6) & 0xF;
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_RxIQExtAnta, 0xF0000000, reg);
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][KEY] = rOFDM0_RxIQExtAnta;
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_RxIQExtAnta, bMaskDWord);
+
+	}
+}
+
+static void _PHY_PathBFillIQKMatrix8723B(
+	struct adapter *padapter,
+	bool bIQKOK,
+	s32 result[][8],
+	u8 final_candidate,
+	bool bTxOnly /* do Tx only */
+)
+{
+	u32 Oldval_1, X, TX1_A, reg;
+	s32	Y, TX1_C;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B IQ Calibration %s !\n", (bIQKOK)?"Success":"Failed"));
+
+	if (final_candidate == 0xFF)
+		return;
+
+	else if (bIQKOK) {
+		Oldval_1 = (PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord) >> 22) & 0x3FF;
+
+		X = result[final_candidate][4];
+		if ((X & 0x00000200) != 0)
+			X = X | 0xFFFFFC00;
+		TX1_A = (X * Oldval_1) >> 8;
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("X = 0x%x, TX1_A = 0x%x\n", X, TX1_A));
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, 0x3FF, TX1_A);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT(27), ((X*Oldval_1>>7) & 0x1));
+
+		Y = result[final_candidate][5];
+		if ((Y & 0x00000200) != 0)
+			Y = Y | 0xFFFFFC00;
+
+		TX1_C = (Y * Oldval_1) >> 8;
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Y = 0x%x, TX1_C = 0x%x\n", Y, TX1_C));
+
+		/* 2 Tx IQC */
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XDTxAFE, 0xF0000000, ((TX1_C&0x3C0)>>6));
+/* 		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC9C][KEY] = rOFDM0_XDTxAFE; */
+/* 		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC9C][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XDTxAFE, bMaskDWord); */
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC94][KEY] = rOFDM0_XCTxAFE;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC94][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XDTxAFE, bMaskDWord);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, 0x003F0000, (TX1_C&0x3F));
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC80][KEY] = rOFDM0_XATxIQImbalance;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC80][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord);
+
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, BIT(25), ((Y*Oldval_1>>7) & 0x1));
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC4C][KEY] = rOFDM0_ECCAThreshold;
+		pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC4C][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskDWord);
+
+		if (bTxOnly) {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("_PHY_PathBFillIQKMatrix8723B only Tx OK\n"));
+
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][KEY] = rOFDM0_XARxIQImbalance;
+/* 			pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XARxIQImbalance, bMaskDWord); */
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][VAL] = 0x40000100;
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][KEY] = rOFDM0_RxIQExtAnta;
+			pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][VAL] = 0x0fffffff & PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_RxIQExtAnta, bMaskDWord);
+			return;
+		}
+
+		/* 2 Rx IQC */
+		reg = result[final_candidate][6];
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBRxIQImbalance, 0x3FF, reg);
+		reg = result[final_candidate][7] & 0x3F;
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBRxIQImbalance, 0xFC00, reg);
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][KEY] = rOFDM0_XARxIQImbalance;
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][VAL] = PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XBRxIQImbalance, bMaskDWord);
+
+		reg = (result[final_candidate][7] >> 6) & 0xF;
+/* 		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_AGCRSSITable, 0x0000F000, reg); */
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][KEY] = rOFDM0_RxIQExtAnta;
+		pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][VAL] = (reg << 28)|(PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_RxIQExtAnta, bMaskDWord)&0x0fffffff);
+	}
+}
+
+/*  */
+/*  2011/07/26 MH Add an API for testing IQK fail case. */
+/*  */
+/*  MP Already declare in odm.c */
+
+void ODM_SetIQCbyRFpath(PDM_ODM_T pDM_Odm, u32 RFpath)
+{
+
+	PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+
+	if (
+		(pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC80][VAL] != 0x0) &&
+		(pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][VAL] != 0x0) &&
+		(pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC80][VAL] != 0x0) &&
+		(pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][VAL] != 0x0)
+	) {
+		if (RFpath) { /* S1: RFpath = 0, S0:RFpath = 1 */
+			/* S0 TX IQC */
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC94][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC94][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC80][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC80][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC4C][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S0][IDX_0xC4C][VAL]);
+			/* S0 RX IQC */
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][KEY], bMaskDWord, pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xC14][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][KEY], bMaskDWord, pRFCalibrateInfo->RxIQC_8723B[PATH_S0][IDX_0xCA0][VAL]);
+		} else {
+			/* S1 TX IQC */
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC94][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC94][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC80][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC80][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC4C][KEY], bMaskDWord, pRFCalibrateInfo->TxIQC_8723B[PATH_S1][IDX_0xC4C][VAL]);
+			/* S1 RX IQC */
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][KEY], bMaskDWord, pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xC14][VAL]);
+			PHY_SetBBReg(pDM_Odm->Adapter, pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][KEY], bMaskDWord, pRFCalibrateInfo->RxIQC_8723B[PATH_S1][IDX_0xCA0][VAL]);
+		}
+	}
+}
+
+static bool ODM_CheckPowerStatus(struct adapter *Adapter)
+{
+	return true;
+}
+
+static void _PHY_SaveADDARegisters8723B(
+	struct adapter *padapter,
+	u32 *ADDAReg,
+	u32 *ADDABackup,
+	u32 RegisterNum
+)
+{
+	u32 i;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	if (ODM_CheckPowerStatus(padapter) == false)
+		return;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Save ADDA parameters.\n"));
+	for (i = 0 ; i < RegisterNum ; i++) {
+		ADDABackup[i] = PHY_QueryBBReg(pDM_Odm->Adapter, ADDAReg[i], bMaskDWord);
+	}
+}
+
+
+static void _PHY_SaveMACRegisters8723B(
+	struct adapter *padapter, u32 *MACReg, u32 *MACBackup
+)
+{
+	u32 i;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T		pDM_Odm = &pHalData->odmpriv;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Save MAC parameters.\n"));
+	for (i = 0 ; i < (IQK_MAC_REG_NUM - 1); i++) {
+		MACBackup[i] = rtw_read8(pDM_Odm->Adapter, MACReg[i]);
+	}
+	MACBackup[i] = rtw_read32(pDM_Odm->Adapter, MACReg[i]);
+
+}
+
+
+static void _PHY_ReloadADDARegisters8723B(
+	struct adapter *padapter,
+	u32 *ADDAReg,
+	u32 *ADDABackup,
+	u32 RegiesterNum
+)
+{
+	u32 i;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Reload ADDA power saving parameters !\n"));
+	for (i = 0 ; i < RegiesterNum; i++) {
+		PHY_SetBBReg(pDM_Odm->Adapter, ADDAReg[i], bMaskDWord, ADDABackup[i]);
+	}
+}
+
+static void _PHY_ReloadMACRegisters8723B(
+	struct adapter *padapter, u32 *MACReg, u32 *MACBackup
+)
+{
+	u32 i;
+
+	for (i = 0 ; i < (IQK_MAC_REG_NUM - 1); i++) {
+		rtw_write8(padapter, MACReg[i], (u8)MACBackup[i]);
+	}
+	rtw_write32(padapter, MACReg[i], MACBackup[i]);
+}
+
+
+static void _PHY_PathADDAOn8723B(
+	struct adapter *padapter,
+	u32 *ADDAReg,
+	bool isPathAOn,
+	bool is2T
+)
+{
+	u32 pathOn;
+	u32 i;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("ADDA ON.\n"));
+
+	pathOn = isPathAOn ? 0x01c00014 : 0x01c00014;
+	if (false == is2T) {
+		pathOn = 0x01c00014;
+		PHY_SetBBReg(pDM_Odm->Adapter, ADDAReg[0], bMaskDWord, 0x01c00014);
+	} else {
+		PHY_SetBBReg(pDM_Odm->Adapter, ADDAReg[0], bMaskDWord, pathOn);
+	}
+
+	for (i = 1 ; i < IQK_ADDA_REG_NUM ; i++) {
+		PHY_SetBBReg(pDM_Odm->Adapter, ADDAReg[i], bMaskDWord, pathOn);
+	}
+
+}
+
+static void _PHY_MACSettingCalibration8723B(
+	struct adapter *padapter, u32 *MACReg, u32 *MACBackup
+)
+{
+	u32 i = 0;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("MAC settings for Calibration.\n"));
+
+	rtw_write8(pDM_Odm->Adapter, MACReg[i], 0x3F);
+
+	for (i = 1 ; i < (IQK_MAC_REG_NUM - 1); i++) {
+		rtw_write8(pDM_Odm->Adapter, MACReg[i], (u8)(MACBackup[i]&(~BIT3)));
+	}
+	rtw_write8(pDM_Odm->Adapter, MACReg[i], (u8)(MACBackup[i]&(~BIT5)));
+
+}
+
+static bool phy_SimularityCompare_8723B(
+	struct adapter *padapter,
+	s32 result[][8],
+	u8  c1,
+	u8  c2
+)
+{
+	u32 i, j, diff, SimularityBitMap, bound = 0;
+	u8 final_candidate[2] = {0xFF, 0xFF}; /* for path A and path B */
+	bool bResult = true;
+	bool is2T = true;
+	s32 tmp1 = 0, tmp2 = 0;
+
+	if (is2T)
+		bound = 8;
+	else
+		bound = 4;
+
+	SimularityBitMap = 0;
+
+	for (i = 0; i < bound; i++) {
+
+		if ((i == 1) || (i == 3) || (i == 5) || (i == 7)) {
+			if ((result[c1][i] & 0x00000200) != 0)
+				tmp1 = result[c1][i] | 0xFFFFFC00;
+			else
+				tmp1 = result[c1][i];
+
+			if ((result[c2][i] & 0x00000200) != 0)
+				tmp2 = result[c2][i] | 0xFFFFFC00;
+			else
+				tmp2 = result[c2][i];
+		} else {
+			tmp1 = result[c1][i];
+			tmp2 = result[c2][i];
+		}
+
+		diff = (tmp1 > tmp2) ? (tmp1 - tmp2) : (tmp2 - tmp1);
+
+		if (diff > MAX_TOLERANCE) {
+			if ((i == 2 || i == 6) && !SimularityBitMap) {
+				if (result[c1][i]+result[c1][i+1] == 0)
+					final_candidate[(i/4)] = c2;
+				else if (result[c2][i]+result[c2][i+1] == 0)
+					final_candidate[(i/4)] = c1;
+				else
+					SimularityBitMap = SimularityBitMap|(1<<i);
+			} else
+				SimularityBitMap = SimularityBitMap|(1<<i);
+		}
+	}
+
+	if (SimularityBitMap == 0) {
+		for (i = 0; i < (bound/4); i++) {
+			if (final_candidate[i] != 0xFF) {
+				for (j = i*4; j < (i+1)*4-2; j++)
+					result[3][j] = result[final_candidate[i]][j];
+				bResult = false;
+			}
+		}
+		return bResult;
+	} else {
+
+		if (!(SimularityBitMap & 0x03)) { /* path A TX OK */
+			for (i = 0; i < 2; i++)
+				result[3][i] = result[c1][i];
+		}
+
+		if (!(SimularityBitMap & 0x0c)) { /* path A RX OK */
+			for (i = 2; i < 4; i++)
+				result[3][i] = result[c1][i];
+		}
+
+		if (!(SimularityBitMap & 0x30)) { /* path B TX OK */
+			for (i = 4; i < 6; i++)
+				result[3][i] = result[c1][i];
+		}
+
+		if (!(SimularityBitMap & 0xc0)) { /* path B RX OK */
+			for (i = 6; i < 8; i++)
+				result[3][i] = result[c1][i];
+		}
+		return false;
+	}
+}
+
+
+
+static void phy_IQCalibrate_8723B(
+	struct adapter *padapter,
+	s32 result[][8],
+	u8 t,
+	bool is2T,
+	u8 RF_Path
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	u32 i;
+	u8 PathAOK, PathBOK;
+	u8 tmp0xc50 = (u8)PHY_QueryBBReg(pDM_Odm->Adapter, 0xC50, bMaskByte0);
+	u8 tmp0xc58 = (u8)PHY_QueryBBReg(pDM_Odm->Adapter, 0xC58, bMaskByte0);
+	u32 ADDA_REG[IQK_ADDA_REG_NUM] = {
+		rFPGA0_XCD_SwitchControl,
+		rBlue_Tooth,
+		rRx_Wait_CCA,
+		rTx_CCK_RFON,
+		rTx_CCK_BBON,
+		rTx_OFDM_RFON,
+		rTx_OFDM_BBON,
+		rTx_To_Rx,
+		rTx_To_Tx,
+		rRx_CCK,
+		rRx_OFDM,
+		rRx_Wait_RIFS,
+		rRx_TO_Rx,
+		rStandby,
+		rSleep,
+		rPMPD_ANAEN
+	};
+	u32 IQK_MAC_REG[IQK_MAC_REG_NUM] = {
+		REG_TXPAUSE,
+		REG_BCN_CTRL,
+		REG_BCN_CTRL_1,
+		REG_GPIO_MUXCFG
+	};
+
+	/* since 92C & 92D have the different define in IQK_BB_REG */
+	u32 IQK_BB_REG_92C[IQK_BB_REG_NUM] = {
+		rOFDM0_TRxPathEnable,
+		rOFDM0_TRMuxPar,
+		rFPGA0_XCD_RFInterfaceSW,
+		rConfig_AntA,
+		rConfig_AntB,
+		rFPGA0_XAB_RFInterfaceSW,
+		rFPGA0_XA_RFInterfaceOE,
+		rFPGA0_XB_RFInterfaceOE,
+		rCCK0_AFESetting
+	};
+	const u32 retryCount = 2;
+
+	/*  Note: IQ calibration must be performed after loading */
+	/* 		PHY_REG.txt , and radio_a, radio_b.txt */
+
+	/* u32 bbvalue; */
+
+	if (t == 0) {
+/* 		 bbvalue = PHY_QueryBBReg(pDM_Odm->Adapter, rFPGA0_RFMOD, bMaskDWord); */
+/* 			RT_DISP(FINIT, INIT_IQK, ("phy_IQCalibrate_8188E() ==>0x%08x\n", bbvalue)); */
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQ Calibration for %s for %d times\n", (is2T ? "2T2R" : "1T1R"), t));
+
+		/*  Save ADDA parameters, turn Path A ADDA on */
+		_PHY_SaveADDARegisters8723B(padapter, ADDA_REG, pDM_Odm->RFCalibrateInfo.ADDA_backup, IQK_ADDA_REG_NUM);
+		_PHY_SaveMACRegisters8723B(padapter, IQK_MAC_REG, pDM_Odm->RFCalibrateInfo.IQK_MAC_backup);
+		_PHY_SaveADDARegisters8723B(padapter, IQK_BB_REG_92C, pDM_Odm->RFCalibrateInfo.IQK_BB_backup, IQK_BB_REG_NUM);
+	}
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQ Calibration for %s for %d times\n", (is2T ? "2T2R" : "1T1R"), t));
+
+	_PHY_PathADDAOn8723B(padapter, ADDA_REG, true, is2T);
+
+/* no serial mode */
+
+	/* save RF path for 8723B */
+/* 	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord); */
+/* 	Path_SEL_RF = PHY_QueryRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xb0, 0xfffff); */
+
+	/* MAC settings */
+	_PHY_MACSettingCalibration8723B(padapter, IQK_MAC_REG, pDM_Odm->RFCalibrateInfo.IQK_MAC_backup);
+
+	/* BB setting */
+	/* PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_RFMOD, BIT24, 0x00); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rCCK0_AFESetting, 0x0f000000, 0xf);
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_TRxPathEnable, bMaskDWord, 0x03a05600);
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_TRMuxPar, bMaskDWord, 0x000800e4);
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_XCD_RFInterfaceSW, bMaskDWord, 0x22204000);
+
+
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_XAB_RFInterfaceSW, BIT10, 0x01); */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_XAB_RFInterfaceSW, BIT26, 0x01); */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_XA_RFInterfaceOE, BIT10, 0x00); */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_XB_RFInterfaceOE, BIT10, 0x00); */
+
+
+/* RX IQ calibration setting for 8723B D cut large current issue when leaving IPS */
+
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x30000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7fb7);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0x43, bRFRegOffsetMask, 0x60fbd);
+
+/* path A TX IQK */
+	for (i = 0 ; i < retryCount ; i++) {
+		PathAOK = phy_PathA_IQK_8723B(padapter, is2T, RF_Path);
+/* 		if (PathAOK == 0x03) { */
+		if (PathAOK == 0x01) {
+			/*  Path A Tx IQK Success */
+			PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+			pDM_Odm->RFCalibrateInfo.TxLOK[ODM_RF_PATH_A] = PHY_QueryRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0x8, bRFRegOffsetMask);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A Tx IQK Success!!\n"));
+				result[t][0] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord)&0x3FF0000)>>16;
+				result[t][1] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord)&0x3FF0000)>>16;
+			break;
+		}
+	}
+
+/* path A RXIQK */
+	for (i = 0 ; i < retryCount ; i++) {
+		PathAOK = phy_PathA_RxIQK8723B(padapter, is2T, RF_Path);
+		if (PathAOK == 0x03) {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path A Rx IQK Success!!\n"));
+/* 				result[t][0] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord)&0x3FF0000)>>16; */
+/* 				result[t][1] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord)&0x3FF0000)>>16; */
+				result[t][2] = (PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_Before_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
+				result[t][3] = (PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
+			break;
+		} else {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A Rx IQK Fail!!\n"));
+		}
+	}
+
+	if (0x00 == PathAOK) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A IQK failed!!\n"));
+	}
+
+/* path B IQK */
+	if (is2T) {
+
+		/* path B TX IQK */
+		for (i = 0 ; i < retryCount ; i++) {
+			PathBOK = phy_PathB_IQK_8723B(padapter);
+			if (PathBOK == 0x01) {
+				/*  Path B Tx IQK Success */
+				PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+				pDM_Odm->RFCalibrateInfo.TxLOK[ODM_RF_PATH_B] = PHY_QueryRFReg(pDM_Odm->Adapter, ODM_RF_PATH_B, 0x8, bRFRegOffsetMask);
+
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B Tx IQK Success!!\n"));
+				result[t][4] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord)&0x3FF0000)>>16;
+				result[t][5] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord)&0x3FF0000)>>16;
+				break;
+			}
+		}
+
+/* path B RX IQK */
+		for (i = 0 ; i < retryCount ; i++) {
+			PathBOK = phy_PathB_RxIQK8723B(padapter, is2T);
+			if (PathBOK == 0x03) {
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path B Rx IQK Success!!\n"));
+/* 				result[t][0] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_Before_IQK_A, bMaskDWord)&0x3FF0000)>>16; */
+/* 				result[t][1] = (PHY_QueryBBReg(pDM_Odm->Adapter, rTx_Power_After_IQK_A, bMaskDWord)&0x3FF0000)>>16; */
+				result[t][6] = (PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_Before_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
+				result[t][7] = (PHY_QueryBBReg(pDM_Odm->Adapter, rRx_Power_After_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
+				break;
+			} else {
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B Rx IQK Fail!!\n"));
+			}
+		}
+
+/* Allen end */
+		if (0x00 == PathBOK) {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B IQK failed!!\n"));
+		}
+	}
+
+	/* Back to BB mode, load original value */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK:Back to BB mode, load original value!\n"));
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0);
+
+	if (t != 0) {
+		/*  Reload ADDA power saving parameters */
+		_PHY_ReloadADDARegisters8723B(padapter, ADDA_REG, pDM_Odm->RFCalibrateInfo.ADDA_backup, IQK_ADDA_REG_NUM);
+
+		/*  Reload MAC parameters */
+		_PHY_ReloadMACRegisters8723B(padapter, IQK_MAC_REG, pDM_Odm->RFCalibrateInfo.IQK_MAC_backup);
+
+		_PHY_ReloadADDARegisters8723B(padapter, IQK_BB_REG_92C, pDM_Odm->RFCalibrateInfo.IQK_BB_backup, IQK_BB_REG_NUM);
+
+		/* Reload RF path */
+/* 		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB); */
+/* 		PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xb0, 0xfffff, Path_SEL_RF); */
+
+		/* Allen initial gain 0xc50 */
+		/*  Restore RX initial gain */
+		PHY_SetBBReg(pDM_Odm->Adapter, 0xc50, bMaskByte0, 0x50);
+		PHY_SetBBReg(pDM_Odm->Adapter, 0xc50, bMaskByte0, tmp0xc50);
+		if (is2T) {
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xc58, bMaskByte0, 0x50);
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xc58, bMaskByte0, tmp0xc58);
+		}
+
+		/* load 0xe30 IQC default value */
+		PHY_SetBBReg(pDM_Odm->Adapter, rTx_IQK_Tone_A, bMaskDWord, 0x01008c00);
+		PHY_SetBBReg(pDM_Odm->Adapter, rRx_IQK_Tone_A, bMaskDWord, 0x01008c00);
+
+	}
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("phy_IQCalibrate_8723B() <==\n"));
+
+}
+
+
+static void phy_LCCalibrate_8723B(PDM_ODM_T pDM_Odm, bool is2T)
+{
+	u8 tmpReg;
+	u32 RF_Amode = 0, RF_Bmode = 0, LC_Cal;
+	struct adapter *padapter = pDM_Odm->Adapter;
+
+	/* Check continuous TX and Packet TX */
+	tmpReg = rtw_read8(pDM_Odm->Adapter, 0xd03);
+
+	if ((tmpReg&0x70) != 0)			/* Deal with contisuous TX case */
+		rtw_write8(pDM_Odm->Adapter, 0xd03, tmpReg&0x8F);	/* disable all continuous TX */
+	else							/*  Deal with Packet TX case */
+		rtw_write8(pDM_Odm->Adapter, REG_TXPAUSE, 0xFF);		/*  block all queues */
+
+	if ((tmpReg&0x70) != 0) {
+		/* 1. Read original RF mode */
+		/* Path-A */
+		RF_Amode = PHY_QueryRFReg(padapter, ODM_RF_PATH_A, RF_AC, bMask12Bits);
+
+		/* Path-B */
+		if (is2T)
+			RF_Bmode = PHY_QueryRFReg(padapter, ODM_RF_PATH_B, RF_AC, bMask12Bits);
+
+		/* 2. Set RF mode = standby mode */
+		/* Path-A */
+		PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_AC, bMask12Bits, (RF_Amode&0x8FFFF)|0x10000);
+
+		/* Path-B */
+		if (is2T)
+			PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_B, RF_AC, bMask12Bits, (RF_Bmode&0x8FFFF)|0x10000);
+	}
+
+	/* 3. Read RF reg18 */
+	LC_Cal = PHY_QueryRFReg(padapter, ODM_RF_PATH_A, RF_CHNLBW, bMask12Bits);
+
+	/* 4. Set LC calibration begin	bit15 */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xB0, bRFRegOffsetMask, 0xDFBE0); /*  LDO ON */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_CHNLBW, bMask12Bits, LC_Cal|0x08000);
+
+	mdelay(100);
+
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xB0, bRFRegOffsetMask, 0xDFFE0); /*  LDO OFF */
+
+	/*  Channel 10 LC calibration issue for 8723bs with 26M xtal */
+	if (pDM_Odm->SupportInterface == ODM_ITRF_SDIO && pDM_Odm->PackageType >= 0x2) {
+		PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_CHNLBW, bMask12Bits, LC_Cal);
+	}
+
+	/* Restore original situation */
+	if ((tmpReg&0x70) != 0) { /* Deal with contisuous TX case */
+		/* Path-A */
+		rtw_write8(pDM_Odm->Adapter, 0xd03, tmpReg);
+		PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_AC, bMask12Bits, RF_Amode);
+
+		/* Path-B */
+		if (is2T)
+			PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_B, RF_AC, bMask12Bits, RF_Bmode);
+	} else /*  Deal with Packet TX case */
+		rtw_write8(pDM_Odm->Adapter, REG_TXPAUSE, 0x00);
+}
+
+/* Analog Pre-distortion calibration */
+#define		APK_BB_REG_NUM	8
+#define		APK_CURVE_REG_NUM 4
+#define		PATH_NUM		2
+
+#define		DP_BB_REG_NUM		7
+#define		DP_RF_REG_NUM		1
+#define		DP_RETRY_LIMIT		10
+#define		DP_PATH_NUM	2
+#define		DP_DPK_NUM			3
+#define		DP_DPK_VALUE_NUM	2
+
+
+
+/* IQK version:V2.5    20140123 */
+/* IQK is controlled by Is2ant, RF path */
+void PHY_IQCalibrate_8723B(
+	struct adapter *padapter,
+	bool bReCovery,
+	bool bRestore,
+	bool Is2ant,	/* false:1ant, true:2-ant */
+	u8 RF_Path	/* 0:S1, 1:S0 */
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	s32 result[4][8];	/* last is final result */
+	u8 i, final_candidate, Indexforchannel;
+	bool bPathAOK, bPathBOK;
+	s32 RegE94, RegE9C, RegEA4, RegEAC, RegEB4, RegEBC, RegEC4, RegECC, RegTmp = 0;
+	bool is12simular, is13simular, is23simular;
+	bool bSingleTone = false, bCarrierSuppression = false;
+	u32 IQK_BB_REG_92C[IQK_BB_REG_NUM] = {
+		rOFDM0_XARxIQImbalance,
+		rOFDM0_XBRxIQImbalance,
+		rOFDM0_ECCAThreshold,
+		rOFDM0_AGCRSSITable,
+		rOFDM0_XATxIQImbalance,
+		rOFDM0_XBTxIQImbalance,
+		rOFDM0_XCTxAFE,
+		rOFDM0_XDTxAFE,
+		rOFDM0_RxIQExtAnta
+	};
+/* 	u32 		Path_SEL_BB = 0; */
+	u32 		GNT_BT_default;
+	u32 		StartTime;
+	s32			ProgressingTime;
+
+	if (ODM_CheckPowerStatus(padapter) == false)
+		return;
+
+	if (!(pDM_Odm->SupportAbility & ODM_RF_CALIBRATION))
+		return;
+
+	/*  20120213<Kordan> Turn on when continuous Tx to pass lab testing. (required by Edlu) */
+	if (bSingleTone || bCarrierSuppression)
+		return;
+
+#if DISABLE_BB_RF
+	return;
+#endif
+	if (pDM_Odm->RFCalibrateInfo.bIQKInProgress)
+		return;
+
+
+	pDM_Odm->RFCalibrateInfo.bIQKInProgress = true;
+
+	if (bRestore) {
+		u32 offset, data;
+		u8 path, bResult = SUCCESS;
+		PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+
+		path = (PHY_QueryBBReg(pDM_Odm->Adapter, rS0S1_PathSwitch, bMaskByte0) == 0x00) ? ODM_RF_PATH_A : ODM_RF_PATH_B;
+
+		/*  Restore TX IQK */
+		for (i = 0; i < 3; ++i) {
+			offset = pRFCalibrateInfo->TxIQC_8723B[path][i][0];
+			data = pRFCalibrateInfo->TxIQC_8723B[path][i][1];
+			if ((offset == 0) || (data == 0)) {
+				DBG_871X(
+					"%s =>path:%s Restore TX IQK result failed\n",
+					__func__,
+					(path == ODM_RF_PATH_A)?"A":"B"
+				);
+				bResult = FAIL;
+				break;
+			}
+			/* RT_TRACE(_module_mp_, _drv_notice_, ("Switch to S1 TxIQC(offset, data) = (0x%X, 0x%X)\n", offset, data)); */
+			PHY_SetBBReg(pDM_Odm->Adapter, offset, bMaskDWord, data);
+		}
+
+		/*  Restore RX IQK */
+		for (i = 0; i < 2; ++i) {
+			offset = pRFCalibrateInfo->RxIQC_8723B[path][i][0];
+			data = pRFCalibrateInfo->RxIQC_8723B[path][i][1];
+			if ((offset == 0) || (data == 0)) {
+				DBG_871X(
+					"%s =>path:%s  Restore RX IQK result failed\n",
+					__func__,
+					(path == ODM_RF_PATH_A)?"A":"B"
+				);
+				bResult = FAIL;
+				break;
+			}
+			/* RT_TRACE(_module_mp_, _drv_notice_, ("Switch to S1 RxIQC (offset, data) = (0x%X, 0x%X)\n", offset, data)); */
+			PHY_SetBBReg(pDM_Odm->Adapter, offset, bMaskDWord, data);
+		}
+
+		if (pDM_Odm->RFCalibrateInfo.TxLOK[ODM_RF_PATH_A] == 0) {
+			DBG_871X("%s => Restore Path-A TxLOK result failed\n", __func__);
+			bResult = FAIL;
+		} else {
+			PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXM_IDAC, bRFRegOffsetMask, pDM_Odm->RFCalibrateInfo.TxLOK[ODM_RF_PATH_A]);
+			PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_B, RF_TXM_IDAC, bRFRegOffsetMask, pDM_Odm->RFCalibrateInfo.TxLOK[ODM_RF_PATH_B]);
+		}
+
+		if (bResult == SUCCESS)
+			return;
+	}
+
+	if (bReCovery) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD, ("PHY_IQCalibrate_8723B: Return due to bReCovery!\n"));
+		_PHY_ReloadADDARegisters8723B(padapter, IQK_BB_REG_92C, pDM_Odm->RFCalibrateInfo.IQK_BB_backup_recover, 9);
+		return;
+	}
+	StartTime = jiffies;
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK:Start!!!\n"));
+
+	/* save default GNT_BT */
+	GNT_BT_default = PHY_QueryBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord);
+	/*  Save RF Path */
+/* 	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord); */
+/* 	Path_SEL_RF = PHY_QueryRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xb0, 0xfffff); */
+
+    /* set GNT_BT = 0, pause BT traffic */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, BIT12, 0x0); */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, BIT11, 0x1); */
+
+
+	for (i = 0; i < 8; i++) {
+		result[0][i] = 0;
+		result[1][i] = 0;
+		result[2][i] = 0;
+		result[3][i] = 0;
+	}
+
+	final_candidate = 0xff;
+	bPathAOK = false;
+	bPathBOK = false;
+	is12simular = false;
+	is23simular = false;
+	is13simular = false;
+
+
+	for (i = 0; i < 3; i++) {
+		phy_IQCalibrate_8723B(padapter, result, i, Is2ant, RF_Path);
+
+		if (i == 1) {
+			is12simular = phy_SimularityCompare_8723B(padapter, result, 0, 1);
+			if (is12simular) {
+				final_candidate = 0;
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK: is12simular final_candidate is %x\n", final_candidate));
+				break;
+			}
+		}
+
+		if (i == 2) {
+			is13simular = phy_SimularityCompare_8723B(padapter, result, 0, 2);
+			if (is13simular) {
+				final_candidate = 0;
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK: is13simular final_candidate is %x\n", final_candidate));
+
+				break;
+			}
+
+			is23simular = phy_SimularityCompare_8723B(padapter, result, 1, 2);
+			if (is23simular) {
+				final_candidate = 1;
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK: is23simular final_candidate is %x\n", final_candidate));
+			} else {
+				for (i = 0; i < 8; i++)
+					RegTmp += result[3][i];
+
+				if (RegTmp != 0)
+					final_candidate = 3;
+				else
+					final_candidate = 0xFF;
+			}
+		}
+	}
+/* 	RT_TRACE(COMP_INIT, DBG_LOUD, ("Release Mutex in IQCalibrate\n")); */
+
+	for (i = 0; i < 4; i++) {
+		RegE94 = result[i][0];
+		RegE9C = result[i][1];
+		RegEA4 = result[i][2];
+		RegEAC = result[i][3];
+		RegEB4 = result[i][4];
+		RegEBC = result[i][5];
+		RegEC4 = result[i][6];
+		RegECC = result[i][7];
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK: RegE94 =%x RegE9C =%x RegEA4 =%x RegEAC =%x RegEB4 =%x RegEBC =%x RegEC4 =%x RegECC =%x\n ", RegE94, RegE9C, RegEA4, RegEAC, RegEB4, RegEBC, RegEC4, RegECC));
+	}
+
+	if (final_candidate != 0xff) {
+		pDM_Odm->RFCalibrateInfo.RegE94 = RegE94 = result[final_candidate][0];
+		pDM_Odm->RFCalibrateInfo.RegE9C = RegE9C = result[final_candidate][1];
+		RegEA4 = result[final_candidate][2];
+		RegEAC = result[final_candidate][3];
+		pDM_Odm->RFCalibrateInfo.RegEB4 = RegEB4 = result[final_candidate][4];
+		pDM_Odm->RFCalibrateInfo.RegEBC = RegEBC = result[final_candidate][5];
+		RegEC4 = result[final_candidate][6];
+		RegECC = result[final_candidate][7];
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK: final_candidate is %x\n", final_candidate));
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK: RegE94 =%x RegE9C =%x RegEA4 =%x RegEAC =%x RegEB4 =%x RegEBC =%x RegEC4 =%x RegECC =%x\n ", RegE94, RegE9C, RegEA4, RegEAC, RegEB4, RegEBC, RegEC4, RegECC));
+		bPathAOK = bPathBOK = true;
+	} else {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK: FAIL use default value\n"));
+
+		pDM_Odm->RFCalibrateInfo.RegE94 = pDM_Odm->RFCalibrateInfo.RegEB4 = 0x100;	/* X default value */
+		pDM_Odm->RFCalibrateInfo.RegE9C = pDM_Odm->RFCalibrateInfo.RegEBC = 0x0;		/* Y default value */
+	}
+
+	{
+		if (RegE94 != 0)
+			_PHY_PathAFillIQKMatrix8723B(padapter, bPathAOK, result, final_candidate, (RegEA4 == 0));
+	}
+	{
+		if (RegEB4 != 0)
+			_PHY_PathBFillIQKMatrix8723B(padapter, bPathBOK, result, final_candidate, (RegEC4 == 0));
+	}
+
+	Indexforchannel = ODM_GetRightChnlPlaceforIQK(pHalData->CurrentChannel);
+
+/* To Fix BSOD when final_candidate is 0xff */
+/* by sherry 20120321 */
+	if (final_candidate < 4) {
+		for (i = 0; i < IQK_Matrix_REG_NUM; i++)
+			pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[Indexforchannel].Value[0][i] = result[final_candidate][i];
+		pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[Indexforchannel].bIQKDone = true;
+	}
+	/* RT_DISP(FINIT, INIT_IQK, ("\nIQK OK Indexforchannel %d.\n", Indexforchannel)); */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("\nIQK OK Indexforchannel %d.\n", Indexforchannel));
+
+	_PHY_SaveADDARegisters8723B(padapter, IQK_BB_REG_92C, pDM_Odm->RFCalibrateInfo.IQK_BB_backup_recover, 9);
+
+	/* restore GNT_BT */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, GNT_BT_default);
+	/*  Restore RF Path */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, Path_SEL_BB); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xb0, 0xfffff, Path_SEL_RF); */
+
+	/* Resotr RX mode table parameter */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xe6177);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0x43, bRFRegOffsetMask, 0x300bd);
+
+	/* set GNT_BT = HW control */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, BIT12, 0x0); */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, BIT11, 0x0); */
+
+	if (Is2ant) {
+		if (RF_Path == 0x0)	/* S1 */
+			ODM_SetIQCbyRFpath(pDM_Odm, 0);
+		else	/* S0 */
+			ODM_SetIQCbyRFpath(pDM_Odm, 1);
+	}
+
+	pDM_Odm->RFCalibrateInfo.bIQKInProgress = false;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK finished\n"));
+	ProgressingTime = jiffies_to_msecs(jiffies - StartTime);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK ProgressingTime = %d\n", ProgressingTime));
+
+
+}
+
+
+void PHY_LCCalibrate_8723B(PDM_ODM_T pDM_Odm)
+{
+	bool		bSingleTone = false, bCarrierSuppression = false;
+	u32 		timeout = 2000, timecount = 0;
+	u32 		StartTime;
+	s32			ProgressingTime;
+
+#if DISABLE_BB_RF
+	return;
+#endif
+
+	if (!(pDM_Odm->SupportAbility & ODM_RF_CALIBRATION))
+		return;
+
+	/*  20120213<Kordan> Turn on when continuous Tx to pass lab testing. (required by Edlu) */
+	if (bSingleTone || bCarrierSuppression)
+		return;
+
+	StartTime = jiffies;
+	while (*(pDM_Odm->pbScanInProcess) && timecount < timeout) {
+		mdelay(50);
+		timecount += 50;
+	}
+
+	pDM_Odm->RFCalibrateInfo.bLCKInProgress = true;
+
+
+	phy_LCCalibrate_8723B(pDM_Odm, false);
+
+
+	pDM_Odm->RFCalibrateInfo.bLCKInProgress = false;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LCK:Finish!!!interface %d\n", pDM_Odm->InterfaceIndex));
+	ProgressingTime = jiffies_to_msecs(jiffies - StartTime);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("LCK ProgressingTime = %d\n", ProgressingTime));
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalPhyRf_8723B.h linux/3rdparty/rtl8723bs/hal/HalPhyRf_8723B.h
--- linux.old/3rdparty/rtl8723bs/hal/HalPhyRf_8723B.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalPhyRf_8723B.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,83 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#ifndef __HAL_PHY_RF_8723B_H__
+#define __HAL_PHY_RF_8723B_H__
+
+/*--------------------------Define Parameters-------------------------------*/
+#define	IQK_DELAY_TIME_8723B		20		/* ms */
+#define IQK_DEFERRED_TIME_8723B		4
+#define	index_mapping_NUM_8723B		15
+#define AVG_THERMAL_NUM_8723B		4
+#define	RF_T_METER_8723B					0x42	/*  */
+
+
+void ConfigureTxpowerTrack_8723B(PTXPWRTRACK_CFG	pConfig);
+
+void DoIQK_8723B(
+	PDM_ODM_T pDM_Odm,
+	u8 DeltaThermalIndex,
+	u8 ThermalValue,
+	u8 Threshold
+);
+
+void ODM_TxPwrTrackSetPwr_8723B(
+	PDM_ODM_T pDM_Odm,
+	PWRTRACK_METHOD Method,
+	u8 RFPath,
+	u8 ChannelMappedIndex
+);
+
+/* 1 7. IQK */
+void PHY_IQCalibrate_8723B(
+	struct adapter *Adapter,
+	bool bReCovery,
+	bool bRestore,
+	bool Is2ant,
+	u8 RF_Path
+);
+
+void ODM_SetIQCbyRFpath(PDM_ODM_T pDM_Odm, u32 RFpath);
+
+/*  */
+/*  LC calibrate */
+/*  */
+void PHY_LCCalibrate_8723B(PDM_ODM_T pDM_Odm);
+
+/*  */
+/*  AP calibrate */
+/*  */
+void PHY_DigitalPredistortion_8723B(struct adapter *padapter);
+
+
+void _PHY_SaveADDARegisters_8723B(
+	struct adapter *padapter,
+	u32 *ADDAReg,
+	u32 *ADDABackup,
+	u32 RegisterNum
+);
+
+void _PHY_PathADDAOn_8723B(
+	struct adapter *padapter,
+	u32 *ADDAReg,
+	bool isPathAOn,
+	bool is2T
+);
+
+void _PHY_MACSettingCalibration_8723B(
+	struct adapter *padapter, u32 *MACReg, u32 *MACBackup
+);
+
+#endif /*  #ifndef __HAL_PHY_RF_8188E_H__ */
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalPhyRf.c linux/3rdparty/rtl8723bs/hal/HalPhyRf.c
--- linux.old/3rdparty/rtl8723bs/hal/HalPhyRf.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalPhyRf.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,662 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+/* include "Mp_Precomp.h" */
+#include "odm_precomp.h"
+
+
+#define CALCULATE_SWINGTALBE_OFFSET(_offset, _direction, _size, _deltaThermal) \
+	do {\
+		for (_offset = 0; _offset < _size; _offset++) {\
+			if (_deltaThermal < thermalThreshold[_direction][_offset]) {\
+				if (_offset != 0)\
+					_offset--;\
+				break;\
+			} \
+		} \
+		if (_offset >= _size)\
+			_offset = _size-1;\
+	} while (0)
+
+
+void ConfigureTxpowerTrack(PDM_ODM_T pDM_Odm, PTXPWRTRACK_CFG pConfig)
+{
+	ConfigureTxpowerTrack_8723B(pConfig);
+}
+
+/*  */
+/*  <20121113, Kordan> This function should be called when TxAGC changed. */
+/*  Otherwise the previous compensation is gone, because we record the */
+/*  delta of temperature between two TxPowerTracking watch dogs. */
+/*  */
+/*  NOTE: If Tx BB swing or Tx scaling is varified during run-time, still */
+/*        need to call this function. */
+/*  */
+void ODM_ClearTxPowerTrackingState(PDM_ODM_T pDM_Odm)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(pDM_Odm->Adapter);
+	u8 p = 0;
+
+	pDM_Odm->BbSwingIdxCckBase = pDM_Odm->DefaultCckIndex;
+	pDM_Odm->BbSwingIdxCck = pDM_Odm->DefaultCckIndex;
+	pDM_Odm->RFCalibrateInfo.CCK_index = 0;
+
+	for (p = ODM_RF_PATH_A; p < MAX_RF_PATH; ++p) {
+		pDM_Odm->BbSwingIdxOfdmBase[p] = pDM_Odm->DefaultOfdmIndex;
+		pDM_Odm->BbSwingIdxOfdm[p] = pDM_Odm->DefaultOfdmIndex;
+		pDM_Odm->RFCalibrateInfo.OFDM_index[p] = pDM_Odm->DefaultOfdmIndex;
+
+		pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = 0;
+		pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[p] = 0;
+		pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[p] = 0;
+		pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = 0;
+
+		/*  Initial Mix mode power tracking */
+		pDM_Odm->Absolute_OFDMSwingIdx[p] = 0;
+		pDM_Odm->Remnant_OFDMSwingIdx[p] = 0;
+	}
+
+	/* Initial at Modify Tx Scaling Mode */
+	pDM_Odm->Modify_TxAGC_Flag_PathA = false;
+	/* Initial at Modify Tx Scaling Mode */
+	pDM_Odm->Modify_TxAGC_Flag_PathB = false;
+	pDM_Odm->Remnant_CCKSwingIdx = 0;
+	pDM_Odm->RFCalibrateInfo.ThermalValue = pHalData->EEPROMThermalMeter;
+	pDM_Odm->RFCalibrateInfo.ThermalValue_IQK = pHalData->EEPROMThermalMeter;
+	pDM_Odm->RFCalibrateInfo.ThermalValue_LCK = pHalData->EEPROMThermalMeter;
+}
+
+void ODM_TXPowerTrackingCallback_ThermalMeter(struct adapter *Adapter)
+{
+
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	u8 ThermalValue = 0, delta, delta_LCK, delta_IQK, p = 0, i = 0;
+	u8 ThermalValue_AVG_count = 0;
+	u32 ThermalValue_AVG = 0;
+
+	u8 OFDM_min_index = 0;  /*  OFDM BB Swing should be less than +3.0dB, which is required by Arthur */
+	u8 Indexforchannel = 0; /*  GetRightChnlPlaceforIQK(pHalData->CurrentChannel) */
+
+	TXPWRTRACK_CFG c;
+
+
+	/* 4 1. The following TWO tables decide the final index of OFDM/CCK swing table. */
+	u8 *deltaSwingTableIdx_TUP_A;
+	u8 *deltaSwingTableIdx_TDOWN_A;
+	u8 *deltaSwingTableIdx_TUP_B;
+	u8 *deltaSwingTableIdx_TDOWN_B;
+
+	/* 4 2. Initilization (7 steps in total) */
+
+	ConfigureTxpowerTrack(pDM_Odm, &c);
+
+	(*c.GetDeltaSwingTable)(
+		pDM_Odm,
+		(u8 **)&deltaSwingTableIdx_TUP_A,
+		(u8 **)&deltaSwingTableIdx_TDOWN_A,
+		(u8 **)&deltaSwingTableIdx_TUP_B,
+		(u8 **)&deltaSwingTableIdx_TDOWN_B
+	);
+
+	/* cosa add for debug */
+	pDM_Odm->RFCalibrateInfo.TXPowerTrackingCallbackCnt++;
+	pDM_Odm->RFCalibrateInfo.bTXPowerTrackingInit = true;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_TX_PWR_TRACK,
+		ODM_DBG_LOUD,
+		(
+			"===>ODM_TXPowerTrackingCallback_ThermalMeter,\npDM_Odm->BbSwingIdxCckBase: %d, pDM_Odm->BbSwingIdxOfdmBase[A]: %d, pDM_Odm->DefaultOfdmIndex: %d\n",
+			pDM_Odm->BbSwingIdxCckBase,
+			pDM_Odm->BbSwingIdxOfdmBase[ODM_RF_PATH_A],
+			pDM_Odm->DefaultOfdmIndex
+		)
+	);
+
+	ThermalValue = (u8)PHY_QueryRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, c.ThermalRegAddr, 0xfc00);	/* 0x42: RF Reg[15:10] 88E */
+	if (
+		!pDM_Odm->RFCalibrateInfo.TxPowerTrackControl ||
+		pHalData->EEPROMThermalMeter == 0 ||
+		pHalData->EEPROMThermalMeter == 0xFF
+	)
+		return;
+
+	/* 4 3. Initialize ThermalValues of RFCalibrateInfo */
+
+	if (pDM_Odm->RFCalibrateInfo.bReloadtxpowerindex)
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+			("reload ofdm index for band switch\n")
+		);
+
+	/* 4 4. Calculate average thermal meter */
+
+	pDM_Odm->RFCalibrateInfo.ThermalValue_AVG[pDM_Odm->RFCalibrateInfo.ThermalValue_AVG_index] = ThermalValue;
+	pDM_Odm->RFCalibrateInfo.ThermalValue_AVG_index++;
+	if (pDM_Odm->RFCalibrateInfo.ThermalValue_AVG_index == c.AverageThermalNum)   /* Average times =  c.AverageThermalNum */
+		pDM_Odm->RFCalibrateInfo.ThermalValue_AVG_index = 0;
+
+	for (i = 0; i < c.AverageThermalNum; i++) {
+		if (pDM_Odm->RFCalibrateInfo.ThermalValue_AVG[i]) {
+			ThermalValue_AVG += pDM_Odm->RFCalibrateInfo.ThermalValue_AVG[i];
+			ThermalValue_AVG_count++;
+		}
+	}
+
+	/* Calculate Average ThermalValue after average enough times */
+	if (ThermalValue_AVG_count) {
+		ThermalValue = (u8)(ThermalValue_AVG / ThermalValue_AVG_count);
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK,
+			ODM_DBG_LOUD,
+			(
+				"AVG Thermal Meter = 0x%X, EFUSE Thermal Base = 0x%X\n",
+				ThermalValue,
+				pHalData->EEPROMThermalMeter
+			)
+		);
+	}
+
+	/* 4 5. Calculate delta, delta_LCK, delta_IQK. */
+	/* delta" here is used to determine whether thermal value changes or not. */
+	delta =
+		(ThermalValue > pDM_Odm->RFCalibrateInfo.ThermalValue) ?
+		(ThermalValue - pDM_Odm->RFCalibrateInfo.ThermalValue) :
+		(pDM_Odm->RFCalibrateInfo.ThermalValue - ThermalValue);
+	delta_LCK =
+		(ThermalValue > pDM_Odm->RFCalibrateInfo.ThermalValue_LCK) ?
+		(ThermalValue - pDM_Odm->RFCalibrateInfo.ThermalValue_LCK) :
+		(pDM_Odm->RFCalibrateInfo.ThermalValue_LCK - ThermalValue);
+	delta_IQK =
+		(ThermalValue > pDM_Odm->RFCalibrateInfo.ThermalValue_IQK) ?
+		(ThermalValue - pDM_Odm->RFCalibrateInfo.ThermalValue_IQK) :
+		(pDM_Odm->RFCalibrateInfo.ThermalValue_IQK - ThermalValue);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_TX_PWR_TRACK,
+		ODM_DBG_LOUD,
+		(
+			"(delta, delta_LCK, delta_IQK) = (%d, %d, %d)\n",
+			delta,
+			delta_LCK,
+			delta_IQK
+		)
+	);
+
+	/* 4 6. If necessary, do LCK. */
+	/*  Delta temperature is equal to or larger than 20 centigrade. */
+	if (delta_LCK >= c.Threshold_IQK) {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK,
+			ODM_DBG_LOUD,
+			(
+				"delta_LCK(%d) >= Threshold_IQK(%d)\n",
+				delta_LCK,
+				c.Threshold_IQK
+			)
+		);
+		pDM_Odm->RFCalibrateInfo.ThermalValue_LCK = ThermalValue;
+		if (c.PHY_LCCalibrate)
+			(*c.PHY_LCCalibrate)(pDM_Odm);
+	}
+
+	/* 3 7. If necessary, move the index of swing table to adjust Tx power. */
+	if (delta > 0 && pDM_Odm->RFCalibrateInfo.TxPowerTrackControl) {
+		/* delta" here is used to record the absolute value of differrence. */
+		delta =
+			ThermalValue > pHalData->EEPROMThermalMeter ?
+			(ThermalValue - pHalData->EEPROMThermalMeter) :
+			(pHalData->EEPROMThermalMeter - ThermalValue);
+
+		if (delta >= TXPWR_TRACK_TABLE_SIZE)
+			delta = TXPWR_TRACK_TABLE_SIZE - 1;
+
+		/* 4 7.1 The Final Power Index = BaseIndex + PowerIndexOffset */
+		if (ThermalValue > pHalData->EEPROMThermalMeter) {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"deltaSwingTableIdx_TUP_A[%d] = %d\n",
+					delta,
+					deltaSwingTableIdx_TUP_A[delta]
+				)
+			);
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[ODM_RF_PATH_A] =
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_A];
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_A] =
+				deltaSwingTableIdx_TUP_A[delta];
+
+			/*  Record delta swing for mix mode power tracking */
+			pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A] =
+				deltaSwingTableIdx_TUP_A[delta];
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"******Temp is higher and pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+					pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A]
+				)
+			);
+
+			if (c.RfPathCount > 1) {
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"deltaSwingTableIdx_TUP_B[%d] = %d\n",
+						delta,
+						deltaSwingTableIdx_TUP_B[delta]
+					)
+				);
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[ODM_RF_PATH_B] =
+					pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_B];
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_B] =
+					deltaSwingTableIdx_TUP_B[delta];
+
+				/*  Record delta swing for mix mode power tracking */
+				pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B] =
+					deltaSwingTableIdx_TUP_B[delta];
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"******Temp is higher and pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B] = %d\n",
+						pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B]
+					)
+				);
+			}
+
+		} else {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"deltaSwingTableIdx_TDOWN_A[%d] = %d\n",
+					delta,
+					deltaSwingTableIdx_TDOWN_A[delta]
+				)
+			);
+
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[ODM_RF_PATH_A] =
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_A];
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_A] =
+				-1 * deltaSwingTableIdx_TDOWN_A[delta];
+
+			/*  Record delta swing for mix mode power tracking */
+			pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A] =
+				-1 * deltaSwingTableIdx_TDOWN_A[delta];
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"******Temp is lower and pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+					pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A]
+				)
+			);
+
+			if (c.RfPathCount > 1) {
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"deltaSwingTableIdx_TDOWN_B[%d] = %d\n",
+						delta,
+						deltaSwingTableIdx_TDOWN_B[delta]
+					)
+				);
+
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[ODM_RF_PATH_B] =
+					pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_B];
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_B] =
+					-1 * deltaSwingTableIdx_TDOWN_B[delta];
+
+				 /*  Record delta swing for mix mode power tracking */
+				pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B] =
+					-1 * deltaSwingTableIdx_TDOWN_B[delta];
+
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"******Temp is lower and pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B] = %d\n",
+						pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B]
+					)
+				);
+			}
+		}
+
+		for (p = ODM_RF_PATH_A; p < c.RfPathCount; p++) {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"\n\n ================================ [Path-%c] Calculating PowerIndexOffset ================================\n",
+					(p == ODM_RF_PATH_A ? 'A' : 'B')
+				)
+			);
+
+			if (
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[p] ==
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[p]
+			) /*  If Thermal value changes but lookup table value still the same */
+				pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = 0;
+			else
+				pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[p] - pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[p];      /*  Power Index Diff between 2 times Power Tracking */
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"[Path-%c] PowerIndexOffset(%d) = DeltaPowerIndex(%d) - DeltaPowerIndexLast(%d)\n",
+					(
+						p == ODM_RF_PATH_A ? 'A' : 'B'),
+						pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p],
+						pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[p],
+						pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[p]
+					)
+				);
+
+			pDM_Odm->RFCalibrateInfo.OFDM_index[p] =
+				pDM_Odm->BbSwingIdxOfdmBase[p] +
+				pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p];
+
+			pDM_Odm->RFCalibrateInfo.CCK_index =
+				pDM_Odm->BbSwingIdxCckBase +
+				pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p];
+
+			pDM_Odm->BbSwingIdxCck =
+				pDM_Odm->RFCalibrateInfo.CCK_index;
+
+			pDM_Odm->BbSwingIdxOfdm[p] =
+				pDM_Odm->RFCalibrateInfo.OFDM_index[p];
+
+			/*  *************Print BB Swing Base and Index Offset************* */
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"The 'CCK' final index(%d) = BaseIndex(%d) + PowerIndexOffset(%d)\n",
+					pDM_Odm->BbSwingIdxCck,
+					pDM_Odm->BbSwingIdxCckBase,
+					pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p]
+				)
+			);
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"The 'OFDM' final index(%d) = BaseIndex[%c](%d) + PowerIndexOffset(%d)\n",
+					pDM_Odm->BbSwingIdxOfdm[p],
+					(p == ODM_RF_PATH_A ? 'A' : 'B'),
+					pDM_Odm->BbSwingIdxOfdmBase[p],
+					pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p]
+				)
+			);
+
+			/* 4 7.1 Handle boundary conditions of index. */
+			if (pDM_Odm->RFCalibrateInfo.OFDM_index[p] > c.SwingTableSize_OFDM-1)
+				pDM_Odm->RFCalibrateInfo.OFDM_index[p] = c.SwingTableSize_OFDM-1;
+			else if (pDM_Odm->RFCalibrateInfo.OFDM_index[p] < OFDM_min_index)
+				pDM_Odm->RFCalibrateInfo.OFDM_index[p] = OFDM_min_index;
+		}
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK,
+			ODM_DBG_LOUD,
+			("\n\n ========================================================================================================\n")
+		);
+		if (pDM_Odm->RFCalibrateInfo.CCK_index > c.SwingTableSize_CCK-1)
+			pDM_Odm->RFCalibrateInfo.CCK_index = c.SwingTableSize_CCK-1;
+		/* else if (pDM_Odm->RFCalibrateInfo.CCK_index < 0) */
+			/* pDM_Odm->RFCalibrateInfo.CCK_index = 0; */
+	} else {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK,
+			ODM_DBG_LOUD,
+			(
+				"The thermal meter is unchanged or TxPowerTracking OFF(%d): ThermalValue: %d , pDM_Odm->RFCalibrateInfo.ThermalValue: %d\n",
+				pDM_Odm->RFCalibrateInfo.TxPowerTrackControl,
+				ThermalValue,
+				pDM_Odm->RFCalibrateInfo.ThermalValue
+			)
+		);
+
+			for (p = ODM_RF_PATH_A; p < c.RfPathCount; p++)
+				pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = 0;
+	}
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_TX_PWR_TRACK,
+		ODM_DBG_LOUD,
+		(
+			"TxPowerTracking: [CCK] Swing Current Index: %d, Swing Base Index: %d\n",
+			pDM_Odm->RFCalibrateInfo.CCK_index,
+			pDM_Odm->BbSwingIdxCckBase
+		)
+	);
+
+	/* Print Swing base & current */
+	for (p = ODM_RF_PATH_A; p < c.RfPathCount; p++) {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK,
+			ODM_DBG_LOUD,
+			(
+				"TxPowerTracking: [OFDM] Swing Current Index: %d, Swing Base Index[%c]: %d\n",
+				pDM_Odm->RFCalibrateInfo.OFDM_index[p],
+				(p == ODM_RF_PATH_A ? 'A' : 'B'),
+				pDM_Odm->BbSwingIdxOfdmBase[p]
+			)
+		);
+	}
+
+	if (
+		(pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_RF_PATH_A] != 0 ||
+		 pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_RF_PATH_B] != 0) &&
+		 pDM_Odm->RFCalibrateInfo.TxPowerTrackControl
+	 ) {
+		/* 4 7.2 Configure the Swing Table to adjust Tx Power. */
+
+		pDM_Odm->RFCalibrateInfo.bTxPowerChanged = true; /*  Always true after Tx Power is adjusted by power tracking. */
+		/*  */
+		/*  2012/04/23 MH According to Luke's suggestion, we can not write BB digital */
+		/*  to increase TX power. Otherwise, EVM will be bad. */
+		/*  */
+		/*  2012/04/25 MH Add for tx power tracking to set tx power in tx agc for 88E. */
+		if (ThermalValue > pDM_Odm->RFCalibrateInfo.ThermalValue) {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"Temperature Increasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+					pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_RF_PATH_A],
+					delta,
+					ThermalValue,
+					pHalData->EEPROMThermalMeter,
+					pDM_Odm->RFCalibrateInfo.ThermalValue
+				)
+			);
+
+			if (c.RfPathCount > 1)
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"Temperature Increasing(B): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+						pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_RF_PATH_B],
+						delta,
+						ThermalValue,
+						pHalData->EEPROMThermalMeter,
+						pDM_Odm->RFCalibrateInfo.ThermalValue
+					)
+				);
+
+		} else if (ThermalValue < pDM_Odm->RFCalibrateInfo.ThermalValue) { /*  Low temperature */
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"Temperature Decreasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+					pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_RF_PATH_A],
+					delta,
+					ThermalValue,
+					pHalData->EEPROMThermalMeter,
+					pDM_Odm->RFCalibrateInfo.ThermalValue
+				)
+			);
+
+			if (c.RfPathCount > 1)
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"Temperature Decreasing(B): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+						pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_RF_PATH_B],
+						delta,
+						ThermalValue,
+						pHalData->EEPROMThermalMeter,
+						pDM_Odm->RFCalibrateInfo.ThermalValue
+					)
+				);
+
+		}
+
+		if (ThermalValue > pHalData->EEPROMThermalMeter) {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"Temperature(%d) higher than PG value(%d)\n",
+					ThermalValue,
+					pHalData->EEPROMThermalMeter
+				)
+			);
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				("**********Enter POWER Tracking MIX_MODE**********\n")
+			);
+			for (p = ODM_RF_PATH_A; p < c.RfPathCount; p++)
+					(*c.ODM_TxPwrTrackSetPwr)(pDM_Odm, MIX_MODE, p, 0);
+		} else {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"Temperature(%d) lower than PG value(%d)\n",
+					ThermalValue,
+					pHalData->EEPROMThermalMeter
+				)
+			);
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				("**********Enter POWER Tracking MIX_MODE**********\n")
+			);
+			for (p = ODM_RF_PATH_A; p < c.RfPathCount; p++)
+				(*c.ODM_TxPwrTrackSetPwr)(pDM_Odm, MIX_MODE, p, Indexforchannel);
+		}
+
+		/*  Record last time Power Tracking result as base. */
+		pDM_Odm->BbSwingIdxCckBase = pDM_Odm->BbSwingIdxCck;
+		for (p = ODM_RF_PATH_A; p < c.RfPathCount; p++)
+			pDM_Odm->BbSwingIdxOfdmBase[p] = pDM_Odm->BbSwingIdxOfdm[p];
+
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+			(
+				"pDM_Odm->RFCalibrateInfo.ThermalValue = %d ThermalValue = %d\n",
+				pDM_Odm->RFCalibrateInfo.ThermalValue,
+				ThermalValue
+			)
+		);
+
+		/* Record last Power Tracking Thermal Value */
+		pDM_Odm->RFCalibrateInfo.ThermalValue = ThermalValue;
+	}
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_TX_PWR_TRACK,
+		ODM_DBG_LOUD,
+		("<===ODM_TXPowerTrackingCallback_ThermalMeter\n")
+	);
+
+	pDM_Odm->RFCalibrateInfo.TXPowercount = 0;
+}
+
+
+
+
+/* 3 ============================================================ */
+/* 3 IQ Calibration */
+/* 3 ============================================================ */
+
+u8 ODM_GetRightChnlPlaceforIQK(u8 chnl)
+{
+	u8 channel_all[ODM_TARGET_CHNL_NUM_2G_5G] = {
+		1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
+		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
+	};
+	u8 place = chnl;
+
+
+	if (chnl > 14) {
+		for (place = 14; place < sizeof(channel_all); place++) {
+			if (channel_all[place] == chnl)
+				return place-13;
+		}
+	}
+	return 0;
+
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalPhyRf.h linux/3rdparty/rtl8723bs/hal/HalPhyRf.h
--- linux.old/3rdparty/rtl8723bs/hal/HalPhyRf.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalPhyRf.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,63 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+ #ifndef __HAL_PHY_RF_H__
+ #define __HAL_PHY_RF_H__
+
+typedef enum _SPUR_CAL_METHOD {
+	PLL_RESET,
+	AFE_PHASE_SEL
+} SPUR_CAL_METHOD;
+
+typedef enum _PWRTRACK_CONTROL_METHOD {
+	BBSWING,
+	TXAGC,
+	MIX_MODE
+} PWRTRACK_METHOD;
+
+typedef void (*FuncSetPwr)(PDM_ODM_T, PWRTRACK_METHOD, u8, u8);
+typedef void (*FuncIQK)(PDM_ODM_T, u8, u8, u8);
+typedef void (*FuncLCK)(PDM_ODM_T);
+typedef void (*FuncSwing)(PDM_ODM_T, u8 **, u8 **, u8 **, u8 **);
+
+typedef struct _TXPWRTRACK_CFG {
+	u8 SwingTableSize_CCK;
+	u8 SwingTableSize_OFDM;
+	u8 Threshold_IQK;
+	u8 AverageThermalNum;
+	u8 RfPathCount;
+	u32 ThermalRegAddr;
+	FuncSetPwr ODM_TxPwrTrackSetPwr;
+	FuncIQK DoIQK;
+	FuncLCK PHY_LCCalibrate;
+	FuncSwing GetDeltaSwingTable;
+} TXPWRTRACK_CFG, *PTXPWRTRACK_CFG;
+
+void ConfigureTxpowerTrack(PDM_ODM_T pDM_Odm, PTXPWRTRACK_CFG pConfig);
+
+
+void ODM_ClearTxPowerTrackingState(PDM_ODM_T pDM_Odm);
+
+void ODM_TXPowerTrackingCallback_ThermalMeter(struct adapter *Adapter);
+
+
+
+#define ODM_TARGET_CHNL_NUM_2G_5G 59
+
+
+u8 ODM_GetRightChnlPlaceforIQK(u8 chnl);
+
+
+#endif	/*  #ifndef __HAL_PHY_RF_H__ */
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/HalPwrSeqCmd.c linux/3rdparty/rtl8723bs/hal/HalPwrSeqCmd.c
--- linux.old/3rdparty/rtl8723bs/hal/HalPwrSeqCmd.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/HalPwrSeqCmd.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,205 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+/*++
+Copyright (c) Realtek Semiconductor Corp. All rights reserved.
+
+Module Name:
+	HalPwrSeqCmd.c
+
+Abstract:
+	Implement HW Power sequence configuration CMD handling routine for Realtek devices.
+
+Major Change History:
+	When       Who               What
+	---------- ---------------   -------------------------------
+	2011-10-26 Lucas            Modify to be compatible with SD4-CE driver.
+	2011-07-07 Roger            Create.
+
+--*/
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <HalPwrSeqCmd.h>
+
+
+/*  */
+/*  Description: */
+/*  This routine deal with the Power Configuration CMDs parsing for RTL8723/RTL8188E Series IC. */
+/*  */
+/*  Assumption: */
+/*  We should follow specific format which was released from HW SD. */
+/*  */
+/*  2011.07.07, added by Roger. */
+/*  */
+u8 HalPwrSeqCmdParsing(
+	struct adapter *padapter,
+	u8 CutVersion,
+	u8 FabVersion,
+	u8 InterfaceType,
+	WLAN_PWR_CFG PwrSeqCmd[]
+)
+{
+	WLAN_PWR_CFG PwrCfgCmd = {0};
+	u8 bPollingBit = false;
+	u32 AryIdx = 0;
+	u8 value = 0;
+	u32 offset = 0;
+	u32 pollingCount = 0; /*  polling autoload done. */
+	u32 maxPollingCnt = 5000;
+
+	do {
+		PwrCfgCmd = PwrSeqCmd[AryIdx];
+
+		RT_TRACE(
+			_module_hal_init_c_,
+			_drv_info_,
+			(
+				"HalPwrSeqCmdParsing: offset(%#x) cut_msk(%#x) fab_msk(%#x) interface_msk(%#x) base(%#x) cmd(%#x) msk(%#x) value(%#x)\n",
+				GET_PWR_CFG_OFFSET(PwrCfgCmd),
+				GET_PWR_CFG_CUT_MASK(PwrCfgCmd),
+				GET_PWR_CFG_FAB_MASK(PwrCfgCmd),
+				GET_PWR_CFG_INTF_MASK(PwrCfgCmd),
+				GET_PWR_CFG_BASE(PwrCfgCmd),
+				GET_PWR_CFG_CMD(PwrCfgCmd),
+				GET_PWR_CFG_MASK(PwrCfgCmd),
+				GET_PWR_CFG_VALUE(PwrCfgCmd)
+			)
+		);
+
+		/* 2 Only Handle the command whose FAB, CUT, and Interface are matched */
+		if (
+			(GET_PWR_CFG_FAB_MASK(PwrCfgCmd) & FabVersion) &&
+			(GET_PWR_CFG_CUT_MASK(PwrCfgCmd) & CutVersion) &&
+			(GET_PWR_CFG_INTF_MASK(PwrCfgCmd) & InterfaceType)
+		) {
+			switch (GET_PWR_CFG_CMD(PwrCfgCmd)) {
+			case PWR_CMD_READ:
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_info_,
+					("HalPwrSeqCmdParsing: PWR_CMD_READ\n")
+				);
+				break;
+
+			case PWR_CMD_WRITE:
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_info_,
+					("HalPwrSeqCmdParsing: PWR_CMD_WRITE\n")
+				);
+				offset = GET_PWR_CFG_OFFSET(PwrCfgCmd);
+
+				/*  */
+				/*  <Roger_Notes> We should deal with interface specific address mapping for some interfaces, e.g., SDIO interface */
+				/*  2011.07.07. */
+				/*  */
+				if (GET_PWR_CFG_BASE(PwrCfgCmd) == PWR_BASEADDR_SDIO) {
+					/*  Read Back SDIO Local value */
+					value = SdioLocalCmd52Read1Byte(padapter, offset);
+
+					value &= ~(GET_PWR_CFG_MASK(PwrCfgCmd));
+					value |= (
+						GET_PWR_CFG_VALUE(PwrCfgCmd) &
+						GET_PWR_CFG_MASK(PwrCfgCmd)
+					);
+
+					/*  Write Back SDIO Local value */
+					SdioLocalCmd52Write1Byte(padapter, offset, value);
+				} else {
+					/*  Read the value from system register */
+					value = rtw_read8(padapter, offset);
+
+					value &= (~(GET_PWR_CFG_MASK(PwrCfgCmd)));
+					value |= (
+						GET_PWR_CFG_VALUE(PwrCfgCmd)
+						&GET_PWR_CFG_MASK(PwrCfgCmd)
+					);
+
+					/*  Write the value back to sytem register */
+					rtw_write8(padapter, offset, value);
+				}
+				break;
+
+			case PWR_CMD_POLLING:
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_info_,
+					("HalPwrSeqCmdParsing: PWR_CMD_POLLING\n")
+				);
+
+				bPollingBit = false;
+				offset = GET_PWR_CFG_OFFSET(PwrCfgCmd);
+				do {
+					if (GET_PWR_CFG_BASE(PwrCfgCmd) == PWR_BASEADDR_SDIO)
+						value = SdioLocalCmd52Read1Byte(padapter, offset);
+					else
+						value = rtw_read8(padapter, offset);
+
+					value = value&GET_PWR_CFG_MASK(PwrCfgCmd);
+					if (
+						value == (GET_PWR_CFG_VALUE(PwrCfgCmd) &
+						GET_PWR_CFG_MASK(PwrCfgCmd))
+					)
+						bPollingBit = true;
+					else
+						udelay(10);
+
+					if (pollingCount++ > maxPollingCnt) {
+						DBG_871X(
+							"Fail to polling Offset[%#x]=%02x\n",
+							offset,
+							value
+						);
+						return false;
+					}
+				} while (!bPollingBit);
+
+				break;
+
+			case PWR_CMD_DELAY:
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_info_,
+					("HalPwrSeqCmdParsing: PWR_CMD_DELAY\n")
+				);
+				if (GET_PWR_CFG_VALUE(PwrCfgCmd) == PWRSEQ_DELAY_US)
+					udelay(GET_PWR_CFG_OFFSET(PwrCfgCmd));
+				else
+					udelay(GET_PWR_CFG_OFFSET(PwrCfgCmd)*1000);
+				break;
+
+			case PWR_CMD_END:
+				/*  When this command is parsed, end the process */
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_info_,
+					("HalPwrSeqCmdParsing: PWR_CMD_END\n")
+				);
+				return true;
+
+			default:
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_err_,
+					("HalPwrSeqCmdParsing: Unknown CMD!!\n")
+				);
+				break;
+			}
+		}
+
+		AryIdx++;/* Add Array Index */
+	} while (1);
+
+	return true;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/hal_sdio.c linux/3rdparty/rtl8723bs/hal/hal_sdio.c
--- linux.old/3rdparty/rtl8723bs/hal/hal_sdio.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/hal_sdio.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,115 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#define _HAL_SDIO_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+
+u8 rtw_hal_sdio_max_txoqt_free_space(struct adapter *padapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+	if (pHalData->SdioTxOQTMaxFreeSpace < 8)
+		pHalData->SdioTxOQTMaxFreeSpace = 8;
+
+	return pHalData->SdioTxOQTMaxFreeSpace;
+}
+
+u8 rtw_hal_sdio_query_tx_freepage(
+	struct adapter *padapter, u8 PageIdx, u8 RequiredPageNum
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+	if ((pHalData->SdioTxFIFOFreePage[PageIdx]+pHalData->SdioTxFIFOFreePage[PUBLIC_QUEUE_IDX]) >= (RequiredPageNum))
+		return true;
+	else
+		return false;
+}
+
+void rtw_hal_sdio_update_tx_freepage(
+	struct adapter *padapter, u8 PageIdx, u8 RequiredPageNum
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 DedicatedPgNum = 0;
+	u8 RequiredPublicFreePgNum = 0;
+	/* _irqL irql; */
+
+	/* spin_lock_bh(&pHalData->SdioTxFIFOFreePageLock); */
+
+	DedicatedPgNum = pHalData->SdioTxFIFOFreePage[PageIdx];
+	if (RequiredPageNum <= DedicatedPgNum) {
+		pHalData->SdioTxFIFOFreePage[PageIdx] -= RequiredPageNum;
+	} else {
+		pHalData->SdioTxFIFOFreePage[PageIdx] = 0;
+		RequiredPublicFreePgNum = RequiredPageNum - DedicatedPgNum;
+		pHalData->SdioTxFIFOFreePage[PUBLIC_QUEUE_IDX] -= RequiredPublicFreePgNum;
+	}
+
+	/* spin_unlock_bh(&pHalData->SdioTxFIFOFreePageLock); */
+}
+
+void rtw_hal_set_sdio_tx_max_length(
+	struct adapter *padapter, u8 numHQ, u8 numNQ, u8 numLQ, u8 numPubQ
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u32 page_size;
+	u32 lenHQ, lenNQ, lenLQ;
+
+	rtw_hal_get_def_var(padapter, HAL_DEF_TX_PAGE_SIZE, &page_size);
+
+	lenHQ = ((numHQ + numPubQ) >> 1) * page_size;
+	lenNQ = ((numNQ + numPubQ) >> 1) * page_size;
+	lenLQ = ((numLQ + numPubQ) >> 1) * page_size;
+
+	pHalData->sdio_tx_max_len[HI_QUEUE_IDX] =
+		(lenHQ > MAX_XMITBUF_SZ) ? MAX_XMITBUF_SZ : lenHQ;
+	pHalData->sdio_tx_max_len[MID_QUEUE_IDX] =
+		(lenNQ > MAX_XMITBUF_SZ) ? MAX_XMITBUF_SZ : lenNQ;
+	pHalData->sdio_tx_max_len[LOW_QUEUE_IDX] =
+		(lenLQ > MAX_XMITBUF_SZ) ? MAX_XMITBUF_SZ : lenLQ;
+}
+
+u32 rtw_hal_get_sdio_tx_max_length(struct adapter *padapter, u8 queue_idx)
+{
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u32 deviceId, max_len;
+
+
+	deviceId = ffaddr2deviceId(pdvobjpriv, queue_idx);
+	switch (deviceId) {
+	case WLAN_TX_HIQ_DEVICE_ID:
+		max_len = pHalData->sdio_tx_max_len[HI_QUEUE_IDX];
+		break;
+
+	case WLAN_TX_MIQ_DEVICE_ID:
+		max_len = pHalData->sdio_tx_max_len[MID_QUEUE_IDX];
+		break;
+
+	case WLAN_TX_LOQ_DEVICE_ID:
+		max_len = pHalData->sdio_tx_max_len[LOW_QUEUE_IDX];
+		break;
+
+	default:
+		max_len = pHalData->sdio_tx_max_len[MID_QUEUE_IDX];
+		break;
+	}
+
+	return max_len;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/Mp_Precomp.h linux/3rdparty/rtl8723bs/hal/Mp_Precomp.h
--- linux.old/3rdparty/rtl8723bs/hal/Mp_Precomp.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/Mp_Precomp.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,33 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 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.
+ *
+ ******************************************************************************/
+#ifndef __MP_PRECOMP_H__
+#define __MP_PRECOMP_H__
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#define BT_TMP_BUF_SIZE	100
+
+#define DCMD_Printf			DBG_BT_INFO
+
+#ifdef bEnable
+#undef bEnable
+#endif
+
+#include "HalBtcOutSrc.h"
+#include "HalBtc8723b1Ant.h"
+#include "HalBtc8723b2Ant.h"
+
+#endif /*  __MP_PRECOMP_H__ */
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_AntDiv.c linux/3rdparty/rtl8723bs/hal/odm_AntDiv.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_AntDiv.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_AntDiv.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,70 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+//============================================================
+// include files
+//============================================================
+
+#include "odm_precomp.h"
+
+//======================================================
+// when antenna test utility is on or some testing
+// need to disable antenna diversity
+// call this function to disable all ODM related mechanisms
+// which will switch antenna.
+//======================================================
+void ODM_StopAntennaSwitchDm(PDM_ODM_T pDM_Odm)
+{
+	// disable ODM antenna diversity
+	pDM_Odm->SupportAbility &= ~ODM_BB_ANT_DIV;
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_ANT_DIV,
+		ODM_DBG_LOUD,
+		("STOP Antenna Diversity\n")
+	);
+}
+
+void ODM_SetAntConfig(PDM_ODM_T pDM_Odm, u8 antSetting)// 0=A, 1=B, 2=C, ....
+{
+	if (antSetting == 0) // ant A
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000000);
+	else if (antSetting == 1)
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+}
+
+//======================================================
+
+
+void ODM_SwAntDivRestAfterLink(PDM_ODM_T pDM_Odm)
+{
+	pSWAT_T pDM_SWAT_Table = &pDM_Odm->DM_SWAT_Table;
+	pFAT_T pDM_FatTable = &pDM_Odm->DM_FatTable;
+	u32 i;
+
+	pDM_Odm->RSSI_test = false;
+	pDM_SWAT_Table->try_flag = 0xff;
+	pDM_SWAT_Table->RSSI_Trying = 0;
+	pDM_SWAT_Table->Double_chk_flag = 0;
+
+	pDM_FatTable->RxIdleAnt = MAIN_ANT;
+
+	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+		pDM_FatTable->MainAnt_Sum[i] = 0;
+		pDM_FatTable->AuxAnt_Sum[i] = 0;
+		pDM_FatTable->MainAnt_Cnt[i] = 0;
+		pDM_FatTable->AuxAnt_Cnt[i] = 0;
+	}
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_AntDiv.h linux/3rdparty/rtl8723bs/hal/odm_AntDiv.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_AntDiv.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_AntDiv.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,38 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODMANTDIV_H__
+#define    __ODMANTDIV_H__
+
+
+
+#define ANT1_2G 0 /*  = ANT2_5G */
+#define ANT2_2G 1 /*  = ANT1_5G */
+
+/* Antenna Diversty Control Type */
+#define	ODM_AUTO_ANT	0
+#define	ODM_FIX_MAIN_ANT	1
+#define	ODM_FIX_AUX_ANT	2
+
+#define	TX_BY_REG	0
+
+#define ANTDIV_ON 1
+#define ANTDIV_OFF 0
+
+#define INIT_ANTDIV_TIMMER 0
+#define CANCEL_ANTDIV_TIMMER 1
+#define RELEASE_ANTDIV_TIMMER 2
+
+#endif /* ifndef	__ODMANTDIV_H__ */
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm.c linux/3rdparty/rtl8723bs/hal/odm.c
--- linux.old/3rdparty/rtl8723bs/hal/odm.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,1446 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+static const u16 dB_Invert_Table[8][12] = {
+	{1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4},
+	{4, 5, 6, 6, 7, 8, 9, 10, 11, 13, 14, 16},
+	{18, 20, 22, 25, 28, 32, 35, 40, 45, 50, 56, 63},
+	{71, 79, 89, 100, 112, 126, 141, 158, 178, 200, 224, 251},
+	{282, 316, 355, 398, 447, 501, 562, 631, 708, 794, 891, 1000},
+	{1122, 1259, 1413, 1585, 1778, 1995, 2239, 2512, 2818, 3162, 3548, 3981},
+	{4467, 5012, 5623, 6310, 7079, 7943, 8913, 10000, 11220, 12589, 14125,
+	 15849},
+	{17783, 19953, 22387, 25119, 28184, 31623, 35481, 39811, 44668, 50119,
+	 56234, 65535}
+ };
+
+/*  Global var */
+
+u32 OFDMSwingTable[OFDM_TABLE_SIZE] = {
+	0x7f8001fe, /*  0, +6.0dB */
+	0x788001e2, /*  1, +5.5dB */
+	0x71c001c7, /*  2, +5.0dB */
+	0x6b8001ae, /*  3, +4.5dB */
+	0x65400195, /*  4, +4.0dB */
+	0x5fc0017f, /*  5, +3.5dB */
+	0x5a400169, /*  6, +3.0dB */
+	0x55400155, /*  7, +2.5dB */
+	0x50800142, /*  8, +2.0dB */
+	0x4c000130, /*  9, +1.5dB */
+	0x47c0011f, /*  10, +1.0dB */
+	0x43c0010f, /*  11, +0.5dB */
+	0x40000100, /*  12, +0dB */
+	0x3c8000f2, /*  13, -0.5dB */
+	0x390000e4, /*  14, -1.0dB */
+	0x35c000d7, /*  15, -1.5dB */
+	0x32c000cb, /*  16, -2.0dB */
+	0x300000c0, /*  17, -2.5dB */
+	0x2d4000b5, /*  18, -3.0dB */
+	0x2ac000ab, /*  19, -3.5dB */
+	0x288000a2, /*  20, -4.0dB */
+	0x26000098, /*  21, -4.5dB */
+	0x24000090, /*  22, -5.0dB */
+	0x22000088, /*  23, -5.5dB */
+	0x20000080, /*  24, -6.0dB */
+	0x1e400079, /*  25, -6.5dB */
+	0x1c800072, /*  26, -7.0dB */
+	0x1b00006c, /*  27. -7.5dB */
+	0x19800066, /*  28, -8.0dB */
+	0x18000060, /*  29, -8.5dB */
+	0x16c0005b, /*  30, -9.0dB */
+	0x15800056, /*  31, -9.5dB */
+	0x14400051, /*  32, -10.0dB */
+	0x1300004c, /*  33, -10.5dB */
+	0x12000048, /*  34, -11.0dB */
+	0x11000044, /*  35, -11.5dB */
+	0x10000040, /*  36, -12.0dB */
+};
+
+u8 CCKSwingTable_Ch1_Ch13[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04}, /*  0, +0dB */
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04}, /*  1, -0.5dB */
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03}, /*  2, -1.0dB */
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03}, /*  3, -1.5dB */
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03}, /*  4, -2.0dB */
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03}, /*  5, -2.5dB */
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03}, /*  6, -3.0dB */
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03}, /*  7, -3.5dB */
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02}, /*  8, -4.0dB */
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02}, /*  9, -4.5dB */
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02}, /*  10, -5.0dB */
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02}, /*  11, -5.5dB */
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02}, /*  12, -6.0dB <== default */
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02}, /*  13, -6.5dB */
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02}, /*  14, -7.0dB */
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02}, /*  15, -7.5dB */
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01}, /*  16, -8.0dB */
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02}, /*  17, -8.5dB */
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01}, /*  18, -9.0dB */
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /*  19, -9.5dB */
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /*  20, -10.0dB */
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01}, /*  21, -10.5dB */
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}, /*  22, -11.0dB */
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01}, /*  23, -11.5dB */
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01}, /*  24, -12.0dB */
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01}, /*  25, -12.5dB */
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01}, /*  26, -13.0dB */
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01}, /*  27, -13.5dB */
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01}, /*  28, -14.0dB */
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01}, /*  29, -14.5dB */
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01}, /*  30, -15.0dB */
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01}, /*  31, -15.5dB */
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}	/*  32, -16.0dB */
+};
+
+u8 CCKSwingTable_Ch14[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00}, /*  0, +0dB */
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00}, /*  1, -0.5dB */
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00}, /*  2, -1.0dB */
+	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00}, /*  3, -1.5dB */
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00}, /*  4, -2.0dB */
+	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00}, /*  5, -2.5dB */
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00}, /*  6, -3.0dB */
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00}, /*  7, -3.5dB */
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00}, /*  8, -4.0dB */
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00}, /*  9, -4.5dB */
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00}, /*  10, -5.0dB */
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00}, /*  11, -5.5dB */
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00}, /*  12, -6.0dB  <== default */
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00}, /*  13, -6.5dB */
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00}, /*  14, -7.0dB */
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00}, /*  15, -7.5dB */
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00}, /*  16, -8.0dB */
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00}, /*  17, -8.5dB */
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00}, /*  18, -9.0dB */
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /*  19, -9.5dB */
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /*  20, -10.0dB */
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00}, /*  21, -10.5dB */
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}, /*  22, -11.0dB */
+	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /*  23, -11.5dB */
+	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /*  24, -12.0dB */
+	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00}, /*  25, -12.5dB */
+	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /*  26, -13.0dB */
+	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /*  27, -13.5dB */
+	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  28, -14.0dB */
+	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  29, -14.5dB */
+	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  30, -15.0dB */
+	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /*  31, -15.5dB */
+	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}	/*  32, -16.0dB */
+};
+
+u32 OFDMSwingTable_New[OFDM_TABLE_SIZE] = {
+	0x0b40002d, /*  0,  -15.0dB */
+	0x0c000030, /*  1,  -14.5dB */
+	0x0cc00033, /*  2,  -14.0dB */
+	0x0d800036, /*  3,  -13.5dB */
+	0x0e400039, /*  4,  -13.0dB */
+	0x0f00003c, /*  5,  -12.5dB */
+	0x10000040, /*  6,  -12.0dB */
+	0x11000044, /*  7,  -11.5dB */
+	0x12000048, /*  8,  -11.0dB */
+	0x1300004c, /*  9,  -10.5dB */
+	0x14400051, /*  10, -10.0dB */
+	0x15800056, /*  11, -9.5dB */
+	0x16c0005b, /*  12, -9.0dB */
+	0x18000060, /*  13, -8.5dB */
+	0x19800066, /*  14, -8.0dB */
+	0x1b00006c, /*  15, -7.5dB */
+	0x1c800072, /*  16, -7.0dB */
+	0x1e400079, /*  17, -6.5dB */
+	0x20000080, /*  18, -6.0dB */
+	0x22000088, /*  19, -5.5dB */
+	0x24000090, /*  20, -5.0dB */
+	0x26000098, /*  21, -4.5dB */
+	0x288000a2, /*  22, -4.0dB */
+	0x2ac000ab, /*  23, -3.5dB */
+	0x2d4000b5, /*  24, -3.0dB */
+	0x300000c0, /*  25, -2.5dB */
+	0x32c000cb, /*  26, -2.0dB */
+	0x35c000d7, /*  27, -1.5dB */
+	0x390000e4, /*  28, -1.0dB */
+	0x3c8000f2, /*  29, -0.5dB */
+	0x40000100, /*  30, +0dB */
+	0x43c0010f, /*  31, +0.5dB */
+	0x47c0011f, /*  32, +1.0dB */
+	0x4c000130, /*  33, +1.5dB */
+	0x50800142, /*  34, +2.0dB */
+	0x55400155, /*  35, +2.5dB */
+	0x5a400169, /*  36, +3.0dB */
+	0x5fc0017f, /*  37, +3.5dB */
+	0x65400195, /*  38, +4.0dB */
+	0x6b8001ae, /*  39, +4.5dB */
+	0x71c001c7, /*  40, +5.0dB */
+	0x788001e2, /*  41, +5.5dB */
+	0x7f8001fe  /*  42, +6.0dB */
+};
+
+u8 CCKSwingTable_Ch1_Ch13_New[CCK_TABLE_SIZE][8] = {
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}, /*   0, -16.0dB */
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01}, /*   1, -15.5dB */
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01}, /*   2, -15.0dB */
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01}, /*   3, -14.5dB */
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01}, /*   4, -14.0dB */
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01}, /*   5, -13.5dB */
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01}, /*   6, -13.0dB */
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01}, /*   7, -12.5dB */
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01}, /*   8, -12.0dB */
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01}, /*   9, -11.5dB */
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}, /*  10, -11.0dB */
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01}, /*  11, -10.5dB */
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /*  12, -10.0dB */
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /*  13, -9.5dB */
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01}, /*  14, -9.0dB */
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02}, /*  15, -8.5dB */
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01}, /*  16, -8.0dB */
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02}, /*  17, -7.5dB */
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02}, /*  18, -7.0dB */
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02}, /*  19, -6.5dB */
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02}, /*  20, -6.0dB */
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02}, /*  21, -5.5dB */
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02}, /*  22, -5.0dB */
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02}, /*  23, -4.5dB */
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02}, /*  24, -4.0dB */
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03}, /*  25, -3.5dB */
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03}, /*  26, -3.0dB */
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03}, /*  27, -2.5dB */
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03}, /*  28, -2.0dB */
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03}, /*  29, -1.5dB */
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03}, /*  30, -1.0dB */
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04}, /*  31, -0.5dB */
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04}	/*  32, +0dB */
+};
+
+u8 CCKSwingTable_Ch14_New[CCK_TABLE_SIZE][8] = {
+	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}, /*   0, -16.0dB */
+	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /*   1, -15.5dB */
+	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /*   2, -15.0dB */
+	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /*   3, -14.5dB */
+	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /*   4, -14.0dB */
+	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /*   5, -13.5dB */
+	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /*   6, -13.0dB */
+	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00}, /*   7, -12.5dB */
+	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /*   8, -12.0dB */
+	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /*   9, -11.5dB */
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}, /*  10, -11.0dB */
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00}, /*  11, -10.5dB */
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /*  12, -10.0dB */
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /*  13, -9.5dB */
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00}, /*  14, -9.0dB */
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00}, /*  15, -8.5dB */
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00}, /*  16, -8.0dB */
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00}, /*  17, -7.5dB */
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00}, /*  18, -7.0dB */
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00}, /*  19, -6.5dB */
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00}, /*  20, -6.0dB */
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00}, /*  21, -5.5dB */
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00}, /*  22, -5.0dB */
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00}, /*  23, -4.5dB */
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00}, /*  24, -4.0dB */
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00}, /*  25, -3.5dB */
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00}, /*  26, -3.0dB */
+	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00}, /*  27, -2.5dB */
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00}, /*  28, -2.0dB */
+	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00}, /*  29, -1.5dB */
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00}, /*  30, -1.0dB */
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00}, /*  31, -0.5dB */
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00}	/*  32, +0dB */
+};
+
+u32 TxScalingTable_Jaguar[TXSCALE_TABLE_SIZE] = {
+	0x081, /*  0,  -12.0dB */
+	0x088, /*  1,  -11.5dB */
+	0x090, /*  2,  -11.0dB */
+	0x099, /*  3,  -10.5dB */
+	0x0A2, /*  4,  -10.0dB */
+	0x0AC, /*  5,  -9.5dB */
+	0x0B6, /*  6,  -9.0dB */
+	0x0C0, /*  7,  -8.5dB */
+	0x0CC, /*  8,  -8.0dB */
+	0x0D8, /*  9,  -7.5dB */
+	0x0E5, /*  10, -7.0dB */
+	0x0F2, /*  11, -6.5dB */
+	0x101, /*  12, -6.0dB */
+	0x110, /*  13, -5.5dB */
+	0x120, /*  14, -5.0dB */
+	0x131, /*  15, -4.5dB */
+	0x143, /*  16, -4.0dB */
+	0x156, /*  17, -3.5dB */
+	0x16A, /*  18, -3.0dB */
+	0x180, /*  19, -2.5dB */
+	0x197, /*  20, -2.0dB */
+	0x1AF, /*  21, -1.5dB */
+	0x1C8, /*  22, -1.0dB */
+	0x1E3, /*  23, -0.5dB */
+	0x200, /*  24, +0  dB */
+	0x21E, /*  25, +0.5dB */
+	0x23E, /*  26, +1.0dB */
+	0x261, /*  27, +1.5dB */
+	0x285, /*  28, +2.0dB */
+	0x2AB, /*  29, +2.5dB */
+	0x2D3, /*  30, +3.0dB */
+	0x2FE, /*  31, +3.5dB */
+	0x32B, /*  32, +4.0dB */
+	0x35C, /*  33, +4.5dB */
+	0x38E, /*  34, +5.0dB */
+	0x3C4, /*  35, +5.5dB */
+	0x3FE  /*  36, +6.0dB */
+};
+
+/*  Local Function predefine. */
+
+/* START------------COMMON INFO RELATED--------------- */
+void odm_CommonInfoSelfInit(PDM_ODM_T pDM_Odm);
+
+void odm_CommonInfoSelfUpdate(PDM_ODM_T pDM_Odm);
+
+void odm_CmnInfoInit_Debug(PDM_ODM_T pDM_Odm);
+
+void odm_BasicDbgMessage(PDM_ODM_T pDM_Odm);
+
+/* END------------COMMON INFO RELATED--------------- */
+
+/* START---------------DIG--------------------------- */
+
+/* Remove by Yuchen */
+
+/* END---------------DIG--------------------------- */
+
+/* START-------BB POWER SAVE----------------------- */
+/* Remove BB power Saving by YuChen */
+/* END---------BB POWER SAVE----------------------- */
+
+void odm_RefreshRateAdaptiveMaskCE(PDM_ODM_T pDM_Odm);
+
+/* Remove by YuChen */
+
+void odm_RSSIMonitorInit(PDM_ODM_T pDM_Odm);
+
+void odm_RSSIMonitorCheckCE(PDM_ODM_T pDM_Odm);
+
+void odm_RSSIMonitorCheck(PDM_ODM_T pDM_Odm);
+
+void odm_SwAntDetectInit(PDM_ODM_T pDM_Odm);
+
+void odm_SwAntDivChkAntSwitchCallback(void *FunctionContext);
+
+
+
+void odm_GlobalAdapterCheck(void);
+
+void odm_RefreshRateAdaptiveMask(PDM_ODM_T pDM_Odm);
+
+void ODM_TXPowerTrackingCheck(PDM_ODM_T pDM_Odm);
+
+void odm_RateAdaptiveMaskInit(PDM_ODM_T pDM_Odm);
+
+void odm_TXPowerTrackingThermalMeterInit(PDM_ODM_T pDM_Odm);
+
+
+void odm_TXPowerTrackingInit(PDM_ODM_T pDM_Odm);
+
+void odm_TXPowerTrackingCheckCE(PDM_ODM_T pDM_Odm);
+
+/* Remove Edca by Yu Chen */
+
+
+#define RxDefaultAnt1		0x65a9
+#define RxDefaultAnt2		0x569a
+
+void odm_InitHybridAntDiv(PDM_ODM_T pDM_Odm);
+
+bool odm_StaDefAntSel(
+	PDM_ODM_T pDM_Odm,
+	u32 OFDM_Ant1_Cnt,
+	u32 OFDM_Ant2_Cnt,
+	u32 CCK_Ant1_Cnt,
+	u32 CCK_Ant2_Cnt,
+	u8 *pDefAnt
+);
+
+void odm_SetRxIdleAnt(PDM_ODM_T pDM_Odm, u8 Ant, bool bDualPath);
+
+
+
+void odm_HwAntDiv(PDM_ODM_T pDM_Odm);
+
+
+/*  */
+/* 3 Export Interface */
+/*  */
+
+/*  */
+/*  2011/09/21 MH Add to describe different team necessary resource allocate?? */
+/*  */
+void ODM_DMInit(PDM_ODM_T pDM_Odm)
+{
+
+	odm_CommonInfoSelfInit(pDM_Odm);
+	odm_CmnInfoInit_Debug(pDM_Odm);
+	odm_DIGInit(pDM_Odm);
+	odm_NHMCounterStatisticsInit(pDM_Odm);
+	odm_AdaptivityInit(pDM_Odm);
+	odm_RateAdaptiveMaskInit(pDM_Odm);
+	ODM_CfoTrackingInit(pDM_Odm);
+	ODM_EdcaTurboInit(pDM_Odm);
+	odm_RSSIMonitorInit(pDM_Odm);
+	odm_TXPowerTrackingInit(pDM_Odm);
+
+	ODM_ClearTxPowerTrackingState(pDM_Odm);
+
+	if (*(pDM_Odm->mp_mode) != 1)
+		odm_PathDiversityInit(pDM_Odm);
+
+	odm_DynamicBBPowerSavingInit(pDM_Odm);
+	odm_DynamicTxPowerInit(pDM_Odm);
+
+	odm_SwAntDetectInit(pDM_Odm);
+}
+
+/*  */
+/*  2011/09/20 MH This is the entry pointer for all team to execute HW out source DM. */
+/*  You can not add any dummy function here, be care, you can only use DM structure */
+/*  to perform any new ODM_DM. */
+/*  */
+void ODM_DMWatchdog(PDM_ODM_T pDM_Odm)
+{
+	odm_CommonInfoSelfUpdate(pDM_Odm);
+	odm_BasicDbgMessage(pDM_Odm);
+	odm_FalseAlarmCounterStatistics(pDM_Odm);
+	odm_NHMCounterStatistics(pDM_Odm);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): RSSI = 0x%x\n", pDM_Odm->RSSI_Min));
+
+	odm_RSSIMonitorCheck(pDM_Odm);
+
+	/* For CE Platform(SPRD or Tablet) */
+	/* 8723A or 8189ES platform */
+	/* NeilChen--2012--08--24-- */
+	/* Fix Leave LPS issue */
+	if ((adapter_to_pwrctl(pDM_Odm->Adapter)->pwr_mode != PS_MODE_ACTIVE) /*  in LPS mode */
+		/*  */
+		/* (pDM_Odm->SupportICType & (ODM_RTL8723A))|| */
+		/* (pDM_Odm->SupportICType & (ODM_RTL8188E) &&(&&(((pDM_Odm->SupportInterface  == ODM_ITRF_SDIO))) */
+		/*  */
+	) {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("----Step1: odm_DIG is in LPS mode\n"));
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("---Step2: 8723AS is in LPS mode\n"));
+			odm_DIGbyRSSI_LPS(pDM_Odm);
+	} else
+		odm_DIG(pDM_Odm);
+
+	{
+		pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+
+		odm_Adaptivity(pDM_Odm, pDM_DigTable->CurIGValue);
+	}
+	odm_CCKPacketDetectionThresh(pDM_Odm);
+
+	if (*(pDM_Odm->pbPowerSaving) == true)
+		return;
+
+
+	odm_RefreshRateAdaptiveMask(pDM_Odm);
+	odm_EdcaTurboCheck(pDM_Odm);
+	odm_PathDiversity(pDM_Odm);
+	ODM_CfoTracking(pDM_Odm);
+
+	ODM_TXPowerTrackingCheck(pDM_Odm);
+
+	/* odm_EdcaTurboCheck(pDM_Odm); */
+
+	/* 2010.05.30 LukeLee: For CE platform, files in IC subfolders may not be included to be compiled, */
+	/*  so compile flags must be left here to prevent from compile errors */
+	pDM_Odm->PhyDbgInfo.NumQryBeaconPkt = 0;
+}
+
+
+/*  */
+/*  Init /.. Fixed HW value. Only init time. */
+/*  */
+void ODM_CmnInfoInit(PDM_ODM_T pDM_Odm, ODM_CMNINFO_E CmnInfo, u32 Value)
+{
+	/*  */
+	/*  This section is used for init value */
+	/*  */
+	switch (CmnInfo) {
+	/*  */
+	/*  Fixed ODM value. */
+	/*  */
+	case ODM_CMNINFO_ABILITY:
+		pDM_Odm->SupportAbility = (u32)Value;
+		break;
+
+	case ODM_CMNINFO_RF_TYPE:
+		pDM_Odm->RFType = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_PLATFORM:
+		pDM_Odm->SupportPlatform = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_INTERFACE:
+		pDM_Odm->SupportInterface = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_MP_TEST_CHIP:
+		pDM_Odm->bIsMPChip = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_IC_TYPE:
+		pDM_Odm->SupportICType = Value;
+		break;
+
+	case ODM_CMNINFO_CUT_VER:
+		pDM_Odm->CutVersion = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_FAB_VER:
+		pDM_Odm->FabVersion = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_RFE_TYPE:
+		pDM_Odm->RFEType = (u8)Value;
+		break;
+
+	case    ODM_CMNINFO_RF_ANTENNA_TYPE:
+		pDM_Odm->AntDivType = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_BOARD_TYPE:
+		pDM_Odm->BoardType = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_PACKAGE_TYPE:
+		pDM_Odm->PackageType = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_EXT_LNA:
+		pDM_Odm->ExtLNA = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_5G_EXT_LNA:
+		pDM_Odm->ExtLNA5G = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_EXT_PA:
+		pDM_Odm->ExtPA = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_5G_EXT_PA:
+		pDM_Odm->ExtPA5G = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_GPA:
+		pDM_Odm->TypeGPA = (ODM_TYPE_GPA_E)Value;
+		break;
+	case ODM_CMNINFO_APA:
+		pDM_Odm->TypeAPA = (ODM_TYPE_APA_E)Value;
+		break;
+	case ODM_CMNINFO_GLNA:
+		pDM_Odm->TypeGLNA = (ODM_TYPE_GLNA_E)Value;
+		break;
+	case ODM_CMNINFO_ALNA:
+		pDM_Odm->TypeALNA = (ODM_TYPE_ALNA_E)Value;
+		break;
+
+	case ODM_CMNINFO_EXT_TRSW:
+		pDM_Odm->ExtTRSW = (u8)Value;
+		break;
+	case ODM_CMNINFO_PATCH_ID:
+		pDM_Odm->PatchID = (u8)Value;
+		break;
+	case ODM_CMNINFO_BINHCT_TEST:
+		pDM_Odm->bInHctTest = (bool)Value;
+		break;
+	case ODM_CMNINFO_BWIFI_TEST:
+		pDM_Odm->bWIFITest = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_SMART_CONCURRENT:
+		pDM_Odm->bDualMacSmartConcurrent = (bool)Value;
+		break;
+
+	/* To remove the compiler warning, must add an empty default statement to handle the other values. */
+	default:
+		/* do nothing */
+		break;
+	}
+
+}
+
+
+void ODM_CmnInfoHook(PDM_ODM_T pDM_Odm, ODM_CMNINFO_E CmnInfo, void *pValue)
+{
+	/*  */
+	/*  Hook call by reference pointer. */
+	/*  */
+	switch (CmnInfo) {
+	/*  */
+	/*  Dynamic call by reference pointer. */
+	/*  */
+	case ODM_CMNINFO_MAC_PHY_MODE:
+		pDM_Odm->pMacPhyMode = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_TX_UNI:
+		pDM_Odm->pNumTxBytesUnicast = (u64 *)pValue;
+		break;
+
+	case ODM_CMNINFO_RX_UNI:
+		pDM_Odm->pNumRxBytesUnicast = (u64 *)pValue;
+		break;
+
+	case ODM_CMNINFO_WM_MODE:
+		pDM_Odm->pwirelessmode = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_BAND:
+		pDM_Odm->pBandType = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_SEC_CHNL_OFFSET:
+		pDM_Odm->pSecChOffset = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_SEC_MODE:
+		pDM_Odm->pSecurity = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_BW:
+		pDM_Odm->pBandWidth = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_CHNL:
+		pDM_Odm->pChannel = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_DMSP_GET_VALUE:
+		pDM_Odm->pbGetValueFromOtherMac = (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_BUDDY_ADAPTOR:
+		pDM_Odm->pBuddyAdapter = (struct adapter **)pValue;
+		break;
+
+	case ODM_CMNINFO_DMSP_IS_MASTER:
+		pDM_Odm->pbMasterOfDMSP = (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_SCAN:
+		pDM_Odm->pbScanInProcess = (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_POWER_SAVING:
+		pDM_Odm->pbPowerSaving = (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_ONE_PATH_CCA:
+		pDM_Odm->pOnePathCCA = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_DRV_STOP:
+		pDM_Odm->pbDriverStopped =  (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_PNP_IN:
+		pDM_Odm->pbDriverIsGoingToPnpSetPowerSleep =  (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_INIT_ON:
+		pDM_Odm->pinit_adpt_in_progress =  (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_ANT_TEST:
+		pDM_Odm->pAntennaTest =  (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_NET_CLOSED:
+		pDM_Odm->pbNet_closed = (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_FORCED_RATE:
+		pDM_Odm->pForcedDataRate = (u16 *)pValue;
+		break;
+
+	case ODM_CMNINFO_FORCED_IGI_LB:
+		pDM_Odm->pu1ForcedIgiLb = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_MP_MODE:
+		pDM_Odm->mp_mode = (u8 *)pValue;
+		break;
+
+	/* case ODM_CMNINFO_RTSTA_AID: */
+	/* pDM_Odm->pAidMap =  (u8 *)pValue; */
+	/* break; */
+
+	/* case ODM_CMNINFO_BT_COEXIST: */
+	/* pDM_Odm->BTCoexist = (bool *)pValue; */
+
+	/* case ODM_CMNINFO_STA_STATUS: */
+	/* pDM_Odm->pODM_StaInfo[] = (PSTA_INFO_T)pValue; */
+	/* break; */
+
+	/* case ODM_CMNINFO_PHY_STATUS: */
+	/* pDM_Odm->pPhyInfo = (ODM_PHY_INFO *)pValue; */
+	/* break; */
+
+	/* case ODM_CMNINFO_MAC_STATUS: */
+	/* pDM_Odm->pMacInfo = (ODM_MAC_INFO *)pValue; */
+	/* break; */
+	/* To remove the compiler warning, must add an empty default statement to handle the other values. */
+	default:
+		/* do nothing */
+		break;
+	}
+
+}
+
+
+void ODM_CmnInfoPtrArrayHook(
+	PDM_ODM_T pDM_Odm,
+	ODM_CMNINFO_E CmnInfo,
+	u16 Index,
+	void *pValue
+)
+{
+	/*  */
+	/*  Hook call by reference pointer. */
+	/*  */
+	switch (CmnInfo) {
+	/*  */
+	/*  Dynamic call by reference pointer. */
+	/*  */
+	case ODM_CMNINFO_STA_STATUS:
+		pDM_Odm->pODM_StaInfo[Index] = (PSTA_INFO_T)pValue;
+		break;
+	/* To remove the compiler warning, must add an empty default statement to handle the other values. */
+	default:
+		/* do nothing */
+		break;
+	}
+
+}
+
+
+/*  */
+/*  Update Band/CHannel/.. The values are dynamic but non-per-packet. */
+/*  */
+void ODM_CmnInfoUpdate(PDM_ODM_T pDM_Odm, u32 CmnInfo, u64 Value)
+{
+	/*  */
+	/*  This init variable may be changed in run time. */
+	/*  */
+	switch (CmnInfo) {
+	case ODM_CMNINFO_LINK_IN_PROGRESS:
+		pDM_Odm->bLinkInProcess = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_ABILITY:
+		pDM_Odm->SupportAbility = (u32)Value;
+		break;
+
+	case ODM_CMNINFO_RF_TYPE:
+		pDM_Odm->RFType = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_WIFI_DIRECT:
+		pDM_Odm->bWIFI_Direct = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_WIFI_DISPLAY:
+		pDM_Odm->bWIFI_Display = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_LINK:
+		pDM_Odm->bLinked = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_STATION_STATE:
+		pDM_Odm->bsta_state = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_RSSI_MIN:
+		pDM_Odm->RSSI_Min = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_DBG_COMP:
+		pDM_Odm->DebugComponents = Value;
+		break;
+
+	case ODM_CMNINFO_DBG_LEVEL:
+		pDM_Odm->DebugLevel = (u32)Value;
+		break;
+	case ODM_CMNINFO_RA_THRESHOLD_HIGH:
+		pDM_Odm->RateAdaptive.HighRSSIThresh = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_RA_THRESHOLD_LOW:
+		pDM_Odm->RateAdaptive.LowRSSIThresh = (u8)Value;
+		break;
+	/*  The following is for BT HS mode and BT coexist mechanism. */
+	case ODM_CMNINFO_BT_ENABLED:
+		pDM_Odm->bBtEnabled = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_BT_HS_CONNECT_PROCESS:
+		pDM_Odm->bBtConnectProcess = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_BT_HS_RSSI:
+		pDM_Odm->btHsRssi = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_BT_OPERATION:
+		pDM_Odm->bBtHsOperation = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_BT_LIMITED_DIG:
+		pDM_Odm->bBtLimitedDig = (bool)Value;
+		break;
+
+	case ODM_CMNINFO_BT_DISABLE_EDCA:
+		pDM_Odm->bBtDisableEdcaTurbo = (bool)Value;
+		break;
+
+/*
+	case	ODM_CMNINFO_OP_MODE:
+		pDM_Odm->OPMode = (u8)Value;
+		break;
+
+	case	ODM_CMNINFO_WM_MODE:
+		pDM_Odm->WirelessMode = (u8)Value;
+		break;
+
+	case	ODM_CMNINFO_BAND:
+		pDM_Odm->BandType = (u8)Value;
+		break;
+
+	case	ODM_CMNINFO_SEC_CHNL_OFFSET:
+		pDM_Odm->SecChOffset = (u8)Value;
+		break;
+
+	case	ODM_CMNINFO_SEC_MODE:
+		pDM_Odm->Security = (u8)Value;
+		break;
+
+	case	ODM_CMNINFO_BW:
+		pDM_Odm->BandWidth = (u8)Value;
+		break;
+
+	case	ODM_CMNINFO_CHNL:
+		pDM_Odm->Channel = (u8)Value;
+		break;
+*/
+	default:
+		/* do nothing */
+		break;
+	}
+
+
+}
+
+void odm_CommonInfoSelfInit(PDM_ODM_T pDM_Odm)
+{
+	pDM_Odm->bCckHighPower = (bool) PHY_QueryBBReg(pDM_Odm->Adapter, ODM_REG(CCK_RPT_FORMAT, pDM_Odm), ODM_BIT(CCK_RPT_FORMAT, pDM_Odm));
+	pDM_Odm->RFPathRxEnable = (u8) PHY_QueryBBReg(pDM_Odm->Adapter, ODM_REG(BB_RX_PATH, pDM_Odm), ODM_BIT(BB_RX_PATH, pDM_Odm));
+
+	ODM_InitDebugSetting(pDM_Odm);
+
+	pDM_Odm->TxRate = 0xFF;
+}
+
+void odm_CommonInfoSelfUpdate(PDM_ODM_T pDM_Odm)
+{
+	u8 EntryCnt = 0;
+	u8 i;
+	PSTA_INFO_T	pEntry;
+
+	if (*(pDM_Odm->pBandWidth) == ODM_BW40M) {
+		if (*(pDM_Odm->pSecChOffset) == 1)
+			pDM_Odm->ControlChannel = *(pDM_Odm->pChannel)-2;
+		else if (*(pDM_Odm->pSecChOffset) == 2)
+			pDM_Odm->ControlChannel = *(pDM_Odm->pChannel)+2;
+	} else
+		pDM_Odm->ControlChannel = *(pDM_Odm->pChannel);
+
+	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+		pEntry = pDM_Odm->pODM_StaInfo[i];
+		if (IS_STA_VALID(pEntry))
+			EntryCnt++;
+	}
+
+	if (EntryCnt == 1)
+		pDM_Odm->bOneEntryOnly = true;
+	else
+		pDM_Odm->bOneEntryOnly = false;
+}
+
+void odm_CmnInfoInit_Debug(PDM_ODM_T pDM_Odm)
+{
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("odm_CmnInfoInit_Debug ==>\n"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("SupportPlatform =%d\n", pDM_Odm->SupportPlatform));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("SupportAbility = 0x%x\n", pDM_Odm->SupportAbility));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("SupportInterface =%d\n", pDM_Odm->SupportInterface));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("SupportICType = 0x%x\n", pDM_Odm->SupportICType));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("CutVersion =%d\n", pDM_Odm->CutVersion));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("FabVersion =%d\n", pDM_Odm->FabVersion));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("RFType =%d\n", pDM_Odm->RFType));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("BoardType =%d\n", pDM_Odm->BoardType));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("ExtLNA =%d\n", pDM_Odm->ExtLNA));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("ExtPA =%d\n", pDM_Odm->ExtPA));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("ExtTRSW =%d\n", pDM_Odm->ExtTRSW));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("PatchID =%d\n", pDM_Odm->PatchID));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("bInHctTest =%d\n", pDM_Odm->bInHctTest));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("bWIFITest =%d\n", pDM_Odm->bWIFITest));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("bDualMacSmartConcurrent =%d\n", pDM_Odm->bDualMacSmartConcurrent));
+
+}
+
+void odm_BasicDbgMessage(PDM_ODM_T pDM_Odm)
+{
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("odm_BasicDbgMsg ==>\n"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("bLinked = %d, RSSI_Min = %d,\n",
+		pDM_Odm->bLinked, pDM_Odm->RSSI_Min));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("RxRate = 0x%x, RSSI_A = %d, RSSI_B = %d\n",
+		pDM_Odm->RxRate, pDM_Odm->RSSI_A, pDM_Odm->RSSI_B));
+}
+
+/* 3 ============================================================ */
+/* 3 DIG */
+/* 3 ============================================================ */
+/*-----------------------------------------------------------------------------
+ * Function:	odm_DIGInit()
+ *
+ * Overview:	Set DIG scheme init value.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *When		Who		Remark
+ *
+ *---------------------------------------------------------------------------
+ */
+
+/* Remove DIG by yuchen */
+
+/* Remove DIG and FA check by Yu Chen */
+
+
+/* 3 ============================================================ */
+/* 3 BB Power Save */
+/* 3 ============================================================ */
+
+/* Remove BB power saving by Yuchen */
+
+/* 3 ============================================================ */
+/* 3 RATR MASK */
+/* 3 ============================================================ */
+/* 3 ============================================================ */
+/* 3 Rate Adaptive */
+/* 3 ============================================================ */
+
+void odm_RateAdaptiveMaskInit(PDM_ODM_T pDM_Odm)
+{
+	PODM_RATE_ADAPTIVE pOdmRA = &pDM_Odm->RateAdaptive;
+
+	pOdmRA->Type = DM_Type_ByDriver;
+	if (pOdmRA->Type == DM_Type_ByDriver)
+		pDM_Odm->bUseRAMask = true;
+	else
+		pDM_Odm->bUseRAMask = false;
+
+	pOdmRA->RATRState = DM_RATR_STA_INIT;
+	pOdmRA->LdpcThres = 35;
+	pOdmRA->bUseLdpc = false;
+	pOdmRA->HighRSSIThresh = 50;
+	pOdmRA->LowRSSIThresh = 20;
+}
+
+u32 ODM_Get_Rate_Bitmap(
+	PDM_ODM_T pDM_Odm,
+	u32 macid,
+	u32 ra_mask,
+	u8 rssi_level
+)
+{
+	PSTA_INFO_T	pEntry;
+	u32 rate_bitmap = 0;
+	u8 WirelessMode;
+
+	pEntry = pDM_Odm->pODM_StaInfo[macid];
+	if (!IS_STA_VALID(pEntry))
+		return ra_mask;
+
+	WirelessMode = pEntry->wireless_mode;
+
+	switch (WirelessMode) {
+	case ODM_WM_B:
+		if (ra_mask & 0x0000000c)		/* 11M or 5.5M enable */
+			rate_bitmap = 0x0000000d;
+		else
+			rate_bitmap = 0x0000000f;
+		break;
+
+	case (ODM_WM_G):
+	case (ODM_WM_A):
+		if (rssi_level == DM_RATR_STA_HIGH)
+			rate_bitmap = 0x00000f00;
+		else
+			rate_bitmap = 0x00000ff0;
+		break;
+
+	case (ODM_WM_B|ODM_WM_G):
+		if (rssi_level == DM_RATR_STA_HIGH)
+			rate_bitmap = 0x00000f00;
+		else if (rssi_level == DM_RATR_STA_MIDDLE)
+			rate_bitmap = 0x00000ff0;
+		else
+			rate_bitmap = 0x00000ff5;
+		break;
+
+	case (ODM_WM_B|ODM_WM_G|ODM_WM_N24G):
+	case (ODM_WM_B|ODM_WM_N24G):
+	case (ODM_WM_G|ODM_WM_N24G):
+	case (ODM_WM_A|ODM_WM_N5G):
+		if (pDM_Odm->RFType == ODM_1T2R || pDM_Odm->RFType == ODM_1T1R) {
+			if (rssi_level == DM_RATR_STA_HIGH)
+				rate_bitmap = 0x000f0000;
+			else if (rssi_level == DM_RATR_STA_MIDDLE)
+				rate_bitmap = 0x000ff000;
+			else {
+				if (*(pDM_Odm->pBandWidth) == ODM_BW40M)
+					rate_bitmap = 0x000ff015;
+				else
+					rate_bitmap = 0x000ff005;
+			}
+		} else {
+			if (rssi_level == DM_RATR_STA_HIGH)
+				rate_bitmap = 0x0f8f0000;
+			else if (rssi_level == DM_RATR_STA_MIDDLE)
+				rate_bitmap = 0x0f8ff000;
+			else {
+				if (*(pDM_Odm->pBandWidth) == ODM_BW40M)
+					rate_bitmap = 0x0f8ff015;
+				else
+					rate_bitmap = 0x0f8ff005;
+			}
+		}
+		break;
+
+	case (ODM_WM_AC|ODM_WM_G):
+		if (rssi_level == 1)
+			rate_bitmap = 0xfc3f0000;
+		else if (rssi_level == 2)
+			rate_bitmap = 0xfffff000;
+		else
+			rate_bitmap = 0xffffffff;
+		break;
+
+	case (ODM_WM_AC|ODM_WM_A):
+
+		if (pDM_Odm->RFType == RF_1T1R) {
+			if (rssi_level == 1)				/*  add by Gary for ac-series */
+				rate_bitmap = 0x003f8000;
+			else if (rssi_level == 2)
+				rate_bitmap = 0x003ff000;
+			else
+				rate_bitmap = 0x003ff010;
+		} else {
+			if (rssi_level == 1)				/*  add by Gary for ac-series */
+				rate_bitmap = 0xfe3f8000;       /*  VHT 2SS MCS3~9 */
+			else if (rssi_level == 2)
+				rate_bitmap = 0xfffff000;       /*  VHT 2SS MCS0~9 */
+			else
+				rate_bitmap = 0xfffff010;       /*  All */
+		}
+		break;
+
+	default:
+		if (pDM_Odm->RFType == RF_1T2R)
+			rate_bitmap = 0x000fffff;
+		else
+			rate_bitmap = 0x0fffffff;
+		break;
+	}
+
+	/* printk("%s ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x\n", __func__, rssi_level, WirelessMode, rate_bitmap); */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD, (" ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x\n", rssi_level, WirelessMode, rate_bitmap));
+
+	return (ra_mask&rate_bitmap);
+
+}
+
+/*-----------------------------------------------------------------------------
+* Function:	odm_RefreshRateAdaptiveMask()
+*
+* Overview:	Update rate table mask according to rssi
+*
+* Input:		NONE
+*
+* Output:		NONE
+*
+* Return:		NONE
+*
+* Revised History:
+*When		Who		Remark
+*05/27/2009	hpfan	Create Version 0.
+*
+* --------------------------------------------------------------------------
+*/
+void odm_RefreshRateAdaptiveMask(PDM_ODM_T pDM_Odm)
+{
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_TRACE, ("odm_RefreshRateAdaptiveMask()---------->\n"));
+	if (!(pDM_Odm->SupportAbility & ODM_BB_RA_MASK)) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_TRACE, ("odm_RefreshRateAdaptiveMask(): Return cos not supported\n"));
+		return;
+	}
+	odm_RefreshRateAdaptiveMaskCE(pDM_Odm);
+}
+
+void odm_RefreshRateAdaptiveMaskCE(PDM_ODM_T pDM_Odm)
+{
+	u8 i;
+	struct adapter *padapter =  pDM_Odm->Adapter;
+
+	if (padapter->bDriverStopped) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_TRACE, ("<---- odm_RefreshRateAdaptiveMask(): driver is going to unload\n"));
+		return;
+	}
+
+	if (!pDM_Odm->bUseRAMask) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD, ("<---- odm_RefreshRateAdaptiveMask(): driver does not control rate adaptive mask\n"));
+		return;
+	}
+
+	/* printk("==> %s\n", __func__); */
+
+	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+		PSTA_INFO_T pstat = pDM_Odm->pODM_StaInfo[i];
+
+		if (IS_STA_VALID(pstat)) {
+			if (IS_MCAST(pstat->hwaddr))  /* if (psta->mac_id == 1) */
+				continue;
+			if (IS_MCAST(pstat->hwaddr))
+				continue;
+
+			if (true == ODM_RAStateCheck(pDM_Odm, pstat->rssi_stat.UndecoratedSmoothedPWDB, false, &pstat->rssi_level)) {
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD, ("RSSI:%d, RSSI_LEVEL:%d\n", pstat->rssi_stat.UndecoratedSmoothedPWDB, pstat->rssi_level));
+				/* printk("RSSI:%d, RSSI_LEVEL:%d\n", pstat->rssi_stat.UndecoratedSmoothedPWDB, pstat->rssi_level); */
+				rtw_hal_update_ra_mask(pstat, pstat->rssi_level);
+			}
+
+		}
+	}
+}
+
+/*  Return Value: bool */
+/*  - true: RATRState is changed. */
+bool ODM_RAStateCheck(
+	PDM_ODM_T pDM_Odm,
+	s32 RSSI,
+	bool bForceUpdate,
+	u8 *pRATRState
+)
+{
+	PODM_RATE_ADAPTIVE pRA = &pDM_Odm->RateAdaptive;
+	const u8 GoUpGap = 5;
+	u8 HighRSSIThreshForRA = pRA->HighRSSIThresh;
+	u8 LowRSSIThreshForRA = pRA->LowRSSIThresh;
+	u8 RATRState;
+
+	/*  Threshold Adjustment: */
+	/*  when RSSI state trends to go up one or two levels, make sure RSSI is high enough. */
+	/*  Here GoUpGap is added to solve the boundary's level alternation issue. */
+	switch (*pRATRState) {
+	case DM_RATR_STA_INIT:
+	case DM_RATR_STA_HIGH:
+		break;
+
+	case DM_RATR_STA_MIDDLE:
+		HighRSSIThreshForRA += GoUpGap;
+		break;
+
+	case DM_RATR_STA_LOW:
+		HighRSSIThreshForRA += GoUpGap;
+		LowRSSIThreshForRA += GoUpGap;
+		break;
+
+	default:
+		ODM_RT_ASSERT(pDM_Odm, false, ("wrong rssi level setting %d !", *pRATRState));
+		break;
+	}
+
+	/*  Decide RATRState by RSSI. */
+	if (RSSI > HighRSSIThreshForRA)
+		RATRState = DM_RATR_STA_HIGH;
+	else if (RSSI > LowRSSIThreshForRA)
+		RATRState = DM_RATR_STA_MIDDLE;
+	else
+		RATRState = DM_RATR_STA_LOW;
+	/* printk("==>%s, RATRState:0x%02x , RSSI:%d\n", __func__, RATRState, RSSI); */
+
+	if (*pRATRState != RATRState || bForceUpdate) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD, ("RSSI Level %d -> %d\n", *pRATRState, RATRState));
+		*pRATRState = RATRState;
+		return true;
+	}
+
+	return false;
+}
+
+
+/*  */
+
+/* 3 ============================================================ */
+/* 3 Dynamic Tx Power */
+/* 3 ============================================================ */
+
+/* Remove BY YuChen */
+
+/* 3 ============================================================ */
+/* 3 RSSI Monitor */
+/* 3 ============================================================ */
+
+void odm_RSSIMonitorInit(PDM_ODM_T pDM_Odm)
+{
+	pRA_T pRA_Table = &pDM_Odm->DM_RA_Table;
+
+	pRA_Table->firstconnect = false;
+
+}
+
+void odm_RSSIMonitorCheck(PDM_ODM_T pDM_Odm)
+{
+	if (!(pDM_Odm->SupportAbility & ODM_BB_RSSI_MONITOR))
+		return;
+
+	odm_RSSIMonitorCheckCE(pDM_Odm);
+
+}	/*  odm_RSSIMonitorCheck */
+
+static void FindMinimumRSSI(struct adapter *padapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	PDM_ODM_T pDM_Odm = &(pHalData->odmpriv);
+
+	/* 1 1.Determine the minimum RSSI */
+
+	if (
+		(pDM_Odm->bLinked != true) &&
+		(pdmpriv->EntryMinUndecoratedSmoothedPWDB == 0)
+	) {
+		pdmpriv->MinUndecoratedPWDBForDM = 0;
+		/* ODM_RT_TRACE(pDM_Odm, COMP_BB_POWERSAVING, DBG_LOUD, ("Not connected to any\n")); */
+	} else
+		pdmpriv->MinUndecoratedPWDBForDM = pdmpriv->EntryMinUndecoratedSmoothedPWDB;
+
+	/* DBG_8192C("%s =>MinUndecoratedPWDBForDM(%d)\n", __func__, pdmpriv->MinUndecoratedPWDBForDM); */
+	/* ODM_RT_TRACE(pDM_Odm, COMP_DIG, DBG_LOUD, ("MinUndecoratedPWDBForDM =%d\n", pHalData->MinUndecoratedPWDBForDM)); */
+}
+
+void odm_RSSIMonitorCheckCE(PDM_ODM_T pDM_Odm)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	int i;
+	int tmpEntryMaxPWDB = 0, tmpEntryMinPWDB = 0xff;
+	u8 sta_cnt = 0;
+	u32 PWDB_rssi[NUM_STA] = {0};/* 0~15]:MACID, [16~31]:PWDB_rssi */
+	bool FirstConnect = false;
+	pRA_T pRA_Table = &pDM_Odm->DM_RA_Table;
+
+	if (pDM_Odm->bLinked != true)
+		return;
+
+	FirstConnect = (pDM_Odm->bLinked) && (pRA_Table->firstconnect == false);
+	pRA_Table->firstconnect = pDM_Odm->bLinked;
+
+	/* if (check_fwstate(&Adapter->mlmepriv, WIFI_AP_STATE|WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE) == true) */
+	{
+		struct sta_info *psta;
+
+		for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+			psta = pDM_Odm->pODM_StaInfo[i];
+			if (IS_STA_VALID(psta)) {
+				if (IS_MCAST(psta->hwaddr))  /* if (psta->mac_id == 1) */
+					continue;
+
+				if (psta->rssi_stat.UndecoratedSmoothedPWDB == (-1))
+					continue;
+
+				if (psta->rssi_stat.UndecoratedSmoothedPWDB < tmpEntryMinPWDB)
+					tmpEntryMinPWDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
+
+				if (psta->rssi_stat.UndecoratedSmoothedPWDB > tmpEntryMaxPWDB)
+					tmpEntryMaxPWDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
+
+				if (psta->rssi_stat.UndecoratedSmoothedPWDB != (-1))
+					PWDB_rssi[sta_cnt++] = (psta->mac_id | (psta->rssi_stat.UndecoratedSmoothedPWDB<<16));
+			}
+		}
+
+		/* printk("%s ==> sta_cnt(%d)\n", __func__, sta_cnt); */
+
+		for (i = 0; i < sta_cnt; i++) {
+			if (PWDB_rssi[i] != (0)) {
+				if (pHalData->fw_ractrl == true)/*  Report every sta's RSSI to FW */
+					rtl8723b_set_rssi_cmd(Adapter, (u8 *)(&PWDB_rssi[i]));
+			}
+		}
+	}
+
+
+
+	if (tmpEntryMaxPWDB != 0)	/*  If associated entry is found */
+		pdmpriv->EntryMaxUndecoratedSmoothedPWDB = tmpEntryMaxPWDB;
+	else
+		pdmpriv->EntryMaxUndecoratedSmoothedPWDB = 0;
+
+	if (tmpEntryMinPWDB != 0xff) /*  If associated entry is found */
+		pdmpriv->EntryMinUndecoratedSmoothedPWDB = tmpEntryMinPWDB;
+	else
+		pdmpriv->EntryMinUndecoratedSmoothedPWDB = 0;
+
+	FindMinimumRSSI(Adapter);/* get pdmpriv->MinUndecoratedPWDBForDM */
+
+	pDM_Odm->RSSI_Min = pdmpriv->MinUndecoratedPWDBForDM;
+	/* ODM_CmnInfoUpdate(&pHalData->odmpriv , ODM_CMNINFO_RSSI_MIN, pdmpriv->MinUndecoratedPWDBForDM); */
+}
+
+/* 3 ============================================================ */
+/* 3 Tx Power Tracking */
+/* 3 ============================================================ */
+
+void odm_TXPowerTrackingInit(PDM_ODM_T pDM_Odm)
+{
+	odm_TXPowerTrackingThermalMeterInit(pDM_Odm);
+}
+
+static u8 getSwingIndex(PDM_ODM_T pDM_Odm)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	u8 i = 0;
+	u32 bbSwing;
+	u32 swingTableSize;
+	u32 *pSwingTable;
+
+	bbSwing = PHY_QueryBBReg(Adapter, rOFDM0_XATxIQImbalance, 0xFFC00000);
+
+	pSwingTable = OFDMSwingTable_New;
+	swingTableSize = OFDM_TABLE_SIZE;
+
+	for (i = 0; i < swingTableSize; ++i) {
+		u32 tableValue = pSwingTable[i];
+
+		if (tableValue >= 0x100000)
+			tableValue >>= 22;
+		if (bbSwing == tableValue)
+			break;
+	}
+	return i;
+}
+
+void odm_TXPowerTrackingThermalMeterInit(PDM_ODM_T pDM_Odm)
+{
+	u8 defaultSwingIndex = getSwingIndex(pDM_Odm);
+	u8 p = 0;
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+
+	pdmpriv->bTXPowerTracking = true;
+	pdmpriv->TXPowercount = 0;
+	pdmpriv->bTXPowerTrackingInit = false;
+
+	if (*(pDM_Odm->mp_mode) != 1)
+		pdmpriv->TxPowerTrackControl = true;
+	else
+		pdmpriv->TxPowerTrackControl = false;
+
+
+	/* MSG_8192C("pdmpriv->TxPowerTrackControl = %d\n", pdmpriv->TxPowerTrackControl); */
+
+	/* pDM_Odm->RFCalibrateInfo.TxPowerTrackControl = true; */
+	pDM_Odm->RFCalibrateInfo.ThermalValue = pHalData->EEPROMThermalMeter;
+	pDM_Odm->RFCalibrateInfo.ThermalValue_IQK = pHalData->EEPROMThermalMeter;
+	pDM_Odm->RFCalibrateInfo.ThermalValue_LCK = pHalData->EEPROMThermalMeter;
+
+	/*  The index of "0 dB" in SwingTable. */
+	pDM_Odm->DefaultOfdmIndex = (defaultSwingIndex >= OFDM_TABLE_SIZE) ? 30 : defaultSwingIndex;
+	pDM_Odm->DefaultCckIndex = 20;
+
+	pDM_Odm->BbSwingIdxCckBase = pDM_Odm->DefaultCckIndex;
+	pDM_Odm->RFCalibrateInfo.CCK_index = pDM_Odm->DefaultCckIndex;
+
+	for (p = ODM_RF_PATH_A; p < MAX_RF_PATH; ++p) {
+		pDM_Odm->BbSwingIdxOfdmBase[p] = pDM_Odm->DefaultOfdmIndex;
+		pDM_Odm->RFCalibrateInfo.OFDM_index[p] = pDM_Odm->DefaultOfdmIndex;
+		pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[p] = 0;
+		pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[p] = 0;
+		pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = 0;
+	}
+
+}
+
+
+void ODM_TXPowerTrackingCheck(PDM_ODM_T pDM_Odm)
+{
+	odm_TXPowerTrackingCheckCE(pDM_Odm);
+}
+
+void odm_TXPowerTrackingCheckCE(PDM_ODM_T pDM_Odm)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+
+	if (!(pDM_Odm->SupportAbility & ODM_RF_TX_PWR_TRACK))
+		return;
+
+	if (!pDM_Odm->RFCalibrateInfo.TM_Trigger) { /* at least delay 1 sec */
+		PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_T_METER_NEW, (BIT17 | BIT16), 0x03);
+
+		/* DBG_871X("Trigger Thermal Meter!!\n"); */
+
+		pDM_Odm->RFCalibrateInfo.TM_Trigger = 1;
+		return;
+	} else {
+		/* DBG_871X("Schedule TxPowerTracking direct call!!\n"); */
+		ODM_TXPowerTrackingCallback_ThermalMeter(Adapter);
+		pDM_Odm->RFCalibrateInfo.TM_Trigger = 0;
+	}
+}
+
+/* 3 ============================================================ */
+/* 3 SW Antenna Diversity */
+/* 3 ============================================================ */
+void odm_SwAntDetectInit(PDM_ODM_T pDM_Odm)
+{
+	pSWAT_T pDM_SWAT_Table = &pDM_Odm->DM_SWAT_Table;
+
+	pDM_SWAT_Table->SWAS_NoLink_BK_Reg92c = rtw_read32(pDM_Odm->Adapter, rDPDT_control);
+	pDM_SWAT_Table->PreAntenna = MAIN_ANT;
+	pDM_SWAT_Table->CurAntenna = MAIN_ANT;
+	pDM_SWAT_Table->SWAS_NoLink_State = 0;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_CfoTracking.c linux/3rdparty/rtl8723bs/hal/odm_CfoTracking.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_CfoTracking.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_CfoTracking.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,338 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+static void odm_SetCrystalCap(void *pDM_VOID, u8 CrystalCap)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
+	bool bEEPROMCheck;
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	bEEPROMCheck = (pHalData->EEPROMVersion >= 0x01) ? true : false;
+
+	if (pCfoTrack->CrystalCap == CrystalCap)
+		return;
+
+	pCfoTrack->CrystalCap = CrystalCap;
+
+	/*  0x2C[23:18] = 0x2C[17:12] = CrystalCap */
+	CrystalCap = CrystalCap & 0x3F;
+	PHY_SetBBReg(
+		pDM_Odm->Adapter,
+		REG_MAC_PHY_CTRL,
+		0x00FFF000,
+		(CrystalCap | (CrystalCap << 6))
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CFO_TRACKING,
+		ODM_DBG_LOUD,
+		(
+			"odm_SetCrystalCap(): CrystalCap = 0x%x\n",
+			CrystalCap
+		)
+	);
+}
+
+static u8 odm_GetDefaultCrytaltalCap(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	u8 CrystalCap = 0x20;
+
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	CrystalCap = pHalData->CrystalCap;
+
+	CrystalCap = CrystalCap & 0x3f;
+
+	return CrystalCap;
+}
+
+static void odm_SetATCStatus(void *pDM_VOID, bool ATCStatus)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
+
+	if (pCfoTrack->bATCStatus == ATCStatus)
+		return;
+
+	PHY_SetBBReg(
+		pDM_Odm->Adapter,
+		ODM_REG(BB_ATC, pDM_Odm),
+		ODM_BIT(BB_ATC, pDM_Odm),
+		ATCStatus
+	);
+	pCfoTrack->bATCStatus = ATCStatus;
+}
+
+static bool odm_GetATCStatus(void *pDM_VOID)
+{
+	bool ATCStatus;
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	ATCStatus = (bool)PHY_QueryBBReg(
+		pDM_Odm->Adapter,
+		ODM_REG(BB_ATC, pDM_Odm),
+		ODM_BIT(BB_ATC, pDM_Odm)
+	);
+	return ATCStatus;
+}
+
+void ODM_CfoTrackingReset(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
+
+	pCfoTrack->DefXCap = odm_GetDefaultCrytaltalCap(pDM_Odm);
+	pCfoTrack->bAdjust = true;
+
+	odm_SetCrystalCap(pDM_Odm, pCfoTrack->DefXCap);
+	odm_SetATCStatus(pDM_Odm, true);
+}
+
+void ODM_CfoTrackingInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
+
+	pCfoTrack->DefXCap =
+		pCfoTrack->CrystalCap = odm_GetDefaultCrytaltalCap(pDM_Odm);
+	pCfoTrack->bATCStatus = odm_GetATCStatus(pDM_Odm);
+	pCfoTrack->bAdjust = true;
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CFO_TRACKING,
+		ODM_DBG_LOUD,
+		("ODM_CfoTracking_init() =========>\n")
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CFO_TRACKING,
+		ODM_DBG_LOUD,
+		(
+			"ODM_CfoTracking_init(): bATCStatus = %d, CrystalCap = 0x%x\n",
+			pCfoTrack->bATCStatus,
+			pCfoTrack->DefXCap
+		)
+	);
+}
+
+void ODM_CfoTracking(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
+	int CFO_kHz_A, CFO_kHz_B, CFO_ave = 0;
+	int CFO_ave_diff;
+	int CrystalCap = (int)pCfoTrack->CrystalCap;
+	u8 Adjust_Xtal = 1;
+
+	/* 4 Support ability */
+	if (!(pDM_Odm->SupportAbility & ODM_BB_CFO_TRACKING)) {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_CFO_TRACKING,
+			ODM_DBG_LOUD,
+			("ODM_CfoTracking(): Return: SupportAbility ODM_BB_CFO_TRACKING is disabled\n")
+		);
+		return;
+	}
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CFO_TRACKING,
+		ODM_DBG_LOUD,
+		("ODM_CfoTracking() =========>\n")
+	);
+
+	if (!pDM_Odm->bLinked || !pDM_Odm->bOneEntryOnly) {
+		/* 4 No link or more than one entry */
+		ODM_CfoTrackingReset(pDM_Odm);
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_CFO_TRACKING,
+			ODM_DBG_LOUD,
+			(
+				"ODM_CfoTracking(): Reset: bLinked = %d, bOneEntryOnly = %d\n",
+				pDM_Odm->bLinked,
+				pDM_Odm->bOneEntryOnly
+			)
+		);
+	} else {
+		/* 3 1. CFO Tracking */
+		/* 4 1.1 No new packet */
+		if (pCfoTrack->packetCount == pCfoTrack->packetCount_pre) {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_CFO_TRACKING,
+				ODM_DBG_LOUD,
+				(
+					"ODM_CfoTracking(): packet counter doesn't change\n"
+				)
+			);
+			return;
+		}
+		pCfoTrack->packetCount_pre = pCfoTrack->packetCount;
+
+		/* 4 1.2 Calculate CFO */
+		CFO_kHz_A =  (int)(pCfoTrack->CFO_tail[0] * 3125)  / 1280;
+		CFO_kHz_B =  (int)(pCfoTrack->CFO_tail[1] * 3125)  / 1280;
+
+		if (pDM_Odm->RFType < ODM_2T2R)
+			CFO_ave = CFO_kHz_A;
+		else
+			CFO_ave = (int)(CFO_kHz_A + CFO_kHz_B) >> 1;
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_CFO_TRACKING,
+			ODM_DBG_LOUD,
+			(
+				"ODM_CfoTracking(): CFO_kHz_A = %dkHz, CFO_kHz_B = %dkHz, CFO_ave = %dkHz\n",
+				CFO_kHz_A,
+				CFO_kHz_B,
+				CFO_ave
+			)
+		);
+
+		/* 4 1.3 Avoid abnormal large CFO */
+		CFO_ave_diff =
+			(pCfoTrack->CFO_ave_pre >= CFO_ave) ?
+			(pCfoTrack->CFO_ave_pre-CFO_ave) :
+			(CFO_ave-pCfoTrack->CFO_ave_pre);
+
+		if (
+			CFO_ave_diff > 20 &&
+			pCfoTrack->largeCFOHit == 0 &&
+			!pCfoTrack->bAdjust
+		) {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CFO_TRACKING, ODM_DBG_LOUD, ("ODM_CfoTracking(): first large CFO hit\n"));
+			pCfoTrack->largeCFOHit = 1;
+			return;
+		} else
+			pCfoTrack->largeCFOHit = 0;
+		pCfoTrack->CFO_ave_pre = CFO_ave;
+
+		/* 4 1.4 Dynamic Xtal threshold */
+		if (pCfoTrack->bAdjust == false) {
+			if (CFO_ave > CFO_TH_XTAL_HIGH || CFO_ave < (-CFO_TH_XTAL_HIGH))
+				pCfoTrack->bAdjust = true;
+		} else {
+			if (CFO_ave < CFO_TH_XTAL_LOW && CFO_ave > (-CFO_TH_XTAL_LOW))
+				pCfoTrack->bAdjust = false;
+		}
+
+		/* 4 1.5 BT case: Disable CFO tracking */
+		if (pDM_Odm->bBtEnabled) {
+			pCfoTrack->bAdjust = false;
+			odm_SetCrystalCap(pDM_Odm, pCfoTrack->DefXCap);
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_CFO_TRACKING,
+				ODM_DBG_LOUD,
+				("ODM_CfoTracking(): Disable CFO tracking for BT!!\n")
+			);
+		}
+
+		/* 4 1.6 Big jump */
+		if (pCfoTrack->bAdjust) {
+			if (CFO_ave > CFO_TH_XTAL_LOW)
+				Adjust_Xtal = Adjust_Xtal+((CFO_ave-CFO_TH_XTAL_LOW)>>2);
+			else if (CFO_ave < (-CFO_TH_XTAL_LOW))
+				Adjust_Xtal = Adjust_Xtal+((CFO_TH_XTAL_LOW-CFO_ave)>>2);
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_CFO_TRACKING,
+				ODM_DBG_LOUD,
+				(
+					"ODM_CfoTracking(): Crystal cap offset = %d\n",
+					Adjust_Xtal
+				)
+			);
+		}
+
+		/* 4 1.7 Adjust Crystal Cap. */
+		if (pCfoTrack->bAdjust) {
+			if (CFO_ave > CFO_TH_XTAL_LOW)
+				CrystalCap = CrystalCap + Adjust_Xtal;
+			else if (CFO_ave < (-CFO_TH_XTAL_LOW))
+				CrystalCap = CrystalCap - Adjust_Xtal;
+
+			if (CrystalCap > 0x3f)
+				CrystalCap = 0x3f;
+			else if (CrystalCap < 0)
+				CrystalCap = 0;
+
+			odm_SetCrystalCap(pDM_Odm, (u8)CrystalCap);
+		}
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_CFO_TRACKING,
+			ODM_DBG_LOUD,
+			(
+				"ODM_CfoTracking(): Crystal cap = 0x%x, Default Crystal cap = 0x%x\n",
+				pCfoTrack->CrystalCap,
+				pCfoTrack->DefXCap
+			)
+		);
+
+		/* 3 2. Dynamic ATC switch */
+		if (CFO_ave < CFO_TH_ATC && CFO_ave > -CFO_TH_ATC) {
+			odm_SetATCStatus(pDM_Odm, false);
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_CFO_TRACKING,
+				ODM_DBG_LOUD,
+				("ODM_CfoTracking(): Disable ATC!!\n")
+			);
+		} else {
+			odm_SetATCStatus(pDM_Odm, true);
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_CFO_TRACKING,
+				ODM_DBG_LOUD,
+				("ODM_CfoTracking(): Enable ATC!!\n")
+			);
+		}
+	}
+}
+
+void ODM_ParsingCFO(void *pDM_VOID, void *pPktinfo_VOID, s8 *pcfotail)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PODM_PACKET_INFO_T pPktinfo = (PODM_PACKET_INFO_T)pPktinfo_VOID;
+	PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
+	u8 i;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_CFO_TRACKING))
+		return;
+
+	if (pPktinfo->StationID != 0) {
+		/* 3 Update CFO report for path-A & path-B */
+		/*  Only paht-A and path-B have CFO tail and short CFO */
+		for (i = ODM_RF_PATH_A; i <= ODM_RF_PATH_B; i++)
+			pCfoTrack->CFO_tail[i] = (int)pcfotail[i];
+
+		/* 3 Update packet counter */
+		if (pCfoTrack->packetCount == 0xffffffff)
+			pCfoTrack->packetCount = 0;
+		else
+			pCfoTrack->packetCount++;
+	}
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_CfoTracking.h linux/3rdparty/rtl8723bs/hal/odm_CfoTracking.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_CfoTracking.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_CfoTracking.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,47 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODMCFOTRACK_H__
+#define    __ODMCFOTRACK_H__
+
+#define		CFO_TH_XTAL_HIGH		20		/*  kHz */
+#define		CFO_TH_XTAL_LOW			10		/*  kHz */
+#define		CFO_TH_ATC			80		/*  kHz */
+
+typedef struct _CFO_TRACKING_ {
+	bool bATCStatus;
+	bool largeCFOHit;
+	bool bAdjust;
+	u8 CrystalCap;
+	u8 DefXCap;
+	int CFO_tail[2];
+	int CFO_ave_pre;
+	u32 packetCount;
+	u32 packetCount_pre;
+
+	bool bForceXtalCap;
+	bool bReset;
+} CFO_TRACKING, *PCFO_TRACKING;
+
+void ODM_CfoTrackingReset(void *pDM_VOID
+);
+
+void ODM_CfoTrackingInit(void *pDM_VOID);
+
+void ODM_CfoTracking(void *pDM_VOID);
+
+void ODM_ParsingCFO(void *pDM_VOID, void *pPktinfo_VOID, s8 *pcfotail);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_debug.c linux/3rdparty/rtl8723bs/hal/odm_debug.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_debug.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_debug.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,52 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+void ODM_InitDebugSetting(PDM_ODM_T pDM_Odm)
+{
+	pDM_Odm->DebugLevel = ODM_DBG_LOUD;
+
+	pDM_Odm->DebugComponents =
+/* BB Functions */
+/* 		ODM_COMP_DIG					| */
+/* 		ODM_COMP_RA_MASK				| */
+/* 		ODM_COMP_DYNAMIC_TXPWR		| */
+/* 		ODM_COMP_FA_CNT				| */
+/* 		ODM_COMP_RSSI_MONITOR			| */
+/* 		ODM_COMP_CCK_PD				| */
+/* 		ODM_COMP_ANT_DIV				| */
+/* 		ODM_COMP_PWR_SAVE				| */
+/* 		ODM_COMP_PWR_TRAIN			| */
+/* 		ODM_COMP_RATE_ADAPTIVE		| */
+/* 		ODM_COMP_PATH_DIV				| */
+/* 		ODM_COMP_DYNAMIC_PRICCA		| */
+/* 		ODM_COMP_RXHP					| */
+/* 		ODM_COMP_MP					| */
+/* 		ODM_COMP_CFO_TRACKING		| */
+
+/* MAC Functions */
+/* 		ODM_COMP_EDCA_TURBO			| */
+/* 		ODM_COMP_EARLY_MODE			| */
+/* RF Functions */
+/* 		ODM_COMP_TX_PWR_TRACK		| */
+/* 		ODM_COMP_RX_GAIN_TRACK		| */
+/* 		ODM_COMP_CALIBRATION			| */
+/* Common */
+/* 		ODM_COMP_COMMON				| */
+/* 		ODM_COMP_INIT					| */
+/* 		ODM_COMP_PSD					| */
+0;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_debug.h linux/3rdparty/rtl8723bs/hal/odm_debug.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_debug.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_debug.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,165 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#ifndef __ODM_DBG_H__
+#define __ODM_DBG_H__
+
+
+/*  */
+/* Define the debug levels */
+/*  */
+/* 1.	DBG_TRACE and DBG_LOUD are used for normal cases. */
+/* So that, they can help SW engineer to develope or trace states changed */
+/* and also help HW enginner to trace every operation to and from HW, */
+/* e.g IO, Tx, Rx. */
+/*  */
+/* 2.	DBG_WARNNING and DBG_SERIOUS are used for unusual or error cases, */
+/* which help us to debug SW or HW. */
+/*  */
+/*  */
+/*  */
+/* Never used in a call to ODM_RT_TRACE()! */
+/*  */
+#define ODM_DBG_OFF					1
+
+/*  */
+/* Fatal bug. */
+/* For example, Tx/Rx/IO locked up, OS hangs, memory access violation, */
+/* resource allocation failed, unexpected HW behavior, HW BUG and so on. */
+/*  */
+#define ODM_DBG_SERIOUS				2
+
+/*  */
+/* Abnormal, rare, or unexpeted cases. */
+/* For example, */
+/* IRP/Packet/OID canceled, */
+/* device suprisely unremoved and so on. */
+/*  */
+#define ODM_DBG_WARNING				3
+
+/*  */
+/* Normal case with useful information about current SW or HW state. */
+/* For example, Tx/Rx descriptor to fill, Tx/Rx descriptor completed status, */
+/* SW protocol state change, dynamic mechanism state change and so on. */
+/*  */
+#define ODM_DBG_LOUD				4
+
+/*  */
+/* Normal case with detail execution flow or information. */
+/*  */
+#define ODM_DBG_TRACE				5
+
+/*  */
+/*  Define the tracing components */
+/*  */
+/*  */
+/* BB Functions */
+#define ODM_COMP_DIG				BIT0
+#define ODM_COMP_RA_MASK			BIT1
+#define ODM_COMP_DYNAMIC_TXPWR		BIT2
+#define ODM_COMP_FA_CNT				BIT3
+#define ODM_COMP_RSSI_MONITOR		BIT4
+#define ODM_COMP_CCK_PD				BIT5
+#define ODM_COMP_ANT_DIV			BIT6
+#define ODM_COMP_PWR_SAVE			BIT7
+#define ODM_COMP_PWR_TRAIN			BIT8
+#define ODM_COMP_RATE_ADAPTIVE		BIT9
+#define ODM_COMP_PATH_DIV			BIT10
+#define ODM_COMP_PSD				BIT11
+#define ODM_COMP_DYNAMIC_PRICCA		BIT12
+#define ODM_COMP_RXHP				BIT13
+#define ODM_COMP_MP					BIT14
+#define ODM_COMP_CFO_TRACKING		BIT15
+/* MAC Functions */
+#define ODM_COMP_EDCA_TURBO			BIT16
+#define ODM_COMP_EARLY_MODE			BIT17
+/* RF Functions */
+#define ODM_COMP_TX_PWR_TRACK		BIT24
+#define ODM_COMP_RX_GAIN_TRACK		BIT25
+#define ODM_COMP_CALIBRATION		BIT26
+/* Common Functions */
+#define ODM_COMP_COMMON				BIT30
+#define ODM_COMP_INIT				BIT31
+
+/*------------------------Export Marco Definition---------------------------*/
+	#define DbgPrint printk
+	#define RT_PRINTK(fmt, args...)\
+		DbgPrint("%s(): " fmt, __func__, ## args)
+	#define RT_DISP(dbgtype, dbgflag, printstr)
+
+#ifndef ASSERT
+	#define ASSERT(expr)
+#endif
+
+#if DBG
+#define ODM_RT_TRACE(pDM_Odm, comp, level, fmt)\
+	if (\
+		(comp & pDM_Odm->DebugComponents) &&\
+		(level <= pDM_Odm->DebugLevel || level == ODM_DBG_SERIOUS)\
+	) {\
+		RT_PRINTK fmt;\
+	}
+
+#define ODM_RT_TRACE_F(pDM_Odm, comp, level, fmt)\
+	if (\
+		(comp & pDM_Odm->DebugComponents) &&\
+		(level <= pDM_Odm->DebugLevel)\
+	) {\
+		RT_PRINTK fmt;\
+	}
+
+#define ODM_RT_ASSERT(pDM_Odm, expr, fmt)\
+	if (!expr) {\
+		DbgPrint("Assertion failed! %s at ......\n", #expr);\
+		DbgPrint(\
+			"      ......%s,%s, line =%d\n",\
+			__FILE__,\
+			__func__,\
+			__LINE__\
+		);\
+		RT_PRINTK fmt;\
+		ASSERT(false);\
+	}
+#define ODM_dbg_enter() { DbgPrint("==> %s\n", __func__); }
+#define ODM_dbg_exit() { DbgPrint("<== %s\n", __func__); }
+#define ODM_dbg_trace(str) { DbgPrint("%s:%s\n", __func__, str); }
+
+#define ODM_PRINT_ADDR(pDM_Odm, comp, level, title_str, ptr)\
+	if (\
+		(comp & pDM_Odm->DebugComponents) &&\
+		(level <= pDM_Odm->DebugLevel)\
+	) {\
+		int __i;\
+		u8 *__ptr = (u8 *)ptr;\
+		DbgPrint("[ODM] ");\
+		DbgPrint(title_str);\
+		DbgPrint(" ");\
+		for (__i = 0; __i < 6; __i++)\
+			DbgPrint("%02X%s", __ptr[__i], (__i == 5) ? "" : "-");\
+		DbgPrint("\n");\
+	}
+#else
+#define ODM_RT_TRACE(pDM_Odm, comp, level, fmt)
+#define ODM_RT_TRACE_F(pDM_Odm, comp, level, fmt)
+#define ODM_RT_ASSERT(pDM_Odm, expr, fmt)
+#define ODM_dbg_enter()
+#define ODM_dbg_exit()
+#define ODM_dbg_trace(str)
+#define ODM_PRINT_ADDR(pDM_Odm, comp, level, title_str, ptr)
+#endif
+
+void ODM_InitDebugSetting(PDM_ODM_T pDM_Odm);
+
+#endif	/*  __ODM_DBG_H__ */
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_DIG.c linux/3rdparty/rtl8723bs/hal/odm_DIG.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_DIG.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_DIG.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,1221 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+#define ADAPTIVITY_VERSION "5.0"
+
+void odm_NHMCounterStatisticsInit(void *pDM_VOID)
+{
+	PDM_ODM_T		pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	/* PHY parameters initialize for n series */
+	rtw_write16(pDM_Odm->Adapter, ODM_REG_NHM_TIMER_11N+2, 0x2710);	/* 0x894[31:16]= 0x2710	Time duration for NHM unit: 4us, 0x2710 =40ms */
+	/* rtw_write16(pDM_Odm->Adapter, ODM_REG_NHM_TIMER_11N+2, 0x4e20);	0x894[31:16]= 0x4e20	Time duration for NHM unit: 4us, 0x4e20 =80ms */
+	rtw_write16(pDM_Odm->Adapter, ODM_REG_NHM_TH9_TH10_11N+2, 0xffff);	/* 0x890[31:16]= 0xffff	th_9, th_10 */
+	/* rtw_write32(pDM_Odm->Adapter, ODM_REG_NHM_TH3_TO_TH0_11N, 0xffffff5c);	0x898 = 0xffffff5c		th_3, th_2, th_1, th_0 */
+	rtw_write32(pDM_Odm->Adapter, ODM_REG_NHM_TH3_TO_TH0_11N, 0xffffff52);	/* 0x898 = 0xffffff52		th_3, th_2, th_1, th_0 */
+	rtw_write32(pDM_Odm->Adapter, ODM_REG_NHM_TH7_TO_TH4_11N, 0xffffffff);	/* 0x89c = 0xffffffff		th_7, th_6, th_5, th_4 */
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_FPGA0_IQK_11N, bMaskByte0, 0xff);		/* 0xe28[7:0]= 0xff		th_8 */
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_NHM_TH9_TH10_11N, BIT10|BIT9|BIT8, 0x7);	/* 0x890[9:8]=3			enable CCX */
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_OFDM_FA_RSTC_11N, BIT7, 0x1);		/* 0xc0c[7]= 1			max power among all RX ants */
+}
+
+void odm_NHMCounterStatistics(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	/*  Get NHM report */
+	odm_GetNHMCounterStatistics(pDM_Odm);
+
+	/*  Reset NHM counter */
+	odm_NHMCounterStatisticsReset(pDM_Odm);
+}
+
+void odm_GetNHMCounterStatistics(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	u32 value32 = 0;
+
+	value32 = PHY_QueryBBReg(pDM_Odm->Adapter, ODM_REG_NHM_CNT_11N, bMaskDWord);
+
+	pDM_Odm->NHM_cnt_0 = (u8)(value32 & bMaskByte0);
+}
+
+void odm_NHMCounterStatisticsReset(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_NHM_TH9_TH10_11N, BIT1, 0);
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_NHM_TH9_TH10_11N, BIT1, 1);
+}
+
+void odm_NHMBBInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	pDM_Odm->adaptivity_flag = 0;
+	pDM_Odm->tolerance_cnt = 3;
+	pDM_Odm->NHMLastTxOkcnt = 0;
+	pDM_Odm->NHMLastRxOkcnt = 0;
+	pDM_Odm->NHMCurTxOkcnt = 0;
+	pDM_Odm->NHMCurRxOkcnt = 0;
+}
+
+/*  */
+void odm_NHMBB(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	/* u8 test_status; */
+	/* Pfalse_ALARM_STATISTICS pFalseAlmCnt = &(pDM_Odm->FalseAlmCnt); */
+
+	pDM_Odm->NHMCurTxOkcnt =
+		*(pDM_Odm->pNumTxBytesUnicast)-pDM_Odm->NHMLastTxOkcnt;
+	pDM_Odm->NHMCurRxOkcnt =
+		*(pDM_Odm->pNumRxBytesUnicast)-pDM_Odm->NHMLastRxOkcnt;
+	pDM_Odm->NHMLastTxOkcnt =
+		*(pDM_Odm->pNumTxBytesUnicast);
+	pDM_Odm->NHMLastRxOkcnt =
+		*(pDM_Odm->pNumRxBytesUnicast);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		(
+			"NHM_cnt_0 =%d, NHMCurTxOkcnt = %llu, NHMCurRxOkcnt = %llu\n",
+			pDM_Odm->NHM_cnt_0,
+			pDM_Odm->NHMCurTxOkcnt,
+			pDM_Odm->NHMCurRxOkcnt
+		)
+	);
+
+
+	if ((pDM_Odm->NHMCurTxOkcnt) + 1 > (u64)(pDM_Odm->NHMCurRxOkcnt<<2) + 1) { /* Tx > 4*Rx possible for adaptivity test */
+		if (pDM_Odm->NHM_cnt_0 >= 190 || pDM_Odm->adaptivity_flag == true) {
+			/* Enable EDCCA since it is possible running Adaptivity testing */
+			/* test_status = 1; */
+			pDM_Odm->adaptivity_flag = true;
+			pDM_Odm->tolerance_cnt = 0;
+		} else {
+			if (pDM_Odm->tolerance_cnt < 3)
+				pDM_Odm->tolerance_cnt = pDM_Odm->tolerance_cnt + 1;
+			else
+				pDM_Odm->tolerance_cnt = 4;
+			/* test_status = 5; */
+			if (pDM_Odm->tolerance_cnt > 3) {
+				/* test_status = 3; */
+				pDM_Odm->adaptivity_flag = false;
+			}
+		}
+	} else { /*  TX<RX */
+		if (pDM_Odm->adaptivity_flag == true && pDM_Odm->NHM_cnt_0 <= 200) {
+			/* test_status = 2; */
+			pDM_Odm->tolerance_cnt = 0;
+		} else {
+			if (pDM_Odm->tolerance_cnt < 3)
+				pDM_Odm->tolerance_cnt = pDM_Odm->tolerance_cnt + 1;
+			else
+				pDM_Odm->tolerance_cnt = 4;
+			/* test_status = 5; */
+			if (pDM_Odm->tolerance_cnt > 3) {
+				/* test_status = 4; */
+				pDM_Odm->adaptivity_flag = false;
+			}
+		}
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("adaptivity_flag = %d\n ", pDM_Odm->adaptivity_flag));
+}
+
+void odm_SearchPwdBLowerBound(void *pDM_VOID, u8 IGI_target)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	u32 value32 = 0;
+	u8 cnt, IGI;
+	bool bAdjust = true;
+	s8 TH_L2H_dmc, TH_H2L_dmc;
+	s8 Diff;
+
+	IGI = 0x50; /*  find H2L, L2H lower bound */
+	ODM_Write_DIG(pDM_Odm, IGI);
+
+
+	Diff = IGI_target-(s8)IGI;
+	TH_L2H_dmc = pDM_Odm->TH_L2H_ini + Diff;
+	if (TH_L2H_dmc > 10)
+		TH_L2H_dmc = 10;
+	TH_H2L_dmc = TH_L2H_dmc - pDM_Odm->TH_EDCCA_HL_diff;
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte0, (u8)TH_L2H_dmc);
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte2, (u8)TH_H2L_dmc);
+
+	mdelay(5);
+
+	while (bAdjust) {
+		for (cnt = 0; cnt < 20; cnt++) {
+			value32 = PHY_QueryBBReg(pDM_Odm->Adapter, ODM_REG_RPT_11N, bMaskDWord);
+
+			if (value32 & BIT30)
+				pDM_Odm->txEdcca1 = pDM_Odm->txEdcca1 + 1;
+			else if (value32 & BIT29)
+				pDM_Odm->txEdcca1 = pDM_Odm->txEdcca1 + 1;
+			else
+				pDM_Odm->txEdcca0 = pDM_Odm->txEdcca0 + 1;
+		}
+		/* DbgPrint("txEdcca1 = %d, txEdcca0 = %d\n", pDM_Odm->txEdcca1, pDM_Odm->txEdcca0); */
+
+		if (pDM_Odm->txEdcca1 > 5) {
+			IGI = IGI-1;
+			TH_L2H_dmc = TH_L2H_dmc + 1;
+			if (TH_L2H_dmc > 10)
+				TH_L2H_dmc = 10;
+			TH_H2L_dmc = TH_L2H_dmc - pDM_Odm->TH_EDCCA_HL_diff;
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte0, (u8)TH_L2H_dmc);
+			PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte2, (u8)TH_H2L_dmc);
+
+			pDM_Odm->TxHangFlg = true;
+			pDM_Odm->txEdcca1 = 0;
+			pDM_Odm->txEdcca0 = 0;
+
+			if (TH_L2H_dmc == 10) {
+				bAdjust = false;
+				pDM_Odm->TxHangFlg = false;
+				pDM_Odm->txEdcca1 = 0;
+				pDM_Odm->txEdcca0 = 0;
+				pDM_Odm->H2L_lb = TH_H2L_dmc;
+				pDM_Odm->L2H_lb = TH_L2H_dmc;
+				pDM_Odm->Adaptivity_IGI_upper = IGI;
+			}
+		} else {
+			bAdjust = false;
+			pDM_Odm->TxHangFlg = false;
+			pDM_Odm->txEdcca1 = 0;
+			pDM_Odm->txEdcca0 = 0;
+			pDM_Odm->H2L_lb = TH_H2L_dmc;
+			pDM_Odm->L2H_lb = TH_L2H_dmc;
+			pDM_Odm->Adaptivity_IGI_upper = IGI;
+		}
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("IGI = 0x%x, H2L_lb = 0x%x, L2H_lb = 0x%x\n", IGI, pDM_Odm->H2L_lb, pDM_Odm->L2H_lb));
+}
+
+void odm_AdaptivityInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	if (pDM_Odm->Carrier_Sense_enable == false)
+		pDM_Odm->TH_L2H_ini = 0xf7; /*  -7 */
+	else
+		pDM_Odm->TH_L2H_ini = 0xa;
+
+	pDM_Odm->AdapEn_RSSI = 20;
+	pDM_Odm->TH_EDCCA_HL_diff = 7;
+
+	pDM_Odm->IGI_Base = 0x32;
+	pDM_Odm->IGI_target = 0x1c;
+	pDM_Odm->ForceEDCCA = 0;
+	pDM_Odm->NHM_disable = false;
+	pDM_Odm->TxHangFlg = true;
+	pDM_Odm->txEdcca0 = 0;
+	pDM_Odm->txEdcca1 = 0;
+	pDM_Odm->H2L_lb = 0;
+	pDM_Odm->L2H_lb = 0;
+	pDM_Odm->Adaptivity_IGI_upper = 0;
+	odm_NHMBBInit(pDM_Odm);
+
+	PHY_SetBBReg(pDM_Odm->Adapter, REG_RD_CTRL, BIT11, 1); /*  stop counting if EDCCA is asserted */
+}
+
+
+void odm_Adaptivity(void *pDM_VOID, u8 IGI)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	s8 TH_L2H_dmc, TH_H2L_dmc;
+	s8 Diff, IGI_target;
+	bool EDCCA_State = false;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_ADAPTIVITY)) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("Go to odm_DynamicEDCCA()\n"));
+		return;
+	}
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_Adaptivity() =====>\n"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("ForceEDCCA =%d, IGI_Base = 0x%x, TH_L2H_ini = %d, TH_EDCCA_HL_diff = %d, AdapEn_RSSI = %d\n",
+		pDM_Odm->ForceEDCCA, pDM_Odm->IGI_Base, pDM_Odm->TH_L2H_ini, pDM_Odm->TH_EDCCA_HL_diff, pDM_Odm->AdapEn_RSSI));
+
+	if (*pDM_Odm->pBandWidth == ODM_BW20M) /* CHANNEL_WIDTH_20 */
+		IGI_target = pDM_Odm->IGI_Base;
+	else if (*pDM_Odm->pBandWidth == ODM_BW40M)
+		IGI_target = pDM_Odm->IGI_Base + 2;
+	else if (*pDM_Odm->pBandWidth == ODM_BW80M)
+		IGI_target = pDM_Odm->IGI_Base + 2;
+	else
+		IGI_target = pDM_Odm->IGI_Base;
+	pDM_Odm->IGI_target = (u8) IGI_target;
+
+	/* Search pwdB lower bound */
+	if (pDM_Odm->TxHangFlg == true) {
+		PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_DBG_RPT_11N, bMaskDWord, 0x208);
+		odm_SearchPwdBLowerBound(pDM_Odm, pDM_Odm->IGI_target);
+	}
+
+	if ((!pDM_Odm->bLinked) || (*pDM_Odm->pChannel > 149)) { /*  Band4 doesn't need adaptivity */
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte0, 0x7f);
+		PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte2, 0x7f);
+		return;
+	}
+
+	if (!pDM_Odm->ForceEDCCA) {
+		if (pDM_Odm->RSSI_Min > pDM_Odm->AdapEn_RSSI)
+			EDCCA_State = 1;
+		else if (pDM_Odm->RSSI_Min < (pDM_Odm->AdapEn_RSSI - 5))
+			EDCCA_State = 0;
+	} else
+		EDCCA_State = 1;
+
+	if (
+		pDM_Odm->bLinked &&
+		pDM_Odm->Carrier_Sense_enable == false &&
+		pDM_Odm->NHM_disable == false &&
+		pDM_Odm->TxHangFlg == false
+	)
+		odm_NHMBB(pDM_Odm);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		(
+			"BandWidth =%s, IGI_target = 0x%x, EDCCA_State =%d\n",
+			(*pDM_Odm->pBandWidth == ODM_BW80M) ? "80M" :
+			((*pDM_Odm->pBandWidth == ODM_BW40M) ? "40M" : "20M"),
+			IGI_target,
+			EDCCA_State
+		)
+	);
+
+	if (EDCCA_State == 1) {
+		Diff = IGI_target-(s8)IGI;
+		TH_L2H_dmc = pDM_Odm->TH_L2H_ini + Diff;
+		if (TH_L2H_dmc > 10)
+			TH_L2H_dmc = 10;
+
+		TH_H2L_dmc = TH_L2H_dmc - pDM_Odm->TH_EDCCA_HL_diff;
+
+		/* replace lower bound to prevent EDCCA always equal  */
+		if (TH_H2L_dmc < pDM_Odm->H2L_lb)
+			TH_H2L_dmc = pDM_Odm->H2L_lb;
+		if (TH_L2H_dmc < pDM_Odm->L2H_lb)
+			TH_L2H_dmc = pDM_Odm->L2H_lb;
+	} else {
+		TH_L2H_dmc = 0x7f;
+		TH_H2L_dmc = 0x7f;
+	}
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("IGI = 0x%x, TH_L2H_dmc = %d, TH_H2L_dmc = %d\n",
+		IGI, TH_L2H_dmc, TH_H2L_dmc));
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte0, (u8)TH_L2H_dmc);
+	PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_ECCAThreshold, bMaskByte2, (u8)TH_H2L_dmc);
+}
+
+void ODM_Write_DIG(void *pDM_VOID, u8 CurrentIGI)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+
+	if (pDM_DigTable->bStopDIG) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("Stop Writing IGI\n"));
+		return;
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_TRACE, ("ODM_REG(IGI_A, pDM_Odm) = 0x%x, ODM_BIT(IGI, pDM_Odm) = 0x%x\n",
+		ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm)));
+
+	if (pDM_DigTable->CurIGValue != CurrentIGI) {
+		/* 1 Check initial gain by upper bound */
+		if (!pDM_DigTable->bPSDInProgress) {
+			if (CurrentIGI > pDM_DigTable->rx_gain_range_max) {
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_TRACE, ("CurrentIGI(0x%02x) is larger than upper bound !!\n", pDM_DigTable->rx_gain_range_max));
+				CurrentIGI = pDM_DigTable->rx_gain_range_max;
+			}
+
+		}
+
+		/* 1 Set IGI value */
+		PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm), CurrentIGI);
+
+		if (pDM_Odm->RFType > ODM_1T1R)
+			PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG(IGI_B, pDM_Odm), ODM_BIT(IGI, pDM_Odm), CurrentIGI);
+
+		pDM_DigTable->CurIGValue = CurrentIGI;
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_TRACE, ("CurrentIGI(0x%02x).\n", CurrentIGI));
+
+}
+
+void odm_PauseDIG(
+	void *pDM_VOID,
+	ODM_Pause_DIG_TYPE PauseType,
+	u8 IGIValue
+)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+	static bool bPaused = false;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG() =========>\n"));
+
+	if (
+		(pDM_Odm->SupportAbility & ODM_BB_ADAPTIVITY) &&
+		pDM_Odm->TxHangFlg == true
+	) {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_DIG,
+			ODM_DBG_LOUD,
+			("odm_PauseDIG(): Dynamic adjust threshold in progress !!\n")
+		);
+		return;
+	}
+
+	if (
+		!bPaused && (!(pDM_Odm->SupportAbility & ODM_BB_DIG) ||
+		!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT))
+	){
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_DIG,
+			ODM_DBG_LOUD,
+			("odm_PauseDIG(): Return: SupportAbility ODM_BB_DIG or ODM_BB_FA_CNT is disabled\n")
+		);
+		return;
+	}
+
+	switch (PauseType) {
+	/* 1 Pause DIG */
+	case ODM_PAUSE_DIG:
+		/* 2 Disable DIG */
+		ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_ABILITY, pDM_Odm->SupportAbility & (~ODM_BB_DIG));
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Pause DIG !!\n"));
+
+		/* 2 Backup IGI value */
+		if (!bPaused) {
+			pDM_DigTable->IGIBackup = pDM_DigTable->CurIGValue;
+			bPaused = true;
+		}
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Backup IGI  = 0x%x\n", pDM_DigTable->IGIBackup));
+
+		/* 2 Write new IGI value */
+		ODM_Write_DIG(pDM_Odm, IGIValue);
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Write new IGI = 0x%x\n", IGIValue));
+		break;
+
+	/* 1 Resume DIG */
+	case ODM_RESUME_DIG:
+		if (bPaused) {
+			/* 2 Write backup IGI value */
+			ODM_Write_DIG(pDM_Odm, pDM_DigTable->IGIBackup);
+			bPaused = false;
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Write original IGI = 0x%x\n", pDM_DigTable->IGIBackup));
+
+			/* 2 Enable DIG */
+			ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_ABILITY, pDM_Odm->SupportAbility | ODM_BB_DIG);
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Resume DIG !!\n"));
+		}
+		break;
+
+	default:
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Wrong  type !!\n"));
+		break;
+	}
+}
+
+bool odm_DigAbort(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	/* SupportAbility */
+	if (!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT)) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Return: SupportAbility ODM_BB_FA_CNT is disabled\n"));
+		return	true;
+	}
+
+	/* SupportAbility */
+	if (!(pDM_Odm->SupportAbility & ODM_BB_DIG)) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Return: SupportAbility ODM_BB_DIG is disabled\n"));
+		return	true;
+	}
+
+	/* ScanInProcess */
+	if (*(pDM_Odm->pbScanInProcess)) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Return: In Scan Progress\n"));
+		return	true;
+	}
+
+	/* add by Neil Chen to avoid PSD is processing */
+	if (pDM_Odm->bDMInitialGainEnable == false) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Return: PSD is Processing\n"));
+		return	true;
+	}
+
+	return	false;
+}
+
+void odm_DIGInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+
+	pDM_DigTable->bStopDIG = false;
+	pDM_DigTable->bPSDInProgress = false;
+	pDM_DigTable->CurIGValue = (u8) PHY_QueryBBReg(pDM_Odm->Adapter, ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm));
+	pDM_DigTable->RssiLowThresh	= DM_DIG_THRESH_LOW;
+	pDM_DigTable->RssiHighThresh	= DM_DIG_THRESH_HIGH;
+	pDM_DigTable->FALowThresh	= DMfalseALARM_THRESH_LOW;
+	pDM_DigTable->FAHighThresh	= DMfalseALARM_THRESH_HIGH;
+	pDM_DigTable->BackoffVal = DM_DIG_BACKOFF_DEFAULT;
+	pDM_DigTable->BackoffVal_range_max = DM_DIG_BACKOFF_MAX;
+	pDM_DigTable->BackoffVal_range_min = DM_DIG_BACKOFF_MIN;
+	pDM_DigTable->PreCCK_CCAThres = 0xFF;
+	pDM_DigTable->CurCCK_CCAThres = 0x83;
+	pDM_DigTable->ForbiddenIGI = DM_DIG_MIN_NIC;
+	pDM_DigTable->LargeFAHit = 0;
+	pDM_DigTable->Recover_cnt = 0;
+	pDM_DigTable->bMediaConnect_0 = false;
+	pDM_DigTable->bMediaConnect_1 = false;
+
+	/* To Initialize pDM_Odm->bDMInitialGainEnable == false to avoid DIG error */
+	pDM_Odm->bDMInitialGainEnable = true;
+
+	pDM_DigTable->DIG_Dynamic_MIN_0 = DM_DIG_MIN_NIC;
+	pDM_DigTable->DIG_Dynamic_MIN_1 = DM_DIG_MIN_NIC;
+
+	/* To Initi BT30 IGI */
+	pDM_DigTable->BT30_CurIGI = 0x32;
+
+	if (pDM_Odm->BoardType & (ODM_BOARD_EXT_PA|ODM_BOARD_EXT_LNA)) {
+		pDM_DigTable->rx_gain_range_max = DM_DIG_MAX_NIC;
+		pDM_DigTable->rx_gain_range_min = DM_DIG_MIN_NIC;
+	} else {
+		pDM_DigTable->rx_gain_range_max = DM_DIG_MAX_NIC;
+		pDM_DigTable->rx_gain_range_min = DM_DIG_MIN_NIC;
+	}
+
+}
+
+
+void odm_DIG(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	/*  Common parameters */
+	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+	Pfalse_ALARM_STATISTICS pFalseAlmCnt = &pDM_Odm->FalseAlmCnt;
+	bool FirstConnect, FirstDisConnect;
+	u8 DIG_MaxOfMin, DIG_Dynamic_MIN;
+	u8 dm_dig_max, dm_dig_min;
+	u8 CurrentIGI = pDM_DigTable->CurIGValue;
+	u8 offset;
+	u32 dm_FA_thres[3];
+	u8 Adap_IGI_Upper = 0;
+	u32 TxTp = 0, RxTp = 0;
+	bool bDFSBand = false;
+	bool bPerformance = true, bFirstTpTarget = false, bFirstCoverage = false;
+
+	if (odm_DigAbort(pDM_Odm) == true)
+		return;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG() ===========================>\n\n"));
+
+	if (pDM_Odm->adaptivity_flag == true)
+		Adap_IGI_Upper = pDM_Odm->Adaptivity_IGI_upper;
+
+
+	/* 1 Update status */
+	DIG_Dynamic_MIN = pDM_DigTable->DIG_Dynamic_MIN_0;
+	FirstConnect = (pDM_Odm->bLinked) && (pDM_DigTable->bMediaConnect_0 == false);
+	FirstDisConnect = (!pDM_Odm->bLinked) && (pDM_DigTable->bMediaConnect_0 == true);
+
+	/* 1 Boundary Decision */
+	/* 2 For WIN\CE */
+	dm_dig_max = 0x5A;
+	dm_dig_min = DM_DIG_MIN_NIC;
+	DIG_MaxOfMin = DM_DIG_MAX_AP;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Absolutly upper bound = 0x%x, lower bound = 0x%x\n", dm_dig_max, dm_dig_min));
+
+	/* 1 Adjust boundary by RSSI */
+	if (pDM_Odm->bLinked && bPerformance) {
+		/* 2 Modify DIG upper bound */
+		/* 4 Modify DIG upper bound for 92E, 8723A\B, 8821 & 8812 BT */
+		if (pDM_Odm->bBtLimitedDig == 1) {
+			offset = 10;
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Coex. case: Force upper bound to RSSI + %d !!!!!!\n", offset));
+		} else
+			offset = 15;
+
+		if ((pDM_Odm->RSSI_Min + offset) > dm_dig_max)
+			pDM_DigTable->rx_gain_range_max = dm_dig_max;
+		else if ((pDM_Odm->RSSI_Min + offset) < dm_dig_min)
+			pDM_DigTable->rx_gain_range_max = dm_dig_min;
+		else
+			pDM_DigTable->rx_gain_range_max = pDM_Odm->RSSI_Min + offset;
+
+		/* 2 Modify DIG lower bound */
+		/* if (pDM_Odm->bOneEntryOnly) */
+		{
+			if (pDM_Odm->RSSI_Min < dm_dig_min)
+				DIG_Dynamic_MIN = dm_dig_min;
+			else if (pDM_Odm->RSSI_Min > DIG_MaxOfMin)
+				DIG_Dynamic_MIN = DIG_MaxOfMin;
+			else
+				DIG_Dynamic_MIN = pDM_Odm->RSSI_Min;
+		}
+	} else {
+		pDM_DigTable->rx_gain_range_max = dm_dig_max;
+		DIG_Dynamic_MIN = dm_dig_min;
+	}
+
+	/* 1 Force Lower Bound for AntDiv */
+	if (pDM_Odm->bLinked && !pDM_Odm->bOneEntryOnly) {
+		if (pDM_Odm->SupportAbility & ODM_BB_ANT_DIV) {
+			if (
+				pDM_Odm->AntDivType == CG_TRX_HW_ANTDIV ||
+				pDM_Odm->AntDivType == CG_TRX_SMART_ANTDIV ||
+				pDM_Odm->AntDivType == S0S1_SW_ANTDIV
+			) {
+				if (pDM_DigTable->AntDiv_RSSI_max > DIG_MaxOfMin)
+					DIG_Dynamic_MIN = DIG_MaxOfMin;
+				else
+					DIG_Dynamic_MIN = (u8) pDM_DigTable->AntDiv_RSSI_max;
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_ANT_DIV,
+					ODM_DBG_LOUD,
+					(
+						"odm_DIG(): Antenna diversity case: Force lower bound to 0x%x !!!!!!\n",
+						DIG_Dynamic_MIN
+					)
+				);
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_ANT_DIV,
+					ODM_DBG_LOUD,
+					(
+						"odm_DIG(): Antenna diversity case: RSSI_max = 0x%x !!!!!!\n",
+						pDM_DigTable->AntDiv_RSSI_max
+					)
+				);
+			}
+		}
+	}
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		(
+			"odm_DIG(): Adjust boundary by RSSI Upper bound = 0x%x, Lower bound = 0x%x\n",
+			pDM_DigTable->rx_gain_range_max,
+			DIG_Dynamic_MIN
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		(
+			"odm_DIG(): Link status: bLinked = %d, RSSI = %d, bFirstConnect = %d, bFirsrDisConnect = %d\n\n",
+			pDM_Odm->bLinked,
+			pDM_Odm->RSSI_Min,
+			FirstConnect,
+			FirstDisConnect
+		)
+	);
+
+	/* 1 Modify DIG lower bound, deal with abnormal case */
+	/* 2 Abnormal false alarm case */
+	if (FirstDisConnect) {
+		pDM_DigTable->rx_gain_range_min = DIG_Dynamic_MIN;
+		pDM_DigTable->ForbiddenIGI = DIG_Dynamic_MIN;
+	} else
+		pDM_DigTable->rx_gain_range_min =
+			odm_ForbiddenIGICheck(pDM_Odm, DIG_Dynamic_MIN, CurrentIGI);
+
+	if (pDM_Odm->bLinked && !FirstConnect) {
+		if (
+			(pDM_Odm->PhyDbgInfo.NumQryBeaconPkt < 5) &&
+			pDM_Odm->bsta_state
+		) {
+			pDM_DigTable->rx_gain_range_min = dm_dig_min;
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_DIG,
+				ODM_DBG_LOUD,
+				(
+					"odm_DIG(): Abnrormal #beacon (%d) case in STA mode: Force lower bound to 0x%x !!!!!!\n\n",
+					pDM_Odm->PhyDbgInfo.NumQryBeaconPkt,
+					pDM_DigTable->rx_gain_range_min
+				)
+			);
+		}
+	}
+
+	/* 2 Abnormal lower bound case */
+	if (pDM_DigTable->rx_gain_range_min > pDM_DigTable->rx_gain_range_max) {
+		pDM_DigTable->rx_gain_range_min = pDM_DigTable->rx_gain_range_max;
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_DIG,
+			ODM_DBG_LOUD,
+			(
+				"odm_DIG(): Abnrormal lower bound case: Force lower bound to 0x%x !!!!!!\n\n",
+				pDM_DigTable->rx_gain_range_min
+			)
+		);
+	}
+
+
+	/* 1 False alarm threshold decision */
+	odm_FAThresholdCheck(pDM_Odm, bDFSBand, bPerformance, RxTp, TxTp, dm_FA_thres);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): False alarm threshold = %d, %d, %d\n\n", dm_FA_thres[0], dm_FA_thres[1], dm_FA_thres[2]));
+
+	/* 1 Adjust initial gain by false alarm */
+	if (pDM_Odm->bLinked && bPerformance) {
+		/* 2 After link */
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_DIG,
+			ODM_DBG_LOUD,
+			("odm_DIG(): Adjust IGI after link\n")
+		);
+
+		if (bFirstTpTarget || (FirstConnect && bPerformance)) {
+			pDM_DigTable->LargeFAHit = 0;
+
+			if (pDM_Odm->RSSI_Min < DIG_MaxOfMin) {
+				if (CurrentIGI < pDM_Odm->RSSI_Min)
+					CurrentIGI = pDM_Odm->RSSI_Min;
+			} else {
+				if (CurrentIGI < DIG_MaxOfMin)
+					CurrentIGI = DIG_MaxOfMin;
+			}
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_DIG,
+				ODM_DBG_LOUD,
+				(
+					"odm_DIG(): First connect case: IGI does on-shot to 0x%x\n",
+					CurrentIGI
+				)
+			);
+
+		} else {
+			if (pFalseAlmCnt->Cnt_all > dm_FA_thres[2])
+				CurrentIGI = CurrentIGI + 4;
+			else if (pFalseAlmCnt->Cnt_all > dm_FA_thres[1])
+				CurrentIGI = CurrentIGI + 2;
+			else if (pFalseAlmCnt->Cnt_all < dm_FA_thres[0])
+				CurrentIGI = CurrentIGI - 2;
+
+			if (
+				(pDM_Odm->PhyDbgInfo.NumQryBeaconPkt < 5) &&
+				(pFalseAlmCnt->Cnt_all < DM_DIG_FA_TH1) &&
+				(pDM_Odm->bsta_state)
+			) {
+				CurrentIGI = pDM_DigTable->rx_gain_range_min;
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_DIG,
+					ODM_DBG_LOUD,
+					(
+						"odm_DIG(): Abnormal #beacon (%d) case: IGI does one-shot to 0x%x\n",
+						pDM_Odm->PhyDbgInfo.NumQryBeaconPkt,
+						CurrentIGI
+					)
+				);
+			}
+		}
+	} else {
+		/* 2 Before link */
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_DIG,
+			ODM_DBG_LOUD,
+			("odm_DIG(): Adjust IGI before link\n")
+		);
+
+		if (FirstDisConnect || bFirstCoverage) {
+			CurrentIGI = dm_dig_min;
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_DIG,
+				ODM_DBG_LOUD,
+				("odm_DIG(): First disconnect case: IGI does on-shot to lower bound\n")
+			);
+		} else {
+			if (pFalseAlmCnt->Cnt_all > dm_FA_thres[2])
+				CurrentIGI = CurrentIGI + 4;
+			else if (pFalseAlmCnt->Cnt_all > dm_FA_thres[1])
+				CurrentIGI = CurrentIGI + 2;
+			else if (pFalseAlmCnt->Cnt_all < dm_FA_thres[0])
+				CurrentIGI = CurrentIGI - 2;
+		}
+	}
+
+	/* 1 Check initial gain by upper/lower bound */
+	if (CurrentIGI < pDM_DigTable->rx_gain_range_min)
+		CurrentIGI = pDM_DigTable->rx_gain_range_min;
+
+	if (CurrentIGI > pDM_DigTable->rx_gain_range_max)
+		CurrentIGI = pDM_DigTable->rx_gain_range_max;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		(
+			"odm_DIG(): CurIGValue = 0x%x, TotalFA = %d\n\n",
+			CurrentIGI,
+			pFalseAlmCnt->Cnt_all
+		)
+	);
+
+	/* 1 Force upper bound and lower bound for adaptivity */
+	if (
+		pDM_Odm->SupportAbility & ODM_BB_ADAPTIVITY &&
+		pDM_Odm->adaptivity_flag == true
+	) {
+		if (CurrentIGI > Adap_IGI_Upper)
+			CurrentIGI = Adap_IGI_Upper;
+
+		if (pDM_Odm->IGI_LowerBound != 0) {
+			if (CurrentIGI < pDM_Odm->IGI_LowerBound)
+				CurrentIGI = pDM_Odm->IGI_LowerBound;
+		}
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Adaptivity case: Force upper bound to 0x%x !!!!!!\n", Adap_IGI_Upper));
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Adaptivity case: Force lower bound to 0x%x !!!!!!\n\n", pDM_Odm->IGI_LowerBound));
+	}
+
+
+	/* 1 Update status */
+	if (pDM_Odm->bBtHsOperation) {
+		if (pDM_Odm->bLinked) {
+			if (pDM_DigTable->BT30_CurIGI > (CurrentIGI))
+				ODM_Write_DIG(pDM_Odm, CurrentIGI);
+			else
+				ODM_Write_DIG(pDM_Odm, pDM_DigTable->BT30_CurIGI);
+
+			pDM_DigTable->bMediaConnect_0 = pDM_Odm->bLinked;
+			pDM_DigTable->DIG_Dynamic_MIN_0 = DIG_Dynamic_MIN;
+		} else {
+			if (pDM_Odm->bLinkInProcess)
+				ODM_Write_DIG(pDM_Odm, 0x1c);
+			else if (pDM_Odm->bBtConnectProcess)
+				ODM_Write_DIG(pDM_Odm, 0x28);
+			else
+				ODM_Write_DIG(pDM_Odm, pDM_DigTable->BT30_CurIGI);/* ODM_Write_DIG(pDM_Odm, pDM_DigTable->CurIGValue); */
+		}
+	} else { /*  BT is not using */
+		ODM_Write_DIG(pDM_Odm, CurrentIGI);/* ODM_Write_DIG(pDM_Odm, pDM_DigTable->CurIGValue); */
+		pDM_DigTable->bMediaConnect_0 = pDM_Odm->bLinked;
+		pDM_DigTable->DIG_Dynamic_MIN_0 = DIG_Dynamic_MIN;
+	}
+}
+
+void odm_DIGbyRSSI_LPS(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	Pfalse_ALARM_STATISTICS pFalseAlmCnt = &pDM_Odm->FalseAlmCnt;
+
+	u8 RSSI_Lower = DM_DIG_MIN_NIC;   /* 0x1E or 0x1C */
+	u8 CurrentIGI = pDM_Odm->RSSI_Min;
+
+	CurrentIGI = CurrentIGI+RSSI_OFFSET_DIG;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		("odm_DIGbyRSSI_LPS() ==>\n")
+	);
+
+	/*  Using FW PS mode to make IGI */
+	/* Adjust by  FA in LPS MODE */
+	if (pFalseAlmCnt->Cnt_all > DM_DIG_FA_TH2_LPS)
+		CurrentIGI = CurrentIGI+4;
+	else if (pFalseAlmCnt->Cnt_all > DM_DIG_FA_TH1_LPS)
+		CurrentIGI = CurrentIGI+2;
+	else if (pFalseAlmCnt->Cnt_all < DM_DIG_FA_TH0_LPS)
+		CurrentIGI = CurrentIGI-2;
+
+
+	/* Lower bound checking */
+
+	/* RSSI Lower bound check */
+	if ((pDM_Odm->RSSI_Min-10) > DM_DIG_MIN_NIC)
+		RSSI_Lower = pDM_Odm->RSSI_Min-10;
+	else
+		RSSI_Lower = DM_DIG_MIN_NIC;
+
+	/* Upper and Lower Bound checking */
+	if (CurrentIGI > DM_DIG_MAX_NIC)
+		CurrentIGI = DM_DIG_MAX_NIC;
+	else if (CurrentIGI < RSSI_Lower)
+		CurrentIGI = RSSI_Lower;
+
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		("odm_DIGbyRSSI_LPS(): pFalseAlmCnt->Cnt_all = %d\n", pFalseAlmCnt->Cnt_all)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		("odm_DIGbyRSSI_LPS(): pDM_Odm->RSSI_Min = %d\n", pDM_Odm->RSSI_Min)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		("odm_DIGbyRSSI_LPS(): CurrentIGI = 0x%x\n", CurrentIGI)
+	);
+
+	ODM_Write_DIG(pDM_Odm, CurrentIGI);
+	/* ODM_Write_DIG(pDM_Odm, pDM_DigTable->CurIGValue); */
+}
+
+/* 3 ============================================================ */
+/* 3 FASLE ALARM CHECK */
+/* 3 ============================================================ */
+
+void odm_FalseAlarmCounterStatistics(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	Pfalse_ALARM_STATISTICS FalseAlmCnt = &(pDM_Odm->FalseAlmCnt);
+	u32 ret_value;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT))
+		return;
+
+	/* hold ofdm counter */
+	/* hold page C counter */
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_OFDM_FA_HOLDC_11N, BIT31, 1);
+	/* hold page D counter */
+	PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_OFDM_FA_RSTD_11N, BIT31, 1);
+
+	ret_value = PHY_QueryBBReg(
+		pDM_Odm->Adapter, ODM_REG_OFDM_FA_TYPE1_11N, bMaskDWord
+	);
+	FalseAlmCnt->Cnt_Fast_Fsync = (ret_value&0xffff);
+	FalseAlmCnt->Cnt_SB_Search_fail = ((ret_value&0xffff0000)>>16);
+
+	ret_value = PHY_QueryBBReg(
+		pDM_Odm->Adapter, ODM_REG_OFDM_FA_TYPE2_11N, bMaskDWord
+	);
+	FalseAlmCnt->Cnt_OFDM_CCA = (ret_value&0xffff);
+	FalseAlmCnt->Cnt_Parity_Fail = ((ret_value&0xffff0000)>>16);
+
+	ret_value = PHY_QueryBBReg(
+		pDM_Odm->Adapter, ODM_REG_OFDM_FA_TYPE3_11N, bMaskDWord
+	);
+	FalseAlmCnt->Cnt_Rate_Illegal = (ret_value&0xffff);
+	FalseAlmCnt->Cnt_Crc8_fail = ((ret_value&0xffff0000)>>16);
+
+	ret_value = PHY_QueryBBReg(
+		pDM_Odm->Adapter, ODM_REG_OFDM_FA_TYPE4_11N, bMaskDWord
+	);
+	FalseAlmCnt->Cnt_Mcs_fail = (ret_value&0xffff);
+
+	FalseAlmCnt->Cnt_Ofdm_fail =
+		FalseAlmCnt->Cnt_Parity_Fail +
+		FalseAlmCnt->Cnt_Rate_Illegal +
+		FalseAlmCnt->Cnt_Crc8_fail +
+		FalseAlmCnt->Cnt_Mcs_fail +
+		FalseAlmCnt->Cnt_Fast_Fsync +
+		FalseAlmCnt->Cnt_SB_Search_fail;
+
+	{
+		/* hold cck counter */
+		PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_CCK_FA_RST_11N, BIT12, 1);
+		PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG_CCK_FA_RST_11N, BIT14, 1);
+
+		ret_value = PHY_QueryBBReg(
+			pDM_Odm->Adapter, ODM_REG_CCK_FA_LSB_11N, bMaskByte0
+		);
+		FalseAlmCnt->Cnt_Cck_fail = ret_value;
+
+		ret_value = PHY_QueryBBReg(
+			pDM_Odm->Adapter, ODM_REG_CCK_FA_MSB_11N, bMaskByte3
+		);
+		FalseAlmCnt->Cnt_Cck_fail += (ret_value&0xff)<<8;
+
+		ret_value = PHY_QueryBBReg(
+			pDM_Odm->Adapter, ODM_REG_CCK_CCA_CNT_11N, bMaskDWord
+		);
+		FalseAlmCnt->Cnt_CCK_CCA =
+			((ret_value&0xFF)<<8) | ((ret_value&0xFF00)>>8);
+	}
+
+	FalseAlmCnt->Cnt_all = (
+		FalseAlmCnt->Cnt_Fast_Fsync +
+		FalseAlmCnt->Cnt_SB_Search_fail +
+		FalseAlmCnt->Cnt_Parity_Fail +
+		FalseAlmCnt->Cnt_Rate_Illegal +
+		FalseAlmCnt->Cnt_Crc8_fail +
+		FalseAlmCnt->Cnt_Mcs_fail +
+		FalseAlmCnt->Cnt_Cck_fail
+	);
+
+	FalseAlmCnt->Cnt_CCA_all =
+		FalseAlmCnt->Cnt_OFDM_CCA + FalseAlmCnt->Cnt_CCK_CCA;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Enter odm_FalseAlarmCounterStatistics\n")
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		(
+			"Cnt_Fast_Fsync =%d, Cnt_SB_Search_fail =%d\n",
+			FalseAlmCnt->Cnt_Fast_Fsync,
+			FalseAlmCnt->Cnt_SB_Search_fail
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		(
+			"Cnt_Parity_Fail =%d, Cnt_Rate_Illegal =%d\n",
+			FalseAlmCnt->Cnt_Parity_Fail,
+			FalseAlmCnt->Cnt_Rate_Illegal
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		(
+			"Cnt_Crc8_fail =%d, Cnt_Mcs_fail =%d\n",
+			FalseAlmCnt->Cnt_Crc8_fail,
+			FalseAlmCnt->Cnt_Mcs_fail
+		)
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_OFDM_CCA =%d\n", FalseAlmCnt->Cnt_OFDM_CCA)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_CCK_CCA =%d\n", FalseAlmCnt->Cnt_CCK_CCA)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_CCA_all =%d\n", FalseAlmCnt->Cnt_CCA_all)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_Ofdm_fail =%d\n",	FalseAlmCnt->Cnt_Ofdm_fail)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_Cck_fail =%d\n",	FalseAlmCnt->Cnt_Cck_fail)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_Ofdm_fail =%d\n",	FalseAlmCnt->Cnt_Ofdm_fail)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Total False Alarm =%d\n",	FalseAlmCnt->Cnt_all)
+	);
+}
+
+
+void odm_FAThresholdCheck(
+	void *pDM_VOID,
+	bool bDFSBand,
+	bool bPerformance,
+	u32 RxTp,
+	u32 TxTp,
+	u32 *dm_FA_thres
+)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	if (pDM_Odm->bLinked && (bPerformance || bDFSBand)) {
+		/*  For NIC */
+		dm_FA_thres[0] = DM_DIG_FA_TH0;
+		dm_FA_thres[1] = DM_DIG_FA_TH1;
+		dm_FA_thres[2] = DM_DIG_FA_TH2;
+	} else {
+		dm_FA_thres[0] = 2000;
+		dm_FA_thres[1] = 4000;
+		dm_FA_thres[2] = 5000;
+	}
+	return;
+}
+
+u8 odm_ForbiddenIGICheck(void *pDM_VOID, u8 DIG_Dynamic_MIN, u8 CurrentIGI)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+	Pfalse_ALARM_STATISTICS pFalseAlmCnt = &(pDM_Odm->FalseAlmCnt);
+	u8 rx_gain_range_min = pDM_DigTable->rx_gain_range_min;
+
+	if (pFalseAlmCnt->Cnt_all > 10000) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Abnormally false alarm case.\n"));
+
+		if (pDM_DigTable->LargeFAHit != 3)
+			pDM_DigTable->LargeFAHit++;
+
+		/* if (pDM_DigTable->ForbiddenIGI < pDM_DigTable->CurIGValue) */
+		if (pDM_DigTable->ForbiddenIGI < CurrentIGI) {
+			pDM_DigTable->ForbiddenIGI = CurrentIGI;
+			/* pDM_DigTable->ForbiddenIGI = pDM_DigTable->CurIGValue; */
+			pDM_DigTable->LargeFAHit = 1;
+		}
+
+		if (pDM_DigTable->LargeFAHit >= 3) {
+			if ((pDM_DigTable->ForbiddenIGI + 2) > pDM_DigTable->rx_gain_range_max)
+				rx_gain_range_min = pDM_DigTable->rx_gain_range_max;
+			else
+				rx_gain_range_min = (pDM_DigTable->ForbiddenIGI + 2);
+			pDM_DigTable->Recover_cnt = 1800;
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Abnormally false alarm case: Recover_cnt = %d\n", pDM_DigTable->Recover_cnt));
+		}
+	} else {
+		if (pDM_DigTable->Recover_cnt != 0) {
+			pDM_DigTable->Recover_cnt--;
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Normal Case: Recover_cnt = %d\n", pDM_DigTable->Recover_cnt));
+		} else {
+			if (pDM_DigTable->LargeFAHit < 3) {
+				if ((pDM_DigTable->ForbiddenIGI - 2) < DIG_Dynamic_MIN) { /* DM_DIG_MIN) */
+					pDM_DigTable->ForbiddenIGI = DIG_Dynamic_MIN; /* DM_DIG_MIN; */
+					rx_gain_range_min = DIG_Dynamic_MIN; /* DM_DIG_MIN; */
+					ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Normal Case: At Lower Bound\n"));
+				} else {
+					pDM_DigTable->ForbiddenIGI -= 2;
+					rx_gain_range_min = (pDM_DigTable->ForbiddenIGI + 2);
+					ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Normal Case: Approach Lower Bound\n"));
+				}
+			} else
+				pDM_DigTable->LargeFAHit = 0;
+		}
+	}
+
+	return rx_gain_range_min;
+
+}
+
+/* 3 ============================================================ */
+/* 3 CCK Packet Detect Threshold */
+/* 3 ============================================================ */
+
+void odm_CCKPacketDetectionThresh(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	Pfalse_ALARM_STATISTICS FalseAlmCnt = &(pDM_Odm->FalseAlmCnt);
+	u8 CurCCK_CCAThres;
+
+
+	if (
+		!(pDM_Odm->SupportAbility & ODM_BB_CCK_PD) ||
+		!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT)
+	) {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_CCK_PD,
+			ODM_DBG_LOUD,
+			("odm_CCKPacketDetectionThresh()  return ==========\n")
+		);
+		return;
+	}
+
+	if (pDM_Odm->ExtLNA)
+		return;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CCK_PD,
+		ODM_DBG_LOUD,
+		("odm_CCKPacketDetectionThresh()  ==========>\n")
+	);
+
+	if (pDM_Odm->bLinked) {
+		if (pDM_Odm->RSSI_Min > 25)
+			CurCCK_CCAThres = 0xcd;
+		else if ((pDM_Odm->RSSI_Min <= 25) && (pDM_Odm->RSSI_Min > 10))
+			CurCCK_CCAThres = 0x83;
+		else {
+			if (FalseAlmCnt->Cnt_Cck_fail > 1000)
+				CurCCK_CCAThres = 0x83;
+			else
+				CurCCK_CCAThres = 0x40;
+		}
+	} else {
+		if (FalseAlmCnt->Cnt_Cck_fail > 1000)
+			CurCCK_CCAThres = 0x83;
+		else
+			CurCCK_CCAThres = 0x40;
+	}
+
+	ODM_Write_CCK_CCA_Thres(pDM_Odm, CurCCK_CCAThres);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CCK_PD,
+		ODM_DBG_LOUD,
+		(
+			"odm_CCKPacketDetectionThresh()  CurCCK_CCAThres = 0x%x\n",
+			CurCCK_CCAThres
+		)
+	);
+}
+
+void ODM_Write_CCK_CCA_Thres(void *pDM_VOID, u8 CurCCK_CCAThres)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+
+	/* modify by Guo.Mingzhi 2012-01-03 */
+	if (pDM_DigTable->CurCCK_CCAThres != CurCCK_CCAThres)
+		rtw_write8(pDM_Odm->Adapter, ODM_REG(CCK_CCA, pDM_Odm), CurCCK_CCAThres);
+
+	pDM_DigTable->PreCCK_CCAThres = pDM_DigTable->CurCCK_CCAThres;
+	pDM_DigTable->CurCCK_CCAThres = CurCCK_CCAThres;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_DIG.h linux/3rdparty/rtl8723bs/hal/odm_DIG.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_DIG.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_DIG.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,195 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#ifndef __ODMDIG_H__
+#define __ODMDIG_H__
+
+typedef struct _Dynamic_Initial_Gain_Threshold_ {
+	bool bStopDIG;
+	bool bPSDInProgress;
+
+	u8 Dig_Enable_Flag;
+	u8 Dig_Ext_Port_Stage;
+
+	int RssiLowThresh;
+	int RssiHighThresh;
+
+	u32 FALowThresh;
+	u32 FAHighThresh;
+
+	u8 CurSTAConnectState;
+	u8 PreSTAConnectState;
+	u8 CurMultiSTAConnectState;
+
+	u8 PreIGValue;
+	u8 CurIGValue;
+	u8 BackupIGValue;		/* MP DIG */
+	u8 BT30_CurIGI;
+	u8 IGIBackup;
+
+	s8 BackoffVal;
+	s8 BackoffVal_range_max;
+	s8 BackoffVal_range_min;
+	u8 rx_gain_range_max;
+	u8 rx_gain_range_min;
+	u8 Rssi_val_min;
+
+	u8 PreCCK_CCAThres;
+	u8 CurCCK_CCAThres;
+	u8 PreCCKPDState;
+	u8 CurCCKPDState;
+	u8 CCKPDBackup;
+
+	u8 LargeFAHit;
+	u8 ForbiddenIGI;
+	u32 Recover_cnt;
+
+	u8 DIG_Dynamic_MIN_0;
+	u8 DIG_Dynamic_MIN_1;
+	bool bMediaConnect_0;
+	bool bMediaConnect_1;
+
+	u32 AntDiv_RSSI_max;
+	u32 RSSI_max;
+
+	u8 *pbP2pLinkInProgress;
+} DIG_T, *pDIG_T;
+
+typedef struct false_ALARM_STATISTICS {
+	u32 Cnt_Parity_Fail;
+	u32 Cnt_Rate_Illegal;
+	u32 Cnt_Crc8_fail;
+	u32 Cnt_Mcs_fail;
+	u32 Cnt_Ofdm_fail;
+	u32 Cnt_Ofdm_fail_pre; /* For RTL8881A */
+	u32 Cnt_Cck_fail;
+	u32 Cnt_all;
+	u32 Cnt_Fast_Fsync;
+	u32 Cnt_SB_Search_fail;
+	u32 Cnt_OFDM_CCA;
+	u32 Cnt_CCK_CCA;
+	u32 Cnt_CCA_all;
+	u32 Cnt_BW_USC; /* Gary */
+	u32 Cnt_BW_LSC; /* Gary */
+} false_ALARM_STATISTICS, *Pfalse_ALARM_STATISTICS;
+
+typedef enum tag_Dynamic_Init_Gain_Operation_Type_Definition {
+	DIG_TYPE_THRESH_HIGH = 0,
+	DIG_TYPE_THRESH_LOW = 1,
+	DIG_TYPE_BACKOFF = 2,
+	DIG_TYPE_RX_GAIN_MIN = 3,
+	DIG_TYPE_RX_GAIN_MAX = 4,
+	DIG_TYPE_ENABLE = 5,
+	DIG_TYPE_DISABLE = 6,
+	DIG_OP_TYPE_MAX
+} DM_DIG_OP_E;
+
+typedef enum tag_ODM_PauseDIG_Type {
+	ODM_PAUSE_DIG = BIT0,
+	ODM_RESUME_DIG = BIT1
+} ODM_Pause_DIG_TYPE;
+
+typedef enum tag_ODM_PauseCCKPD_Type {
+	ODM_PAUSE_CCKPD = BIT0,
+	ODM_RESUME_CCKPD = BIT1
+} ODM_Pause_CCKPD_TYPE;
+
+#define		DM_DIG_THRESH_HIGH			40
+#define		DM_DIG_THRESH_LOW			35
+
+#define		DMfalseALARM_THRESH_LOW	400
+#define		DMfalseALARM_THRESH_HIGH	1000
+
+#define		DM_DIG_MAX_NIC				0x3e
+#define		DM_DIG_MIN_NIC				0x1e /* 0x22//0x1c */
+#define		DM_DIG_MAX_OF_MIN_NIC		0x3e
+
+#define		DM_DIG_MAX_AP					0x3e
+#define		DM_DIG_MIN_AP					0x1c
+#define		DM_DIG_MAX_OF_MIN			0x2A	/* 0x32 */
+#define		DM_DIG_MIN_AP_DFS				0x20
+
+#define		DM_DIG_MAX_NIC_HP			0x46
+#define		DM_DIG_MIN_NIC_HP				0x2e
+
+#define		DM_DIG_MAX_AP_HP				0x42
+#define		DM_DIG_MIN_AP_HP				0x30
+
+#define		DM_DIG_FA_TH0				0x200/* 0x20 */
+
+#define		DM_DIG_FA_TH1					0x300
+#define		DM_DIG_FA_TH2					0x400
+/* this is for 92d */
+#define		DM_DIG_FA_TH0_92D				0x100
+#define		DM_DIG_FA_TH1_92D				0x400
+#define		DM_DIG_FA_TH2_92D				0x600
+
+#define		DM_DIG_BACKOFF_MAX			12
+#define		DM_DIG_BACKOFF_MIN			-4
+#define		DM_DIG_BACKOFF_DEFAULT		10
+
+#define			DM_DIG_FA_TH0_LPS				4 /*  4 in lps */
+#define			DM_DIG_FA_TH1_LPS				15 /*  15 lps */
+#define			DM_DIG_FA_TH2_LPS				30 /*  30 lps */
+#define			RSSI_OFFSET_DIG				0x05
+
+void odm_NHMCounterStatisticsInit(void *pDM_VOID);
+
+void odm_NHMCounterStatistics(void *pDM_VOID);
+
+void odm_NHMBBInit(void *pDM_VOID);
+
+void odm_NHMBB(void *pDM_VOID);
+
+void odm_NHMCounterStatisticsReset(void *pDM_VOID);
+
+void odm_GetNHMCounterStatistics(void *pDM_VOID);
+
+void odm_SearchPwdBLowerBound(void *pDM_VOID, u8 IGI_target);
+
+void odm_AdaptivityInit(void *pDM_VOID);
+
+void odm_Adaptivity(void *pDM_VOID, u8 IGI);
+
+void ODM_Write_DIG(void *pDM_VOID, u8 CurrentIGI);
+
+void odm_PauseDIG(void *pDM_VOID, ODM_Pause_DIG_TYPE PauseType, u8 IGIValue);
+
+void odm_DIGInit(void *pDM_VOID);
+
+void odm_DIG(void *pDM_VOID);
+
+void odm_DIGbyRSSI_LPS(void *pDM_VOID);
+
+void odm_FalseAlarmCounterStatistics(void *pDM_VOID);
+
+void odm_FAThresholdCheck(
+	void *pDM_VOID,
+	bool bDFSBand,
+	bool bPerformance,
+	u32 RxTp,
+	u32 TxTp,
+	u32 *dm_FA_thres
+);
+
+u8 odm_ForbiddenIGICheck(void *pDM_VOID, u8 DIG_Dynamic_MIN, u8 CurrentIGI);
+
+bool odm_DigAbort(void *pDM_VOID);
+
+void odm_CCKPacketDetectionThresh(void *pDM_VOID);
+
+void ODM_Write_CCK_CCA_Thres(void *pDM_VOID, u8 CurCCK_CCAThres);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_DynamicBBPowerSaving.c linux/3rdparty/rtl8723bs/hal/odm_DynamicBBPowerSaving.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_DynamicBBPowerSaving.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_DynamicBBPowerSaving.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,89 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+void odm_DynamicBBPowerSavingInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pPS_T pDM_PSTable = &pDM_Odm->DM_PSTable;
+
+	pDM_PSTable->PreCCAState = CCA_MAX;
+	pDM_PSTable->CurCCAState = CCA_MAX;
+	pDM_PSTable->PreRFState = RF_MAX;
+	pDM_PSTable->CurRFState = RF_MAX;
+	pDM_PSTable->Rssi_val_min = 0;
+	pDM_PSTable->initialize = 0;
+}
+
+void ODM_RF_Saving(void *pDM_VOID, u8 bForceInNormal)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pPS_T pDM_PSTable = &pDM_Odm->DM_PSTable;
+	u8 Rssi_Up_bound = 30;
+	u8 Rssi_Low_bound = 25;
+
+	if (pDM_Odm->PatchID == 40) { /* RT_CID_819x_FUNAI_TV */
+		Rssi_Up_bound = 50;
+		Rssi_Low_bound = 45;
+	}
+
+	if (pDM_PSTable->initialize == 0) {
+
+		pDM_PSTable->Reg874 = (PHY_QueryBBReg(pDM_Odm->Adapter, 0x874, bMaskDWord)&0x1CC000)>>14;
+		pDM_PSTable->RegC70 = (PHY_QueryBBReg(pDM_Odm->Adapter, 0xc70, bMaskDWord)&BIT3)>>3;
+		pDM_PSTable->Reg85C = (PHY_QueryBBReg(pDM_Odm->Adapter, 0x85c, bMaskDWord)&0xFF000000)>>24;
+		pDM_PSTable->RegA74 = (PHY_QueryBBReg(pDM_Odm->Adapter, 0xa74, bMaskDWord)&0xF000)>>12;
+		/* Reg818 = PHY_QueryBBReg(padapter, 0x818, bMaskDWord); */
+		pDM_PSTable->initialize = 1;
+	}
+
+	if (!bForceInNormal) {
+		if (pDM_Odm->RSSI_Min != 0xFF) {
+			if (pDM_PSTable->PreRFState == RF_Normal) {
+				if (pDM_Odm->RSSI_Min >= Rssi_Up_bound)
+					pDM_PSTable->CurRFState = RF_Save;
+				else
+					pDM_PSTable->CurRFState = RF_Normal;
+			} else {
+				if (pDM_Odm->RSSI_Min <= Rssi_Low_bound)
+					pDM_PSTable->CurRFState = RF_Normal;
+				else
+					pDM_PSTable->CurRFState = RF_Save;
+			}
+		} else
+			pDM_PSTable->CurRFState = RF_MAX;
+	} else
+		pDM_PSTable->CurRFState = RF_Normal;
+
+	if (pDM_PSTable->PreRFState != pDM_PSTable->CurRFState) {
+		if (pDM_PSTable->CurRFState == RF_Save) {
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x874, 0x1C0000, 0x2); /* Reg874[20:18]=3'b010 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xc70, BIT3, 0); /* RegC70[3]= 1'b0 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x85c, 0xFF000000, 0x63); /* Reg85C[31:24]= 0x63 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x874, 0xC000, 0x2); /* Reg874[15:14]=2'b10 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xa74, 0xF000, 0x3); /* RegA75[7:4]= 0x3 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x818, BIT28, 0x0); /* Reg818[28]= 1'b0 */
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x818, BIT28, 0x1); /* Reg818[28]= 1'b1 */
+		} else {
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x874, 0x1CC000, pDM_PSTable->Reg874);
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xc70, BIT3, pDM_PSTable->RegC70);
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x85c, 0xFF000000, pDM_PSTable->Reg85C);
+			PHY_SetBBReg(pDM_Odm->Adapter, 0xa74, 0xF000, pDM_PSTable->RegA74);
+			PHY_SetBBReg(pDM_Odm->Adapter, 0x818, BIT28, 0x0);
+		}
+		pDM_PSTable->PreRFState = pDM_PSTable->CurRFState;
+	}
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_DynamicBBPowerSaving.h linux/3rdparty/rtl8723bs/hal/odm_DynamicBBPowerSaving.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_DynamicBBPowerSaving.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_DynamicBBPowerSaving.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,39 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODMDYNAMICBBPOWERSAVING_H__
+#define    __ODMDYNAMICBBPOWERSAVING_H__
+
+typedef struct _Dynamic_Power_Saving_ {
+	u8 PreCCAState;
+	u8 CurCCAState;
+
+	u8 PreRFState;
+	u8 CurRFState;
+
+	int Rssi_val_min;
+
+	u8 initialize;
+	u32 Reg874, RegC70, Reg85C, RegA74;
+
+} PS_T, *pPS_T;
+
+#define dm_RF_Saving ODM_RF_Saving
+
+void ODM_RF_Saving(void *pDM_VOID, u8 bForceInNormal);
+
+void odm_DynamicBBPowerSavingInit(void *pDM_VOID);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_DynamicTxPower.c linux/3rdparty/rtl8723bs/hal/odm_DynamicTxPower.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_DynamicTxPower.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_DynamicTxPower.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,30 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+void odm_DynamicTxPowerInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+
+	pdmpriv->bDynamicTxPowerEnable = false;
+
+	pdmpriv->LastDTPLvl = TxHighPwrLevel_Normal;
+	pdmpriv->DynamicTxHighPowerLvl = TxHighPwrLevel_Normal;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_DynamicTxPower.h linux/3rdparty/rtl8723bs/hal/odm_DynamicTxPower.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_DynamicTxPower.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_DynamicTxPower.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,37 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODMDYNAMICTXPOWER_H__
+#define    __ODMDYNAMICTXPOWER_H__
+
+#define		TX_POWER_NEAR_FIELD_THRESH_LVL2	74
+#define		TX_POWER_NEAR_FIELD_THRESH_LVL1	67
+#define		TX_POWER_NEAR_FIELD_THRESH_AP		0x3F
+#define		TX_POWER_NEAR_FIELD_THRESH_8812	60
+
+#define		TxHighPwrLevel_Normal		0
+#define		TxHighPwrLevel_Level1		1
+#define		TxHighPwrLevel_Level2		2
+#define		TxHighPwrLevel_BT1			3
+#define		TxHighPwrLevel_BT2			4
+#define		TxHighPwrLevel_15			5
+#define		TxHighPwrLevel_35			6
+#define		TxHighPwrLevel_50			7
+#define		TxHighPwrLevel_70			8
+#define		TxHighPwrLevel_100			9
+
+void odm_DynamicTxPowerInit(void *pDM_VOID);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_EdcaTurboCheck.c linux/3rdparty/rtl8723bs/hal/odm_EdcaTurboCheck.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_EdcaTurboCheck.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_EdcaTurboCheck.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,186 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+static u32 edca_setting_DL_GMode[HT_IOT_PEER_MAX] = {
+/*UNKNOWN, REALTEK_90, ALTEK_92SE	BROADCOM, LINK	ATHEROS,
+ *CISCO, MERU, MARVELL, 92U_AP, SELF_AP
+ */
+	0x4322, 0xa44f, 0x5e4322, 0xa42b, 0x5e4322, 0x4322,
+	0xa42b, 0x5ea42b, 0xa44f, 0x5e4322, 0x5ea42b
+};
+
+static u32 edca_setting_UL[HT_IOT_PEER_MAX] = {
+/*UNKNOWN, REALTEK_90, REALTEK_92SE, BROADCOM, RALINK, ATHEROS,
+ *CISCO, MERU, MARVELL, 92U_AP, SELF_AP(DownLink/Tx)
+ */
+	0x5e4322, 0xa44f, 0x5e4322, 0x5ea32b, 0x5ea422,	0x5ea322,
+	0x3ea430, 0x5ea42b, 0x5ea44f, 0x5e4322, 0x5e4322};
+
+static u32 edca_setting_DL[HT_IOT_PEER_MAX] = {
+/*UNKNOWN, REALTEK_90, REALTEK_92SE, BROADCOM, RALINK, ATHEROS,
+ *CISCO, MERU, MARVELL, 92U_AP, SELF_AP(UpLink/Rx)
+ */
+	0xa44f, 0x5ea44f, 0x5e4322, 0x5ea42b, 0xa44f, 0xa630,
+	0x5ea630, 0x5ea42b, 0xa44f, 0xa42b, 0xa42b};
+
+void ODM_EdcaTurboInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	struct adapter *Adapter = pDM_Odm->Adapter;
+
+	pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = false;
+	pDM_Odm->DM_EDCA_Table.bIsCurRDLState = false;
+	Adapter->recvpriv.bIsAnyNonBEPkts = false;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial VO PARAM: 0x%x\n",
+		      rtw_read32(pDM_Odm->Adapter, ODM_EDCA_VO_PARAM)));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial VI PARAM: 0x%x\n",
+		      rtw_read32(pDM_Odm->Adapter, ODM_EDCA_VI_PARAM)));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial BE PARAM: 0x%x\n",
+		      rtw_read32(pDM_Odm->Adapter, ODM_EDCA_BE_PARAM)));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial BK PARAM: 0x%x\n",
+		      rtw_read32(pDM_Odm->Adapter, ODM_EDCA_BK_PARAM)));
+}	/*  ODM_InitEdcaTurbo */
+
+void odm_EdcaTurboCheck(void *pDM_VOID)
+{
+	/*  In HW integration first stage, we provide 4 different handles to
+	 *  operate at the same time. In stage2/3, we need to prove universal
+	 *  interface and merge all HW dynamic mechanism.
+	 */
+	PDM_ODM_T		pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("odm_EdcaTurboCheck ========================>\n"));
+
+	if (!(pDM_Odm->SupportAbility & ODM_MAC_EDCA_TURBO))
+		return;
+
+	odm_EdcaTurboCheckCE(pDM_Odm);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("<========================odm_EdcaTurboCheck\n"));
+}	/*  odm_CheckEdcaTurbo */
+
+void odm_EdcaTurboCheckCE(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(Adapter);
+	struct recv_priv *precvpriv = &(Adapter->recvpriv);
+	struct registry_priv *pregpriv = &Adapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &(Adapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u32 EDCA_BE_UL = 0x5ea42b;
+	u32 EDCA_BE_DL = 0x5ea42b;
+	u32 iot_peer = 0;
+	u8 wirelessmode = 0xFF;		/* invalid value */
+	u32 trafficIndex;
+	u32 edca_param;
+	u64	cur_tx_bytes = 0;
+	u64	cur_rx_bytes = 0;
+	u8 bbtchange = false;
+	u8 biasonrx = false;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+
+	if (!pDM_Odm->bLinked) {
+		precvpriv->bIsAnyNonBEPkts = false;
+		return;
+	}
+
+	if ((pregpriv->wifi_spec == 1)) {
+		precvpriv->bIsAnyNonBEPkts = false;
+		return;
+	}
+
+	if (pDM_Odm->pwirelessmode)
+		wirelessmode = *(pDM_Odm->pwirelessmode);
+
+	iot_peer = pmlmeinfo->assoc_AP_vendor;
+
+	if (iot_peer >=  HT_IOT_PEER_MAX) {
+		precvpriv->bIsAnyNonBEPkts = false;
+		return;
+	}
+
+	/*  Check if the status needs to be changed. */
+	if ((bbtchange) || (!precvpriv->bIsAnyNonBEPkts)) {
+		cur_tx_bytes = pdvobjpriv->traffic_stat.cur_tx_bytes;
+		cur_rx_bytes = pdvobjpriv->traffic_stat.cur_rx_bytes;
+
+		/* traffic, TX or RX */
+		if (biasonrx) {
+			if (cur_tx_bytes > (cur_rx_bytes << 2)) {
+				/*  Uplink TP is present. */
+				trafficIndex = UP_LINK;
+			} else { /*  Balance TP is present. */
+				trafficIndex = DOWN_LINK;
+			}
+		} else {
+			if (cur_rx_bytes > (cur_tx_bytes << 2)) {
+				/*  Downlink TP is present. */
+				trafficIndex = DOWN_LINK;
+			} else { /*  Balance TP is present. */
+				trafficIndex = UP_LINK;
+			}
+		}
+
+		/* 92D txop can't be set to 0x3e for cisco1250 */
+		if ((iot_peer == HT_IOT_PEER_CISCO) &&
+		    (wirelessmode == ODM_WM_N24G)) {
+			EDCA_BE_DL = edca_setting_DL[iot_peer];
+			EDCA_BE_UL = edca_setting_UL[iot_peer];
+		} else if ((iot_peer == HT_IOT_PEER_CISCO) &&
+			   ((wirelessmode == ODM_WM_G) ||
+			    (wirelessmode == (ODM_WM_B | ODM_WM_G)) ||
+			    (wirelessmode == ODM_WM_A) ||
+			    (wirelessmode == ODM_WM_B))) {
+			EDCA_BE_DL = edca_setting_DL_GMode[iot_peer];
+		} else if ((iot_peer == HT_IOT_PEER_AIRGO) &&
+			   ((wirelessmode == ODM_WM_G) ||
+			    (wirelessmode == ODM_WM_A))) {
+			EDCA_BE_DL = 0xa630;
+		} else if (iot_peer == HT_IOT_PEER_MARVELL) {
+			EDCA_BE_DL = edca_setting_DL[iot_peer];
+			EDCA_BE_UL = edca_setting_UL[iot_peer];
+		} else if (iot_peer == HT_IOT_PEER_ATHEROS) {
+			/*  Set DL EDCA for Atheros peer to 0x3ea42b. */
+			EDCA_BE_DL = edca_setting_DL[iot_peer];
+		}
+
+		if (trafficIndex == DOWN_LINK)
+			edca_param = EDCA_BE_DL;
+		else
+			edca_param = EDCA_BE_UL;
+
+		rtw_write32(Adapter, REG_EDCA_BE_PARAM, edca_param);
+
+		pDM_Odm->DM_EDCA_Table.prv_traffic_idx = trafficIndex;
+
+		pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = true;
+	} else {
+		/*  Turn Off EDCA turbo here. */
+		/*  Restore original EDCA according to the declaration of AP. */
+		 if (pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA) {
+			rtw_write32(Adapter, REG_EDCA_BE_PARAM, pHalData->AcParam_BE);
+			pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = false;
+		}
+	}
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_EdcaTurboCheck.h linux/3rdparty/rtl8723bs/hal/odm_EdcaTurboCheck.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_EdcaTurboCheck.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_EdcaTurboCheck.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,31 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#ifndef __ODMEDCATURBOCHECK_H__
+#define __ODMEDCATURBOCHECK_H__
+
+typedef struct _EDCA_TURBO_ {
+	bool bCurrentTurboEDCA;
+	bool bIsCurRDLState;
+
+	u32 prv_traffic_idx; /*  edca turbo */
+} EDCA_T, *pEDCA_T;
+
+void odm_EdcaTurboCheck(void *pDM_VOID);
+void ODM_EdcaTurboInit(void *pDM_VOID);
+
+void odm_EdcaTurboCheckCE(void *pDM_VOID);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm.h linux/3rdparty/rtl8723bs/hal/odm.h
--- linux.old/3rdparty/rtl8723bs/hal/odm.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,1465 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+
+#ifndef	__HALDMOUTSRC_H__
+#define __HALDMOUTSRC_H__
+
+
+#include "odm_EdcaTurboCheck.h"
+#include "odm_DIG.h"
+#include "odm_PathDiv.h"
+#include "odm_DynamicBBPowerSaving.h"
+#include "odm_DynamicTxPower.h"
+#include "odm_CfoTracking.h"
+#include "odm_NoiseMonitor.h"
+
+#define	TP_MODE		0
+#define	RSSI_MODE		1
+#define	TRAFFIC_LOW	0
+#define	TRAFFIC_HIGH	1
+#define	NONE			0
+
+
+/* 3 Tx Power Tracking */
+/* 3 ============================================================ */
+#define		DPK_DELTA_MAPPING_NUM	13
+#define		index_mapping_HP_NUM	15
+#define	OFDM_TABLE_SIZE		43
+#define	CCK_TABLE_SIZE			33
+#define TXSCALE_TABLE_SIZE		37
+#define TXPWR_TRACK_TABLE_SIZE	30
+#define DELTA_SWINGIDX_SIZE     30
+#define BAND_NUM				4
+
+/* 3 PSD Handler */
+/* 3 ============================================================ */
+
+#define	AFH_PSD		1	/* 0:normal PSD scan, 1: only do 20 pts PSD */
+#define	MODE_40M		0	/* 0:20M, 1:40M */
+#define	PSD_TH2		3
+#define	PSD_CHMIN		20   /*  Minimum channel number for BT AFH */
+#define	SIR_STEP_SIZE	3
+#define   Smooth_Size_1		5
+#define	Smooth_TH_1	3
+#define   Smooth_Size_2		10
+#define	Smooth_TH_2	4
+#define   Smooth_Size_3		20
+#define	Smooth_TH_3	4
+#define   Smooth_Step_Size 5
+#define	Adaptive_SIR	1
+#define	PSD_RESCAN		4
+#define	PSD_SCAN_INTERVAL	700 /* ms */
+
+/* 8723A High Power IGI Setting */
+#define		DM_DIG_HIGH_PWR_IGI_LOWER_BOUND	0x22
+#define			DM_DIG_Gmode_HIGH_PWR_IGI_LOWER_BOUND 0x28
+#define		DM_DIG_HIGH_PWR_THRESHOLD	0x3a
+#define		DM_DIG_LOW_PWR_THRESHOLD	0x14
+
+/* ANT Test */
+#define			ANTTESTALL		0x00		/* Ant A or B will be Testing */
+#define		ANTTESTA		0x01		/* Ant A will be Testing */
+#define		ANTTESTB		0x02		/* Ant B will be testing */
+
+#define	PS_MODE_ACTIVE 0x01
+
+/* for 8723A Ant Definition--2012--06--07 due to different IC may be different ANT define */
+#define		MAIN_ANT		1		/* Ant A or Ant Main */
+#define		AUX_ANT		2		/* AntB or Ant Aux */
+#define		MAX_ANT		3		/*  3 for AP using */
+
+
+/* Antenna Diversity Type */
+#define	SW_ANTDIV	0
+#define	HW_ANTDIV	1
+/*  structure and define */
+
+/* Remove DIG by Yuchen */
+
+/* Remoce BB power saving by Yuchn */
+
+/* Remove DIG by yuchen */
+
+typedef struct _Dynamic_Primary_CCA {
+	u8 PriCCA_flag;
+	u8 intf_flag;
+	u8 intf_type;
+	u8 DupRTS_flag;
+	u8 Monitor_flag;
+	u8 CH_offset;
+	u8 	MF_state;
+} Pri_CCA_T, *pPri_CCA_T;
+
+typedef struct _Rate_Adaptive_Table_ {
+	u8 firstconnect;
+} RA_T, *pRA_T;
+
+typedef struct _RX_High_Power_ {
+	u8 RXHP_flag;
+	u8 PSD_func_trigger;
+	u8 PSD_bitmap_RXHP[80];
+	u8 Pre_IGI;
+	u8 Cur_IGI;
+	u8 Pre_pw_th;
+	u8 Cur_pw_th;
+	bool First_time_enter;
+	bool RXHP_enable;
+	u8 TP_Mode;
+	RT_TIMER PSDTimer;
+} RXHP_T, *pRXHP_T;
+
+#define ASSOCIATE_ENTRY_NUM					32 /*  Max size of AsocEntry[]. */
+#define	ODM_ASSOCIATE_ENTRY_NUM				ASSOCIATE_ENTRY_NUM
+
+/*  This indicates two different the steps. */
+/*  In SWAW_STEP_PEAK, driver needs to switch antenna and listen to the signal on the air. */
+/*  In SWAW_STEP_DETERMINE, driver just compares the signal captured in SWAW_STEP_PEAK */
+/*  with original RSSI to determine if it is necessary to switch antenna. */
+#define SWAW_STEP_PEAK		0
+#define SWAW_STEP_DETERMINE	1
+
+#define	TP_MODE		0
+#define	RSSI_MODE		1
+#define	TRAFFIC_LOW	0
+#define	TRAFFIC_HIGH	1
+#define	TRAFFIC_UltraLOW	2
+
+typedef struct _SW_Antenna_Switch_ {
+	u8 Double_chk_flag;
+	u8 try_flag;
+	s32 PreRSSI;
+	u8 CurAntenna;
+	u8 PreAntenna;
+	u8 RSSI_Trying;
+	u8 TestMode;
+	u8 bTriggerAntennaSwitch;
+	u8 SelectAntennaMap;
+	u8 RSSI_target;
+	u8 reset_idx;
+	u16 Single_Ant_Counter;
+	u16 Dual_Ant_Counter;
+	u16 Aux_FailDetec_Counter;
+	u16 Retry_Counter;
+
+	/*  Before link Antenna Switch check */
+	u8 SWAS_NoLink_State;
+	u32 SWAS_NoLink_BK_Reg860;
+	u32 SWAS_NoLink_BK_Reg92c;
+	u32 SWAS_NoLink_BK_Reg948;
+	bool ANTA_ON;	/* To indicate Ant A is or not */
+	bool ANTB_ON;	/* To indicate Ant B is on or not */
+	bool Pre_Aux_FailDetec;
+	bool RSSI_AntDect_bResult;
+	u8 Ant5G;
+	u8 Ant2G;
+
+	s32 RSSI_sum_A;
+	s32 RSSI_sum_B;
+	s32 RSSI_cnt_A;
+	s32 RSSI_cnt_B;
+
+	u64 lastTxOkCnt;
+	u64 lastRxOkCnt;
+	u64 TXByteCnt_A;
+	u64 TXByteCnt_B;
+	u64 RXByteCnt_A;
+	u64 RXByteCnt_B;
+	u8 TrafficLoad;
+	u8 Train_time;
+	u8 Train_time_flag;
+	RT_TIMER SwAntennaSwitchTimer;
+	RT_TIMER SwAntennaSwitchTimer_8723B;
+	u32 PktCnt_SWAntDivByCtrlFrame;
+	bool bSWAntDivByCtrlFrame;
+} SWAT_T, *pSWAT_T;
+
+/* Remove Edca by YuChen */
+
+
+typedef struct _ODM_RATE_ADAPTIVE {
+	u8 Type;				/*  DM_Type_ByFW/DM_Type_ByDriver */
+	u8 LdpcThres;			/*  if RSSI > LdpcThres => switch from LPDC to BCC */
+	bool bUseLdpc;
+	bool bLowerRtsRate;
+	u8 HighRSSIThresh;		/*  if RSSI > HighRSSIThresh	=> RATRState is DM_RATR_STA_HIGH */
+	u8 LowRSSIThresh;		/*  if RSSI <= LowRSSIThresh	=> RATRState is DM_RATR_STA_LOW */
+	u8 RATRState;			/*  Current RSSI level, DM_RATR_STA_HIGH/DM_RATR_STA_MIDDLE/DM_RATR_STA_LOW */
+
+} ODM_RATE_ADAPTIVE, *PODM_RATE_ADAPTIVE;
+
+
+#define IQK_MAC_REG_NUM		4
+#define IQK_ADDA_REG_NUM		16
+#define IQK_BB_REG_NUM_MAX	10
+#define IQK_BB_REG_NUM		9
+#define HP_THERMAL_NUM		8
+
+#define AVG_THERMAL_NUM		8
+#define IQK_Matrix_REG_NUM	8
+#define IQK_Matrix_Settings_NUM	14+24+21 /*  Channels_2_4G_NUM + Channels_5G_20M_NUM + Channels_5G */
+
+#define		DM_Type_ByFW			0
+#define		DM_Type_ByDriver		1
+
+/*  */
+/*  Declare for common info */
+/*  */
+#define MAX_PATH_NUM_92CS		2
+#define MAX_PATH_NUM_8188E		1
+#define MAX_PATH_NUM_8192E		2
+#define MAX_PATH_NUM_8723B		1
+#define MAX_PATH_NUM_8812A		2
+#define MAX_PATH_NUM_8821A		1
+#define MAX_PATH_NUM_8814A		4
+#define MAX_PATH_NUM_8822B		2
+
+
+#define IQK_THRESHOLD			8
+#define DPK_THRESHOLD			4
+
+typedef struct _ODM_Phy_Status_Info_ {
+	/*  */
+	/*  Be care, if you want to add any element please insert between */
+	/*  RxPWDBAll & SignalStrength. */
+	/*  */
+	u8 RxPWDBAll;
+
+	u8 SignalQuality;			/*  in 0-100 index. */
+	s8 RxMIMOSignalQuality[4];	/* per-path's EVM */
+	u8 RxMIMOEVMdbm[4];		/* per-path's EVM dbm */
+
+	u8 RxMIMOSignalStrength[4];/*  in 0~100 index */
+
+	u16 Cfo_short[4];			/*  per-path's Cfo_short */
+	u16 Cfo_tail[4];			/*  per-path's Cfo_tail */
+
+	s8 RxPower;				/*  in dBm Translate from PWdB */
+	s8 RecvSignalPower;		/*  Real power in dBm for this packet, no beautification and aggregation. Keep this raw info to be used for the other procedures. */
+	u8 BTRxRSSIPercentage;
+	u8 SignalStrength;			/*  in 0-100 index. */
+
+	s8 RxPwr[4];				/* per-path's pwdb */
+
+	u8 RxSNR[4];				/* per-path's SNR */
+	u8 BandWidth;
+	u8 btCoexPwrAdjust;
+} ODM_PHY_INFO_T, *PODM_PHY_INFO_T;
+
+
+typedef struct _ODM_Per_Pkt_Info_ {
+	/* u8 Rate; */
+	u8 DataRate;
+	u8 StationID;
+	bool bPacketMatchBSSID;
+	bool bPacketToSelf;
+	bool bPacketBeacon;
+	bool bToSelf;
+} ODM_PACKET_INFO_T, *PODM_PACKET_INFO_T;
+
+
+typedef struct _ODM_Phy_Dbg_Info_ {
+	/* ODM Write, debug info */
+	s8 RxSNRdB[4];
+	u32 NumQryPhyStatus;
+	u32 NumQryPhyStatusCCK;
+	u32 NumQryPhyStatusOFDM;
+	u8 NumQryBeaconPkt;
+	/* Others */
+	s32 RxEVM[4];
+
+} ODM_PHY_DBG_INFO_T;
+
+
+typedef struct _ODM_Mac_Status_Info_ {
+	u8 test;
+} ODM_MAC_INFO;
+
+
+typedef enum tag_Dynamic_ODM_Support_Ability_Type {
+	/*  BB Team */
+	ODM_DIG				= 0x00000001,
+	ODM_HIGH_POWER		= 0x00000002,
+	ODM_CCK_CCA_TH		= 0x00000004,
+	ODM_FA_STATISTICS	= 0x00000008,
+	ODM_RAMASK			= 0x00000010,
+	ODM_RSSI_MONITOR	= 0x00000020,
+	ODM_SW_ANTDIV		= 0x00000040,
+	ODM_HW_ANTDIV		= 0x00000080,
+	ODM_BB_PWRSV		= 0x00000100,
+	ODM_2TPATHDIV		= 0x00000200,
+	ODM_1TPATHDIV		= 0x00000400,
+	ODM_PSD2AFH			= 0x00000800
+} ODM_Ability_E;
+
+/*  */
+/*  2011/20/20 MH For MP driver RT_WLAN_STA =  STA_INFO_T */
+/*  Please declare below ODM relative info in your STA info structure. */
+/*  */
+typedef struct _ODM_STA_INFO {
+	/*  Driver Write */
+	bool bUsed;				/*  record the sta status link or not? */
+	/* u8 WirelessMode;		 */
+	u8 IOTPeer;			/*  Enum value.	HT_IOT_PEER_E */
+
+	/*  ODM Write */
+	/* 1 PHY_STATUS_INFO */
+	u8 RSSI_Path[4];		/*  */
+	u8 RSSI_Ave;
+	u8 RXEVM[4];
+	u8 RXSNR[4];
+
+	/*  ODM Write */
+	/* 1 TX_INFO (may changed by IC) */
+	/* TX_INFO_T		pTxInfo;		Define in IC folder. Move lower layer. */
+
+	/*  */
+	/* 	Please use compile flag to disabe the strcutrue for other IC except 88E. */
+	/* 	Move To lower layer. */
+	/*  */
+	/*  ODM Write Wilson will handle this part(said by Luke.Lee) */
+	/* TX_RPT_T		pTxRpt;			Define in IC folder. Move lower layer. */
+} ODM_STA_INFO_T, *PODM_STA_INFO_T;
+
+/*  */
+/*  2011/10/20 MH Define Common info enum for all team. */
+/*  */
+typedef enum _ODM_Common_Info_Definition {
+	/*  Fixed value: */
+
+	/* HOOK BEFORE REG INIT----------- */
+	ODM_CMNINFO_PLATFORM = 0,
+	ODM_CMNINFO_ABILITY,					/*  ODM_ABILITY_E */
+	ODM_CMNINFO_INTERFACE,				/*  ODM_INTERFACE_E */
+	ODM_CMNINFO_MP_TEST_CHIP,
+	ODM_CMNINFO_IC_TYPE,					/*  ODM_IC_TYPE_E */
+	ODM_CMNINFO_CUT_VER,					/*  ODM_CUT_VERSION_E */
+	ODM_CMNINFO_FAB_VER,					/*  ODM_FAB_E */
+	ODM_CMNINFO_RF_TYPE,					/*  ODM_RF_PATH_E or ODM_RF_TYPE_E? */
+	ODM_CMNINFO_RFE_TYPE,
+	ODM_CMNINFO_BOARD_TYPE,				/*  ODM_BOARD_TYPE_E */
+	ODM_CMNINFO_PACKAGE_TYPE,
+	ODM_CMNINFO_EXT_LNA,					/*  true */
+	ODM_CMNINFO_5G_EXT_LNA,
+	ODM_CMNINFO_EXT_PA,
+	ODM_CMNINFO_5G_EXT_PA,
+	ODM_CMNINFO_GPA,
+	ODM_CMNINFO_APA,
+	ODM_CMNINFO_GLNA,
+	ODM_CMNINFO_ALNA,
+	ODM_CMNINFO_EXT_TRSW,
+	ODM_CMNINFO_PATCH_ID,				/* CUSTOMER ID */
+	ODM_CMNINFO_BINHCT_TEST,
+	ODM_CMNINFO_BWIFI_TEST,
+	ODM_CMNINFO_SMART_CONCURRENT,
+	/* HOOK BEFORE REG INIT----------- */
+
+
+	/*  Dynamic value: */
+/*  POINTER REFERENCE----------- */
+	ODM_CMNINFO_MAC_PHY_MODE,	/*  ODM_MAC_PHY_MODE_E */
+	ODM_CMNINFO_TX_UNI,
+	ODM_CMNINFO_RX_UNI,
+	ODM_CMNINFO_WM_MODE,		/*  ODM_WIRELESS_MODE_E */
+	ODM_CMNINFO_BAND,		/*  ODM_BAND_TYPE_E */
+	ODM_CMNINFO_SEC_CHNL_OFFSET,	/*  ODM_SEC_CHNL_OFFSET_E */
+	ODM_CMNINFO_SEC_MODE,		/*  ODM_SECURITY_E */
+	ODM_CMNINFO_BW,			/*  ODM_BW_E */
+	ODM_CMNINFO_CHNL,
+	ODM_CMNINFO_FORCED_RATE,
+
+	ODM_CMNINFO_DMSP_GET_VALUE,
+	ODM_CMNINFO_BUDDY_ADAPTOR,
+	ODM_CMNINFO_DMSP_IS_MASTER,
+	ODM_CMNINFO_SCAN,
+	ODM_CMNINFO_POWER_SAVING,
+	ODM_CMNINFO_ONE_PATH_CCA,	/*  ODM_CCA_PATH_E */
+	ODM_CMNINFO_DRV_STOP,
+	ODM_CMNINFO_PNP_IN,
+	ODM_CMNINFO_INIT_ON,
+	ODM_CMNINFO_ANT_TEST,
+	ODM_CMNINFO_NET_CLOSED,
+	ODM_CMNINFO_MP_MODE,
+	/* ODM_CMNINFO_RTSTA_AID,	 For win driver only? */
+	ODM_CMNINFO_FORCED_IGI_LB,
+	ODM_CMNINFO_IS1ANTENNA,
+	ODM_CMNINFO_RFDEFAULTPATH,
+/*  POINTER REFERENCE----------- */
+
+/* CALL BY VALUE------------- */
+	ODM_CMNINFO_WIFI_DIRECT,
+	ODM_CMNINFO_WIFI_DISPLAY,
+	ODM_CMNINFO_LINK_IN_PROGRESS,
+	ODM_CMNINFO_LINK,
+	ODM_CMNINFO_STATION_STATE,
+	ODM_CMNINFO_RSSI_MIN,
+	ODM_CMNINFO_DBG_COMP,			/*  u64 */
+	ODM_CMNINFO_DBG_LEVEL,			/*  u32 */
+	ODM_CMNINFO_RA_THRESHOLD_HIGH,		/*  u8 */
+	ODM_CMNINFO_RA_THRESHOLD_LOW,		/*  u8 */
+	ODM_CMNINFO_RF_ANTENNA_TYPE,		/*  u8 */
+	ODM_CMNINFO_BT_ENABLED,
+	ODM_CMNINFO_BT_HS_CONNECT_PROCESS,
+	ODM_CMNINFO_BT_HS_RSSI,
+	ODM_CMNINFO_BT_OPERATION,
+	ODM_CMNINFO_BT_LIMITED_DIG,		/* Need to Limited Dig or not */
+	ODM_CMNINFO_BT_DISABLE_EDCA,
+/* CALL BY VALUE------------- */
+
+	/*  Dynamic ptr array hook itms. */
+	ODM_CMNINFO_STA_STATUS,
+	ODM_CMNINFO_PHY_STATUS,
+	ODM_CMNINFO_MAC_STATUS,
+
+	ODM_CMNINFO_MAX,
+
+
+} ODM_CMNINFO_E;
+
+/*  2011/10/20 MH Define ODM support ability.  ODM_CMNINFO_ABILITY */
+typedef enum _ODM_Support_Ability_Definition {
+	/*  */
+	/*  BB ODM section BIT 0-15 */
+	/*  */
+	ODM_BB_DIG			= BIT0,
+	ODM_BB_RA_MASK			= BIT1,
+	ODM_BB_DYNAMIC_TXPWR		= BIT2,
+	ODM_BB_FA_CNT			= BIT3,
+	ODM_BB_RSSI_MONITOR		= BIT4,
+	ODM_BB_CCK_PD			= BIT5,
+	ODM_BB_ANT_DIV			= BIT6,
+	ODM_BB_PWR_SAVE			= BIT7,
+	ODM_BB_PWR_TRAIN		= BIT8,
+	ODM_BB_RATE_ADAPTIVE		= BIT9,
+	ODM_BB_PATH_DIV			= BIT10,
+	ODM_BB_PSD			= BIT11,
+	ODM_BB_RXHP			= BIT12,
+	ODM_BB_ADAPTIVITY		= BIT13,
+	ODM_BB_CFO_TRACKING		= BIT14,
+
+	/*  MAC DM section BIT 16-23 */
+	ODM_MAC_EDCA_TURBO		= BIT16,
+	ODM_MAC_EARLY_MODE		= BIT17,
+
+	/*  RF ODM section BIT 24-31 */
+	ODM_RF_TX_PWR_TRACK		= BIT24,
+	ODM_RF_RX_GAIN_TRACK	= BIT25,
+	ODM_RF_CALIBRATION		= BIT26,
+} ODM_ABILITY_E;
+
+/* 	ODM_CMNINFO_INTERFACE */
+typedef enum tag_ODM_Support_Interface_Definition {
+	ODM_ITRF_SDIO	=	0x4,
+	ODM_ITRF_ALL	=	0x7,
+} ODM_INTERFACE_E;
+
+/*  ODM_CMNINFO_IC_TYPE */
+typedef enum tag_ODM_Support_IC_Type_Definition {
+	ODM_RTL8723B	=	BIT8,
+} ODM_IC_TYPE_E;
+
+/* ODM_CMNINFO_CUT_VER */
+typedef enum tag_ODM_Cut_Version_Definition {
+	ODM_CUT_A		=	0,
+	ODM_CUT_B		=	1,
+	ODM_CUT_C		=	2,
+	ODM_CUT_D		=	3,
+	ODM_CUT_E		=	4,
+	ODM_CUT_F		=	5,
+
+	ODM_CUT_I		=	8,
+	ODM_CUT_J		=	9,
+	ODM_CUT_K		=	10,
+	ODM_CUT_TEST	=	15,
+} ODM_CUT_VERSION_E;
+
+/*  ODM_CMNINFO_FAB_VER */
+typedef enum tag_ODM_Fab_Version_Definition {
+	ODM_TSMC	=	0,
+	ODM_UMC		=	1,
+} ODM_FAB_E;
+
+/*  ODM_CMNINFO_RF_TYPE */
+/*  */
+/*  For example 1T2R (A+AB = BIT0|BIT4|BIT5) */
+/*  */
+typedef enum tag_ODM_RF_Path_Bit_Definition {
+	ODM_RF_TX_A	=	BIT0,
+	ODM_RF_TX_B	=	BIT1,
+	ODM_RF_TX_C	=	BIT2,
+	ODM_RF_TX_D	=	BIT3,
+	ODM_RF_RX_A	=	BIT4,
+	ODM_RF_RX_B	=	BIT5,
+	ODM_RF_RX_C	=	BIT6,
+	ODM_RF_RX_D	=	BIT7,
+} ODM_RF_PATH_E;
+
+
+typedef enum tag_ODM_RF_Type_Definition {
+	ODM_1T1R	=	0,
+	ODM_1T2R	=	1,
+	ODM_2T2R	=	2,
+	ODM_2T3R	=	3,
+	ODM_2T4R	=	4,
+	ODM_3T3R	=	5,
+	ODM_3T4R	=	6,
+	ODM_4T4R	=	7,
+} ODM_RF_TYPE_E;
+
+
+/*  */
+/*  ODM Dynamic common info value definition */
+/*  */
+
+/* typedef enum _MACPHY_MODE_8192D{ */
+/* 	SINGLEMAC_SINGLEPHY, */
+/* 	DUALMAC_DUALPHY, */
+/* 	DUALMAC_SINGLEPHY, */
+/* MACPHY_MODE_8192D,*PMACPHY_MODE_8192D; */
+/*  Above is the original define in MP driver. Please use the same define. THX. */
+typedef enum tag_ODM_MAC_PHY_Mode_Definition {
+	ODM_SMSP	= 0,
+	ODM_DMSP	= 1,
+	ODM_DMDP	= 2,
+} ODM_MAC_PHY_MODE_E;
+
+
+typedef enum tag_BT_Coexist_Definition {
+	ODM_BT_BUSY		= 1,
+	ODM_BT_ON		= 2,
+	ODM_BT_OFF		= 3,
+	ODM_BT_NONE		= 4,
+} ODM_BT_COEXIST_E;
+
+/*  ODM_CMNINFO_OP_MODE */
+typedef enum tag_Operation_Mode_Definition {
+	ODM_NO_LINK      = BIT0,
+	ODM_LINK         = BIT1,
+	ODM_SCAN         = BIT2,
+	ODM_POWERSAVE    = BIT3,
+	ODM_AP_MODE      = BIT4,
+	ODM_CLIENT_MODE  = BIT5,
+	ODM_AD_HOC       = BIT6,
+	ODM_WIFI_DIRECT  = BIT7,
+	ODM_WIFI_DISPLAY = BIT8,
+} ODM_OPERATION_MODE_E;
+
+/*  ODM_CMNINFO_WM_MODE */
+typedef enum tag_Wireless_Mode_Definition {
+	ODM_WM_UNKNOW     = 0x0,
+	ODM_WM_B          = BIT0,
+	ODM_WM_G          = BIT1,
+	ODM_WM_A          = BIT2,
+	ODM_WM_N24G       = BIT3,
+	ODM_WM_N5G        = BIT4,
+	ODM_WM_AUTO       = BIT5,
+	ODM_WM_AC         = BIT6,
+} ODM_WIRELESS_MODE_E;
+
+/*  ODM_CMNINFO_BAND */
+typedef enum tag_Band_Type_Definition {
+	ODM_BAND_2_4G = 0,
+	ODM_BAND_5G,
+	ODM_BAND_ON_BOTH,
+	ODM_BANDMAX
+} ODM_BAND_TYPE_E;
+
+/*  ODM_CMNINFO_SEC_CHNL_OFFSET */
+typedef enum tag_Secondary_Channel_Offset_Definition {
+	ODM_DONT_CARE	= 0,
+	ODM_BELOW		= 1,
+	ODM_ABOVE		= 2
+} ODM_SEC_CHNL_OFFSET_E;
+
+/*  ODM_CMNINFO_SEC_MODE */
+typedef enum tag_Security_Definition {
+	ODM_SEC_OPEN		= 0,
+	ODM_SEC_WEP40		= 1,
+	ODM_SEC_TKIP		= 2,
+	ODM_SEC_RESERVE		= 3,
+	ODM_SEC_AESCCMP		= 4,
+	ODM_SEC_WEP104		= 5,
+	ODM_WEP_WPA_MIXED	= 6, /*  WEP + WPA */
+	ODM_SEC_SMS4		= 7,
+} ODM_SECURITY_E;
+
+/*  ODM_CMNINFO_BW */
+typedef enum tag_Bandwidth_Definition {
+	ODM_BW20M		= 0,
+	ODM_BW40M		= 1,
+	ODM_BW80M		= 2,
+	ODM_BW160M		= 3,
+	ODM_BW10M		= 4,
+} ODM_BW_E;
+
+
+/*  ODM_CMNINFO_BOARD_TYPE */
+/*  For non-AC-series IC , ODM_BOARD_5G_EXT_PA and ODM_BOARD_5G_EXT_LNA are ignored */
+/*  For AC-series IC, external PA & LNA can be indivisuallly added on 2.4G and/or 5G */
+typedef enum tag_Board_Definition {
+	ODM_BOARD_DEFAULT    = 0,      /*  The DEFAULT case. */
+	ODM_BOARD_MINICARD   = BIT(0), /*  0 = non-mini card, 1 = mini card. */
+	ODM_BOARD_SLIM       = BIT(1), /*  0 = non-slim card, 1 = slim card */
+	ODM_BOARD_BT         = BIT(2), /*  0 = without BT card, 1 = with BT */
+	ODM_BOARD_EXT_PA     = BIT(3), /*  0 = no 2G ext-PA, 1 = existing 2G ext-PA */
+	ODM_BOARD_EXT_LNA    = BIT(4), /*  0 = no 2G ext-LNA, 1 = existing 2G ext-LNA */
+	ODM_BOARD_EXT_TRSW   = BIT(5), /*  0 = no ext-TRSW, 1 = existing ext-TRSW */
+	ODM_BOARD_EXT_PA_5G  = BIT(6), /*  0 = no 5G ext-PA, 1 = existing 5G ext-PA */
+	ODM_BOARD_EXT_LNA_5G = BIT(7), /*  0 = no 5G ext-LNA, 1 = existing 5G ext-LNA */
+} ODM_BOARD_TYPE_E;
+
+typedef enum tag_ODM_Package_Definition {
+	ODM_PACKAGE_DEFAULT      = 0,
+	ODM_PACKAGE_QFN68        = BIT(0),
+	ODM_PACKAGE_TFBGA90      = BIT(1),
+	ODM_PACKAGE_TFBGA79      = BIT(2),
+} ODM_Package_TYPE_E;
+
+typedef enum tag_ODM_TYPE_GPA_Definition {
+	TYPE_GPA0 = 0,
+	TYPE_GPA1 = BIT(1)|BIT(0)
+} ODM_TYPE_GPA_E;
+
+typedef enum tag_ODM_TYPE_APA_Definition {
+	TYPE_APA0 = 0,
+	TYPE_APA1 = BIT(1)|BIT(0)
+} ODM_TYPE_APA_E;
+
+typedef enum tag_ODM_TYPE_GLNA_Definition {
+	TYPE_GLNA0 = 0,
+	TYPE_GLNA1 = BIT(2)|BIT(0),
+	TYPE_GLNA2 = BIT(3)|BIT(1),
+	TYPE_GLNA3 = BIT(3)|BIT(2)|BIT(1)|BIT(0)
+} ODM_TYPE_GLNA_E;
+
+typedef enum tag_ODM_TYPE_ALNA_Definition {
+	TYPE_ALNA0 = 0,
+	TYPE_ALNA1 = BIT(2)|BIT(0),
+	TYPE_ALNA2 = BIT(3)|BIT(1),
+	TYPE_ALNA3 = BIT(3)|BIT(2)|BIT(1)|BIT(0)
+} ODM_TYPE_ALNA_E;
+
+/*  ODM_CMNINFO_ONE_PATH_CCA */
+typedef enum tag_CCA_Path {
+	ODM_CCA_2R			= 0,
+	ODM_CCA_1R_A		= 1,
+	ODM_CCA_1R_B		= 2,
+} ODM_CCA_PATH_E;
+
+
+typedef struct _ODM_RA_Info_ {
+	u8 RateID;
+	u32 RateMask;
+	u32 RAUseRate;
+	u8 RateSGI;
+	u8 RssiStaRA;
+	u8 PreRssiStaRA;
+	u8 SGIEnable;
+	u8 DecisionRate;
+	u8 PreRate;
+	u8 HighestRate;
+	u8 LowestRate;
+	u32 NscUp;
+	u32 NscDown;
+	u16 RTY[5];
+	u32 TOTAL;
+	u16 DROP;
+	u8 Active;
+	u16 RptTime;
+	u8 RAWaitingCounter;
+	u8 RAPendingCounter;
+	u8 PTActive;  /*  on or off */
+	u8 PTTryState;  /*  0 trying state, 1 for decision state */
+	u8 PTStage;  /*  0~6 */
+	u8 PTStopCount; /* Stop PT counter */
+	u8 PTPreRate;  /*  if rate change do PT */
+	u8 PTPreRssi; /*  if RSSI change 5% do PT */
+	u8 PTModeSS;  /*  decide whitch rate should do PT */
+	u8 RAstage;  /*  StageRA, decide how many times RA will be done between PT */
+	u8 PTSmoothFactor;
+} ODM_RA_INFO_T, *PODM_RA_INFO_T;
+
+typedef struct _IQK_MATRIX_REGS_SETTING {
+	bool bIQKDone;
+	s32 Value[3][IQK_Matrix_REG_NUM];
+	bool bBWIqkResultSaved[3];
+} IQK_MATRIX_REGS_SETTING, *PIQK_MATRIX_REGS_SETTING;
+
+
+/* Remove PATHDIV_PARA struct to odm_PathDiv.h */
+
+typedef struct ODM_RF_Calibration_Structure {
+	/* for tx power tracking */
+
+	u32 RegA24; /*  for TempCCK */
+	s32 RegE94;
+	s32 RegE9C;
+	s32 RegEB4;
+	s32 RegEBC;
+
+	u8 TXPowercount;
+	bool bTXPowerTrackingInit;
+	bool bTXPowerTracking;
+	u8 TxPowerTrackControl; /* for mp mode, turn off txpwrtracking as default */
+	u8 TM_Trigger;
+	u8 InternalPA5G[2];	/* pathA / pathB */
+
+	u8 ThermalMeter[2];    /*  ThermalMeter, index 0 for RFIC0, and 1 for RFIC1 */
+	u8 ThermalValue;
+	u8 ThermalValue_LCK;
+	u8 ThermalValue_IQK;
+	u8 ThermalValue_DPK;
+	u8 ThermalValue_AVG[AVG_THERMAL_NUM];
+	u8 ThermalValue_AVG_index;
+	u8 ThermalValue_RxGain;
+	u8 ThermalValue_Crystal;
+	u8 ThermalValue_DPKstore;
+	u8 ThermalValue_DPKtrack;
+	bool TxPowerTrackingInProgress;
+
+	bool bReloadtxpowerindex;
+	u8 bRfPiEnable;
+	u32 TXPowerTrackingCallbackCnt; /* cosa add for debug */
+
+
+	/*  Tx power Tracking ------------------------- */
+	u8 bCCKinCH14;
+	u8 CCK_index;
+	u8 OFDM_index[MAX_RF_PATH];
+	s8 PowerIndexOffset[MAX_RF_PATH];
+	s8 DeltaPowerIndex[MAX_RF_PATH];
+	s8 DeltaPowerIndexLast[MAX_RF_PATH];
+	bool bTxPowerChanged;
+
+	u8 ThermalValue_HP[HP_THERMAL_NUM];
+	u8 ThermalValue_HP_index;
+	IQK_MATRIX_REGS_SETTING IQKMatrixRegSetting[IQK_Matrix_Settings_NUM];
+	bool bNeedIQK;
+	bool bIQKInProgress;
+	u8 Delta_IQK;
+	u8 Delta_LCK;
+	s8 BBSwingDiff2G, BBSwingDiff5G; /*  Unit: dB */
+	u8 DeltaSwingTableIdx_2GCCKA_P[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GCCKA_N[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GCCKB_P[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GCCKB_N[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GA_P[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GA_N[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GB_P[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GB_N[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_5GA_P[BAND_NUM][DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_5GA_N[BAND_NUM][DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_5GB_P[BAND_NUM][DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_5GB_N[BAND_NUM][DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GA_P_8188E[DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_2GA_N_8188E[DELTA_SWINGIDX_SIZE];
+
+	/*  */
+
+	/* for IQK */
+	u32 RegC04;
+	u32 Reg874;
+	u32 RegC08;
+	u32 RegB68;
+	u32 RegB6C;
+	u32 Reg870;
+	u32 Reg860;
+	u32 Reg864;
+
+	bool bIQKInitialized;
+	bool bLCKInProgress;
+	bool bAntennaDetected;
+	u32 ADDA_backup[IQK_ADDA_REG_NUM];
+	u32 IQK_MAC_backup[IQK_MAC_REG_NUM];
+	u32 IQK_BB_backup_recover[9];
+	u32 IQK_BB_backup[IQK_BB_REG_NUM];
+	u32 TxIQC_8723B[2][3][2]; /*  { {S1: 0xc94, 0xc80, 0xc4c} , {S0: 0xc9c, 0xc88, 0xc4c}} */
+	u32 RxIQC_8723B[2][2][2]; /*  { {S1: 0xc14, 0xca0} ,           {S0: 0xc14, 0xca0}} */
+
+
+	/* for APK */
+	u32 APKoutput[2][2]; /* path A/B; output1_1a/output1_2a */
+	u8 bAPKdone;
+	u8 bAPKThermalMeterIgnore;
+
+	/*  DPK */
+	bool bDPKFail;
+	u8 bDPdone;
+	u8 bDPPathAOK;
+	u8 bDPPathBOK;
+
+	u32 TxLOK[2];
+
+} ODM_RF_CAL_T, *PODM_RF_CAL_T;
+/*  */
+/*  ODM Dynamic common info value definition */
+/*  */
+
+typedef struct _FAST_ANTENNA_TRAINNING_ {
+	u8 Bssid[6];
+	u8 antsel_rx_keep_0;
+	u8 antsel_rx_keep_1;
+	u8 antsel_rx_keep_2;
+	u8 antsel_rx_keep_3;
+	u32 antSumRSSI[7];
+	u32 antRSSIcnt[7];
+	u32 antAveRSSI[7];
+	u8 FAT_State;
+	u32 TrainIdx;
+	u8 antsel_a[ODM_ASSOCIATE_ENTRY_NUM];
+	u8 antsel_b[ODM_ASSOCIATE_ENTRY_NUM];
+	u8 antsel_c[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 MainAnt_Sum[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 AuxAnt_Sum[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 MainAnt_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 AuxAnt_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
+	u8 RxIdleAnt;
+	bool	bBecomeLinked;
+	u32 MinMaxRSSI;
+	u8 idx_AntDiv_counter_2G;
+	u8 idx_AntDiv_counter_5G;
+	u32 AntDiv_2G_5G;
+	u32 CCK_counter_main;
+	u32 CCK_counter_aux;
+	u32 OFDM_counter_main;
+	u32 OFDM_counter_aux;
+
+
+	u32 CCK_CtrlFrame_Cnt_main;
+	u32 CCK_CtrlFrame_Cnt_aux;
+	u32 OFDM_CtrlFrame_Cnt_main;
+	u32 OFDM_CtrlFrame_Cnt_aux;
+	u32 MainAnt_CtrlFrame_Sum;
+	u32 AuxAnt_CtrlFrame_Sum;
+	u32 MainAnt_CtrlFrame_Cnt;
+	u32 AuxAnt_CtrlFrame_Cnt;
+
+} FAT_T, *pFAT_T;
+
+typedef enum _FAT_STATE {
+	FAT_NORMAL_STATE			= 0,
+	FAT_TRAINING_STATE		= 1,
+} FAT_STATE_E, *PFAT_STATE_E;
+
+typedef enum _ANT_DIV_TYPE {
+	NO_ANTDIV			= 0xFF,
+	CG_TRX_HW_ANTDIV		= 0x01,
+	CGCS_RX_HW_ANTDIV	= 0x02,
+	FIXED_HW_ANTDIV		= 0x03,
+	CG_TRX_SMART_ANTDIV	= 0x04,
+	CGCS_RX_SW_ANTDIV	= 0x05,
+	S0S1_SW_ANTDIV          = 0x06 /* 8723B intrnal switch S0 S1 */
+} ANT_DIV_TYPE_E, *PANT_DIV_TYPE_E;
+
+typedef struct _ODM_PATH_DIVERSITY_ {
+	u8 RespTxPath;
+	u8 PathSel[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 PathA_Sum[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 PathB_Sum[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 PathA_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
+	u32 PathB_Cnt[ODM_ASSOCIATE_ENTRY_NUM];
+} PATHDIV_T, *pPATHDIV_T;
+
+
+typedef enum _BASEBAND_CONFIG_PHY_REG_PG_VALUE_TYPE{
+	PHY_REG_PG_RELATIVE_VALUE = 0,
+	PHY_REG_PG_EXACT_VALUE = 1
+} PHY_REG_PG_TYPE;
+
+
+/*  */
+/*  Antenna detection information from single tone mechanism, added by Roger, 2012.11.27. */
+/*  */
+typedef struct _ANT_DETECTED_INFO {
+	bool bAntDetected;
+	u32 dBForAntA;
+	u32 dBForAntB;
+	u32 dBForAntO;
+} ANT_DETECTED_INFO, *PANT_DETECTED_INFO;
+
+/*  */
+/*  2011/09/22 MH Copy from SD4 defined structure. We use to support PHY DM integration. */
+/*  */
+typedef  struct DM_Out_Source_Dynamic_Mechanism_Structure {
+	/* RT_TIMER	FastAntTrainingTimer; */
+	/*  */
+	/* 	Add for different team use temporarily */
+	/*  */
+	struct adapter *Adapter;		/*  For CE/NIC team */
+	/*  WHen you use Adapter or priv pointer, you must make sure the pointer is ready. */
+	bool odm_ready;
+
+	PHY_REG_PG_TYPE PhyRegPgValueType;
+	u8 PhyRegPgVersion;
+
+	u64	DebugComponents;
+	u32 DebugLevel;
+
+	u32 NumQryPhyStatusAll;	/* CCK + OFDM */
+	u32 LastNumQryPhyStatusAll;
+	u32 RxPWDBAve;
+	bool MPDIG_2G;		/* off MPDIG */
+	u8 Times_2G;
+
+/*  ODM HANDLE, DRIVER NEEDS NOT TO HOOK------ */
+	bool bCckHighPower;
+	u8 RFPathRxEnable;		/*  ODM_CMNINFO_RFPATH_ENABLE */
+	u8 ControlChannel;
+/*  ODM HANDLE, DRIVER NEEDS NOT TO HOOK------ */
+
+/* REMOVED COMMON INFO---------- */
+	/* u8 		PseudoMacPhyMode; */
+	/* bool			*BTCoexist; */
+	/* bool			PseudoBtCoexist; */
+	/* u8 		OPMode; */
+	/* bool			bAPMode; */
+	/* bool			bClientMode; */
+	/* bool			bAdHocMode; */
+	/* bool			bSlaveOfDMSP; */
+/* REMOVED COMMON INFO---------- */
+
+
+/* 1  COMMON INFORMATION */
+
+	/*  */
+	/*  Init Value */
+	/*  */
+/* HOOK BEFORE REG INIT----------- */
+	/*  ODM Platform info AP/ADSL/CE/MP = 1/2/3/4 */
+	u8 SupportPlatform;
+	/*  ODM Support Ability DIG/RATR/TX_PWR_TRACK/ ?K?K = 1/2/3/?K */
+	u32 SupportAbility;
+	/*  ODM PCIE/USB/SDIO = 1/2/3 */
+	u8 SupportInterface;
+	/*  ODM composite or independent. Bit oriented/ 92C+92D+ .... or any other type = 1/2/3/... */
+	u32 SupportICType;
+	/*  Cut Version TestChip/A-cut/B-cut... = 0/1/2/3/... */
+	u8 CutVersion;
+	/*  Fab Version TSMC/UMC = 0/1 */
+	u8 FabVersion;
+	/*  RF Type 4T4R/3T3R/2T2R/1T2R/1T1R/... */
+	u8 RFType;
+	u8 RFEType;
+	/*  Board Type Normal/HighPower/MiniCard/SLIM/Combo/... = 0/1/2/3/4/... */
+	u8 BoardType;
+	u8 PackageType;
+	u8 TypeGLNA;
+	u8 TypeGPA;
+	u8 TypeALNA;
+	u8 TypeAPA;
+	/*  with external LNA  NO/Yes = 0/1 */
+	u8 ExtLNA;
+	u8 ExtLNA5G;
+	/*  with external PA  NO/Yes = 0/1 */
+	u8 ExtPA;
+	u8 ExtPA5G;
+	/*  with external TRSW  NO/Yes = 0/1 */
+	u8 ExtTRSW;
+	u8 PatchID; /* Customer ID */
+	bool bInHctTest;
+	bool bWIFITest;
+
+	bool bDualMacSmartConcurrent;
+	u32 BK_SupportAbility;
+	u8 AntDivType;
+/* HOOK BEFORE REG INIT----------- */
+
+	/*  */
+	/*  Dynamic Value */
+	/*  */
+/*  POINTER REFERENCE----------- */
+
+	u8 u8_temp;
+	bool bool_temp;
+	struct adapter *adapter_temp;
+
+	/*  MAC PHY Mode SMSP/DMSP/DMDP = 0/1/2 */
+	u8 *pMacPhyMode;
+	/* TX Unicast byte count */
+	u64 *pNumTxBytesUnicast;
+	/* RX Unicast byte count */
+	u64 *pNumRxBytesUnicast;
+	/*  Wireless mode B/G/A/N = BIT0/BIT1/BIT2/BIT3 */
+	u8 *pwirelessmode; /* ODM_WIRELESS_MODE_E */
+	/*  Frequence band 2.4G/5G = 0/1 */
+	u8 *pBandType;
+	/*  Secondary channel offset don't_care/below/above = 0/1/2 */
+	u8 *pSecChOffset;
+	/*  Security mode Open/WEP/AES/TKIP = 0/1/2/3 */
+	u8 *pSecurity;
+	/*  BW info 20M/40M/80M = 0/1/2 */
+	u8 *pBandWidth;
+	/*  Central channel location Ch1/Ch2/.... */
+	u8 *pChannel; /* central channel number */
+	bool DPK_Done;
+	/*  Common info for 92D DMSP */
+
+	bool *pbGetValueFromOtherMac;
+	struct adapter **pBuddyAdapter;
+	bool *pbMasterOfDMSP; /* MAC0: master, MAC1: slave */
+	/*  Common info for Status */
+	bool *pbScanInProcess;
+	bool *pbPowerSaving;
+	/*  CCA Path 2-path/path-A/path-B = 0/1/2; using ODM_CCA_PATH_E. */
+	u8 *pOnePathCCA;
+	/* pMgntInfo->AntennaTest */
+	u8 *pAntennaTest;
+	bool *pbNet_closed;
+	u8 *mp_mode;
+	/* u8 	*pAidMap; */
+	u8 *pu1ForcedIgiLb;
+/*  For 8723B IQK----------- */
+	bool *pIs1Antenna;
+	u8 *pRFDefaultPath;
+	/*  0:S1, 1:S0 */
+
+/*  POINTER REFERENCE----------- */
+	u16 *pForcedDataRate;
+/* CALL BY VALUE------------- */
+	bool bLinkInProcess;
+	bool bWIFI_Direct;
+	bool bWIFI_Display;
+	bool bLinked;
+
+	bool bsta_state;
+	u8 RSSI_Min;
+	u8 InterfaceIndex; /*  Add for 92D  dual MAC: 0--Mac0 1--Mac1 */
+	bool bIsMPChip;
+	bool bOneEntryOnly;
+	/*  Common info for BTDM */
+	bool bBtEnabled;			/*  BT is disabled */
+	bool bBtConnectProcess;	/*  BT HS is under connection progress. */
+	u8 btHsRssi;				/*  BT HS mode wifi rssi value. */
+	bool bBtHsOperation;		/*  BT HS mode is under progress */
+	bool bBtDisableEdcaTurbo;	/*  Under some condition, don't enable the EDCA Turbo */
+	bool bBtLimitedDig;			/*  BT is busy. */
+/* CALL BY VALUE------------- */
+	u8 RSSI_A;
+	u8 RSSI_B;
+	u64 RSSI_TRSW;
+	u64 RSSI_TRSW_H;
+	u64 RSSI_TRSW_L;
+	u64 RSSI_TRSW_iso;
+
+	u8 RxRate;
+	bool bNoisyState;
+	u8 TxRate;
+	u8 LinkedInterval;
+	u8 preChannel;
+	u32 TxagcOffsetValueA;
+	bool IsTxagcOffsetPositiveA;
+	u32 TxagcOffsetValueB;
+	bool IsTxagcOffsetPositiveB;
+	u64	lastTxOkCnt;
+	u64	lastRxOkCnt;
+	u32 BbSwingOffsetA;
+	bool IsBbSwingOffsetPositiveA;
+	u32 BbSwingOffsetB;
+	bool IsBbSwingOffsetPositiveB;
+	s8 TH_L2H_ini;
+	s8 TH_EDCCA_HL_diff;
+	s8 IGI_Base;
+	u8 IGI_target;
+	bool ForceEDCCA;
+	u8 AdapEn_RSSI;
+	s8 Force_TH_H;
+	s8 Force_TH_L;
+	u8 IGI_LowerBound;
+	u8 antdiv_rssi;
+	u8 AntType;
+	u8 pre_AntType;
+	u8 antdiv_period;
+	u8 antdiv_select;
+	u8 NdpaPeriod;
+	bool H2C_RARpt_connect;
+
+	/*  add by Yu Cehn for adaptivtiy */
+	bool adaptivity_flag;
+	bool NHM_disable;
+	bool TxHangFlg;
+	bool Carrier_Sense_enable;
+	u8 tolerance_cnt;
+	u64 NHMCurTxOkcnt;
+	u64 NHMCurRxOkcnt;
+	u64 NHMLastTxOkcnt;
+	u64 NHMLastRxOkcnt;
+	u8 txEdcca1;
+	u8 txEdcca0;
+	s8 H2L_lb;
+	s8 L2H_lb;
+	u8 Adaptivity_IGI_upper;
+	u8 NHM_cnt_0;
+
+
+	ODM_NOISE_MONITOR noise_level;/* ODM_MAX_CHANNEL_NUM]; */
+	/*  */
+	/* 2 Define STA info. */
+	/*  _ODM_STA_INFO */
+	/*  2012/01/12 MH For MP, we need to reduce one array pointer for default port.?? */
+	PSTA_INFO_T pODM_StaInfo[ODM_ASSOCIATE_ENTRY_NUM];
+
+	/*  */
+	/*  2012/02/14 MH Add to share 88E ra with other SW team. */
+	/*  We need to colelct all support abilit to a proper area. */
+	/*  */
+	bool RaSupport88E;
+
+	/*  Define ........... */
+
+	/*  Latest packet phy info (ODM write) */
+	ODM_PHY_DBG_INFO_T PhyDbgInfo;
+	/* PHY_INFO_88E		PhyInfo; */
+
+	/*  Latest packet phy info (ODM write) */
+	ODM_MAC_INFO *pMacInfo;
+	/* MAC_INFO_88E		MacInfo; */
+
+	/*  Different Team independt structure?? */
+
+	/*  */
+	/* TX_RTP_CMN		TX_retrpo; */
+	/* TX_RTP_88E		TX_retrpo; */
+	/* TX_RTP_8195		TX_retrpo; */
+
+	/*  */
+	/* ODM Structure */
+	/*  */
+	FAT_T DM_FatTable;
+	DIG_T DM_DigTable;
+	PS_T DM_PSTable;
+	Pri_CCA_T DM_PriCCA;
+	RXHP_T DM_RXHP_Table;
+	RA_T DM_RA_Table;
+	false_ALARM_STATISTICS FalseAlmCnt;
+	false_ALARM_STATISTICS FlaseAlmCntBuddyAdapter;
+	SWAT_T DM_SWAT_Table;
+	bool RSSI_test;
+	CFO_TRACKING DM_CfoTrack;
+
+	EDCA_T DM_EDCA_Table;
+	u32 WMMEDCA_BE;
+	PATHDIV_T DM_PathDiv;
+	/*  Copy from SD4 structure */
+	/*  */
+	/*  ================================================== */
+	/*  */
+
+	/* common */
+	/* u8 DM_Type; */
+	/* u8    PSD_Report_RXHP[80];    Add By Gary */
+	/* u8    PSD_func_flag;                Add By Gary */
+	/* for DIG */
+	/* u8 bDMInitialGainEnable; */
+	/* u8 binitialized;  for dm_initial_gain_Multi_STA use. */
+	/* for Antenna diversity */
+	/* u8 AntDivCfg; 0:OFF , 1:ON, 2:by efuse */
+	/* PSTA_INFO_T RSSI_target; */
+
+	bool *pbDriverStopped;
+	bool *pbDriverIsGoingToPnpSetPowerSleep;
+	bool *pinit_adpt_in_progress;
+
+	/* PSD */
+	bool bUserAssignLevel;
+	RT_TIMER PSDTimer;
+	u8 RSSI_BT;			/* come from BT */
+	bool bPSDinProcess;
+	bool bPSDactive;
+	bool bDMInitialGainEnable;
+
+	/* MPT DIG */
+	RT_TIMER MPT_DIGTimer;
+
+	/* for rate adaptive, in fact,  88c/92c fw will handle this */
+	u8 bUseRAMask;
+
+	ODM_RATE_ADAPTIVE RateAdaptive;
+
+	ANT_DETECTED_INFO AntDetectedInfo; /*  Antenna detected information for RSSI tool */
+
+	ODM_RF_CAL_T RFCalibrateInfo;
+
+	/*  */
+	/*  TX power tracking */
+	/*  */
+	u8 BbSwingIdxOfdm[MAX_RF_PATH];
+	u8 BbSwingIdxOfdmCurrent;
+	u8 BbSwingIdxOfdmBase[MAX_RF_PATH];
+	bool BbSwingFlagOfdm;
+	u8 BbSwingIdxCck;
+	u8 BbSwingIdxCckCurrent;
+	u8 BbSwingIdxCckBase;
+	u8 DefaultOfdmIndex;
+	u8 DefaultCckIndex;
+	bool BbSwingFlagCck;
+
+	s8 Absolute_OFDMSwingIdx[MAX_RF_PATH];
+	s8 Remnant_OFDMSwingIdx[MAX_RF_PATH];
+	s8 Remnant_CCKSwingIdx;
+	s8 Modify_TxAGC_Value;       /* Remnat compensate value at TxAGC */
+	bool Modify_TxAGC_Flag_PathA;
+	bool Modify_TxAGC_Flag_PathB;
+	bool Modify_TxAGC_Flag_PathC;
+	bool Modify_TxAGC_Flag_PathD;
+	bool Modify_TxAGC_Flag_PathA_CCK;
+
+	s8 KfreeOffset[MAX_RF_PATH];
+	/*  */
+	/*  ODM system resource. */
+	/*  */
+
+	/*  ODM relative time. */
+	RT_TIMER PathDivSwitchTimer;
+	/* 2011.09.27 add for Path Diversity */
+	RT_TIMER CCKPathDiversityTimer;
+	RT_TIMER FastAntTrainingTimer;
+
+	/*  ODM relative workitem. */
+
+	#if (BEAMFORMING_SUPPORT == 1)
+	RT_BEAMFORMING_INFO BeamformingInfo;
+	#endif
+} DM_ODM_T, *PDM_ODM_T; /*  DM_Dynamic_Mechanism_Structure */
+
+#define ODM_RF_PATH_MAX 2
+
+typedef enum _ODM_RF_RADIO_PATH {
+	ODM_RF_PATH_A = 0,   /* Radio Path A */
+	ODM_RF_PATH_B = 1,   /* Radio Path B */
+	ODM_RF_PATH_C = 2,   /* Radio Path C */
+	ODM_RF_PATH_D = 3,   /* Radio Path D */
+	ODM_RF_PATH_AB,
+	ODM_RF_PATH_AC,
+	ODM_RF_PATH_AD,
+	ODM_RF_PATH_BC,
+	ODM_RF_PATH_BD,
+	ODM_RF_PATH_CD,
+	ODM_RF_PATH_ABC,
+	ODM_RF_PATH_ACD,
+	ODM_RF_PATH_BCD,
+	ODM_RF_PATH_ABCD,
+	/*   ODM_RF_PATH_MAX,    Max RF number 90 support */
+} ODM_RF_RADIO_PATH_E, *PODM_RF_RADIO_PATH_E;
+
+ typedef enum _ODM_RF_CONTENT {
+	odm_radioa_txt = 0x1000,
+	odm_radiob_txt = 0x1001,
+	odm_radioc_txt = 0x1002,
+	odm_radiod_txt = 0x1003
+} ODM_RF_CONTENT;
+
+typedef enum _ODM_BB_Config_Type {
+	CONFIG_BB_PHY_REG,
+	CONFIG_BB_AGC_TAB,
+	CONFIG_BB_AGC_TAB_2G,
+	CONFIG_BB_AGC_TAB_5G,
+	CONFIG_BB_PHY_REG_PG,
+	CONFIG_BB_PHY_REG_MP,
+	CONFIG_BB_AGC_TAB_DIFF,
+} ODM_BB_Config_Type, *PODM_BB_Config_Type;
+
+typedef enum _ODM_RF_Config_Type {
+	CONFIG_RF_RADIO,
+	CONFIG_RF_TXPWR_LMT,
+} ODM_RF_Config_Type, *PODM_RF_Config_Type;
+
+typedef enum _ODM_FW_Config_Type {
+	CONFIG_FW_NIC,
+	CONFIG_FW_NIC_2,
+	CONFIG_FW_AP,
+	CONFIG_FW_WoWLAN,
+	CONFIG_FW_WoWLAN_2,
+	CONFIG_FW_AP_WoWLAN,
+	CONFIG_FW_BT,
+} ODM_FW_Config_Type;
+
+/*  Status code */
+typedef enum _RT_STATUS {
+	RT_STATUS_SUCCESS,
+	RT_STATUS_FAILURE,
+	RT_STATUS_PENDING,
+	RT_STATUS_RESOURCE,
+	RT_STATUS_INVALID_CONTEXT,
+	RT_STATUS_INVALID_PARAMETER,
+	RT_STATUS_NOT_SUPPORT,
+	RT_STATUS_OS_API_FAILED,
+} RT_STATUS, *PRT_STATUS;
+
+#ifdef REMOVE_PACK
+#pragma pack()
+#endif
+
+/* include "odm_function.h" */
+
+/* 3 =========================================================== */
+/* 3 DIG */
+/* 3 =========================================================== */
+
+/* Remove DIG by Yuchen */
+
+/* 3 =========================================================== */
+/* 3 AGC RX High Power Mode */
+/* 3 =========================================================== */
+#define          LNA_Low_Gain_1                      0x64
+#define          LNA_Low_Gain_2                      0x5A
+#define          LNA_Low_Gain_3                      0x58
+
+#define          FA_RXHP_TH1                           5000
+#define          FA_RXHP_TH2                           1500
+#define          FA_RXHP_TH3                             800
+#define          FA_RXHP_TH4                             600
+#define          FA_RXHP_TH5                             500
+
+/* 3 =========================================================== */
+/* 3 EDCA */
+/* 3 =========================================================== */
+
+/* 3 =========================================================== */
+/* 3 Dynamic Tx Power */
+/* 3 =========================================================== */
+/* Dynamic Tx Power Control Threshold */
+
+/* 3 =========================================================== */
+/* 3 Rate Adaptive */
+/* 3 =========================================================== */
+#define		DM_RATR_STA_INIT			0
+#define		DM_RATR_STA_HIGH			1
+#define		DM_RATR_STA_MIDDLE			2
+#define		DM_RATR_STA_LOW				3
+
+/* 3 =========================================================== */
+/* 3 BB Power Save */
+/* 3 =========================================================== */
+
+typedef enum tag_1R_CCA_Type_Definition {
+	CCA_1R = 0,
+	CCA_2R = 1,
+	CCA_MAX = 2,
+} DM_1R_CCA_E;
+
+typedef enum tag_RF_Type_Definition {
+	RF_Save = 0,
+	RF_Normal = 1,
+	RF_MAX = 2,
+} DM_RF_E;
+
+/* 3 =========================================================== */
+/* 3 Antenna Diversity */
+/* 3 =========================================================== */
+typedef enum tag_SW_Antenna_Switch_Definition {
+	Antenna_A = 1,
+	Antenna_B = 2,
+	Antenna_MAX = 3,
+} DM_SWAS_E;
+
+
+/*  Maximal number of antenna detection mechanism needs to perform, added by Roger, 2011.12.28. */
+#define	MAX_ANTENNA_DETECTION_CNT	10
+
+/*  */
+/*  Extern Global Variables. */
+/*  */
+extern	u32 OFDMSwingTable[OFDM_TABLE_SIZE];
+extern	u8 CCKSwingTable_Ch1_Ch13[CCK_TABLE_SIZE][8];
+extern	u8 CCKSwingTable_Ch14[CCK_TABLE_SIZE][8];
+
+extern	u32 OFDMSwingTable_New[OFDM_TABLE_SIZE];
+extern	u8 CCKSwingTable_Ch1_Ch13_New[CCK_TABLE_SIZE][8];
+extern	u8 CCKSwingTable_Ch14_New[CCK_TABLE_SIZE][8];
+
+extern  u32 TxScalingTable_Jaguar[TXSCALE_TABLE_SIZE];
+
+/*  */
+/*  check Sta pointer valid or not */
+/*  */
+#define IS_STA_VALID(pSta)		(pSta)
+/*  20100514 Joseph: Add definition for antenna switching test after link. */
+/*  This indicates two different the steps. */
+/*  In SWAW_STEP_PEAK, driver needs to switch antenna and listen to the signal on the air. */
+/*  In SWAW_STEP_DETERMINE, driver just compares the signal captured in SWAW_STEP_PEAK */
+/*  with original RSSI to determine if it is necessary to switch antenna. */
+#define SWAW_STEP_PEAK		0
+#define SWAW_STEP_DETERMINE	1
+
+/* Remove DIG by yuchen */
+
+void ODM_SetAntenna(PDM_ODM_T pDM_Odm, u8 Antenna);
+
+
+/* Remove BB power saving by Yuchen */
+
+#define dm_CheckTXPowerTracking ODM_TXPowerTrackingCheck
+void ODM_TXPowerTrackingCheck(PDM_ODM_T pDM_Odm);
+
+bool ODM_RAStateCheck(
+	PDM_ODM_T pDM_Odm,
+	s32	RSSI,
+	bool bForceUpdate,
+	u8 *pRATRState
+);
+
+#define dm_SWAW_RSSI_Check	ODM_SwAntDivChkPerPktRssi
+void ODM_SwAntDivChkPerPktRssi(
+	PDM_ODM_T pDM_Odm,
+	u8 StationID,
+	PODM_PHY_INFO_T pPhyInfo
+);
+
+u32 ODM_Get_Rate_Bitmap(
+	PDM_ODM_T pDM_Odm,
+	u32 macid,
+	u32 ra_mask,
+	u8 rssi_level
+);
+
+#if (BEAMFORMING_SUPPORT == 1)
+BEAMFORMING_CAP Beamforming_GetEntryBeamCapByMacId(PMGNT_INFO pMgntInfo, u8 MacId);
+#endif
+
+void odm_TXPowerTrackingInit(PDM_ODM_T pDM_Odm);
+
+void ODM_DMInit(PDM_ODM_T pDM_Odm);
+
+void ODM_DMWatchdog(PDM_ODM_T pDM_Odm); /*  For common use in the future */
+
+void ODM_CmnInfoInit(PDM_ODM_T pDM_Odm, ODM_CMNINFO_E CmnInfo, u32 Value);
+
+void ODM_CmnInfoHook(PDM_ODM_T pDM_Odm, ODM_CMNINFO_E CmnInfo, void *pValue);
+
+void ODM_CmnInfoPtrArrayHook(
+	PDM_ODM_T pDM_Odm,
+	ODM_CMNINFO_E CmnInfo,
+	u16 Index,
+	void *pValue
+);
+
+void ODM_CmnInfoUpdate(PDM_ODM_T pDM_Odm, u32 CmnInfo, u64 Value);
+
+void ODM_InitAllTimers(PDM_ODM_T pDM_Odm);
+
+void ODM_CancelAllTimers(PDM_ODM_T pDM_Odm);
+
+void ODM_ReleaseAllTimers(PDM_ODM_T pDM_Odm);
+
+void ODM_AntselStatistics_88C(
+	PDM_ODM_T pDM_Odm,
+	u8 MacId,
+	u32 PWDBAll,
+	bool isCCKrate
+);
+
+void ODM_DynamicARFBSelect(PDM_ODM_T pDM_Odm, u8 rate, bool Collision_State);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_HWConfig.c linux/3rdparty/rtl8723bs/hal/odm_HWConfig.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_HWConfig.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_HWConfig.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,535 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+#define READ_AND_CONFIG_MP(ic, txt) (ODM_ReadAndConfig_MP_##ic##txt(pDM_Odm))
+#define READ_AND_CONFIG     READ_AND_CONFIG_MP
+#define GET_VERSION_MP(ic, txt) (ODM_GetVersion_MP_##ic##txt())
+#define GET_VERSION(ic, txt) (pDM_Odm->bIsMPChip?GET_VERSION_MP(ic, txt):GET_VERSION_TC(ic, txt))
+
+static u8 odm_QueryRxPwrPercentage(s8 AntPower)
+{
+	if ((AntPower <= -100) || (AntPower >= 20))
+		return	0;
+	else if (AntPower >= 0)
+		return	100;
+	else
+		return	(100+AntPower);
+
+}
+
+static s32 odm_SignalScaleMapping_92CSeries(PDM_ODM_T pDM_Odm, s32 CurrSig)
+{
+	s32 RetSig = 0;
+
+	if (pDM_Odm->SupportInterface  == ODM_ITRF_SDIO) {
+		if (CurrSig >= 51 && CurrSig <= 100)
+			RetSig = 100;
+		else if (CurrSig >= 41 && CurrSig <= 50)
+			RetSig = 80 + ((CurrSig - 40)*2);
+		else if (CurrSig >= 31 && CurrSig <= 40)
+			RetSig = 66 + (CurrSig - 30);
+		else if (CurrSig >= 21 && CurrSig <= 30)
+			RetSig = 54 + (CurrSig - 20);
+		else if (CurrSig >= 10 && CurrSig <= 20)
+			RetSig = 42 + (((CurrSig - 10) * 2) / 3);
+		else if (CurrSig >= 5 && CurrSig <= 9)
+			RetSig = 22 + (((CurrSig - 5) * 3) / 2);
+		else if (CurrSig >= 1 && CurrSig <= 4)
+			RetSig = 6 + (((CurrSig - 1) * 3) / 2);
+		else
+			RetSig = CurrSig;
+	}
+
+	return RetSig;
+}
+
+s32 odm_SignalScaleMapping(PDM_ODM_T pDM_Odm, s32 CurrSig)
+{
+	return odm_SignalScaleMapping_92CSeries(pDM_Odm, CurrSig);
+}
+
+static u8 odm_EVMdbToPercentage(s8 Value)
+{
+	/*  */
+	/*  -33dB~0dB to 0%~99% */
+	/*  */
+	s8 ret_val;
+
+	ret_val = Value;
+	ret_val /= 2;
+
+	/* DbgPrint("Value =%d\n", Value); */
+	/* ODM_RT_DISP(FRX, RX_PHY_SQ, ("EVMdbToPercentage92C Value =%d / %x\n", ret_val, ret_val)); */
+
+	if (ret_val >= 0)
+		ret_val = 0;
+	if (ret_val <= -33)
+		ret_val = -33;
+
+	ret_val = 0 - ret_val;
+	ret_val *= 3;
+
+	if (ret_val == 99)
+		ret_val = 100;
+
+	return ret_val;
+}
+
+static void odm_RxPhyStatus92CSeries_Parsing(
+	PDM_ODM_T pDM_Odm,
+	PODM_PHY_INFO_T pPhyInfo,
+	u8 *pPhyStatus,
+	PODM_PACKET_INFO_T pPktinfo
+)
+{
+	u8 i, Max_spatial_stream;
+	s8 rx_pwr[4], rx_pwr_all = 0;
+	u8 EVM, PWDB_ALL = 0, PWDB_ALL_BT;
+	u8 RSSI, total_rssi = 0;
+	bool isCCKrate = false;
+	u8 rf_rx_num = 0;
+	u8 cck_highpwr = 0;
+	u8 LNA_idx, VGA_idx;
+	PPHY_STATUS_RPT_8192CD_T pPhyStaRpt = (PPHY_STATUS_RPT_8192CD_T)pPhyStatus;
+
+	isCCKrate = (pPktinfo->DataRate <= DESC_RATE11M) ? true : false;
+	pPhyInfo->RxMIMOSignalQuality[ODM_RF_PATH_A] = -1;
+	pPhyInfo->RxMIMOSignalQuality[ODM_RF_PATH_B] = -1;
+
+
+	if (isCCKrate) {
+		u8 cck_agc_rpt;
+
+		pDM_Odm->PhyDbgInfo.NumQryPhyStatusCCK++;
+		/*  */
+		/*  (1)Hardware does not provide RSSI for CCK */
+		/*  (2)PWDB, Average PWDB cacluated by hardware (for rate adaptive) */
+		/*  */
+
+		/* if (pHalData->eRFPowerState == eRfOn) */
+		cck_highpwr = pDM_Odm->bCckHighPower;
+		/* else */
+		/* cck_highpwr = false; */
+
+		cck_agc_rpt =  pPhyStaRpt->cck_agc_rpt_ofdm_cfosho_a ;
+
+		/* 2011.11.28 LukeLee: 88E use different LNA & VGA gain table */
+		/* The RSSI formula should be modified according to the gain table */
+		/* In 88E, cck_highpwr is always set to 1 */
+		LNA_idx = ((cck_agc_rpt & 0xE0)>>5);
+		VGA_idx = (cck_agc_rpt & 0x1F);
+		rx_pwr_all = odm_CCKRSSI_8723B(LNA_idx, VGA_idx);
+		PWDB_ALL = odm_QueryRxPwrPercentage(rx_pwr_all);
+		if (PWDB_ALL > 100)
+			PWDB_ALL = 100;
+
+		pPhyInfo->RxPWDBAll = PWDB_ALL;
+		pPhyInfo->BTRxRSSIPercentage = PWDB_ALL;
+		pPhyInfo->RecvSignalPower = rx_pwr_all;
+		/*  */
+		/*  (3) Get Signal Quality (EVM) */
+		/*  */
+		/* if (pPktinfo->bPacketMatchBSSID) */
+		{
+			u8 SQ, SQ_rpt;
+
+			if (pPhyInfo->RxPWDBAll > 40 && !pDM_Odm->bInHctTest)
+				SQ = 100;
+			else {
+				SQ_rpt = pPhyStaRpt->cck_sig_qual_ofdm_pwdb_all;
+
+				if (SQ_rpt > 64)
+					SQ = 0;
+				else if (SQ_rpt < 20)
+					SQ = 100;
+				else
+					SQ = ((64-SQ_rpt) * 100) / 44;
+
+			}
+
+			/* DbgPrint("cck SQ = %d\n", SQ); */
+			pPhyInfo->SignalQuality = SQ;
+			pPhyInfo->RxMIMOSignalQuality[ODM_RF_PATH_A] = SQ;
+			pPhyInfo->RxMIMOSignalQuality[ODM_RF_PATH_B] = -1;
+		}
+	} else { /* is OFDM rate */
+		pDM_Odm->PhyDbgInfo.NumQryPhyStatusOFDM++;
+
+		/*  */
+		/*  (1)Get RSSI for HT rate */
+		/*  */
+
+		for (i = ODM_RF_PATH_A; i < ODM_RF_PATH_MAX; i++) {
+			/*  2008/01/30 MH we will judge RF RX path now. */
+			if (pDM_Odm->RFPathRxEnable & BIT(i))
+				rf_rx_num++;
+			/* else */
+				/* continue; */
+
+			rx_pwr[i] = ((pPhyStaRpt->path_agc[i].gain&0x3F)*2) - 110;
+
+
+			pPhyInfo->RxPwr[i] = rx_pwr[i];
+
+			/* Translate DBM to percentage. */
+			RSSI = odm_QueryRxPwrPercentage(rx_pwr[i]);
+			total_rssi += RSSI;
+			/* RT_DISP(FRX, RX_PHY_SS, ("RF-%d RXPWR =%x RSSI =%d\n", i, rx_pwr[i], RSSI)); */
+
+			pPhyInfo->RxMIMOSignalStrength[i] = (u8) RSSI;
+
+			/* Get Rx snr value in DB */
+			pPhyInfo->RxSNR[i] = pDM_Odm->PhyDbgInfo.RxSNRdB[i] = (s32)(pPhyStaRpt->path_rxsnr[i]/2);
+		}
+
+
+		/*  */
+		/*  (2)PWDB, Average PWDB cacluated by hardware (for rate adaptive) */
+		/*  */
+		rx_pwr_all = (((pPhyStaRpt->cck_sig_qual_ofdm_pwdb_all) >> 1)&0x7f)-110;
+
+		PWDB_ALL_BT = PWDB_ALL = odm_QueryRxPwrPercentage(rx_pwr_all);
+		/* RT_DISP(FRX, RX_PHY_SS, ("PWDB_ALL =%d\n", PWDB_ALL)); */
+
+		pPhyInfo->RxPWDBAll = PWDB_ALL;
+		/* ODM_RT_TRACE(pDM_Odm, ODM_COMP_RSSI_MONITOR, ODM_DBG_LOUD, ("ODM OFDM RSSI =%d\n", pPhyInfo->RxPWDBAll)); */
+		pPhyInfo->BTRxRSSIPercentage = PWDB_ALL_BT;
+		pPhyInfo->RxPower = rx_pwr_all;
+		pPhyInfo->RecvSignalPower = rx_pwr_all;
+
+		{/* pMgntInfo->CustomerID != RT_CID_819x_Lenovo */
+			/*  */
+			/*  (3)EVM of HT rate */
+			/*  */
+			if (pPktinfo->DataRate >= DESC_RATEMCS8 && pPktinfo->DataRate <= DESC_RATEMCS15)
+				Max_spatial_stream = 2; /* both spatial stream make sense */
+			else
+				Max_spatial_stream = 1; /* only spatial stream 1 makes sense */
+
+			for (i = 0; i < Max_spatial_stream; i++) {
+				/*  Do not use shift operation like "rx_evmX >>= 1" because the compilor of free build environment */
+				/*  fill most significant bit to "zero" when doing shifting operation which may change a negative */
+				/*  value to positive one, then the dbm value (which is supposed to be negative)  is not correct anymore. */
+				EVM = odm_EVMdbToPercentage((pPhyStaRpt->stream_rxevm[i]));	/* dbm */
+
+				/* RT_DISP(FRX, RX_PHY_SQ, ("RXRATE =%x RXEVM =%x EVM =%s%d\n", */
+				/* GET_RX_STATUS_DESC_RX_MCS(pDesc), pDrvInfo->rxevm[i], "%", EVM)); */
+
+				/* if (pPktinfo->bPacketMatchBSSID) */
+				{
+					if (i == ODM_RF_PATH_A) /*  Fill value in RFD, Get the first spatial stream only */
+						pPhyInfo->SignalQuality = (u8)(EVM & 0xff);
+
+					pPhyInfo->RxMIMOSignalQuality[i] = (u8)(EVM & 0xff);
+				}
+			}
+		}
+
+		ODM_ParsingCFO(pDM_Odm, pPktinfo, pPhyStaRpt->path_cfotail);
+
+	}
+
+	/* UI BSS List signal strength(in percentage), make it good looking, from 0~100. */
+	/* It is assigned to the BSS List in GetValueFromBeaconOrProbeRsp(). */
+	if (isCCKrate) {
+#ifdef CONFIG_SKIP_SIGNAL_SCALE_MAPPING
+		pPhyInfo->SignalStrength = (u8)PWDB_ALL;
+#else
+		pPhyInfo->SignalStrength = (u8)(odm_SignalScaleMapping(pDM_Odm, PWDB_ALL));/* PWDB_ALL; */
+#endif
+	} else {
+		if (rf_rx_num != 0) {
+#ifdef CONFIG_SKIP_SIGNAL_SCALE_MAPPING
+			total_rssi /= rf_rx_num;
+			pPhyInfo->SignalStrength = (u8)total_rssi;
+#else
+			pPhyInfo->SignalStrength = (u8)(odm_SignalScaleMapping(pDM_Odm, total_rssi /= rf_rx_num));
+#endif
+		}
+	}
+
+	/* DbgPrint("isCCKrate = %d, pPhyInfo->RxPWDBAll = %d, pPhyStaRpt->cck_agc_rpt_ofdm_cfosho_a = 0x%x\n", */
+		/* isCCKrate, pPhyInfo->RxPWDBAll, pPhyStaRpt->cck_agc_rpt_ofdm_cfosho_a); */
+}
+
+static void odm_Process_RSSIForDM(
+	PDM_ODM_T pDM_Odm, PODM_PHY_INFO_T pPhyInfo, PODM_PACKET_INFO_T pPktinfo
+)
+{
+
+	s32 UndecoratedSmoothedPWDB, UndecoratedSmoothedCCK, UndecoratedSmoothedOFDM, RSSI_Ave;
+	u8 isCCKrate = 0;
+	u8 RSSI_max, RSSI_min, i;
+	u32 OFDM_pkt = 0;
+	u32 Weighting = 0;
+	PSTA_INFO_T pEntry;
+
+
+	if (pPktinfo->StationID == 0xFF)
+		return;
+
+	pEntry = pDM_Odm->pODM_StaInfo[pPktinfo->StationID];
+
+	if (!IS_STA_VALID(pEntry))
+		return;
+
+	if ((!pPktinfo->bPacketMatchBSSID))
+		return;
+
+	if (pPktinfo->bPacketBeacon)
+		pDM_Odm->PhyDbgInfo.NumQryBeaconPkt++;
+
+	isCCKrate = ((pPktinfo->DataRate <= DESC_RATE11M)) ? true : false;
+	pDM_Odm->RxRate = pPktinfo->DataRate;
+
+	/* Statistic for antenna/path diversity------------------ */
+	if (pDM_Odm->SupportAbility & ODM_BB_ANT_DIV) {
+
+	}
+
+	/* Smart Antenna Debug Message------------------ */
+
+	UndecoratedSmoothedCCK = pEntry->rssi_stat.UndecoratedSmoothedCCK;
+	UndecoratedSmoothedOFDM = pEntry->rssi_stat.UndecoratedSmoothedOFDM;
+	UndecoratedSmoothedPWDB = pEntry->rssi_stat.UndecoratedSmoothedPWDB;
+
+	if (pPktinfo->bPacketToSelf || pPktinfo->bPacketBeacon) {
+
+		if (!isCCKrate) { /* ofdm rate */
+			if (pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_B] == 0) {
+				RSSI_Ave = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_A];
+				pDM_Odm->RSSI_A = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_A];
+				pDM_Odm->RSSI_B = 0;
+			} else {
+				/* DbgPrint("pRfd->Status.RxMIMOSignalStrength[0] = %d, pRfd->Status.RxMIMOSignalStrength[1] = %d\n", */
+					/* pRfd->Status.RxMIMOSignalStrength[0], pRfd->Status.RxMIMOSignalStrength[1]); */
+				pDM_Odm->RSSI_A =  pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_A];
+				pDM_Odm->RSSI_B = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_B];
+
+				if (
+					pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_A] >
+					pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_B]
+				) {
+					RSSI_max = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_A];
+					RSSI_min = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_B];
+				} else {
+					RSSI_max = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_B];
+					RSSI_min = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_A];
+				}
+
+				if ((RSSI_max-RSSI_min) < 3)
+					RSSI_Ave = RSSI_max;
+				else if ((RSSI_max-RSSI_min) < 6)
+					RSSI_Ave = RSSI_max - 1;
+				else if ((RSSI_max-RSSI_min) < 10)
+					RSSI_Ave = RSSI_max - 2;
+				else
+					RSSI_Ave = RSSI_max - 3;
+			}
+
+			/* 1 Process OFDM RSSI */
+			if (UndecoratedSmoothedOFDM <= 0)	/*  initialize */
+				UndecoratedSmoothedOFDM = pPhyInfo->RxPWDBAll;
+			else {
+				if (pPhyInfo->RxPWDBAll > (u32)UndecoratedSmoothedOFDM) {
+					UndecoratedSmoothedOFDM =
+							((UndecoratedSmoothedOFDM*(Rx_Smooth_Factor-1)) +
+							RSSI_Ave)/Rx_Smooth_Factor;
+					UndecoratedSmoothedOFDM = UndecoratedSmoothedOFDM + 1;
+				} else {
+					UndecoratedSmoothedOFDM =
+							((UndecoratedSmoothedOFDM*(Rx_Smooth_Factor-1)) +
+							RSSI_Ave)/Rx_Smooth_Factor;
+				}
+			}
+
+			pEntry->rssi_stat.PacketMap = (pEntry->rssi_stat.PacketMap<<1) | BIT0;
+
+		} else {
+			RSSI_Ave = pPhyInfo->RxPWDBAll;
+			pDM_Odm->RSSI_A = (u8) pPhyInfo->RxPWDBAll;
+			pDM_Odm->RSSI_B = 0;
+
+			/* 1 Process CCK RSSI */
+			if (UndecoratedSmoothedCCK <= 0)	/*  initialize */
+				UndecoratedSmoothedCCK = pPhyInfo->RxPWDBAll;
+			else {
+				if (pPhyInfo->RxPWDBAll > (u32)UndecoratedSmoothedCCK) {
+					UndecoratedSmoothedCCK =
+							((UndecoratedSmoothedCCK*(Rx_Smooth_Factor-1)) +
+							pPhyInfo->RxPWDBAll)/Rx_Smooth_Factor;
+					UndecoratedSmoothedCCK = UndecoratedSmoothedCCK + 1;
+				} else {
+					UndecoratedSmoothedCCK =
+							((UndecoratedSmoothedCCK*(Rx_Smooth_Factor-1)) +
+							pPhyInfo->RxPWDBAll)/Rx_Smooth_Factor;
+				}
+			}
+			pEntry->rssi_stat.PacketMap = pEntry->rssi_stat.PacketMap<<1;
+		}
+
+		/* if (pEntry) */
+		{
+			/* 2011.07.28 LukeLee: modified to prevent unstable CCK RSSI */
+			if (pEntry->rssi_stat.ValidBit >= 64)
+				pEntry->rssi_stat.ValidBit = 64;
+			else
+				pEntry->rssi_stat.ValidBit++;
+
+			for (i = 0; i < pEntry->rssi_stat.ValidBit; i++)
+				OFDM_pkt += (u8)(pEntry->rssi_stat.PacketMap>>i)&BIT0;
+
+			if (pEntry->rssi_stat.ValidBit == 64) {
+				Weighting = ((OFDM_pkt<<4) > 64)?64:(OFDM_pkt<<4);
+				UndecoratedSmoothedPWDB = (Weighting*UndecoratedSmoothedOFDM+(64-Weighting)*UndecoratedSmoothedCCK)>>6;
+			} else {
+				if (pEntry->rssi_stat.ValidBit != 0)
+					UndecoratedSmoothedPWDB = (OFDM_pkt*UndecoratedSmoothedOFDM+(pEntry->rssi_stat.ValidBit-OFDM_pkt)*UndecoratedSmoothedCCK)/pEntry->rssi_stat.ValidBit;
+				else
+					UndecoratedSmoothedPWDB = 0;
+			}
+
+			pEntry->rssi_stat.UndecoratedSmoothedCCK = UndecoratedSmoothedCCK;
+			pEntry->rssi_stat.UndecoratedSmoothedOFDM = UndecoratedSmoothedOFDM;
+			pEntry->rssi_stat.UndecoratedSmoothedPWDB = UndecoratedSmoothedPWDB;
+
+			/* DbgPrint("OFDM_pkt =%d, Weighting =%d\n", OFDM_pkt, Weighting); */
+			/* DbgPrint("UndecoratedSmoothedOFDM =%d, UndecoratedSmoothedPWDB =%d, UndecoratedSmoothedCCK =%d\n", */
+			/* UndecoratedSmoothedOFDM, UndecoratedSmoothedPWDB, UndecoratedSmoothedCCK); */
+
+		}
+
+	}
+}
+
+
+/*  */
+/*  Endianness before calling this API */
+/*  */
+static void ODM_PhyStatusQuery_92CSeries(
+	PDM_ODM_T pDM_Odm,
+	PODM_PHY_INFO_T pPhyInfo,
+	u8 *pPhyStatus,
+	PODM_PACKET_INFO_T pPktinfo
+)
+{
+
+	odm_RxPhyStatus92CSeries_Parsing(pDM_Odm, pPhyInfo, pPhyStatus, pPktinfo);
+
+	if (!pDM_Odm->RSSI_test)
+		odm_Process_RSSIForDM(pDM_Odm, pPhyInfo, pPktinfo);
+}
+
+void ODM_PhyStatusQuery(
+	PDM_ODM_T pDM_Odm,
+	PODM_PHY_INFO_T pPhyInfo,
+	u8 *pPhyStatus,
+	PODM_PACKET_INFO_T pPktinfo
+)
+{
+
+	ODM_PhyStatusQuery_92CSeries(pDM_Odm, pPhyInfo, pPhyStatus, pPktinfo);
+}
+
+/*  */
+/*  If you want to add a new IC, Please follow below template and generate a new one. */
+/*  */
+/*  */
+
+HAL_STATUS ODM_ConfigRFWithHeaderFile(
+	PDM_ODM_T pDM_Odm,
+	ODM_RF_Config_Type ConfigType,
+	ODM_RF_RADIO_PATH_E eRFPath
+)
+{
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				("===>ODM_ConfigRFWithHeaderFile (%s)\n", (pDM_Odm->bIsMPChip) ? "MPChip" : "TestChip"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				("pDM_Odm->SupportPlatform: 0x%X, pDM_Odm->SupportInterface: 0x%X, pDM_Odm->BoardType: 0x%X\n",
+				pDM_Odm->SupportPlatform, pDM_Odm->SupportInterface, pDM_Odm->BoardType));
+
+	if (ConfigType == CONFIG_RF_RADIO)
+		READ_AND_CONFIG(8723B, _RadioA);
+	else if (ConfigType == CONFIG_RF_TXPWR_LMT)
+		READ_AND_CONFIG(8723B, _TXPWR_LMT);
+
+	return HAL_STATUS_SUCCESS;
+}
+
+HAL_STATUS ODM_ConfigRFWithTxPwrTrackHeaderFile(PDM_ODM_T pDM_Odm)
+{
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				 ("===>ODM_ConfigRFWithTxPwrTrackHeaderFile (%s)\n", (pDM_Odm->bIsMPChip) ? "MPChip" : "TestChip"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				 ("pDM_Odm->SupportPlatform: 0x%X, pDM_Odm->SupportInterface: 0x%X, pDM_Odm->BoardType: 0x%X\n",
+				 pDM_Odm->SupportPlatform, pDM_Odm->SupportInterface, pDM_Odm->BoardType));
+
+	if (pDM_Odm->SupportInterface == ODM_ITRF_SDIO)
+		READ_AND_CONFIG(8723B, _TxPowerTrack_SDIO);
+
+	return HAL_STATUS_SUCCESS;
+}
+
+HAL_STATUS ODM_ConfigBBWithHeaderFile(
+	PDM_ODM_T pDM_Odm, ODM_BB_Config_Type ConfigType
+)
+{
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				("===>ODM_ConfigBBWithHeaderFile (%s)\n", (pDM_Odm->bIsMPChip) ? "MPChip" : "TestChip"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				("pDM_Odm->SupportPlatform: 0x%X, pDM_Odm->SupportInterface: 0x%X, pDM_Odm->BoardType: 0x%X\n",
+				pDM_Odm->SupportPlatform, pDM_Odm->SupportInterface, pDM_Odm->BoardType));
+
+	if (ConfigType == CONFIG_BB_PHY_REG)
+		READ_AND_CONFIG(8723B, _PHY_REG);
+	else if (ConfigType == CONFIG_BB_AGC_TAB)
+		READ_AND_CONFIG(8723B, _AGC_TAB);
+	else if (ConfigType == CONFIG_BB_PHY_REG_PG)
+		READ_AND_CONFIG(8723B, _PHY_REG_PG);
+
+	return HAL_STATUS_SUCCESS;
+}
+
+HAL_STATUS ODM_ConfigMACWithHeaderFile(PDM_ODM_T pDM_Odm)
+{
+	u8 result = HAL_STATUS_SUCCESS;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		(
+			"===>ODM_ConfigMACWithHeaderFile (%s)\n",
+			(pDM_Odm->bIsMPChip) ? "MPChip" : "TestChip"
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		(
+			"pDM_Odm->SupportPlatform: 0x%X, pDM_Odm->SupportInterface: 0x%X, pDM_Odm->BoardType: 0x%X\n",
+			pDM_Odm->SupportPlatform,
+			pDM_Odm->SupportInterface,
+			pDM_Odm->BoardType
+		)
+	);
+
+	READ_AND_CONFIG(8723B, _MAC_REG);
+
+	return result;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_HWConfig.h linux/3rdparty/rtl8723bs/hal/odm_HWConfig.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_HWConfig.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_HWConfig.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,162 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+
+#ifndef __HALHWOUTSRC_H__
+#define __HALHWOUTSRC_H__
+
+
+/*--------------------------Define -------------------------------------------*/
+/* define READ_NEXT_PAIR(v1, v2, i) do { i += 2; v1 = Array[i]; v2 = Array[i+1]; } while (0) */
+#define AGC_DIFF_CONFIG_MP(ic, band) (ODM_ReadAndConfig_MP_##ic##_AGC_TAB_DIFF(pDM_Odm, Array_MP_##ic##_AGC_TAB_DIFF_##band, \
+	sizeof(Array_MP_##ic##_AGC_TAB_DIFF_##band)/sizeof(u32)))
+#define AGC_DIFF_CONFIG_TC(ic, band) (ODM_ReadAndConfig_TC_##ic##_AGC_TAB_DIFF(pDM_Odm, Array_TC_##ic##_AGC_TAB_DIFF_##band, \
+	sizeof(Array_TC_##ic##_AGC_TAB_DIFF_##band)/sizeof(u32)))
+
+#define AGC_DIFF_CONFIG(ic, band)\
+	do {\
+		if (pDM_Odm->bIsMPChip)\
+			AGC_DIFF_CONFIG_MP(ic, band);\
+		else\
+			AGC_DIFF_CONFIG_TC(ic, band);\
+	} while (0)
+
+
+/*  */
+/*  structure and define */
+/*  */
+
+typedef struct _Phy_Rx_AGC_Info {
+	#if (ODM_ENDIAN_TYPE == ODM_ENDIAN_LITTLE)
+		u8 gain:7, trsw:1;
+	#else
+		u8 trsw:1, gain:7;
+	#endif
+} PHY_RX_AGC_INFO_T, *pPHY_RX_AGC_INFO_T;
+
+typedef struct _Phy_Status_Rpt_8192cd {
+	PHY_RX_AGC_INFO_T path_agc[2];
+	u8 ch_corr[2];
+	u8 cck_sig_qual_ofdm_pwdb_all;
+	u8 cck_agc_rpt_ofdm_cfosho_a;
+	u8 cck_rpt_b_ofdm_cfosho_b;
+	u8 rsvd_1;/* ch_corr_msb; */
+	u8 noise_power_db_msb;
+	s8 path_cfotail[2];
+	u8 pcts_mask[2];
+	s8 stream_rxevm[2];
+	u8 path_rxsnr[2];
+	u8 noise_power_db_lsb;
+	u8 rsvd_2[3];
+	u8 stream_csi[2];
+	u8 stream_target_csi[2];
+	s8	sig_evm;
+	u8 rsvd_3;
+
+#if (ODM_ENDIAN_TYPE == ODM_ENDIAN_LITTLE)
+	u8 antsel_rx_keep_2:1;	/* ex_intf_flg:1; */
+	u8 sgi_en:1;
+	u8 rxsc:2;
+	u8 idle_long:1;
+	u8 r_ant_train_en:1;
+	u8 ant_sel_b:1;
+	u8 ant_sel:1;
+#else	/*  _BIG_ENDIAN_ */
+	u8 ant_sel:1;
+	u8 ant_sel_b:1;
+	u8 r_ant_train_en:1;
+	u8 idle_long:1;
+	u8 rxsc:2;
+	u8 sgi_en:1;
+	u8 antsel_rx_keep_2:1;	/* ex_intf_flg:1; */
+#endif
+} PHY_STATUS_RPT_8192CD_T, *PPHY_STATUS_RPT_8192CD_T;
+
+
+typedef struct _Phy_Status_Rpt_8812 {
+	/* 2012.05.24 LukeLee: This structure should take big/little endian in consideration later..... */
+
+	/* DWORD 0 */
+	u8 gain_trsw[2];
+#if (ODM_ENDIAN_TYPE == ODM_ENDIAN_LITTLE)
+	u16 chl_num:10;
+	u16 sub_chnl:4;
+	u16 r_RFMOD:2;
+#else	/*  _BIG_ENDIAN_ */
+	u16 r_RFMOD:2;
+	u16 sub_chnl:4;
+	u16 chl_num:10;
+#endif
+
+	/* DWORD 1 */
+	u8 pwdb_all;
+	u8 cfosho[4];	/*  DW 1 byte 1 DW 2 byte 0 */
+
+	/* DWORD 2 */
+	s8 cfotail[4]; /*  DW 2 byte 1 DW 3 byte 0 */
+
+	/* DWORD 3 */
+	s8 rxevm[2]; /*  DW 3 byte 1 DW 3 byte 2 */
+	s8 rxsnr[2]; /*  DW 3 byte 3 DW 4 byte 0 */
+
+	/* DWORD 4 */
+	u8 PCTS_MSK_RPT[2];
+	u8 pdsnr[2]; /*  DW 4 byte 3 DW 5 Byte 0 */
+
+	/* DWORD 5 */
+	u8 csi_current[2];
+	u8 rx_gain_c;
+
+	/* DWORD 6 */
+	u8 rx_gain_d;
+	s8 sigevm;
+	u8 resvd_0;
+	u8 antidx_anta:3;
+	u8 antidx_antb:3;
+	u8 resvd_1:2;
+} PHY_STATUS_RPT_8812_T, *PPHY_STATUS_RPT_8812_T;
+
+
+void ODM_PhyStatusQuery(
+	PDM_ODM_T pDM_Odm,
+	PODM_PHY_INFO_T pPhyInfo,
+	u8 *pPhyStatus,
+	PODM_PACKET_INFO_T pPktinfo
+);
+
+HAL_STATUS ODM_ConfigRFWithTxPwrTrackHeaderFile(PDM_ODM_T pDM_Odm);
+
+HAL_STATUS ODM_ConfigRFWithHeaderFile(
+	PDM_ODM_T pDM_Odm,
+	ODM_RF_Config_Type ConfigType,
+	ODM_RF_RADIO_PATH_E eRFPath
+);
+
+HAL_STATUS ODM_ConfigBBWithHeaderFile(
+	PDM_ODM_T pDM_Odm, ODM_BB_Config_Type ConfigType
+);
+
+HAL_STATUS ODM_ConfigMACWithHeaderFile(PDM_ODM_T pDM_Odm);
+
+HAL_STATUS ODM_ConfigFWWithHeaderFile(
+	PDM_ODM_T pDM_Odm,
+	ODM_FW_Config_Type ConfigType,
+	u8 *pFirmware,
+	u32 *pSize
+);
+
+s32 odm_SignalScaleMapping(PDM_ODM_T pDM_Odm, s32 CurrSig);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_interface.h linux/3rdparty/rtl8723bs/hal/odm_interface.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_interface.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_interface.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,59 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+
+#ifndef	__ODM_INTERFACE_H__
+#define __ODM_INTERFACE_H__
+
+
+
+/*  =========== Constant/Structure/Enum/... Define */
+
+/*  =========== Macro Define */
+
+#define _reg_all(_name)			ODM_##_name
+#define _reg_ic(_name, _ic)		ODM_##_name##_ic
+#define _bit_all(_name)			BIT_##_name
+#define _bit_ic(_name, _ic)		BIT_##_name##_ic
+
+/*===================================
+
+#define ODM_REG_DIG_11N		0xC50
+#define ODM_REG_DIG_11AC	0xDDD
+
+ODM_REG(DIG, _pDM_Odm)
+=====================================*/
+
+#define _reg_11N(_name)			ODM_REG_##_name##_11N
+#define _bit_11N(_name)			ODM_BIT_##_name##_11N
+
+#define _cat(_name, _ic_type, _func) _func##_11N(_name)
+
+/*  _name: name of register or bit. */
+/*  Example: "ODM_REG(R_A_AGC_CORE1, pDM_Odm)" */
+/*         gets "ODM_R_A_AGC_CORE1" or "ODM_R_A_AGC_CORE1_8192C", depends on SupportICType. */
+#define ODM_REG(_name, _pDM_Odm)	_cat(_name, _pDM_Odm->SupportICType, _reg)
+#define ODM_BIT(_name, _pDM_Odm)	_cat(_name, _pDM_Odm->SupportICType, _bit)
+
+typedef enum _ODM_H2C_CMD {
+	ODM_H2C_RSSI_REPORT = 0,
+	ODM_H2C_PSD_RESULT = 1,
+	ODM_H2C_PathDiv = 2,
+	ODM_H2C_WIFI_CALIBRATION = 3,
+	ODM_MAX_H2CCMD
+} ODM_H2C_CMD;
+
+
+#endif	/*  __ODM_INTERFACE_H__ */
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_NoiseMonitor.c linux/3rdparty/rtl8723bs/hal/odm_NoiseMonitor.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_NoiseMonitor.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_NoiseMonitor.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,175 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+/*  This function is for inband noise test utility only */
+/*  To obtain the inband noise level(dbm), do the following. */
+/*  1. disable DIG and Power Saving */
+/*  2. Set initial gain = 0x1a */
+/*  3. Stop updating idle time pwer report (for driver read) */
+/* - 0x80c[25] */
+
+#define Valid_Min				-35
+#define Valid_Max			10
+#define ValidCnt				5
+
+static s16 odm_InbandNoise_Monitor_NSeries(
+	PDM_ODM_T pDM_Odm,
+	u8 bPauseDIG,
+	u8 IGIValue,
+	u32 max_time
+)
+{
+	u32 tmp4b;
+	u8 max_rf_path = 0, rf_path;
+	u8 reg_c50, reg_c58, valid_done = 0;
+	struct noise_level noise_data;
+	u32 start  = 0, func_start = 0, func_end = 0;
+
+	func_start = jiffies;
+	pDM_Odm->noise_level.noise_all = 0;
+
+	if ((pDM_Odm->RFType == ODM_1T2R) || (pDM_Odm->RFType == ODM_2T2R))
+		max_rf_path = 2;
+	else
+		max_rf_path = 1;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("odm_DebugControlInbandNoise_Nseries() ==>\n"));
+
+	memset(&noise_data, 0, sizeof(struct noise_level));
+
+	/*  */
+	/*  Step 1. Disable DIG && Set initial gain. */
+	/*  */
+
+	if (bPauseDIG)
+		odm_PauseDIG(pDM_Odm, ODM_PAUSE_DIG, IGIValue);
+	/*  */
+	/*  Step 2. Disable all power save for read registers */
+	/*  */
+	/* dcmd_DebugControlPowerSave(padapter, PSDisable); */
+
+	/*  */
+	/*  Step 3. Get noise power level */
+	/*  */
+	start = jiffies;
+	while (1) {
+
+		/* Stop updating idle time pwer report (for driver read) */
+		PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_TxGainStage, BIT25, 1);
+
+		/* Read Noise Floor Report */
+		tmp4b = PHY_QueryBBReg(pDM_Odm->Adapter, 0x8f8, bMaskDWord);
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("Noise Floor Report (0x8f8) = 0x%08x\n", tmp4b));
+
+		/* PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XAAGCCore1, bMaskByte0, TestInitialGain); */
+		/* if (max_rf_path == 2) */
+		/* PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBAGCCore1, bMaskByte0, TestInitialGain); */
+
+		/* update idle time pwer report per 5us */
+		PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_TxGainStage, BIT25, 0);
+
+		noise_data.value[ODM_RF_PATH_A] = (u8)(tmp4b&0xff);
+		noise_data.value[ODM_RF_PATH_B]  = (u8)((tmp4b&0xff00)>>8);
+
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("value_a = 0x%x(%d), value_b = 0x%x(%d)\n",
+			noise_data.value[ODM_RF_PATH_A], noise_data.value[ODM_RF_PATH_A], noise_data.value[ODM_RF_PATH_B], noise_data.value[ODM_RF_PATH_B]));
+
+		for (rf_path = ODM_RF_PATH_A; rf_path < max_rf_path; rf_path++) {
+			noise_data.sval[rf_path] = (s8)noise_data.value[rf_path];
+			noise_data.sval[rf_path] /= 2;
+		}
+
+
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("sval_a = %d, sval_b = %d\n",
+			noise_data.sval[ODM_RF_PATH_A], noise_data.sval[ODM_RF_PATH_B]));
+		/* mdelay(10); */
+		/* msleep(10); */
+
+		for (rf_path = ODM_RF_PATH_A; rf_path < max_rf_path; rf_path++) {
+			if ((noise_data.valid_cnt[rf_path] < ValidCnt) && (noise_data.sval[rf_path] < Valid_Max && noise_data.sval[rf_path] >= Valid_Min)) {
+				noise_data.valid_cnt[rf_path]++;
+				noise_data.sum[rf_path] += noise_data.sval[rf_path];
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("RF_Path:%d Valid sval = %d\n", rf_path, noise_data.sval[rf_path]));
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("Sum of sval = %d,\n", noise_data.sum[rf_path]));
+				if (noise_data.valid_cnt[rf_path] == ValidCnt) {
+					valid_done++;
+					ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("After divided, RF_Path:%d , sum = %d\n", rf_path, noise_data.sum[rf_path]));
+				}
+
+			}
+
+		}
+
+		/* printk("####### valid_done:%d #############\n", valid_done); */
+		if ((valid_done == max_rf_path) || (jiffies_to_msecs(jiffies - start) > max_time)) {
+			for (rf_path = ODM_RF_PATH_A; rf_path < max_rf_path; rf_path++) {
+				/* printk("%s PATH_%d - sum = %d, valid_cnt = %d\n", __func__, rf_path, noise_data.sum[rf_path], noise_data.valid_cnt[rf_path]); */
+				if (noise_data.valid_cnt[rf_path])
+					noise_data.sum[rf_path] /= noise_data.valid_cnt[rf_path];
+				else
+					noise_data.sum[rf_path]  = 0;
+			}
+			break;
+		}
+	}
+	reg_c50 = (s32)PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XAAGCCore1, bMaskByte0);
+	reg_c50 &= ~BIT7;
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("0x%x = 0x%02x(%d)\n", rOFDM0_XAAGCCore1, reg_c50, reg_c50));
+	pDM_Odm->noise_level.noise[ODM_RF_PATH_A] = -110 + reg_c50 + noise_data.sum[ODM_RF_PATH_A];
+	pDM_Odm->noise_level.noise_all += pDM_Odm->noise_level.noise[ODM_RF_PATH_A];
+
+	if (max_rf_path == 2) {
+		reg_c58 = (s32)PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XBAGCCore1, bMaskByte0);
+		reg_c58 &= ~BIT7;
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("0x%x = 0x%02x(%d)\n", rOFDM0_XBAGCCore1, reg_c58, reg_c58));
+		pDM_Odm->noise_level.noise[ODM_RF_PATH_B] = -110 + reg_c58 + noise_data.sum[ODM_RF_PATH_B];
+		pDM_Odm->noise_level.noise_all += pDM_Odm->noise_level.noise[ODM_RF_PATH_B];
+	}
+	pDM_Odm->noise_level.noise_all /= max_rf_path;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_COMMON,
+		ODM_DBG_LOUD,
+		(
+			"noise_a = %d, noise_b = %d\n",
+			pDM_Odm->noise_level.noise[ODM_RF_PATH_A],
+			pDM_Odm->noise_level.noise[ODM_RF_PATH_B]
+		)
+	);
+
+	/*  */
+	/*  Step 4. Recover the Dig */
+	/*  */
+	if (bPauseDIG)
+		odm_PauseDIG(pDM_Odm, ODM_RESUME_DIG, IGIValue);
+
+	func_end = jiffies_to_msecs(jiffies - func_start);
+	/* printk("%s noise_a = %d, noise_b = %d noise_all:%d (%d ms)\n", __func__, */
+	/* pDM_Odm->noise_level.noise[ODM_RF_PATH_A], */
+	/* pDM_Odm->noise_level.noise[ODM_RF_PATH_B], */
+	/* pDM_Odm->noise_level.noise_all, func_end); */
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("odm_DebugControlInbandNoise_Nseries() <==\n"));
+	return pDM_Odm->noise_level.noise_all;
+
+}
+
+s16 ODM_InbandNoise_Monitor(void *pDM_VOID, u8 bPauseDIG, u8 IGIValue, u32 max_time)
+{
+	return odm_InbandNoise_Monitor_NSeries(pDM_VOID, bPauseDIG, IGIValue, max_time);
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_NoiseMonitor.h linux/3rdparty/rtl8723bs/hal/odm_NoiseMonitor.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_NoiseMonitor.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_NoiseMonitor.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,47 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ *****************************************************************************/
+#ifndef	__ODMNOISEMONITOR_H__
+#define __ODMNOISEMONITOR_H__
+
+#define	ODM_MAX_CHANNEL_NUM					38/* 14+24 */
+struct noise_level {
+	/* u8 value_a, value_b; */
+	u8 value[MAX_RF_PATH];
+	/* s8 sval_a, sval_b; */
+	s8 sval[MAX_RF_PATH];
+
+	/* s32 noise_a = 0, noise_b = 0, sum_a = 0, sum_b = 0; */
+	/* s32 noise[ODM_RF_PATH_MAX]; */
+	s32 sum[MAX_RF_PATH];
+	/* u8 valid_cnt_a = 0, valid_cnt_b = 0, */
+	u8 valid[MAX_RF_PATH];
+	u8 valid_cnt[MAX_RF_PATH];
+
+};
+
+
+typedef struct _ODM_NOISE_MONITOR_ {
+	s8 noise[MAX_RF_PATH];
+	s16 noise_all;
+} ODM_NOISE_MONITOR;
+
+s16 ODM_InbandNoise_Monitor(
+	void *pDM_VOID,
+	u8 bPauseDIG,
+	u8 IGIValue,
+	u32 max_time
+);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_PathDiv.c linux/3rdparty/rtl8723bs/hal/odm_PathDiv.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_PathDiv.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_PathDiv.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,42 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+void odm_PathDiversityInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_PATH_DIV))
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_PATH_DIV,
+			ODM_DBG_LOUD,
+			("Return: Not Support PathDiv\n")
+		);
+}
+
+void odm_PathDiversity(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_PATH_DIV))
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_PATH_DIV,
+			ODM_DBG_LOUD,
+			("Return: Not Support PathDiv\n")
+		);
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_PathDiv.h linux/3rdparty/rtl8723bs/hal/odm_PathDiv.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_PathDiv.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_PathDiv.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,29 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODMPATHDIV_H__
+#define    __ODMPATHDIV_H__
+
+void
+odm_PathDiversityInit(
+	void *pDM_VOID
+	);
+
+void
+odm_PathDiversity(
+	void *pDM_VOID
+	);
+
+ #endif		 /* ifndef  __ODMPATHDIV_H__ */
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_precomp.h linux/3rdparty/rtl8723bs/hal/odm_precomp.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_precomp.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_precomp.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,60 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODM_PRECOMP_H__
+#define __ODM_PRECOMP_H__
+
+#include "odm_types.h"
+
+#define		TEST_FALG___		1
+
+/* 2 Config Flags and Structs - defined by each ODM Type */
+
+	/* include <basic_types.h> */
+	/* include <osdep_service.h> */
+	/* include <drv_types.h> */
+	/* include <rtw_byteorder.h> */
+	/* include <hal_intf.h> */
+#define BEAMFORMING_SUPPORT 0
+
+/* 2 Hardware Parameter Files */
+
+/* 2 OutSrc Header Files */
+
+#include "odm.h"
+#include "odm_HWConfig.h"
+#include "odm_debug.h"
+#include "odm_RegDefine11N.h"
+#include "odm_AntDiv.h"
+#include "odm_EdcaTurboCheck.h"
+#include "odm_DIG.h"
+#include "odm_PathDiv.h"
+#include "odm_DynamicBBPowerSaving.h"
+#include "odm_DynamicTxPower.h"
+#include "odm_CfoTracking.h"
+#include "odm_NoiseMonitor.h"
+#include "HalPhyRf.h"
+#include "HalPhyRf_8723B.h"/* for IQK, LCK, Power-tracking */
+#include "rtl8723b_hal.h"
+#include "odm_interface.h"
+#include "odm_reg.h"
+#include "HalHWImg8723B_MAC.h"
+#include "HalHWImg8723B_RF.h"
+#include "HalHWImg8723B_BB.h"
+#include "Hal8723BReg.h"
+#include "odm_RTL8723B.h"
+#include "odm_RegConfig8723B.h"
+
+#endif	/*  __ODM_PRECOMP_H__ */
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_RegConfig8723B.c linux/3rdparty/rtl8723bs/hal/odm_RegConfig8723B.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_RegConfig8723B.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_RegConfig8723B.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,257 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+void odm_ConfigRFReg_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Addr,
+	u32 Data,
+	ODM_RF_RADIO_PATH_E RF_PATH,
+	u32 RegAddr
+)
+{
+	if (Addr == 0xfe || Addr == 0xffe)
+		msleep(50);
+	else {
+		PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH, RegAddr, bRFRegOffsetMask, Data);
+		/*  Add 1us delay between BB/RF register setting. */
+		udelay(1);
+
+		/* For disable/enable test in high temperature, the B6 value will fail to fill. Suggestion by BB Stanley, 2013.06.25. */
+		if (Addr == 0xb6) {
+			u32 getvalue = 0;
+			u8 count = 0;
+
+			getvalue = PHY_QueryRFReg(
+				pDM_Odm->Adapter, RF_PATH, Addr, bMaskDWord
+			);
+
+			udelay(1);
+
+			while ((getvalue>>8) != (Data>>8)) {
+				count++;
+				PHY_SetRFReg(pDM_Odm->Adapter, RF_PATH, RegAddr, bRFRegOffsetMask, Data);
+				udelay(1);
+				getvalue = PHY_QueryRFReg(pDM_Odm->Adapter, RF_PATH, Addr, bMaskDWord);
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_INIT,
+					ODM_DBG_TRACE,
+					(
+						"===> ODM_ConfigRFWithHeaderFile: [B6] getvalue 0x%x, Data 0x%x, count %d\n",
+						getvalue,
+						Data,
+						count
+					)
+				);
+				if (count > 5)
+					break;
+			}
+		}
+
+		if (Addr == 0xb2) {
+			u32 getvalue = 0;
+			u8 count = 0;
+
+			getvalue = PHY_QueryRFReg(
+				pDM_Odm->Adapter, RF_PATH, Addr, bMaskDWord
+			);
+
+			udelay(1);
+
+			while (getvalue != Data) {
+				count++;
+				PHY_SetRFReg(
+					pDM_Odm->Adapter,
+					RF_PATH,
+					RegAddr,
+					bRFRegOffsetMask,
+					Data
+				);
+				udelay(1);
+				/* Do LCK againg */
+				PHY_SetRFReg(
+					pDM_Odm->Adapter,
+					RF_PATH,
+					0x18,
+					bRFRegOffsetMask,
+					0x0fc07
+				);
+				udelay(1);
+				getvalue = PHY_QueryRFReg(
+					pDM_Odm->Adapter, RF_PATH, Addr, bMaskDWord
+				);
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_INIT,
+					ODM_DBG_TRACE,
+					(
+						"===> ODM_ConfigRFWithHeaderFile: [B2] getvalue 0x%x, Data 0x%x, count %d\n",
+						getvalue,
+						Data,
+						count
+					)
+				);
+
+				if (count > 5)
+					break;
+			}
+		}
+	}
+}
+
+
+void odm_ConfigRF_RadioA_8723B(PDM_ODM_T pDM_Odm, u32 Addr, u32 Data)
+{
+	u32  content = 0x1000; /*  RF_Content: radioa_txt */
+	u32 maskforPhySet = (u32)(content&0xE000);
+
+	odm_ConfigRFReg_8723B(
+		pDM_Odm,
+		Addr,
+		Data,
+		ODM_RF_PATH_A,
+		Addr|maskforPhySet
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> ODM_ConfigRFWithHeaderFile: [RadioA] %08X %08X\n",
+			Addr,
+			Data
+		)
+	);
+}
+
+void odm_ConfigMAC_8723B(PDM_ODM_T pDM_Odm, u32 Addr, u8 Data)
+{
+	rtw_write8(pDM_Odm->Adapter, Addr, Data);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> ODM_ConfigMACWithHeaderFile: [MAC_REG] %08X %08X\n",
+			Addr,
+			Data
+		)
+	);
+}
+
+void odm_ConfigBB_AGC_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+)
+{
+	PHY_SetBBReg(pDM_Odm->Adapter, Addr, Bitmask, Data);
+	/*  Add 1us delay between BB/RF register setting. */
+	udelay(1);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> ODM_ConfigBBWithHeaderFile: [AGC_TAB] %08X %08X\n",
+			Addr,
+			Data
+		)
+	);
+}
+
+void odm_ConfigBB_PHY_REG_PG_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Band,
+	u32 RfPath,
+	u32 TxNum,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+)
+{
+	if (Addr == 0xfe || Addr == 0xffe)
+		msleep(50);
+	else {
+		PHY_StoreTxPowerByRate(pDM_Odm->Adapter, Band, RfPath, TxNum, Addr, Bitmask, Data);
+	}
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		(
+			"===> ODM_ConfigBBWithHeaderFile: [PHY_REG] %08X %08X %08X\n",
+			Addr,
+			Bitmask,
+			Data
+		)
+	);
+}
+
+void odm_ConfigBB_PHY_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+)
+{
+	if (Addr == 0xfe)
+		msleep(50);
+	else if (Addr == 0xfd)
+		mdelay(5);
+	else if (Addr == 0xfc)
+		mdelay(1);
+	else if (Addr == 0xfb)
+		udelay(50);
+	else if (Addr == 0xfa)
+		udelay(5);
+	else if (Addr == 0xf9)
+		udelay(1);
+	else {
+		PHY_SetBBReg(pDM_Odm->Adapter, Addr, Bitmask, Data);
+	}
+
+	/*  Add 1us delay between BB/RF register setting. */
+	udelay(1);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_TRACE, ("===> ODM_ConfigBBWithHeaderFile: [PHY_REG] %08X %08X\n", Addr, Data));
+}
+
+void odm_ConfigBB_TXPWR_LMT_8723B(
+	PDM_ODM_T pDM_Odm,
+	u8 *Regulation,
+	u8 *Band,
+	u8 *Bandwidth,
+	u8 *RateSection,
+	u8 *RfPath,
+	u8 *Channel,
+	u8 *PowerLimit
+)
+{
+	PHY_SetTxPowerLimit(
+		pDM_Odm->Adapter,
+		Regulation,
+		Band,
+		Bandwidth,
+		RateSection,
+		RfPath,
+		Channel,
+		PowerLimit
+	);
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_RegConfig8723B.h linux/3rdparty/rtl8723bs/hal/odm_RegConfig8723B.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_RegConfig8723B.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_RegConfig8723B.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,65 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#ifndef __INC_ODM_REGCONFIG_H_8723B
+#define __INC_ODM_REGCONFIG_H_8723B
+
+void odm_ConfigRFReg_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Addr,
+	u32 Data,
+	ODM_RF_RADIO_PATH_E RF_PATH,
+	u32 RegAddr
+);
+
+void odm_ConfigRF_RadioA_8723B(PDM_ODM_T pDM_Odm, u32 Addr, u32 Data);
+
+void odm_ConfigMAC_8723B(PDM_ODM_T pDM_Odm, u32 Addr, u8 Data);
+
+void odm_ConfigBB_AGC_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+);
+
+void odm_ConfigBB_PHY_REG_PG_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Band,
+	u32 RfPath,
+	u32 TxNum,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+);
+
+void odm_ConfigBB_PHY_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+);
+
+void odm_ConfigBB_TXPWR_LMT_8723B(
+	PDM_ODM_T pDM_Odm,
+	u8 *Regulation,
+	u8 *Band,
+	u8 *Bandwidth,
+	u8 *RateSection,
+	u8 *RfPath,
+	u8 *Channel,
+	u8 *PowerLimit
+);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_RegDefine11N.h linux/3rdparty/rtl8723bs/hal/odm_RegDefine11N.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_RegDefine11N.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_RegDefine11N.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,172 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#ifndef	__ODM_REGDEFINE11N_H__
+#define __ODM_REGDEFINE11N_H__
+
+
+/* 2 RF REG LIST */
+#define	ODM_REG_RF_MODE_11N				0x00
+#define	ODM_REG_RF_0B_11N				0x0B
+#define	ODM_REG_CHNBW_11N				0x18
+#define	ODM_REG_T_METER_11N				0x24
+#define	ODM_REG_RF_25_11N				0x25
+#define	ODM_REG_RF_26_11N				0x26
+#define	ODM_REG_RF_27_11N				0x27
+#define	ODM_REG_RF_2B_11N				0x2B
+#define	ODM_REG_RF_2C_11N				0x2C
+#define	ODM_REG_RXRF_A3_11N				0x3C
+#define	ODM_REG_T_METER_92D_11N			0x42
+#define	ODM_REG_T_METER_88E_11N			0x42
+
+/* 2 BB REG LIST */
+/* PAGE 8 */
+#define	ODM_REG_BB_CTRL_11N				0x800
+#define	ODM_REG_RF_PIN_11N				0x804
+#define	ODM_REG_PSD_CTRL_11N				0x808
+#define	ODM_REG_TX_ANT_CTRL_11N			0x80C
+#define	ODM_REG_BB_PWR_SAV5_11N		0x818
+#define	ODM_REG_CCK_RPT_FORMAT_11N		0x824
+#define	ODM_REG_RX_DEFUALT_A_11N		0x858
+#define	ODM_REG_RX_DEFUALT_B_11N		0x85A
+#define	ODM_REG_BB_PWR_SAV3_11N		0x85C
+#define	ODM_REG_ANTSEL_CTRL_11N			0x860
+#define	ODM_REG_RX_ANT_CTRL_11N			0x864
+#define	ODM_REG_PIN_CTRL_11N				0x870
+#define	ODM_REG_BB_PWR_SAV1_11N		0x874
+#define	ODM_REG_ANTSEL_PATH_11N			0x878
+#define	ODM_REG_BB_3WIRE_11N			0x88C
+#define	ODM_REG_SC_CNT_11N				0x8C4
+#define	ODM_REG_PSD_DATA_11N				0x8B4
+#define	ODM_REG_PSD_DATA_11N				0x8B4
+#define	ODM_REG_NHM_TIMER_11N			0x894
+#define	ODM_REG_NHM_TH9_TH10_11N		0x890
+#define	ODM_REG_NHM_TH3_TO_TH0_11N		0x898
+#define	ODM_REG_NHM_TH7_TO_TH4_11N		0x89c
+#define	ODM_REG_NHM_CNT_11N				0x8d8
+/* PAGE 9 */
+#define	ODM_REG_DBG_RPT_11N				0x908
+#define	ODM_REG_ANT_MAPPING1_11N		0x914
+#define	ODM_REG_ANT_MAPPING2_11N		0x918
+/* PAGE A */
+#define	ODM_REG_CCK_ANTDIV_PARA1_11N	0xA00
+#define	ODM_REG_CCK_CCA_11N				0xA0A
+#define	ODM_REG_CCK_ANTDIV_PARA2_11N	0xA0C
+#define	ODM_REG_CCK_ANTDIV_PARA3_11N	0xA10
+#define	ODM_REG_CCK_ANTDIV_PARA4_11N	0xA14
+#define	ODM_REG_CCK_FILTER_PARA1_11N	0xA22
+#define	ODM_REG_CCK_FILTER_PARA2_11N	0xA23
+#define	ODM_REG_CCK_FILTER_PARA3_11N	0xA24
+#define	ODM_REG_CCK_FILTER_PARA4_11N	0xA25
+#define	ODM_REG_CCK_FILTER_PARA5_11N	0xA26
+#define	ODM_REG_CCK_FILTER_PARA6_11N	0xA27
+#define	ODM_REG_CCK_FILTER_PARA7_11N	0xA28
+#define	ODM_REG_CCK_FILTER_PARA8_11N	0xA29
+#define	ODM_REG_CCK_FA_RST_11N			0xA2C
+#define	ODM_REG_CCK_FA_MSB_11N			0xA58
+#define	ODM_REG_CCK_FA_LSB_11N			0xA5C
+#define	ODM_REG_CCK_CCA_CNT_11N			0xA60
+#define	ODM_REG_BB_PWR_SAV4_11N		0xA74
+/* PAGE B */
+#define	ODM_REG_LNA_SWITCH_11N			0xB2C
+#define	ODM_REG_PATH_SWITCH_11N			0xB30
+#define	ODM_REG_RSSI_CTRL_11N			0xB38
+#define	ODM_REG_CONFIG_ANTA_11N			0xB68
+#define	ODM_REG_RSSI_BT_11N				0xB9C
+/* PAGE C */
+#define	ODM_REG_OFDM_FA_HOLDC_11N		0xC00
+#define	ODM_REG_BB_RX_PATH_11N			0xC04
+#define	ODM_REG_TRMUX_11N				0xC08
+#define	ODM_REG_OFDM_FA_RSTC_11N		0xC0C
+#define	ODM_REG_RXIQI_MATRIX_11N			0xC14
+#define	ODM_REG_TXIQK_MATRIX_LSB1_11N	0xC4C
+#define	ODM_REG_IGI_A_11N					0xC50
+#define	ODM_REG_ANTDIV_PARA2_11N		0xC54
+#define	ODM_REG_IGI_B_11N					0xC58
+#define	ODM_REG_ANTDIV_PARA3_11N		0xC5C
+#define   ODM_REG_L1SBD_PD_CH_11N			0XC6C
+#define	ODM_REG_BB_PWR_SAV2_11N		0xC70
+#define	ODM_REG_RX_OFF_11N				0xC7C
+#define	ODM_REG_TXIQK_MATRIXA_11N		0xC80
+#define	ODM_REG_TXIQK_MATRIXB_11N		0xC88
+#define	ODM_REG_TXIQK_MATRIXA_LSB2_11N	0xC94
+#define	ODM_REG_TXIQK_MATRIXB_LSB2_11N	0xC9C
+#define	ODM_REG_RXIQK_MATRIX_LSB_11N	0xCA0
+#define	ODM_REG_ANTDIV_PARA1_11N		0xCA4
+#define	ODM_REG_OFDM_FA_TYPE1_11N		0xCF0
+/* PAGE D */
+#define	ODM_REG_OFDM_FA_RSTD_11N		0xD00
+#define	ODM_REG_BB_ATC_11N				0xD2C
+#define	ODM_REG_OFDM_FA_TYPE2_11N		0xDA0
+#define	ODM_REG_OFDM_FA_TYPE3_11N		0xDA4
+#define	ODM_REG_OFDM_FA_TYPE4_11N		0xDA8
+#define	ODM_REG_RPT_11N					0xDF4
+/* PAGE E */
+#define	ODM_REG_TXAGC_A_6_18_11N		0xE00
+#define	ODM_REG_TXAGC_A_24_54_11N		0xE04
+#define	ODM_REG_TXAGC_A_1_MCS32_11N	0xE08
+#define	ODM_REG_TXAGC_A_MCS0_3_11N		0xE10
+#define	ODM_REG_TXAGC_A_MCS4_7_11N		0xE14
+#define	ODM_REG_TXAGC_A_MCS8_11_11N	0xE18
+#define	ODM_REG_TXAGC_A_MCS12_15_11N	0xE1C
+#define	ODM_REG_FPGA0_IQK_11N			0xE28
+#define	ODM_REG_TXIQK_TONE_A_11N		0xE30
+#define	ODM_REG_RXIQK_TONE_A_11N		0xE34
+#define	ODM_REG_TXIQK_PI_A_11N			0xE38
+#define	ODM_REG_RXIQK_PI_A_11N			0xE3C
+#define	ODM_REG_TXIQK_11N				0xE40
+#define	ODM_REG_RXIQK_11N				0xE44
+#define	ODM_REG_IQK_AGC_PTS_11N			0xE48
+#define	ODM_REG_IQK_AGC_RSP_11N			0xE4C
+#define	ODM_REG_BLUETOOTH_11N			0xE6C
+#define	ODM_REG_RX_WAIT_CCA_11N			0xE70
+#define	ODM_REG_TX_CCK_RFON_11N			0xE74
+#define	ODM_REG_TX_CCK_BBON_11N			0xE78
+#define	ODM_REG_OFDM_RFON_11N			0xE7C
+#define	ODM_REG_OFDM_BBON_11N			0xE80
+#define		ODM_REG_TX2RX_11N				0xE84
+#define	ODM_REG_TX2TX_11N				0xE88
+#define	ODM_REG_RX_CCK_11N				0xE8C
+#define	ODM_REG_RX_OFDM_11N				0xED0
+#define	ODM_REG_RX_WAIT_RIFS_11N		0xED4
+#define	ODM_REG_RX2RX_11N				0xED8
+#define	ODM_REG_STANDBY_11N				0xEDC
+#define	ODM_REG_SLEEP_11N				0xEE0
+#define	ODM_REG_PMPD_ANAEN_11N			0xEEC
+#define	ODM_REG_IGI_C_11N					0xF84
+#define	ODM_REG_IGI_D_11N					0xF88
+
+/* 2 MAC REG LIST */
+#define	ODM_REG_BB_RST_11N				0x02
+#define	ODM_REG_ANTSEL_PIN_11N			0x4C
+#define	ODM_REG_EARLY_MODE_11N			0x4D0
+#define	ODM_REG_RSSI_MONITOR_11N		0x4FE
+#define	ODM_REG_EDCA_VO_11N				0x500
+#define	ODM_REG_EDCA_VI_11N				0x504
+#define	ODM_REG_EDCA_BE_11N				0x508
+#define	ODM_REG_EDCA_BK_11N				0x50C
+#define	ODM_REG_TXPAUSE_11N				0x522
+#define	ODM_REG_RESP_TX_11N				0x6D8
+#define	ODM_REG_ANT_TRAIN_PARA1_11N		0x7b0
+#define	ODM_REG_ANT_TRAIN_PARA2_11N		0x7b4
+
+
+/* DIG Related */
+#define	ODM_BIT_IGI_11N					0x0000007F
+#define	ODM_BIT_CCK_RPT_FORMAT_11N		BIT9
+#define	ODM_BIT_BB_RX_PATH_11N			0xF
+#define	ODM_BIT_BB_ATC_11N				BIT11
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_reg.h linux/3rdparty/rtl8723bs/hal/odm_reg.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_reg.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_reg.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,103 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+/*  File Name: odm_reg.h */
+/*  Description: */
+/*  This file is for general register definition. */
+#ifndef	__HAL_ODM_REG_H__
+#define __HAL_ODM_REG_H__
+
+/*  Register Definition */
+
+/* MAC REG */
+#define	ODM_BB_RESET				0x002
+#define	ODM_DUMMY				0x4fe
+#define	RF_T_METER_OLD				0x24
+#define	RF_T_METER_NEW				0x42
+
+#define	ODM_EDCA_VO_PARAM			0x500
+#define	ODM_EDCA_VI_PARAM			0x504
+#define	ODM_EDCA_BE_PARAM			0x508
+#define	ODM_EDCA_BK_PARAM			0x50C
+#define	ODM_TXPAUSE				0x522
+
+/* BB REG */
+#define	ODM_FPGA_PHY0_PAGE8			0x800
+#define	ODM_PSD_SETTING				0x808
+#define	ODM_AFE_SETTING				0x818
+#define	ODM_TXAGC_B_24_54			0x834
+#define	ODM_TXAGC_B_MCS32_5			0x838
+#define	ODM_TXAGC_B_MCS0_MCS3			0x83c
+#define	ODM_TXAGC_B_MCS4_MCS7			0x848
+#define	ODM_TXAGC_B_MCS8_MCS11			0x84c
+#define	ODM_ANALOG_REGISTER			0x85c
+#define	ODM_RF_INTERFACE_OUTPUT			0x860
+#define	ODM_TXAGC_B_MCS12_MCS15			0x868
+#define	ODM_TXAGC_B_11_A_2_11			0x86c
+#define	ODM_AD_DA_LSB_MASK			0x874
+#define	ODM_ENABLE_3_WIRE			0x88c
+#define	ODM_PSD_REPORT				0x8b4
+#define	ODM_R_ANT_SELECT			0x90c
+#define	ODM_CCK_ANT_SELECT			0xa07
+#define	ODM_CCK_PD_THRESH			0xa0a
+#define	ODM_CCK_RF_REG1				0xa11
+#define	ODM_CCK_MATCH_FILTER			0xa20
+#define	ODM_CCK_RAKE_MAC			0xa2e
+#define	ODM_CCK_CNT_RESET			0xa2d
+#define	ODM_CCK_TX_DIVERSITY			0xa2f
+#define	ODM_CCK_FA_CNT_MSB			0xa5b
+#define	ODM_CCK_FA_CNT_LSB			0xa5c
+#define	ODM_CCK_NEW_FUNCTION			0xa75
+#define	ODM_OFDM_PHY0_PAGE_C			0xc00
+#define	ODM_OFDM_RX_ANT				0xc04
+#define	ODM_R_A_RXIQI				0xc14
+#define	ODM_R_A_AGC_CORE1			0xc50
+#define	ODM_R_A_AGC_CORE2			0xc54
+#define	ODM_R_B_AGC_CORE1			0xc58
+#define	ODM_R_AGC_PAR				0xc70
+#define	ODM_R_HTSTF_AGC_PAR			0xc7c
+#define	ODM_TX_PWR_TRAINING_A			0xc90
+#define	ODM_TX_PWR_TRAINING_B			0xc98
+#define	ODM_OFDM_FA_CNT1			0xcf0
+#define	ODM_OFDM_PHY0_PAGE_D			0xd00
+#define	ODM_OFDM_FA_CNT2			0xda0
+#define	ODM_OFDM_FA_CNT3			0xda4
+#define	ODM_OFDM_FA_CNT4			0xda8
+#define	ODM_TXAGC_A_6_18			0xe00
+#define	ODM_TXAGC_A_24_54			0xe04
+#define	ODM_TXAGC_A_1_MCS32			0xe08
+#define	ODM_TXAGC_A_MCS0_MCS3			0xe10
+#define	ODM_TXAGC_A_MCS4_MCS7			0xe14
+#define	ODM_TXAGC_A_MCS8_MCS11			0xe18
+#define	ODM_TXAGC_A_MCS12_MCS15			0xe1c
+
+/* RF REG */
+#define	ODM_GAIN_SETTING			0x00
+#define	ODM_CHANNEL				0x18
+
+/* Ant Detect Reg */
+#define	ODM_DPDT				0x300
+
+/* PSD Init */
+#define	ODM_PSDREG				0x808
+
+/* 92D Path Div */
+#define	PATHDIV_REG				0xB30
+#define	PATHDIV_TRI				0xBA0
+
+/*  Bitmap Definition */
+
+#define	BIT_FA_RESET				BIT0
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_RTL8723B.c linux/3rdparty/rtl8723bs/hal/odm_RTL8723B.c
--- linux.old/3rdparty/rtl8723bs/hal/odm_RTL8723B.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_RTL8723B.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,45 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+
+#include "odm_precomp.h"
+
+s8 odm_CCKRSSI_8723B(u8 LNA_idx, u8 VGA_idx)
+{
+	s8 rx_pwr_all = 0x00;
+
+	switch (LNA_idx) {
+	/* 46  53 73 95 201301231630 */
+	/*  46 53 77 99 201301241630 */
+
+	case 6:
+		rx_pwr_all = -34 - (2 * VGA_idx);
+		break;
+	case 4:
+		rx_pwr_all = -14 - (2 * VGA_idx);
+		break;
+	case 1:
+		rx_pwr_all = 6 - (2 * VGA_idx);
+		break;
+	case 0:
+		rx_pwr_all = 16 - (2 * VGA_idx);
+		break;
+	default:
+		/* rx_pwr_all = -53+(2*(31-VGA_idx)); */
+		/* DbgPrint("wrong LNA index\n"); */
+		break;
+
+	}
+	return rx_pwr_all;
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_RTL8723B.h linux/3rdparty/rtl8723bs/hal/odm_RTL8723B.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_RTL8723B.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_RTL8723B.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,22 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#ifndef	__ODM_RTL8723B_H__
+#define __ODM_RTL8723B_H__
+
+#define	DM_DIG_MIN_NIC_8723	0x1C
+
+s8 odm_CCKRSSI_8723B(u8 LNA_idx, u8 VGA_idx);
+
+#endif
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/odm_types.h linux/3rdparty/rtl8723bs/hal/odm_types.h
--- linux.old/3rdparty/rtl8723bs/hal/odm_types.h	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/odm_types.h	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,102 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ ******************************************************************************/
+#ifndef __ODM_TYPES_H__
+#define __ODM_TYPES_H__
+
+#include <drv_types.h>
+
+/*  Deifne HW endian support */
+#define	ODM_ENDIAN_BIG	0
+#define	ODM_ENDIAN_LITTLE	1
+
+#define GET_ODM(__padapter)	((PDM_ODM_T)(&((GET_HAL_DATA(__padapter))->odmpriv)))
+
+typedef enum _HAL_STATUS {
+	HAL_STATUS_SUCCESS,
+	HAL_STATUS_FAILURE,
+	/*RT_STATUS_PENDING,
+	RT_STATUS_RESOURCE,
+	RT_STATUS_INVALID_CONTEXT,
+	RT_STATUS_INVALID_PARAMETER,
+	RT_STATUS_NOT_SUPPORT,
+	RT_STATUS_OS_API_FAILED,*/
+} HAL_STATUS, *PHAL_STATUS;
+
+
+/*  */
+/*  Declare for ODM spin lock defintion temporarily fro compile pass. */
+/*  */
+typedef enum _RT_SPINLOCK_TYPE {
+	RT_TX_SPINLOCK = 1,
+	RT_RX_SPINLOCK = 2,
+	RT_RM_SPINLOCK = 3,
+	RT_CAM_SPINLOCK = 4,
+	RT_SCAN_SPINLOCK = 5,
+	RT_LOG_SPINLOCK = 7,
+	RT_BW_SPINLOCK = 8,
+	RT_CHNLOP_SPINLOCK = 9,
+	RT_RF_OPERATE_SPINLOCK = 10,
+	RT_INITIAL_SPINLOCK = 11,
+	RT_RF_STATE_SPINLOCK = 12, /*  For RF state. Added by Bruce, 2007-10-30. */
+	/* Shall we define Ndis 6.2 SpinLock Here ? */
+	RT_PORT_SPINLOCK = 16,
+	RT_H2C_SPINLOCK = 20, /*  For H2C cmd. Added by tynli. 2009.11.09. */
+
+	RT_BTData_SPINLOCK = 25,
+
+	RT_WAPI_OPTION_SPINLOCK = 26,
+	RT_WAPI_RX_SPINLOCK = 27,
+
+	/*  add for 92D CCK control issue */
+	RT_CCK_PAGEA_SPINLOCK = 28,
+	RT_BUFFER_SPINLOCK = 29,
+	RT_CHANNEL_AND_BANDWIDTH_SPINLOCK = 30,
+	RT_GEN_TEMP_BUF_SPINLOCK = 31,
+	RT_AWB_SPINLOCK = 32,
+	RT_FW_PS_SPINLOCK = 33,
+	RT_HW_TIMER_SPIN_LOCK = 34,
+	RT_MPT_WI_SPINLOCK = 35,
+	RT_P2P_SPIN_LOCK = 36,	/*  Protect P2P context */
+	RT_DBG_SPIN_LOCK = 37,
+	RT_IQK_SPINLOCK = 38,
+	RT_PENDED_OID_SPINLOCK = 39,
+	RT_CHNLLIST_SPINLOCK = 40,
+	RT_INDIC_SPINLOCK = 41,	/* protect indication */
+} RT_SPINLOCK_TYPE;
+
+	#if defined(__LITTLE_ENDIAN)
+		#define	ODM_ENDIAN_TYPE			ODM_ENDIAN_LITTLE
+	#else
+		#define	ODM_ENDIAN_TYPE			ODM_ENDIAN_BIG
+	#endif
+
+	typedef struct timer_list		RT_TIMER, *PRT_TIMER;
+	typedef  void *RT_TIMER_CALL_BACK;
+	#define	STA_INFO_T			struct sta_info
+	#define	PSTA_INFO_T		struct sta_info *
+
+	#define SET_TX_DESC_ANTSEL_A_88E(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 24, 1, __Value)
+	#define SET_TX_DESC_ANTSEL_B_88E(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 25, 1, __Value)
+	#define SET_TX_DESC_ANTSEL_C_88E(__pTxDesc, __Value) SET_BITS_TO_LE_4BYTE(__pTxDesc+28, 29, 1, __Value)
+
+	/* define useless flag to avoid compile warning */
+	#define	USE_WORKITEM 0
+	#define   FPGA_TWO_MAC_VERIFICATION	0
+
+#define READ_NEXT_PAIR(v1, v2, i) do { if (i+2 >= ArrayLen) break; i += 2; v1 = Array[i]; v2 = Array[i+1]; } while (0)
+#define COND_ELSE  2
+#define COND_ENDIF 3
+
+#endif /*  __ODM_TYPES_H__ */
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/rtl8723b_cmd.c linux/3rdparty/rtl8723bs/hal/rtl8723b_cmd.c
--- linux.old/3rdparty/rtl8723bs/hal/rtl8723b_cmd.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/rtl8723b_cmd.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,2358 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+#define _RTL8723B_CMD_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+#include "hal_com_h2c.h"
+
+#define MAX_H2C_BOX_NUMS	4
+#define MESSAGE_BOX_SIZE	4
+
+#define RTL8723B_MAX_CMD_LEN	7
+#define RTL8723B_EX_MESSAGE_BOX_SIZE	4
+
+static u8 _is_fw_read_cmd_down(struct adapter *padapter, u8 msgbox_num)
+{
+	u8 read_down = false;
+	int retry_cnts = 100;
+
+	u8 valid;
+
+	/* DBG_8192C(" _is_fw_read_cmd_down , reg_1cc(%x), msg_box(%d)...\n", rtw_read8(padapter, REG_HMETFR), msgbox_num); */
+
+	do {
+		valid = rtw_read8(padapter, REG_HMETFR) & BIT(msgbox_num);
+		if (0 == valid) {
+			read_down = true;
+		}
+#ifdef CONFIG_WOWLAN
+		else
+			msleep(1);
+#endif
+	} while ((!read_down) && (retry_cnts--));
+
+	return read_down;
+
+}
+
+
+/*****************************************
+* H2C Msg format :
+*| 31 - 8		|7-5	| 4 - 0	|
+*| h2c_msg	|Class	|CMD_ID	|
+*| 31-0						|
+*| Ext msg					|
+*
+******************************************/
+s32 FillH2CCmd8723B(struct adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer)
+{
+	u8 h2c_box_num;
+	u32 msgbox_addr;
+	u32 msgbox_ex_addr = 0;
+	struct hal_com_data *pHalData;
+	u32 h2c_cmd = 0;
+	u32 h2c_cmd_ex = 0;
+	s32 ret = _FAIL;
+
+	padapter = GET_PRIMARY_ADAPTER(padapter);
+	pHalData = GET_HAL_DATA(padapter);
+	if (mutex_lock_interruptible(&(adapter_to_dvobj(padapter)->h2c_fwcmd_mutex)))
+		return ret;
+
+	if (!pCmdBuffer) {
+		goto exit;
+	}
+
+	if (CmdLen > RTL8723B_MAX_CMD_LEN) {
+		goto exit;
+	}
+
+	if (padapter->bSurpriseRemoved == true)
+		goto exit;
+
+	/* pay attention to if  race condition happened in  H2C cmd setting. */
+	do {
+		h2c_box_num = pHalData->LastHMEBoxNum;
+
+		if (!_is_fw_read_cmd_down(padapter, h2c_box_num)) {
+			DBG_8192C(" fw read cmd failed...\n");
+			/* DBG_8192C(" 0x1c0: 0x%8x\n", rtw_read32(padapter, 0x1c0)); */
+			/* DBG_8192C(" 0x1c4: 0x%8x\n", rtw_read32(padapter, 0x1c4)); */
+			goto exit;
+		}
+
+		if (CmdLen <= 3)
+			memcpy((u8 *)(&h2c_cmd)+1, pCmdBuffer, CmdLen);
+		else {
+			memcpy((u8 *)(&h2c_cmd)+1, pCmdBuffer, 3);
+			memcpy((u8 *)(&h2c_cmd_ex), pCmdBuffer+3, CmdLen-3);
+/* 			*(u8 *)(&h2c_cmd) |= BIT(7); */
+		}
+
+		*(u8 *)(&h2c_cmd) |= ElementID;
+
+		if (CmdLen > 3) {
+			msgbox_ex_addr = REG_HMEBOX_EXT0_8723B + (h2c_box_num*RTL8723B_EX_MESSAGE_BOX_SIZE);
+			rtw_write32(padapter, msgbox_ex_addr, h2c_cmd_ex);
+		}
+		msgbox_addr = REG_HMEBOX_0 + (h2c_box_num*MESSAGE_BOX_SIZE);
+		rtw_write32(padapter, msgbox_addr, h2c_cmd);
+
+		/* DBG_8192C("MSG_BOX:%d, CmdLen(%d), CmdID(0x%x), reg:0x%x =>h2c_cmd:0x%.8x, reg:0x%x =>h2c_cmd_ex:0x%.8x\n" */
+		/* 	, pHalData->LastHMEBoxNum , CmdLen, ElementID, msgbox_addr, h2c_cmd, msgbox_ex_addr, h2c_cmd_ex); */
+
+		pHalData->LastHMEBoxNum = (h2c_box_num+1) % MAX_H2C_BOX_NUMS;
+
+	} while (0);
+
+	ret = _SUCCESS;
+
+exit:
+
+	mutex_unlock(&(adapter_to_dvobj(padapter)->h2c_fwcmd_mutex));
+	return ret;
+}
+
+static void ConstructBeacon(struct adapter *padapter, u8 *pframe, u32 *pLength)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	u32 rate_len, pktlen;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+
+	/* DBG_871X("%s\n", __func__); */
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
+	/* pmlmeext->mgnt_seq++; */
+	SetFrameSubType(pframe, WIFI_BEACON);
+
+	pframe += sizeof(struct ieee80211_hdr_3addr);
+	pktlen = sizeof (struct ieee80211_hdr_3addr);
+
+	/* timestamp will be inserted by hardware */
+	pframe += 8;
+	pktlen += 8;
+
+	/*  beacon interval: 2 bytes */
+	memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
+
+	pframe += 2;
+	pktlen += 2;
+
+	/*  capability info: 2 bytes */
+	memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
+
+	pframe += 2;
+	pktlen += 2;
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
+		/* DBG_871X("ie len =%d\n", cur_network->IELength); */
+		pktlen += cur_network->IELength - sizeof(struct ndis_802_11_fix_ie);
+		memcpy(pframe, cur_network->IEs+sizeof(struct ndis_802_11_fix_ie), pktlen);
+
+		goto _ConstructBeacon;
+	}
+
+	/* below for ad-hoc mode */
+
+	/*  SSID */
+	pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &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, &pktlen);
+
+	/*  DS parameter set */
+	pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pktlen);
+
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+		u32 ATIMWindow;
+		/*  IBSS Parameter Set... */
+		/* ATIMWindow = cur->Configuration.ATIMWindow; */
+		ATIMWindow = 0;
+		pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pktlen);
+	}
+
+
+	/* todo: ERP IE */
+
+
+	/*  EXTERNDED SUPPORTED RATE */
+	if (rate_len > 8)
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pktlen);
+
+
+	/* todo:HT for adhoc */
+
+_ConstructBeacon:
+
+	if ((pktlen + TXDESC_SIZE) > 512) {
+		DBG_871X("beacon frame too large\n");
+		return;
+	}
+
+	*pLength = pktlen;
+
+	/* DBG_871X("%s bcn_sz =%d\n", __func__, pktlen); */
+
+}
+
+static void ConstructPSPoll(struct adapter *padapter, u8 *pframe, u32 *pLength)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	/* DBG_871X("%s\n", __func__); */
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	/*  Frame control. */
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+	SetPwrMgt(fctrl);
+	SetFrameSubType(pframe, WIFI_PSPOLL);
+
+	/*  AID. */
+	SetDuration(pframe, (pmlmeinfo->aid | 0xc000));
+
+	/*  BSSID. */
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	/*  TA. */
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+
+	*pLength = 16;
+}
+
+static void ConstructNullFunctionData(
+	struct adapter *padapter,
+	u8 *pframe,
+	u32 *pLength,
+	u8 *StaAddr,
+	u8 bQoS,
+	u8 AC,
+	u8 bEosp,
+	u8 bForcePowerSave
+)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	u32 pktlen;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *cur_network = &pmlmepriv->cur_network;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+
+	/* DBG_871X("%s:%d\n", __func__, bForcePowerSave); */
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+	if (bForcePowerSave)
+		SetPwrMgt(fctrl);
+
+	switch (cur_network->network.InfrastructureMode) {
+	case Ndis802_11Infrastructure:
+		SetToDs(fctrl);
+		memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, StaAddr, ETH_ALEN);
+		break;
+	case Ndis802_11APMode:
+		SetFrDs(fctrl);
+		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		break;
+	case Ndis802_11IBSS:
+	default:
+		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
+		memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+		break;
+	}
+
+	SetSeqNum(pwlanhdr, 0);
+
+	if (bQoS == true) {
+		struct ieee80211_qos_hdr *pwlanqoshdr;
+
+		SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+
+		pwlanqoshdr = (struct ieee80211_qos_hdr *)pframe;
+		SetPriority(&pwlanqoshdr->qos_ctrl, AC);
+		SetEOSP(&pwlanqoshdr->qos_ctrl, bEosp);
+
+		pktlen = sizeof(struct ieee80211_qos_hdr);
+	} else {
+		SetFrameSubType(pframe, WIFI_DATA_NULL);
+
+		pktlen = sizeof(struct ieee80211_hdr_3addr);
+	}
+
+	*pLength = pktlen;
+}
+
+
+#ifdef CONFIG_WOWLAN
+/*  */
+/*  Description: */
+/* 	Construct the ARP response packet to support ARP offload. */
+/*  */
+static void ConstructARPResponse(
+	struct adapter *padapter,
+	u8 *pframe,
+	u32 *pLength,
+	u8 *pIPAddress
+)
+{
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	static u8 	ARPLLCHeader[8] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00, 0x08, 0x06};
+	u8 		*pARPRspPkt = pframe;
+	/* for TKIP Cal MIC */
+	u8 		*payload = pframe;
+	u8 	EncryptionHeadOverhead = 0;
+	/* DBG_871X("%s:%d\n", __func__, bForcePowerSave); */
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	/*  */
+	/*  MAC Header. */
+	/*  */
+	SetFrameType(fctrl, WIFI_DATA);
+	/* SetFrameSubType(fctrl, 0); */
+	SetToDs(fctrl);
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0);
+	SetDuration(pwlanhdr, 0);
+	/* SET_80211_HDR_FRAME_CONTROL(pARPRspPkt, 0); */
+	/* SET_80211_HDR_TYPE_AND_SUBTYPE(pARPRspPkt, Type_Data); */
+	/* SET_80211_HDR_TO_DS(pARPRspPkt, 1); */
+	/* SET_80211_HDR_ADDRESS1(pARPRspPkt, pMgntInfo->Bssid); */
+	/* SET_80211_HDR_ADDRESS2(pARPRspPkt, Adapter->CurrentAddress); */
+	/* SET_80211_HDR_ADDRESS3(pARPRspPkt, pMgntInfo->Bssid); */
+
+	/* SET_80211_HDR_DURATION(pARPRspPkt, 0); */
+	/* SET_80211_HDR_FRAGMENT_SEQUENCE(pARPRspPkt, 0); */
+	*pLength = 24;
+
+	/*  */
+	/*  Security Header: leave space for it if necessary. */
+	/*  */
+
+	switch (psecuritypriv->dot11PrivacyAlgrthm) {
+	case _WEP40_:
+	case _WEP104_:
+		EncryptionHeadOverhead = 4;
+		break;
+	case _TKIP_:
+		EncryptionHeadOverhead = 8;
+		break;
+	case _AES_:
+		EncryptionHeadOverhead = 8;
+		break;
+	default:
+		EncryptionHeadOverhead = 0;
+	}
+
+	if (EncryptionHeadOverhead > 0) {
+		memset(&(pframe[*pLength]), 0, EncryptionHeadOverhead);
+		*pLength += EncryptionHeadOverhead;
+		SetPrivacy(fctrl);
+	}
+
+	/*  */
+	/*  Frame Body. */
+	/*  */
+	pARPRspPkt = (u8 *)(pframe + *pLength);
+	payload = pARPRspPkt; /* Get Payload pointer */
+	/*  LLC header */
+	memcpy(pARPRspPkt, ARPLLCHeader, 8);
+	*pLength += 8;
+
+	/*  ARP element */
+	pARPRspPkt += 8;
+	SET_ARP_PKT_HW(pARPRspPkt, 0x0100);
+	SET_ARP_PKT_PROTOCOL(pARPRspPkt, 0x0008);	/*  IP protocol */
+	SET_ARP_PKT_HW_ADDR_LEN(pARPRspPkt, 6);
+	SET_ARP_PKT_PROTOCOL_ADDR_LEN(pARPRspPkt, 4);
+	SET_ARP_PKT_OPERATION(pARPRspPkt, 0x0200); /*  ARP response */
+	SET_ARP_PKT_SENDER_MAC_ADDR(pARPRspPkt, myid(&(padapter->eeprompriv)));
+	SET_ARP_PKT_SENDER_IP_ADDR(pARPRspPkt, pIPAddress);
+	{
+		SET_ARP_PKT_TARGET_MAC_ADDR(pARPRspPkt, get_my_bssid(&(pmlmeinfo->network)));
+		SET_ARP_PKT_TARGET_IP_ADDR(pARPRspPkt, pIPAddress);
+		DBG_871X("%s Target Mac Addr:" MAC_FMT "\n", __func__, MAC_ARG(get_my_bssid(&(pmlmeinfo->network))));
+		DBG_871X("%s Target IP Addr" IP_FMT "\n", __func__, IP_ARG(pIPAddress));
+	}
+
+	*pLength += 28;
+
+	if (psecuritypriv->dot11PrivacyAlgrthm == _TKIP_) {
+		u8 mic[8];
+		struct mic_data	micdata;
+		struct sta_info *psta = NULL;
+		u8 priority[4] = {
+			0x0, 0x0, 0x0, 0x0
+		};
+		u8 null_key[16] = {
+			0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+			0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0
+		};
+
+		DBG_871X("%s(): Add MIC\n", __func__);
+
+		psta = rtw_get_stainfo(&padapter->stapriv, get_my_bssid(&(pmlmeinfo->network)));
+		if (psta != NULL) {
+			if (!memcmp(&psta->dot11tkiptxmickey.skey[0], null_key, 16)) {
+				DBG_871X("%s(): STA dot11tkiptxmickey == 0\n", __func__);
+			}
+			/* start to calculate the mic code */
+			rtw_secmicsetkey(&micdata, &psta->dot11tkiptxmickey.skey[0]);
+		}
+
+		rtw_secmicappend(&micdata, pwlanhdr->addr3, 6);  /* DA */
+
+		rtw_secmicappend(&micdata, pwlanhdr->addr2, 6); /* SA */
+
+		priority[0] = 0;
+		rtw_secmicappend(&micdata, &priority[0], 4);
+
+		rtw_secmicappend(&micdata, payload, 36); /* payload length = 8 + 28 */
+
+		rtw_secgetmic(&micdata, &(mic[0]));
+
+		pARPRspPkt += 28;
+		memcpy(pARPRspPkt, &(mic[0]), 8);
+
+		*pLength += 8;
+	}
+}
+
+#ifdef CONFIG_PNO_SUPPORT
+static void ConstructPnoInfo(
+	struct adapter *padapter, u8 *pframe, u32 *pLength
+)
+{
+
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+
+	u8 *pPnoInfoPkt = pframe;
+	pPnoInfoPkt = (u8 *)(pframe + *pLength);
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->ssid_num, 4);
+
+	*pLength += 4;
+	pPnoInfoPkt += 4;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->fast_scan_period, 4);
+
+	*pLength += 4;
+	pPnoInfoPkt += 4;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->fast_scan_iterations, 4);
+
+	*pLength += 4;
+	pPnoInfoPkt += 4;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->slow_scan_period, 4);
+
+	*pLength += 4;
+	pPnoInfoPkt += 4;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->ssid_length,
+			MAX_PNO_LIST_COUNT);
+
+	*pLength += MAX_PNO_LIST_COUNT;
+	pPnoInfoPkt += MAX_PNO_LIST_COUNT;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->ssid_cipher_info,
+			MAX_PNO_LIST_COUNT);
+
+	*pLength += MAX_PNO_LIST_COUNT;
+	pPnoInfoPkt += MAX_PNO_LIST_COUNT;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->ssid_channel_info,
+			MAX_PNO_LIST_COUNT);
+
+	*pLength += MAX_PNO_LIST_COUNT;
+	pPnoInfoPkt += MAX_PNO_LIST_COUNT;
+}
+
+static void ConstructSSIDList(
+	struct adapter *padapter, u8 *pframe, u32 *pLength
+)
+{
+	int i = 0;
+	u8 *pSSIDListPkt = pframe;
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+
+	pSSIDListPkt = (u8 *)(pframe + *pLength);
+
+	for (i = 0; i < pwrctl->pnlo_info->ssid_num ; i++) {
+		memcpy(pSSIDListPkt, &pwrctl->pno_ssid_list->node[i].SSID,
+			pwrctl->pnlo_info->ssid_length[i]);
+
+		*pLength += WLAN_SSID_MAXLEN;
+		pSSIDListPkt += WLAN_SSID_MAXLEN;
+	}
+}
+
+static void ConstructScanInfo(
+	struct adapter *padapter, u8 *pframe, u32 *pLength
+)
+{
+	int i = 0;
+	u8 *pScanInfoPkt = pframe;
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+
+	pScanInfoPkt = (u8 *)(pframe + *pLength);
+
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->channel_num, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->orig_ch, 1);
+
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->orig_bw, 1);
+
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->orig_40_offset, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->orig_80_offset, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->periodScan, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->period_scan_time, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->enableRFE, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->rfe_type, 8);
+
+	*pLength += 8;
+	pScanInfoPkt += 8;
+
+	for (i = 0; i < MAX_SCAN_LIST_COUNT; i++) {
+		memcpy(pScanInfoPkt, &pwrctl->pscan_info->ssid_channel_info[i], 4);
+		*pLength += 4;
+		pScanInfoPkt += 4;
+	}
+}
+#endif
+
+#ifdef CONFIG_GTK_OL
+static void ConstructGTKResponse(
+	struct adapter *padapter, u8 *pframe, u32 *pLength
+)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	u16 *fctrl;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	static u8 LLCHeader[8] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00, 0x88, 0x8E};
+	static u8 GTKbody_a[11] = {0x01, 0x03, 0x00, 0x5F, 0x02, 0x03, 0x12, 0x00, 0x10, 0x42, 0x0B};
+	u8 *pGTKRspPkt = pframe;
+	u8 EncryptionHeadOverhead = 0;
+	/* DBG_871X("%s:%d\n", __func__, bForcePowerSave); */
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	/*  */
+	/*  MAC Header. */
+	/*  */
+	SetFrameType(fctrl, WIFI_DATA);
+	/* SetFrameSubType(fctrl, 0); */
+	SetToDs(fctrl);
+	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0);
+	SetDuration(pwlanhdr, 0);
+
+	*pLength = 24;
+
+	/*  */
+	/*  Security Header: leave space for it if necessary. */
+	/*  */
+
+	switch (psecuritypriv->dot11PrivacyAlgrthm) {
+	case _WEP40_:
+	case _WEP104_:
+		EncryptionHeadOverhead = 4;
+		break;
+	case _TKIP_:
+		EncryptionHeadOverhead = 8;
+		break;
+	case _AES_:
+		EncryptionHeadOverhead = 8;
+		break;
+	default:
+		EncryptionHeadOverhead = 0;
+	}
+
+	if (EncryptionHeadOverhead > 0) {
+		memset(&(pframe[*pLength]), 0, EncryptionHeadOverhead);
+		*pLength += EncryptionHeadOverhead;
+		/* GTK's privacy bit is done by FW */
+		/* SetPrivacy(fctrl); */
+	}
+
+	/*  */
+	/*  Frame Body. */
+	/*  */
+	pGTKRspPkt =  (u8 *)(pframe + *pLength);
+	/*  LLC header */
+	memcpy(pGTKRspPkt, LLCHeader, 8);
+	*pLength += 8;
+
+	/*  GTK element */
+	pGTKRspPkt += 8;
+
+	/* GTK frame body after LLC, part 1 */
+	memcpy(pGTKRspPkt, GTKbody_a, 11);
+	*pLength += 11;
+	pGTKRspPkt += 11;
+	/* GTK frame body after LLC, part 2 */
+	memset(&(pframe[*pLength]), 0, 88);
+	*pLength += 88;
+	pGTKRspPkt += 88;
+
+}
+#endif /* CONFIG_GTK_OL */
+
+#ifdef CONFIG_PNO_SUPPORT
+static void ConstructProbeReq(struct adapter *padapter, u8 *pframe, u32 *pLength)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	u16 *fctrl;
+	u32 pktlen;
+	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};
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+	mac = myid(&(padapter->eeprompriv));
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	/* 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, 0);
+	SetFrameSubType(pframe, WIFI_PROBEREQ);
+
+	pktlen = sizeof(struct ieee80211_hdr_3addr);
+	pframe += pktlen;
+
+	pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &pktlen);
+
+	get_rate_set(padapter, bssrate, &bssrate_len);
+
+	if (bssrate_len > 8) {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, bssrate, &pktlen);
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (bssrate_len - 8), (bssrate + 8), &pktlen);
+	} else
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, bssrate_len, bssrate, &pktlen);
+
+	*pLength = pktlen;
+}
+#endif /* CONFIG_PNO_SUPPORT */
+#endif /* CONFIG_WOWLAN */
+
+#ifdef CONFIG_AP_WOWLAN
+static void ConstructProbeRsp(struct adapter *padapter, u8 *pframe, u32 *pLength, u8 *StaAddr, bool bHideSSID)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	u16 *fctrl;
+	u8 *mac, *bssid;
+	u32 pktlen;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+	u8 *pwps_ie;
+	uint wps_ielen;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	/* DBG_871X("%s\n", __func__); */
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	mac = myid(&(padapter->eeprompriv));
+	bssid = cur_network->MacAddress;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+	memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
+
+	DBG_871X("%s FW Mac Addr:" MAC_FMT "\n", __func__, MAC_ARG(mac));
+	DBG_871X("%s FW IP Addr" IP_FMT "\n", __func__, IP_ARG(StaAddr));
+
+	SetSeqNum(pwlanhdr, 0);
+	SetFrameSubType(fctrl, WIFI_PROBERSP);
+
+	pktlen = sizeof(struct ieee80211_hdr_3addr);
+	pframe += pktlen;
+
+	if (cur_network->IELength > MAX_IE_SZ)
+		return;
+
+	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;
+		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;
+			pktlen += wps_ielen+2;
+		}
+
+		if ((wps_offset+wps_ielen+2+remainder_ielen) <= MAX_IE_SZ) {
+			memcpy(pframe, premainder_ie, remainder_ielen);
+			pframe += remainder_ielen;
+			pktlen += remainder_ielen;
+		}
+	} else {
+		memcpy(pframe, cur_network->IEs, cur_network->IELength);
+		pframe += cur_network->IELength;
+		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 = pframe + sizeof(struct 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) {
+				DBG_871X_LEVEL(_drv_warning_, 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;
+			pktlen += ssid_ielen_diff;
+		}
+	}
+
+	*pLength = pktlen;
+
+}
+#endif /*  CONFIG_AP_WOWLAN */
+
+/*  To check if reserved page content is destroyed by beacon beacuse beacon is too large. */
+/*  2010.06.23. Added by tynli. */
+void CheckFwRsvdPageContent(struct adapter *Adapter)
+{
+}
+
+static void rtl8723b_set_FwRsvdPage_cmd(struct adapter *padapter, PRSVDPAGE_LOC rsvdpageloc)
+{
+	u8 u1H2CRsvdPageParm[H2C_RSVDPAGE_LOC_LEN] = {0};
+
+	DBG_871X("8723BRsvdPageLoc: ProbeRsp =%d PsPoll =%d Null =%d QoSNull =%d BTNull =%d\n",
+		rsvdpageloc->LocProbeRsp, rsvdpageloc->LocPsPoll,
+		rsvdpageloc->LocNullData, rsvdpageloc->LocQosNull,
+		rsvdpageloc->LocBTQosNull);
+
+	SET_8723B_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1H2CRsvdPageParm, rsvdpageloc->LocProbeRsp);
+	SET_8723B_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1H2CRsvdPageParm, rsvdpageloc->LocPsPoll);
+	SET_8723B_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1H2CRsvdPageParm, rsvdpageloc->LocNullData);
+	SET_8723B_H2CCMD_RSVDPAGE_LOC_QOS_NULL_DATA(u1H2CRsvdPageParm, rsvdpageloc->LocQosNull);
+	SET_8723B_H2CCMD_RSVDPAGE_LOC_BT_QOS_NULL_DATA(u1H2CRsvdPageParm, rsvdpageloc->LocBTQosNull);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CRsvdPageParm:", u1H2CRsvdPageParm, H2C_RSVDPAGE_LOC_LEN);
+	FillH2CCmd8723B(padapter, H2C_8723B_RSVD_PAGE, H2C_RSVDPAGE_LOC_LEN, u1H2CRsvdPageParm);
+}
+
+static void rtl8723b_set_FwAoacRsvdPage_cmd(struct adapter *padapter, PRSVDPAGE_LOC rsvdpageloc)
+{
+#ifdef CONFIG_WOWLAN
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8 u1H2CAoacRsvdPageParm[H2C_AOAC_RSVDPAGE_LOC_LEN] = {0};
+
+	DBG_871X("8723BAOACRsvdPageLoc: RWC =%d ArpRsp =%d NbrAdv =%d GtkRsp =%d GtkInfo =%d ProbeReq =%d NetworkList =%d\n",
+			rsvdpageloc->LocRemoteCtrlInfo, rsvdpageloc->LocArpRsp,
+			rsvdpageloc->LocNbrAdv, rsvdpageloc->LocGTKRsp,
+			rsvdpageloc->LocGTKInfo, rsvdpageloc->LocProbeReq,
+			rsvdpageloc->LocNetList);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		SET_H2CCMD_AOAC_RSVDPAGE_LOC_REMOTE_WAKE_CTRL_INFO(u1H2CAoacRsvdPageParm, rsvdpageloc->LocRemoteCtrlInfo);
+		SET_H2CCMD_AOAC_RSVDPAGE_LOC_ARP_RSP(u1H2CAoacRsvdPageParm, rsvdpageloc->LocArpRsp);
+		/* SET_H2CCMD_AOAC_RSVDPAGE_LOC_NEIGHBOR_ADV(u1H2CAoacRsvdPageParm, rsvdpageloc->LocNbrAdv); */
+		SET_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_RSP(u1H2CAoacRsvdPageParm, rsvdpageloc->LocGTKRsp);
+		SET_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_INFO(u1H2CAoacRsvdPageParm, rsvdpageloc->LocGTKInfo);
+#ifdef CONFIG_GTK_OL
+		SET_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_EXT_MEM(u1H2CAoacRsvdPageParm, rsvdpageloc->LocGTKEXTMEM);
+#endif /*  CONFIG_GTK_OL */
+		RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CAoacRsvdPageParm:", u1H2CAoacRsvdPageParm, H2C_AOAC_RSVDPAGE_LOC_LEN);
+		FillH2CCmd8723B(padapter, H2C_8723B_AOAC_RSVD_PAGE, H2C_AOAC_RSVDPAGE_LOC_LEN, u1H2CAoacRsvdPageParm);
+	} else {
+#ifdef CONFIG_PNO_SUPPORT
+		if (!pwrpriv->pno_in_resume) {
+			DBG_871X("NLO_INFO =%d\n", rsvdpageloc->LocPNOInfo);
+			memset(&u1H2CAoacRsvdPageParm, 0, sizeof(u1H2CAoacRsvdPageParm));
+			SET_H2CCMD_AOAC_RSVDPAGE_LOC_NLO_INFO(u1H2CAoacRsvdPageParm, rsvdpageloc->LocPNOInfo);
+			FillH2CCmd8723B(padapter, H2C_AOAC_RSVDPAGE3, H2C_AOAC_RSVDPAGE_LOC_LEN, u1H2CAoacRsvdPageParm);
+			msleep(10);
+		}
+#endif
+	}
+
+#endif /*  CONFIG_WOWLAN */
+}
+
+#ifdef CONFIG_AP_WOWLAN
+static void rtl8723b_set_ap_wow_rsvdpage_cmd(
+	struct adapter *padapter, PRSVDPAGE_LOC rsvdpageloc
+)
+{
+	u8 header;
+	u8 rsvdparm[H2C_AOAC_RSVDPAGE_LOC_LEN] = {0};
+
+	header = rtw_read8(padapter, REG_BCNQ_BDNY);
+
+	DBG_871X("%s: beacon: %d, probeRsp: %d, header:0x%02x\n", __func__,
+			rsvdpageloc->LocApOffloadBCN,
+			rsvdpageloc->LocProbeRsp,
+			header);
+
+	SET_H2CCMD_AP_WOWLAN_RSVDPAGE_LOC_BCN(rsvdparm,
+			rsvdpageloc->LocApOffloadBCN + header);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_BCN_RSVDPAGE,
+			H2C_BCN_RSVDPAGE_LEN, rsvdparm);
+
+	msleep(10);
+
+	memset(&rsvdparm, 0, sizeof(rsvdparm));
+
+	SET_H2CCMD_AP_WOWLAN_RSVDPAGE_LOC_ProbeRsp(
+			rsvdparm,
+			rsvdpageloc->LocProbeRsp + header);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_PROBERSP_RSVDPAGE,
+			H2C_PROBERSP_RSVDPAGE_LEN, rsvdparm);
+
+	msleep(10);
+}
+#endif /* CONFIG_AP_WOWLAN */
+
+void rtl8723b_set_FwMediaStatusRpt_cmd(struct adapter *padapter, u8 mstatus, u8 macid)
+{
+	u8 u1H2CMediaStatusRptParm[H2C_MEDIA_STATUS_RPT_LEN] = {0};
+	u8 macid_end = 0;
+
+	DBG_871X("%s(): mstatus = %d macid =%d\n", __func__, mstatus, macid);
+
+	SET_8723B_H2CCMD_MSRRPT_PARM_OPMODE(u1H2CMediaStatusRptParm, mstatus);
+	SET_8723B_H2CCMD_MSRRPT_PARM_MACID_IND(u1H2CMediaStatusRptParm, 0);
+	SET_8723B_H2CCMD_MSRRPT_PARM_MACID(u1H2CMediaStatusRptParm, macid);
+	SET_8723B_H2CCMD_MSRRPT_PARM_MACID_END(u1H2CMediaStatusRptParm, macid_end);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CMediaStatusRptParm:", u1H2CMediaStatusRptParm, H2C_MEDIA_STATUS_RPT_LEN);
+	FillH2CCmd8723B(padapter, H2C_8723B_MEDIA_STATUS_RPT, H2C_MEDIA_STATUS_RPT_LEN, u1H2CMediaStatusRptParm);
+}
+
+#ifdef CONFIG_WOWLAN
+static void rtl8723b_set_FwKeepAlive_cmd(struct adapter *padapter, u8 benable, u8 pkt_type)
+{
+	u8 u1H2CKeepAliveParm[H2C_KEEP_ALIVE_CTRL_LEN] = {0};
+	u8 adopt = 1, check_period = 5;
+
+	DBG_871X("%s(): benable = %d\n", __func__, benable);
+	SET_8723B_H2CCMD_KEEPALIVE_PARM_ENABLE(u1H2CKeepAliveParm, benable);
+	SET_8723B_H2CCMD_KEEPALIVE_PARM_ADOPT(u1H2CKeepAliveParm, adopt);
+	SET_8723B_H2CCMD_KEEPALIVE_PARM_PKT_TYPE(u1H2CKeepAliveParm, pkt_type);
+	SET_8723B_H2CCMD_KEEPALIVE_PARM_CHECK_PERIOD(u1H2CKeepAliveParm, check_period);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CKeepAliveParm:", u1H2CKeepAliveParm, H2C_KEEP_ALIVE_CTRL_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_KEEP_ALIVE, H2C_KEEP_ALIVE_CTRL_LEN, u1H2CKeepAliveParm);
+}
+
+static void rtl8723b_set_FwDisconDecision_cmd(struct adapter *padapter, u8 benable)
+{
+	u8 u1H2CDisconDecisionParm[H2C_DISCON_DECISION_LEN] = {0};
+	u8 adopt = 1, check_period = 10, trypkt_num = 0;
+
+	DBG_871X("%s(): benable = %d\n", __func__, benable);
+	SET_8723B_H2CCMD_DISCONDECISION_PARM_ENABLE(u1H2CDisconDecisionParm, benable);
+	SET_8723B_H2CCMD_DISCONDECISION_PARM_ADOPT(u1H2CDisconDecisionParm, adopt);
+	SET_8723B_H2CCMD_DISCONDECISION_PARM_CHECK_PERIOD(u1H2CDisconDecisionParm, check_period);
+	SET_8723B_H2CCMD_DISCONDECISION_PARM_TRY_PKT_NUM(u1H2CDisconDecisionParm, trypkt_num);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CDisconDecisionParm:", u1H2CDisconDecisionParm, H2C_DISCON_DECISION_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_DISCON_DECISION, H2C_DISCON_DECISION_LEN, u1H2CDisconDecisionParm);
+}
+#endif /*  CONFIG_WOWLAN */
+
+void rtl8723b_set_FwMacIdConfig_cmd(struct adapter *padapter, u8 mac_id, u8 raid, u8 bw, u8 sgi, u32 mask)
+{
+	u8 u1H2CMacIdConfigParm[H2C_MACID_CFG_LEN] = {0};
+
+	DBG_871X("%s(): mac_id =%d raid = 0x%x bw =%d mask = 0x%x\n", __func__, mac_id, raid, bw, mask);
+
+	SET_8723B_H2CCMD_MACID_CFG_MACID(u1H2CMacIdConfigParm, mac_id);
+	SET_8723B_H2CCMD_MACID_CFG_RAID(u1H2CMacIdConfigParm, raid);
+	SET_8723B_H2CCMD_MACID_CFG_SGI_EN(u1H2CMacIdConfigParm, sgi ? 1 : 0);
+	SET_8723B_H2CCMD_MACID_CFG_BW(u1H2CMacIdConfigParm, bw);
+	SET_8723B_H2CCMD_MACID_CFG_RATE_MASK0(u1H2CMacIdConfigParm, (u8)(mask & 0x000000ff));
+	SET_8723B_H2CCMD_MACID_CFG_RATE_MASK1(u1H2CMacIdConfigParm, (u8)((mask & 0x0000ff00) >> 8));
+	SET_8723B_H2CCMD_MACID_CFG_RATE_MASK2(u1H2CMacIdConfigParm, (u8)((mask & 0x00ff0000) >> 16));
+	SET_8723B_H2CCMD_MACID_CFG_RATE_MASK3(u1H2CMacIdConfigParm, (u8)((mask & 0xff000000) >> 24));
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CMacIdConfigParm:", u1H2CMacIdConfigParm, H2C_MACID_CFG_LEN);
+	FillH2CCmd8723B(padapter, H2C_8723B_MACID_CFG, H2C_MACID_CFG_LEN, u1H2CMacIdConfigParm);
+}
+
+static void rtl8723b_set_FwRssiSetting_cmd(struct adapter *padapter, u8 *param)
+{
+	u8 u1H2CRssiSettingParm[H2C_RSSI_SETTING_LEN] = {0};
+	u8 mac_id = *param;
+	u8 rssi = *(param+2);
+	u8 uldl_state = 0;
+
+	/* DBG_871X("%s(): param =%.2x-%.2x-%.2x\n", __func__, *param, *(param+1), *(param+2)); */
+	/* DBG_871X("%s(): mac_id =%d rssi =%d\n", __func__, mac_id, rssi); */
+
+	SET_8723B_H2CCMD_RSSI_SETTING_MACID(u1H2CRssiSettingParm, mac_id);
+	SET_8723B_H2CCMD_RSSI_SETTING_RSSI(u1H2CRssiSettingParm, rssi);
+	SET_8723B_H2CCMD_RSSI_SETTING_ULDL_STATE(u1H2CRssiSettingParm, uldl_state);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_notice_, "u1H2CRssiSettingParm:", u1H2CRssiSettingParm, H2C_RSSI_SETTING_LEN);
+	FillH2CCmd8723B(padapter, H2C_8723B_RSSI_SETTING, H2C_RSSI_SETTING_LEN, u1H2CRssiSettingParm);
+}
+
+void rtl8723b_set_FwPwrMode_cmd(struct adapter *padapter, u8 psmode)
+{
+	int i;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	u8 u1H2CPwrModeParm[H2C_PWRMODE_LEN] = {0};
+	u8 PowerState = 0, awake_intvl = 1, byte5 = 0, rlbm = 0;
+
+	if (pwrpriv->dtim > 0)
+		DBG_871X("%s(): FW LPS mode = %d, SmartPS =%d, dtim =%d\n", __func__, psmode, pwrpriv->smart_ps, pwrpriv->dtim);
+	else
+		DBG_871X("%s(): FW LPS mode = %d, SmartPS =%d\n", __func__, psmode, pwrpriv->smart_ps);
+
+#ifdef CONFIG_WOWLAN
+	if (psmode == PS_MODE_DTIM) { /* For WOWLAN LPS, DTIM = (awake_intvl - 1) */
+		awake_intvl = 3;/* DTIM =2 */
+		rlbm = 2;
+	} else
+#endif /* CONFIG_WOWLAN */
+	{
+		if (pwrpriv->dtim > 0 && pwrpriv->dtim < 16)
+			awake_intvl = pwrpriv->dtim+1;/* DTIM = (awake_intvl - 1) */
+		else
+			awake_intvl = 3;/* DTIM =2 */
+
+		rlbm = 2;
+	}
+
+
+	if (padapter->registrypriv.wifi_spec == 1) {
+		awake_intvl = 2;
+		rlbm = 2;
+	}
+
+	if (psmode > 0) {
+		if (rtw_btcoex_IsBtControlLps(padapter) == true) {
+			PowerState = rtw_btcoex_RpwmVal(padapter);
+			byte5 = rtw_btcoex_LpsVal(padapter);
+
+			if ((rlbm == 2) && (byte5 & BIT(4))) {
+				/*  Keep awake interval to 1 to prevent from */
+				/*  decreasing coex performance */
+				awake_intvl = 2;
+				rlbm = 2;
+			}
+		} else {
+			PowerState = 0x00;/*  AllON(0x0C), RFON(0x04), RFOFF(0x00) */
+			byte5 = 0x40;
+		}
+	} else {
+		PowerState = 0x0C;/*  AllON(0x0C), RFON(0x04), RFOFF(0x00) */
+		byte5 = 0x40;
+	}
+
+	SET_8723B_H2CCMD_PWRMODE_PARM_MODE(u1H2CPwrModeParm, (psmode > 0) ? 1 : 0);
+	SET_8723B_H2CCMD_PWRMODE_PARM_SMART_PS(u1H2CPwrModeParm, pwrpriv->smart_ps);
+	SET_8723B_H2CCMD_PWRMODE_PARM_RLBM(u1H2CPwrModeParm, rlbm);
+	SET_8723B_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1H2CPwrModeParm, awake_intvl);
+	SET_8723B_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1H2CPwrModeParm, padapter->registrypriv.uapsd_enable);
+	SET_8723B_H2CCMD_PWRMODE_PARM_PWR_STATE(u1H2CPwrModeParm, PowerState);
+	SET_8723B_H2CCMD_PWRMODE_PARM_BYTE5(u1H2CPwrModeParm, byte5);
+	if (psmode != PS_MODE_ACTIVE) {
+		if (pmlmeext->adaptive_tsf_done == false && pmlmeext->bcn_cnt > 0) {
+			u8 ratio_20_delay, ratio_80_delay;
+
+			/* byte 6 for adaptive_early_32k */
+			/* 0:3] = DrvBcnEarly  (ms) , [4:7] = DrvBcnTimeOut  (ms) */
+			/*  20% for DrvBcnEarly, 80% for DrvBcnTimeOut */
+			ratio_20_delay = 0;
+			ratio_80_delay = 0;
+			pmlmeext->DrvBcnEarly = 0xff;
+			pmlmeext->DrvBcnTimeOut = 0xff;
+
+			DBG_871X("%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;
+
+				DBG_871X(
+					"%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 && pmlmeext->DrvBcnEarly == 0xff) {
+					pmlmeext->DrvBcnEarly = i;
+					DBG_871X("%s(): DrvBcnEarly = %d\n", __func__, pmlmeext->DrvBcnEarly);
+				}
+
+				if (ratio_80_delay > 80 && pmlmeext->DrvBcnTimeOut == 0xff) {
+					pmlmeext->DrvBcnTimeOut = i;
+					DBG_871X("%s(): DrvBcnTimeOut = %d\n", __func__, pmlmeext->DrvBcnTimeOut);
+				}
+
+				/* reset adaptive_early_32k cnt */
+				pmlmeext->bcn_delay_cnt[i] = 0;
+				pmlmeext->bcn_delay_ratio[i] = 0;
+
+			}
+
+			pmlmeext->bcn_cnt = 0;
+			pmlmeext->adaptive_tsf_done = true;
+
+		} else {
+			DBG_871X("%s(): DrvBcnEarly = %d\n", __func__, pmlmeext->DrvBcnEarly);
+			DBG_871X("%s(): DrvBcnTimeOut = %d\n", __func__, pmlmeext->DrvBcnTimeOut);
+		}
+
+/* offload to FW if fw version > v15.10
+		pmlmeext->DrvBcnEarly = 0;
+		pmlmeext->DrvBcnTimeOut =7;
+
+		if ((pmlmeext->DrvBcnEarly!= 0Xff) && (pmlmeext->DrvBcnTimeOut!= 0xff))
+			u1H2CPwrModeParm[H2C_PWRMODE_LEN-1] = BIT(0) | ((pmlmeext->DrvBcnEarly<<1)&0x0E) |((pmlmeext->DrvBcnTimeOut<<4)&0xf0) ;
+*/
+
+	}
+
+	rtw_btcoex_RecordPwrMode(padapter, u1H2CPwrModeParm, H2C_PWRMODE_LEN);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CPwrModeParm:", u1H2CPwrModeParm, H2C_PWRMODE_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_SET_PWR_MODE, H2C_PWRMODE_LEN, u1H2CPwrModeParm);
+}
+
+void rtl8723b_set_FwPsTuneParam_cmd(struct adapter *padapter)
+{
+	u8 u1H2CPsTuneParm[H2C_PSTUNEPARAM_LEN] = {0};
+	u8 bcn_to_limit = 10; /* 10 * 100 * awakeinterval (ms) */
+	u8 dtim_timeout = 5; /* ms wait broadcast data timer */
+	u8 ps_timeout = 20;  /* ms Keep awake when tx */
+	u8 dtim_period = 3;
+
+	/* DBG_871X("%s(): FW LPS mode = %d\n", __func__, psmode); */
+
+	SET_8723B_H2CCMD_PSTUNE_PARM_BCN_TO_LIMIT(u1H2CPsTuneParm, bcn_to_limit);
+	SET_8723B_H2CCMD_PSTUNE_PARM_DTIM_TIMEOUT(u1H2CPsTuneParm, dtim_timeout);
+	SET_8723B_H2CCMD_PSTUNE_PARM_PS_TIMEOUT(u1H2CPsTuneParm, ps_timeout);
+	SET_8723B_H2CCMD_PSTUNE_PARM_ADOPT(u1H2CPsTuneParm, 1);
+	SET_8723B_H2CCMD_PSTUNE_PARM_DTIM_PERIOD(u1H2CPsTuneParm, dtim_period);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CPsTuneParm:", u1H2CPsTuneParm, H2C_PSTUNEPARAM_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_PS_TUNING_PARA, H2C_PSTUNEPARAM_LEN, u1H2CPsTuneParm);
+}
+
+void rtl8723b_set_FwPwrModeInIPS_cmd(struct adapter *padapter, u8 cmd_param)
+{
+	/* BIT0:enable, BIT1:NoConnect32k */
+
+	DBG_871X("%s()\n", __func__);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_FWLPS_IN_IPS_, 1, &cmd_param);
+}
+
+#ifdef CONFIG_WOWLAN
+static void rtl8723b_set_FwWoWlanCtrl_Cmd(struct adapter *padapter, u8 bFuncEn)
+{
+	struct security_priv *psecpriv = &padapter->securitypriv;
+	u8 u1H2CWoWlanCtrlParm[H2C_WOWLAN_LEN] = {0};
+	u8 discont_wake = 1, gpionum = 0, gpio_dur = 0, hw_unicast = 0;
+	u8 sdio_wakeup_enable = 1;
+	u8 gpio_high_active = 0; /* 0: low active, 1: high active */
+	u8 magic_pkt = 0;
+
+#ifdef CONFIG_GPIO_WAKEUP
+	gpionum = WAKEUP_GPIO_IDX;
+	sdio_wakeup_enable = 0;
+#endif
+
+#ifdef CONFIG_PNO_SUPPORT
+	if (!ppwrpriv->wowlan_pno_enable)
+		magic_pkt = 1;
+#endif
+
+	if (psecpriv->dot11PrivacyAlgrthm == _WEP40_ || psecpriv->dot11PrivacyAlgrthm == _WEP104_)
+		hw_unicast = 1;
+
+	DBG_871X("%s(): bFuncEn =%d\n", __func__, bFuncEn);
+
+	SET_H2CCMD_WOWLAN_FUNC_ENABLE(u1H2CWoWlanCtrlParm, bFuncEn);
+	SET_H2CCMD_WOWLAN_PATTERN_MATCH_ENABLE(u1H2CWoWlanCtrlParm, 0);
+	SET_H2CCMD_WOWLAN_MAGIC_PKT_ENABLE(u1H2CWoWlanCtrlParm, magic_pkt);
+	SET_H2CCMD_WOWLAN_UNICAST_PKT_ENABLE(u1H2CWoWlanCtrlParm, hw_unicast);
+	SET_H2CCMD_WOWLAN_ALL_PKT_DROP(u1H2CWoWlanCtrlParm, 0);
+	SET_H2CCMD_WOWLAN_GPIO_ACTIVE(u1H2CWoWlanCtrlParm, gpio_high_active);
+	SET_H2CCMD_WOWLAN_DISCONNECT_WAKE_UP(u1H2CWoWlanCtrlParm, discont_wake);
+	SET_H2CCMD_WOWLAN_GPIONUM(u1H2CWoWlanCtrlParm, gpionum);
+	SET_H2CCMD_WOWLAN_DATAPIN_WAKE_UP(u1H2CWoWlanCtrlParm, sdio_wakeup_enable);
+	SET_H2CCMD_WOWLAN_GPIO_DURATION(u1H2CWoWlanCtrlParm, gpio_dur);
+	/* SET_H2CCMD_WOWLAN_GPIO_PULSE_EN(u1H2CWoWlanCtrlParm, 1); */
+	SET_H2CCMD_WOWLAN_GPIO_PULSE_COUNT(u1H2CWoWlanCtrlParm, 0x09);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CWoWlanCtrlParm:", u1H2CWoWlanCtrlParm, H2C_WOWLAN_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_WOWLAN, H2C_WOWLAN_LEN, u1H2CWoWlanCtrlParm);
+}
+
+static void rtl8723b_set_FwRemoteWakeCtrl_Cmd(struct adapter *padapter, u8 benable)
+{
+	u8 u1H2CRemoteWakeCtrlParm[H2C_REMOTE_WAKE_CTRL_LEN] = {0};
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	struct pwrctrl_priv *ppwrpriv = adapter_to_pwrctl(padapter);
+
+	DBG_871X("%s(): Enable =%d\n", __func__, benable);
+
+	if (!ppwrpriv->wowlan_pno_enable) {
+		SET_H2CCMD_REMOTE_WAKECTRL_ENABLE(u1H2CRemoteWakeCtrlParm, benable);
+		SET_H2CCMD_REMOTE_WAKE_CTRL_ARP_OFFLOAD_EN(u1H2CRemoteWakeCtrlParm, 1);
+#ifdef CONFIG_GTK_OL
+		if (psecuritypriv->binstallKCK_KEK &&
+		    psecuritypriv->dot11PrivacyAlgrthm == _AES_) {
+			SET_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(u1H2CRemoteWakeCtrlParm, 1);
+		} else {
+			DBG_871X("no kck or security is not AES\n");
+			SET_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(u1H2CRemoteWakeCtrlParm, 0);
+		}
+#endif /* CONFIG_GTK_OL */
+
+		SET_H2CCMD_REMOTE_WAKE_CTRL_FW_UNICAST_EN(u1H2CRemoteWakeCtrlParm, 1);
+
+		if ((psecuritypriv->dot11PrivacyAlgrthm == _AES_) ||
+		    (psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_))
+			SET_H2CCMD_REMOTE_WAKE_CTRL_ARP_ACTION(u1H2CRemoteWakeCtrlParm, 0);
+		else
+			SET_H2CCMD_REMOTE_WAKE_CTRL_ARP_ACTION(u1H2CRemoteWakeCtrlParm, 1);
+	}
+#ifdef CONFIG_PNO_SUPPORT
+	else {
+		SET_H2CCMD_REMOTE_WAKECTRL_ENABLE(u1H2CRemoteWakeCtrlParm, benable);
+		SET_H2CCMD_REMOTE_WAKE_CTRL_NLO_OFFLOAD_EN(u1H2CRemoteWakeCtrlParm, benable);
+	}
+#endif
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CRemoteWakeCtrlParm:", u1H2CRemoteWakeCtrlParm, H2C_REMOTE_WAKE_CTRL_LEN);
+	FillH2CCmd8723B(padapter, H2C_8723B_REMOTE_WAKE_CTRL,
+		H2C_REMOTE_WAKE_CTRL_LEN, u1H2CRemoteWakeCtrlParm);
+#ifdef CONFIG_PNO_SUPPORT
+	if (ppwrpriv->wowlan_pno_enable && ppwrpriv->pno_in_resume == false) {
+		res = rtw_read8(padapter, REG_PNO_STATUS);
+		DBG_871X("cmd: 0x81 REG_PNO_STATUS: 0x%02x\n", res);
+		while (!(res&BIT(7)) && count < 25) {
+			DBG_871X("[%d] cmd: 0x81 REG_PNO_STATUS: 0x%02x\n", count, res);
+			res = rtw_read8(padapter, REG_PNO_STATUS);
+			count++;
+			msleep(2);
+		}
+		DBG_871X("cmd: 0x81 REG_PNO_STATUS: 0x%02x\n", res);
+	}
+#endif /* CONFIG_PNO_SUPPORT */
+}
+
+static void rtl8723b_set_FwAOACGlobalInfo_Cmd(struct adapter *padapter,  u8 group_alg, u8 pairwise_alg)
+{
+	u8 u1H2CAOACGlobalInfoParm[H2C_AOAC_GLOBAL_INFO_LEN] = {0};
+
+	DBG_871X("%s(): group_alg =%d pairwise_alg =%d\n", __func__, group_alg, pairwise_alg);
+
+	SET_H2CCMD_AOAC_GLOBAL_INFO_PAIRWISE_ENC_ALG(u1H2CAOACGlobalInfoParm, pairwise_alg);
+	SET_H2CCMD_AOAC_GLOBAL_INFO_GROUP_ENC_ALG(u1H2CAOACGlobalInfoParm, group_alg);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CAOACGlobalInfoParm:", u1H2CAOACGlobalInfoParm, H2C_AOAC_GLOBAL_INFO_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_AOAC_GLOBAL_INFO, H2C_AOAC_GLOBAL_INFO_LEN, u1H2CAOACGlobalInfoParm);
+}
+
+#ifdef CONFIG_PNO_SUPPORT
+static void rtl8723b_set_FwScanOffloadInfo_cmd(struct adapter *padapter, PRSVDPAGE_LOC rsvdpageloc, u8 enable)
+{
+	u8 u1H2CScanOffloadInfoParm[H2C_SCAN_OFFLOAD_CTRL_LEN] = {0};
+	u8 res = 0, count = 0;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+	DBG_871X("%s: loc_probe_packet:%d, loc_scan_info: %d loc_ssid_info:%d\n",
+		__func__, rsvdpageloc->LocProbePacket, rsvdpageloc->LocScanInfo, rsvdpageloc->LocSSIDInfo);
+
+	SET_H2CCMD_AOAC_NLO_FUN_EN(u1H2CScanOffloadInfoParm, enable);
+	SET_H2CCMD_AOAC_RSVDPAGE_LOC_SCAN_INFO(u1H2CScanOffloadInfoParm, rsvdpageloc->LocScanInfo);
+	SET_H2CCMD_AOAC_RSVDPAGE_LOC_PROBE_PACKET(u1H2CScanOffloadInfoParm, rsvdpageloc->LocProbePacket);
+	SET_H2CCMD_AOAC_RSVDPAGE_LOC_SSID_INFO(u1H2CScanOffloadInfoParm, rsvdpageloc->LocSSIDInfo);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CScanOffloadInfoParm:", u1H2CScanOffloadInfoParm, H2C_SCAN_OFFLOAD_CTRL_LEN);
+	FillH2CCmd8723B(padapter, H2C_8723B_D0_SCAN_OFFLOAD_INFO, H2C_SCAN_OFFLOAD_CTRL_LEN, u1H2CScanOffloadInfoParm);
+
+	msleep(20);
+}
+#endif /* CONFIG_PNO_SUPPORT */
+
+static void rtl8723b_set_FwWoWlanRelated_cmd(struct adapter *padapter, u8 enable)
+{
+	struct security_priv *psecpriv = &padapter->securitypriv;
+	struct pwrctrl_priv *ppwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_info *psta = NULL;
+	u8 pkt_type = 0;
+
+	DBG_871X_LEVEL(_drv_always_, "+%s()+: enable =%d\n", __func__, enable);
+	if (enable) {
+		rtl8723b_set_FwAOACGlobalInfo_Cmd(padapter, psecpriv->dot118021XGrpPrivacy, psecpriv->dot11PrivacyAlgrthm);
+
+		rtl8723b_set_FwJoinBssRpt_cmd(padapter, RT_MEDIA_CONNECT);	/* RT_MEDIA_CONNECT will confuse in the future */
+
+		if (!(ppwrpriv->wowlan_pno_enable)) {
+			psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(pmlmepriv));
+			if (psta != NULL)
+				rtl8723b_set_FwMediaStatusRpt_cmd(padapter, RT_MEDIA_CONNECT, psta->mac_id);
+		} else
+			DBG_871X("%s(): Disconnected, no FwMediaStatusRpt CONNECT\n", __func__);
+
+		msleep(2);
+
+		if (!(ppwrpriv->wowlan_pno_enable)) {
+			rtl8723b_set_FwDisconDecision_cmd(padapter, enable);
+			msleep(2);
+
+			if ((psecpriv->dot11PrivacyAlgrthm != _WEP40_) || (psecpriv->dot11PrivacyAlgrthm != _WEP104_))
+				pkt_type = 1;
+
+			rtl8723b_set_FwKeepAlive_cmd(padapter, enable, pkt_type);
+			msleep(2);
+		}
+
+		rtl8723b_set_FwWoWlanCtrl_Cmd(padapter, enable);
+		msleep(2);
+
+		rtl8723b_set_FwRemoteWakeCtrl_Cmd(padapter, enable);
+	} else {
+		rtl8723b_set_FwRemoteWakeCtrl_Cmd(padapter, enable);
+		msleep(2);
+		rtl8723b_set_FwWoWlanCtrl_Cmd(padapter, enable);
+	}
+
+	DBG_871X_LEVEL(_drv_always_, "-%s()-\n", __func__);
+}
+
+void rtl8723b_set_wowlan_cmd(struct adapter *padapter, u8 enable)
+{
+	rtl8723b_set_FwWoWlanRelated_cmd(padapter, enable);
+}
+#endif /* CONFIG_WOWLAN */
+
+#ifdef CONFIG_AP_WOWLAN
+static void rtl8723b_set_FwAPWoWlanCtrl_Cmd(struct adapter *padapter, u8 bFuncEn)
+{
+	u8 u1H2CAPWoWlanCtrlParm[H2C_WOWLAN_LEN] = {0};
+	u8 gpionum = 0, gpio_dur = 0;
+	u8 gpio_high_active = 1; /* 0: low active, 1: high active */
+	u8 gpio_pulse = bFuncEn;
+#ifdef CONFIG_GPIO_WAKEUP
+	gpionum = WAKEUP_GPIO_IDX;
+#endif
+
+	DBG_871X("%s(): bFuncEn =%d\n", __func__, bFuncEn);
+
+	if (bFuncEn)
+		gpio_dur = 16;
+	else
+		gpio_dur = 0;
+
+	SET_H2CCMD_AP_WOW_GPIO_CTRL_INDEX(u1H2CAPWoWlanCtrlParm,
+			gpionum);
+	SET_H2CCMD_AP_WOW_GPIO_CTRL_PLUS(u1H2CAPWoWlanCtrlParm,
+			gpio_pulse);
+	SET_H2CCMD_AP_WOW_GPIO_CTRL_HIGH_ACTIVE(u1H2CAPWoWlanCtrlParm,
+			gpio_high_active);
+	SET_H2CCMD_AP_WOW_GPIO_CTRL_EN(u1H2CAPWoWlanCtrlParm,
+			bFuncEn);
+	SET_H2CCMD_AP_WOW_GPIO_CTRL_DURATION(u1H2CAPWoWlanCtrlParm,
+			gpio_dur);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_AP_WOW_GPIO_CTRL,
+			H2C_AP_WOW_GPIO_CTRL_LEN, u1H2CAPWoWlanCtrlParm);
+}
+
+static void rtl8723b_set_Fw_AP_Offload_Cmd(struct adapter *padapter, u8 bFuncEn)
+{
+	u8 u1H2CAPOffloadCtrlParm[H2C_WOWLAN_LEN] = {0};
+
+	DBG_871X("%s(): bFuncEn =%d\n", __func__, bFuncEn);
+
+	SET_H2CCMD_AP_WOWLAN_EN(u1H2CAPOffloadCtrlParm, bFuncEn);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_AP_OFFLOAD,
+			H2C_AP_OFFLOAD_LEN, u1H2CAPOffloadCtrlParm);
+}
+
+static void rtl8723b_set_AP_FwWoWlan_cmd(struct adapter *padapter, u8 enable)
+{
+	DBG_871X_LEVEL(_drv_always_, "+%s()+: enable =%d\n", __func__, enable);
+	if (enable) {
+		rtl8723b_set_FwJoinBssRpt_cmd(padapter, RT_MEDIA_CONNECT);
+		issue_beacon(padapter, 0);
+	}
+
+	rtl8723b_set_FwAPWoWlanCtrl_Cmd(padapter, enable);
+	msleep(10);
+	rtl8723b_set_Fw_AP_Offload_Cmd(padapter, enable);
+	msleep(10);
+	DBG_871X_LEVEL(_drv_always_, "-%s()-\n", __func__);
+	return ;
+}
+
+void rtl8723b_set_ap_wowlan_cmd(struct adapter *padapter, u8 enable)
+{
+	rtl8723b_set_AP_FwWoWlan_cmd(padapter, enable);
+}
+#endif /* CONFIG_AP_WOWLAN */
+
+/*  */
+/*  Description: Fill the reserved packets that FW will use to RSVD page. */
+/* 			Now we just send 4 types packet to rsvd page. */
+/* 			(1)Beacon, (2)Ps-poll, (3)Null data, (4)ProbeRsp. */
+/* 	Input: */
+/* 	    bDLFinished - false: At the first time we will send all the packets as a large packet to Hw, */
+/* 						so we need to set the packet length to total lengh. */
+/* 			      true: At the second time, we should send the first packet (default:beacon) */
+/* 						to Hw again and set the lengh in descriptor to the real beacon lengh. */
+/*  2009.10.15 by tynli. */
+static void rtl8723b_set_FwRsvdPagePkt(
+	struct adapter *padapter, bool bDLFinished
+)
+{
+	struct hal_com_data *pHalData;
+	struct xmit_frame *pcmdframe;
+	struct pkt_attrib *pattrib;
+	struct xmit_priv *pxmitpriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	struct pwrctrl_priv *pwrctl;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u32 BeaconLength = 0, PSPollLength = 0;
+	u32 NullDataLength = 0, QosNullLength = 0, BTQosNullLength = 0;
+	u8 *ReservedPagePacket;
+	u8 TxDescLen = TXDESC_SIZE, TxDescOffset = TXDESC_OFFSET;
+	u8 TotalPageNum = 0, CurtPktPageNum = 0, RsvdPageNum = 0;
+	u16 BufIndex, PageSize = 128;
+	u32 TotalPacketLen, MaxRsvdPageBufSize = 0;
+	RSVDPAGE_LOC RsvdPageLoc;
+#ifdef CONFIG_WOWLAN
+	u32 ARPLegnth = 0, GTKLegnth = 0;
+	u8 currentip[4];
+	u8 cur_dot11txpn[8];
+#ifdef CONFIG_GTK_OL
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta;
+	u8 kek[RTW_KEK_LEN];
+	u8 kck[RTW_KCK_LEN];
+#endif
+#endif
+
+	/* DBG_871X("%s---->\n", __func__); */
+
+	pHalData = GET_HAL_DATA(padapter);
+	pxmitpriv = &padapter->xmitpriv;
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+	pwrctl = adapter_to_pwrctl(padapter);
+
+	RsvdPageNum = BCNQ_PAGE_NUM_8723B + WOWLAN_PAGE_NUM_8723B;
+	MaxRsvdPageBufSize = RsvdPageNum*PageSize;
+
+	pcmdframe = rtw_alloc_cmdxmitframe(pxmitpriv);
+	if (pcmdframe == NULL) {
+		DBG_871X("%s: alloc ReservedPagePacket fail!\n", __func__);
+		return;
+	}
+
+	ReservedPagePacket = pcmdframe->buf_addr;
+	memset(&RsvdPageLoc, 0, sizeof(RSVDPAGE_LOC));
+
+	/* 3 (1) beacon */
+	BufIndex = TxDescOffset;
+	ConstructBeacon(padapter, &ReservedPagePacket[BufIndex], &BeaconLength);
+
+	/*  When we count the first page size, we need to reserve description size for the RSVD */
+	/*  packet, it will be filled in front of the packet in TXPKTBUF. */
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + BeaconLength);
+	/* If we don't add 1 more page, the WOWLAN function has a problem. Baron thinks it's a bug of firmware */
+	if (CurtPktPageNum == 1)
+		CurtPktPageNum += 1;
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3 (2) ps-poll */
+	RsvdPageLoc.LocPsPoll = TotalPageNum;
+	ConstructPSPoll(padapter, &ReservedPagePacket[BufIndex], &PSPollLength);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], PSPollLength, true, false, false);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: PS-POLL %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (PSPollLength+TxDescLen)); */
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + PSPollLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3 (3) null data */
+	RsvdPageLoc.LocNullData = TotalPageNum;
+	ConstructNullFunctionData(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&NullDataLength,
+		get_my_bssid(&pmlmeinfo->network),
+		false, 0, 0, false
+	);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], NullDataLength, false, false, false);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: NULL DATA %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (NullDataLength+TxDescLen)); */
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + NullDataLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3 (5) Qos null data */
+	RsvdPageLoc.LocQosNull = TotalPageNum;
+	ConstructNullFunctionData(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&QosNullLength,
+		get_my_bssid(&pmlmeinfo->network),
+		true, 0, 0, false
+	);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], QosNullLength, false, false, false);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: QOS NULL DATA %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (QosNullLength+TxDescLen)); */
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + QosNullLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3 (6) BT Qos null data */
+	RsvdPageLoc.LocBTQosNull = TotalPageNum;
+	ConstructNullFunctionData(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&BTQosNullLength,
+		get_my_bssid(&pmlmeinfo->network),
+		true, 0, 0, false
+	);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], BTQosNullLength, false, true, false);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: BT QOS NULL DATA %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (BTQosNullLength+TxDescLen)); */
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + BTQosNullLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+#ifdef CONFIG_WOWLAN
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+	/* if (pwrctl->wowlan_mode == true) { */
+		/* BufIndex += (CurtPktPageNum*PageSize); */
+
+	/* 3(7) ARP RSP */
+	rtw_get_current_ip_address(padapter, currentip);
+	RsvdPageLoc.LocArpRsp = TotalPageNum;
+	{
+	ConstructARPResponse(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&ARPLegnth,
+		currentip
+		);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], ARPLegnth, false, false, true);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: ARP RSP %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (ARPLegnth+TxDescLen)); */
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + ARPLegnth);
+	}
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3(8) SEC IV */
+	rtw_get_sec_iv(padapter, cur_dot11txpn, get_my_bssid(&pmlmeinfo->network));
+	RsvdPageLoc.LocRemoteCtrlInfo = TotalPageNum;
+	memcpy(ReservedPagePacket+BufIndex-TxDescLen, cur_dot11txpn, _AES_IV_LEN_);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: SEC IV %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], _AES_IV_LEN_); */
+
+	CurtPktPageNum = (u8)PageNum_128(_AES_IV_LEN_);
+
+	TotalPageNum += CurtPktPageNum;
+
+#ifdef CONFIG_GTK_OL
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* if the ap staion info. exists, get the kek, kck from staion info. */
+	psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
+	if (psta == NULL) {
+		memset(kek, 0, RTW_KEK_LEN);
+		memset(kck, 0, RTW_KCK_LEN);
+		DBG_8192C("%s, KEK, KCK download rsvd page all zero\n", __func__);
+	} else {
+		memcpy(kek, psta->kek, RTW_KEK_LEN);
+		memcpy(kck, psta->kck, RTW_KCK_LEN);
+	}
+
+	/* 3(9) KEK, KCK */
+	RsvdPageLoc.LocGTKInfo = TotalPageNum;
+	memcpy(ReservedPagePacket+BufIndex-TxDescLen, kck, RTW_KCK_LEN);
+	memcpy(ReservedPagePacket+BufIndex-TxDescLen+RTW_KCK_LEN, kek, RTW_KEK_LEN);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: KEK KCK %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (TxDescLen + RTW_KCK_LEN + RTW_KEK_LEN)); */
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + RTW_KCK_LEN + RTW_KEK_LEN);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3(10) GTK Response */
+	RsvdPageLoc.LocGTKRsp = TotalPageNum;
+	ConstructGTKResponse(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&GTKLegnth
+	);
+
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], GTKLegnth, false, false, true);
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: GTK RSP %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (TxDescLen + GTKLegnth)); */
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + GTKLegnth);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* below page is empty for GTK extension memory */
+	/* 3(11) GTK EXT MEM */
+	RsvdPageLoc.LocGTKEXTMEM = TotalPageNum;
+
+	CurtPktPageNum = 2;
+
+	TotalPageNum += CurtPktPageNum;
+
+	TotalPacketLen = BufIndex-TxDescLen + 256; /* extension memory for FW */
+#else
+	TotalPacketLen = BufIndex-TxDescLen + sizeof (union pn48); /* IV len */
+#endif /* CONFIG_GTK_OL */
+	} else
+#endif /* CONFIG_WOWLAN */
+	{
+#ifdef CONFIG_PNO_SUPPORT
+		if (pwrctl->pno_in_resume == false && pwrctl->pno_inited == true) {
+			/* Probe Request */
+			RsvdPageLoc.LocProbePacket = TotalPageNum;
+			ConstructProbeReq(
+				padapter,
+				&ReservedPagePacket[BufIndex],
+				&ProbeReqLength);
+
+			rtl8723b_fill_fake_txdesc(padapter,
+				&ReservedPagePacket[BufIndex-TxDescLen],
+				ProbeReqLength, false, false, false);
+#ifdef CONFIG_PNO_SET_DEBUG
+	{
+			int gj;
+			printk("probe req pkt =>\n");
+			for (gj = 0; gj < ProbeReqLength+TxDescLen; gj++) {
+				printk(" %02x ", ReservedPagePacket[BufIndex-TxDescLen+gj]);
+				if ((gj+1)%8 == 0)
+					printk("\n");
+			}
+			printk(" <=end\n");
+	}
+#endif
+			CurtPktPageNum =
+				(u8)PageNum_128(TxDescLen + ProbeReqLength);
+
+			TotalPageNum += CurtPktPageNum;
+
+			BufIndex += (CurtPktPageNum*PageSize);
+
+			/* PNO INFO Page */
+			RsvdPageLoc.LocPNOInfo = TotalPageNum;
+			ConstructPnoInfo(padapter, &ReservedPagePacket[BufIndex-TxDescLen], &PNOLength);
+#ifdef CONFIG_PNO_SET_DEBUG
+	{
+			int gj;
+			printk("PNO pkt =>\n");
+			for (gj = 0; gj < PNOLength; gj++) {
+				printk(" %02x ", ReservedPagePacket[BufIndex-TxDescLen+gj]);
+				if ((gj + 1)%8 == 0)
+					printk("\n");
+			}
+			printk(" <=end\n");
+	}
+#endif
+
+			CurtPktPageNum = (u8)PageNum_128(PNOLength);
+			TotalPageNum += CurtPktPageNum;
+			BufIndex += (CurtPktPageNum*PageSize);
+
+			/* SSID List Page */
+			RsvdPageLoc.LocSSIDInfo = TotalPageNum;
+			ConstructSSIDList(padapter, &ReservedPagePacket[BufIndex-TxDescLen], &SSIDLegnth);
+#ifdef CONFIG_PNO_SET_DEBUG
+	{
+			int gj;
+			printk("SSID list pkt =>\n");
+			for (gj = 0; gj < SSIDLegnth; gj++) {
+				printk(" %02x ", ReservedPagePacket[BufIndex-TxDescLen+gj]);
+				if ((gj + 1)%8 == 0)
+					printk("\n");
+			}
+			printk(" <=end\n");
+	}
+#endif
+			CurtPktPageNum = (u8)PageNum_128(SSIDLegnth);
+			TotalPageNum += CurtPktPageNum;
+			BufIndex += (CurtPktPageNum*PageSize);
+
+			/* Scan Info Page */
+			RsvdPageLoc.LocScanInfo = TotalPageNum;
+			ConstructScanInfo(padapter, &ReservedPagePacket[BufIndex-TxDescLen], &ScanInfoLength);
+#ifdef CONFIG_PNO_SET_DEBUG
+	{
+			int gj;
+			printk("Scan info pkt =>\n");
+			for (gj = 0; gj < ScanInfoLength; gj++) {
+				printk(" %02x ", ReservedPagePacket[BufIndex-TxDescLen+gj]);
+				if ((gj + 1)%8 == 0)
+					printk("\n");
+			}
+			printk(" <=end\n");
+	}
+#endif
+			CurtPktPageNum = (u8)PageNum_128(ScanInfoLength);
+			TotalPageNum += CurtPktPageNum;
+			BufIndex += (CurtPktPageNum*PageSize);
+
+			TotalPacketLen = BufIndex + ScanInfoLength;
+		} else {
+		TotalPacketLen = BufIndex + BTQosNullLength;
+	}
+#else /* CONFIG_PNO_SUPPORT */
+		TotalPacketLen = BufIndex + BTQosNullLength;
+#endif
+	}
+
+	if (TotalPacketLen > MaxRsvdPageBufSize) {
+		DBG_871X("%s(): ERROR: The rsvd page size is not enough!!TotalPacketLen %d, MaxRsvdPageBufSize %d\n", __func__,
+			TotalPacketLen, MaxRsvdPageBufSize);
+		goto error;
+	} else {
+		/*  update attribute */
+		pattrib = &pcmdframe->attrib;
+		update_mgntframe_attrib(padapter, pattrib);
+		pattrib->qsel = 0x10;
+		pattrib->pktlen = pattrib->last_txcmdsz = TotalPacketLen - TxDescOffset;
+		dump_mgntframe_and_wait(padapter, pcmdframe, 100);
+	}
+
+	DBG_871X("%s: Set RSVD page location to Fw , TotalPacketLen(%d), TotalPageNum(%d)\n", __func__, TotalPacketLen, TotalPageNum);
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		rtl8723b_set_FwRsvdPage_cmd(padapter, &RsvdPageLoc);
+		rtl8723b_set_FwAoacRsvdPage_cmd(padapter, &RsvdPageLoc);
+	} else {
+		rtl8723b_set_FwAoacRsvdPage_cmd(padapter, &RsvdPageLoc);
+#ifdef CONFIG_PNO_SUPPORT
+		if (pwrctl->pno_in_resume)
+			rtl8723b_set_FwScanOffloadInfo_cmd(padapter,
+					&RsvdPageLoc, 0);
+		else
+			rtl8723b_set_FwScanOffloadInfo_cmd(padapter,
+					&RsvdPageLoc, 1);
+#endif
+	}
+	return;
+
+error:
+
+	rtw_free_xmitframe(pxmitpriv, pcmdframe);
+}
+
+#ifdef CONFIG_AP_WOWLAN
+/*  */
+/* Description: Fill the reserved packets that FW will use to RSVD page. */
+/* Now we just send 2 types packet to rsvd page. (1)Beacon, (2)ProbeRsp. */
+/*  */
+/* Input: bDLFinished */
+/*  */
+/* false: At the first time we will send all the packets as a large packet to Hw, */
+/* 	 so we need to set the packet length to total lengh. */
+/*  */
+/* true: At the second time, we should send the first packet (default:beacon) */
+/* 	to Hw again and set the lengh in descriptor to the real beacon lengh. */
+/*  2009.10.15 by tynli. */
+static void rtl8723b_set_AP_FwRsvdPagePkt(
+	struct adapter *padapter, bool bDLFinished
+)
+{
+	struct hal_com_data *pHalData;
+	struct xmit_frame *pcmdframe;
+	struct pkt_attrib *pattrib;
+	struct xmit_priv *pxmitpriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	struct pwrctrl_priv *pwrctl;
+	u32 BeaconLength = 0, ProbeRspLength = 0;
+	u8 *ReservedPagePacket;
+	u8 TxDescLen = TXDESC_SIZE, TxDescOffset = TXDESC_OFFSET;
+	u8 TotalPageNum = 0, CurtPktPageNum = 0, RsvdPageNum = 0;
+	u8 currentip[4];
+	u16 BufIndex, PageSize = 128;
+	u32 TotalPacketLen = 0, MaxRsvdPageBufSize = 0;
+	RSVDPAGE_LOC RsvdPageLoc;
+
+	/* DBG_871X("%s---->\n", __func__); */
+	DBG_8192C("+" FUNC_ADPT_FMT ": iface_type =%d\n",
+		FUNC_ADPT_ARG(padapter), get_iface_type(padapter));
+
+	pHalData = GET_HAL_DATA(padapter);
+	pxmitpriv = &padapter->xmitpriv;
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+	pwrctl = adapter_to_pwrctl(padapter);
+
+	RsvdPageNum = BCNQ_PAGE_NUM_8723B + AP_WOWLAN_PAGE_NUM_8723B;
+	MaxRsvdPageBufSize = RsvdPageNum*PageSize;
+
+	pcmdframe = rtw_alloc_cmdxmitframe(pxmitpriv);
+	if (pcmdframe == NULL) {
+		DBG_871X("%s: alloc ReservedPagePacket fail!\n", __func__);
+		return;
+	}
+
+	ReservedPagePacket = pcmdframe->buf_addr;
+	memset(&RsvdPageLoc, 0, sizeof(RSVDPAGE_LOC));
+
+	/* 3 (1) beacon */
+	BufIndex = TxDescOffset;
+	ConstructBeacon(padapter, &ReservedPagePacket[BufIndex], &BeaconLength);
+
+	/*  When we count the first page size, we need to reserve description size for the RSVD */
+	/*  packet, it will be filled in front of the packet in TXPKTBUF. */
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + BeaconLength);
+	/* If we don't add 1 more page, the WOWLAN function has a problem. Baron thinks it's a bug of firmware */
+	if (CurtPktPageNum == 1)
+		CurtPktPageNum += 1;
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 2 (4) probe response */
+	RsvdPageLoc.LocProbeRsp = TotalPageNum;
+
+	rtw_get_current_ip_address(padapter, currentip);
+
+	ConstructProbeRsp(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&ProbeRspLength,
+		currentip,
+		false);
+	rtl8723b_fill_fake_txdesc(padapter,
+			&ReservedPagePacket[BufIndex-TxDescLen],
+			ProbeRspLength,
+			false, false, false);
+
+	DBG_871X("%s(): HW_VAR_SET_TX_CMD: PROBE RSP %p %d\n",
+		__func__, &ReservedPagePacket[BufIndex-TxDescLen],
+		(ProbeRspLength+TxDescLen));
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + ProbeRspLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	TotalPacketLen = BufIndex + ProbeRspLength;
+
+	if (TotalPacketLen > MaxRsvdPageBufSize) {
+		DBG_871X("%s(): ERROR: The rsvd page size is not enough \
+				!!TotalPacketLen %d, MaxRsvdPageBufSize %d\n",
+				__func__, TotalPacketLen, MaxRsvdPageBufSize);
+		goto error;
+	} else {
+		/*  update attribute */
+		pattrib = &pcmdframe->attrib;
+		update_mgntframe_attrib(padapter, pattrib);
+		pattrib->qsel = 0x10;
+		pattrib->pktlen = TotalPacketLen - TxDescOffset;
+		pattrib->last_txcmdsz = TotalPacketLen - TxDescOffset;
+		dump_mgntframe_and_wait(padapter, pcmdframe, 100);
+	}
+
+	DBG_871X("%s: Set RSVD page location to Fw , TotalPacketLen(%d), TotalPageNum(%d)\n", __func__, TotalPacketLen, TotalPageNum);
+	rtl8723b_set_ap_wow_rsvdpage_cmd(padapter, &RsvdPageLoc);
+
+	return;
+error:
+	rtw_free_xmitframe(pxmitpriv, pcmdframe);
+}
+#endif /* CONFIG_AP_WOWLAN */
+
+void rtl8723b_download_rsvd_page(struct adapter *padapter, u8 mstatus)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+#ifdef CONFIG_AP_WOWLAN
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+#endif
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	bool bcn_valid = false;
+	u8 DLBcnCount = 0;
+	u32 poll = 0;
+	u8 val8;
+
+	DBG_8192C("+" FUNC_ADPT_FMT ": iface_type =%d mstatus(%x)\n",
+		FUNC_ADPT_ARG(padapter), get_iface_type(padapter), mstatus);
+
+	if (mstatus == RT_MEDIA_CONNECT) {
+		bool bRecover = false;
+		u8 v8;
+
+		/*  We should set AID, correct TSF, HW seq enable before set JoinBssReport to Fw in 88/92C. */
+		/*  Suggested by filen. Added by tynli. */
+		rtw_write16(padapter, REG_BCN_PSR_RPT, (0xC000|pmlmeinfo->aid));
+
+		/*  set REG_CR bit 8 */
+		v8 = rtw_read8(padapter, REG_CR+1);
+		v8 |= BIT(0); /*  ENSWBCN */
+		rtw_write8(padapter, REG_CR+1, v8);
+
+		/*  Disable Hw protection for a time which revserd for Hw sending beacon. */
+		/*  Fix download reserved page packet fail that access collision with the protection time. */
+		/*  2010.05.11. Added by tynli. */
+		val8 = rtw_read8(padapter, REG_BCN_CTRL);
+		val8 &= ~EN_BCN_FUNCTION;
+		val8 |= DIS_TSF_UDT;
+		rtw_write8(padapter, REG_BCN_CTRL, val8);
+
+		/*  Set FWHW_TXQ_CTRL 0x422[6]= 0 to tell Hw the packet is not a real beacon frame. */
+		if (pHalData->RegFwHwTxQCtrl & BIT(6))
+			bRecover = true;
+
+		/*  To tell Hw the packet is not a real beacon frame. */
+		rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl & ~BIT(6));
+		pHalData->RegFwHwTxQCtrl &= ~BIT(6);
+
+		/*  Clear beacon valid check bit. */
+		rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
+		rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
+
+		DLBcnCount = 0;
+		poll = 0;
+		do {
+#ifdef CONFIG_AP_WOWLAN
+			if (pwrpriv->wowlan_ap_mode)
+				rtl8723b_set_AP_FwRsvdPagePkt(padapter, 0);
+			else
+				rtl8723b_set_FwRsvdPagePkt(padapter, 0);
+#else
+			/*  download rsvd page. */
+			rtl8723b_set_FwRsvdPagePkt(padapter, 0);
+#endif
+			DLBcnCount++;
+			do {
+				yield();
+				/* mdelay(10); */
+				/*  check rsvd page download OK. */
+				rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bcn_valid));
+				poll++;
+			} while (!bcn_valid && (poll%10) != 0 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+		} while (!bcn_valid && DLBcnCount <= 100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+		if (padapter->bSurpriseRemoved || padapter->bDriverStopped) {
+		} else if (!bcn_valid)
+			DBG_871X(ADPT_FMT": 1 DL RSVD page failed! DLBcnCount:%u, poll:%u\n",
+				ADPT_ARG(padapter), DLBcnCount, poll);
+		else {
+			struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+			pwrctl->fw_psmode_iface_id = padapter->iface_id;
+			DBG_871X(ADPT_FMT": 1 DL RSVD page success! DLBcnCount:%u, poll:%u\n",
+				ADPT_ARG(padapter), DLBcnCount, poll);
+		}
+
+		/*  2010.05.11. Added by tynli. */
+		val8 = rtw_read8(padapter, REG_BCN_CTRL);
+		val8 |= EN_BCN_FUNCTION;
+		val8 &= ~DIS_TSF_UDT;
+		rtw_write8(padapter, REG_BCN_CTRL, val8);
+
+		/*  To make sure that if there exists an adapter which would like to send beacon. */
+		/*  If exists, the origianl value of 0x422[6] will be 1, we should check this to */
+		/*  prevent from setting 0x422[6] to 0 after download reserved page, or it will cause */
+		/*  the beacon cannot be sent by HW. */
+		/*  2010.06.23. Added by tynli. */
+		if (bRecover) {
+			rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl | BIT(6));
+			pHalData->RegFwHwTxQCtrl |= BIT(6);
+		}
+
+		/*  Clear CR[8] or beacon packet will not be send to TxBuf anymore. */
+		v8 = rtw_read8(padapter, REG_CR+1);
+		v8 &= ~BIT(0); /*  ~ENSWBCN */
+		rtw_write8(padapter, REG_CR+1, v8);
+	}
+}
+
+void rtl8723b_set_rssi_cmd(struct adapter *padapter, u8 *param)
+{
+	rtl8723b_set_FwRssiSetting_cmd(padapter, param);
+}
+
+void rtl8723b_set_FwJoinBssRpt_cmd(struct adapter *padapter, u8 mstatus)
+{
+	if (mstatus == 1)
+		rtl8723b_download_rsvd_page(padapter, RT_MEDIA_CONNECT);
+}
+
+/* arg[0] = macid */
+/* arg[1] = raid */
+/* arg[2] = shortGIrate */
+/* arg[3] = init_rate */
+void rtl8723b_Add_RateATid(
+	struct adapter *padapter,
+	u32 bitmap,
+	u8 *arg,
+	u8 rssi_level
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct sta_info *psta;
+	u8 mac_id = arg[0];
+	u8 raid = arg[1];
+	u8 shortGI = arg[2];
+	u8 bw;
+	u32 mask = bitmap&0x0FFFFFFF;
+
+	psta = pmlmeinfo->FW_sta_info[mac_id].psta;
+	if (psta == NULL)
+		return;
+
+	bw = psta->bw_mode;
+
+	if (rssi_level != DM_RATR_STA_INIT)
+		mask = ODM_Get_Rate_Bitmap(&pHalData->odmpriv, mac_id, mask, rssi_level);
+
+	DBG_871X("%s(): mac_id =%d raid = 0x%x bw =%d mask = 0x%x\n", __func__, mac_id, raid, bw, mask);
+	rtl8723b_set_FwMacIdConfig_cmd(padapter, mac_id, raid, bw, shortGI, mask);
+}
+
+static void ConstructBtNullFunctionData(
+	struct adapter *padapter,
+	u8 *pframe,
+	u32 *pLength,
+	u8 *StaAddr,
+	u8 bQoS,
+	u8 AC,
+	u8 bEosp,
+	u8 bForcePowerSave
+)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	u32 pktlen;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	u8 bssid[ETH_ALEN];
+
+
+	DBG_871X("+" FUNC_ADPT_FMT ": qos =%d eosp =%d ps =%d\n",
+		FUNC_ADPT_ARG(padapter), bQoS, bEosp, bForcePowerSave);
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (NULL == StaAddr) {
+		memcpy(bssid, myid(&padapter->eeprompriv), ETH_ALEN);
+		StaAddr = bssid;
+	}
+
+	fctrl = &pwlanhdr->frame_control;
+	*fctrl = 0;
+	if (bForcePowerSave)
+		SetPwrMgt(fctrl);
+
+	SetFrDs(fctrl);
+	memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
+	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
+	memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
+
+	SetDuration(pwlanhdr, 0);
+	SetSeqNum(pwlanhdr, 0);
+
+	if (bQoS == true) {
+		struct ieee80211_qos_hdr *pwlanqoshdr;
+
+		SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+
+		pwlanqoshdr = (struct ieee80211_qos_hdr *)pframe;
+		SetPriority(&pwlanqoshdr->qos_ctrl, AC);
+		SetEOSP(&pwlanqoshdr->qos_ctrl, bEosp);
+
+		pktlen = sizeof(struct ieee80211_qos_hdr);
+	} else {
+		SetFrameSubType(pframe, WIFI_DATA_NULL);
+
+		pktlen = sizeof(struct ieee80211_hdr_3addr);
+	}
+
+	*pLength = pktlen;
+}
+
+static void SetFwRsvdPagePkt_BTCoex(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+	struct xmit_frame *pcmdframe;
+	struct pkt_attrib *pattrib;
+	struct xmit_priv *pxmitpriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	u32 BeaconLength = 0;
+	u32 BTQosNullLength = 0;
+	u8 *ReservedPagePacket;
+	u8 TxDescLen, TxDescOffset;
+	u8 TotalPageNum = 0, CurtPktPageNum = 0, RsvdPageNum = 0;
+	u16 BufIndex, PageSize;
+	u32 TotalPacketLen, MaxRsvdPageBufSize = 0;
+	RSVDPAGE_LOC RsvdPageLoc;
+
+
+/* 	DBG_8192C("+" FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(padapter)); */
+
+	pHalData = GET_HAL_DATA(padapter);
+	pxmitpriv = &padapter->xmitpriv;
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+	TxDescLen = TXDESC_SIZE;
+	TxDescOffset = TXDESC_OFFSET;
+	PageSize = PAGE_SIZE_TX_8723B;
+
+	RsvdPageNum = BCNQ_PAGE_NUM_8723B;
+	MaxRsvdPageBufSize = RsvdPageNum*PageSize;
+
+	pcmdframe = rtw_alloc_cmdxmitframe(pxmitpriv);
+	if (pcmdframe == NULL) {
+		DBG_8192C("%s: alloc ReservedPagePacket fail!\n", __func__);
+		return;
+	}
+
+	ReservedPagePacket = pcmdframe->buf_addr;
+	memset(&RsvdPageLoc, 0, sizeof(RSVDPAGE_LOC));
+
+	/* 3 (1) beacon */
+	BufIndex = TxDescOffset;
+	ConstructBeacon(padapter, &ReservedPagePacket[BufIndex], &BeaconLength);
+
+	/*  When we count the first page size, we need to reserve description size for the RSVD */
+	/*  packet, it will be filled in front of the packet in TXPKTBUF. */
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + BeaconLength);
+	/* If we don't add 1 more page, the WOWLAN function has a problem. Baron thinks it's a bug of firmware */
+	if (CurtPktPageNum == 1)
+		CurtPktPageNum += 1;
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/*  Jump to lastest page */
+	if (BufIndex < (MaxRsvdPageBufSize - PageSize)) {
+		BufIndex = TxDescOffset + (MaxRsvdPageBufSize - PageSize);
+		TotalPageNum = BCNQ_PAGE_NUM_8723B - 1;
+	}
+
+	/* 3 (6) BT Qos null data */
+	RsvdPageLoc.LocBTQosNull = TotalPageNum;
+	ConstructBtNullFunctionData(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&BTQosNullLength,
+		NULL,
+		true, 0, 0, false
+	);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], BTQosNullLength, false, true, false);
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + BTQosNullLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	TotalPacketLen = BufIndex + BTQosNullLength;
+	if (TotalPacketLen > MaxRsvdPageBufSize) {
+		DBG_8192C(FUNC_ADPT_FMT ": ERROR: The rsvd page size is not enough!!TotalPacketLen %d, MaxRsvdPageBufSize %d\n",
+			FUNC_ADPT_ARG(padapter), TotalPacketLen, MaxRsvdPageBufSize);
+		goto error;
+	}
+
+	/*  update attribute */
+	pattrib = &pcmdframe->attrib;
+	update_mgntframe_attrib(padapter, pattrib);
+	pattrib->qsel = 0x10;
+	pattrib->pktlen = pattrib->last_txcmdsz = TotalPacketLen - TxDescOffset;
+	dump_mgntframe_and_wait(padapter, pcmdframe, 100);
+
+/* 	DBG_8192C(FUNC_ADPT_FMT ": Set RSVD page location to Fw, TotalPacketLen(%d), TotalPageNum(%d)\n", */
+/* 		FUNC_ADPT_ARG(padapter), TotalPacketLen, TotalPageNum); */
+	rtl8723b_set_FwRsvdPage_cmd(padapter, &RsvdPageLoc);
+	rtl8723b_set_FwAoacRsvdPage_cmd(padapter, &RsvdPageLoc);
+
+	return;
+
+error:
+	rtw_free_xmitframe(pxmitpriv, pcmdframe);
+}
+
+void rtl8723b_download_BTCoex_AP_mode_rsvd_page(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	u8 bRecover = false;
+	u8 bcn_valid = false;
+	u8 DLBcnCount = 0;
+	u32 poll = 0;
+	u8 val8;
+
+
+	DBG_8192C("+" FUNC_ADPT_FMT ": iface_type =%d fw_state = 0x%08X\n",
+		FUNC_ADPT_ARG(padapter), get_iface_type(padapter), get_fwstate(&padapter->mlmepriv));
+
+#ifdef CONFIG_DEBUG
+	if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == false) {
+		DBG_8192C(FUNC_ADPT_FMT ": [WARNING] not in AP mode!!\n",
+			FUNC_ADPT_ARG(padapter));
+	}
+#endif /*  CONFIG_DEBUG */
+
+	pHalData = GET_HAL_DATA(padapter);
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	/*  We should set AID, correct TSF, HW seq enable before set JoinBssReport to Fw in 88/92C. */
+	/*  Suggested by filen. Added by tynli. */
+	rtw_write16(padapter, REG_BCN_PSR_RPT, (0xC000|pmlmeinfo->aid));
+
+	/*  set REG_CR bit 8 */
+	val8 = rtw_read8(padapter, REG_CR+1);
+	val8 |= BIT(0); /*  ENSWBCN */
+	rtw_write8(padapter,  REG_CR+1, val8);
+
+	/*  Disable Hw protection for a time which revserd for Hw sending beacon. */
+	/*  Fix download reserved page packet fail that access collision with the protection time. */
+	/*  2010.05.11. Added by tynli. */
+	val8 = rtw_read8(padapter, REG_BCN_CTRL);
+	val8 &= ~EN_BCN_FUNCTION;
+	val8 |= DIS_TSF_UDT;
+	rtw_write8(padapter, REG_BCN_CTRL, val8);
+
+	/*  Set FWHW_TXQ_CTRL 0x422[6]= 0 to tell Hw the packet is not a real beacon frame. */
+	if (pHalData->RegFwHwTxQCtrl & BIT(6))
+		bRecover = true;
+
+	/*  To tell Hw the packet is not a real beacon frame. */
+	pHalData->RegFwHwTxQCtrl &= ~BIT(6);
+	rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
+
+	/*  Clear beacon valid check bit. */
+	rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
+	rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
+
+	DLBcnCount = 0;
+	poll = 0;
+	do {
+		SetFwRsvdPagePkt_BTCoex(padapter);
+		DLBcnCount++;
+		do {
+			yield();
+/* 			mdelay(10); */
+			/*  check rsvd page download OK. */
+			rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, &bcn_valid);
+			poll++;
+		} while (!bcn_valid && (poll%10) != 0 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+	} while (!bcn_valid && (DLBcnCount <= 100) && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
+
+	if (true == bcn_valid) {
+		struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+		pwrctl->fw_psmode_iface_id = padapter->iface_id;
+		DBG_8192C(ADPT_FMT": DL RSVD page success! DLBcnCount:%d, poll:%d\n",
+			ADPT_ARG(padapter), DLBcnCount, poll);
+	} else {
+		DBG_8192C(ADPT_FMT": DL RSVD page fail! DLBcnCount:%d, poll:%d\n",
+			ADPT_ARG(padapter), DLBcnCount, poll);
+		DBG_8192C(ADPT_FMT": DL RSVD page fail! bSurpriseRemoved =%d\n",
+			ADPT_ARG(padapter), padapter->bSurpriseRemoved);
+		DBG_8192C(ADPT_FMT": DL RSVD page fail! bDriverStopped =%d\n",
+			ADPT_ARG(padapter), padapter->bDriverStopped);
+	}
+
+	/*  2010.05.11. Added by tynli. */
+	val8 = rtw_read8(padapter, REG_BCN_CTRL);
+	val8 |= EN_BCN_FUNCTION;
+	val8 &= ~DIS_TSF_UDT;
+	rtw_write8(padapter, REG_BCN_CTRL, val8);
+
+	/*  To make sure that if there exists an adapter which would like to send beacon. */
+	/*  If exists, the origianl value of 0x422[6] will be 1, we should check this to */
+	/*  prevent from setting 0x422[6] to 0 after download reserved page, or it will cause */
+	/*  the beacon cannot be sent by HW. */
+	/*  2010.06.23. Added by tynli. */
+	if (bRecover) {
+		pHalData->RegFwHwTxQCtrl |= BIT(6);
+		rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
+	}
+
+	/*  Clear CR[8] or beacon packet will not be send to TxBuf anymore. */
+	val8 = rtw_read8(padapter, REG_CR+1);
+	val8 &= ~BIT(0); /*  ~ENSWBCN */
+	rtw_write8(padapter, REG_CR+1, val8);
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/rtl8723b_dm.c linux/3rdparty/rtl8723bs/hal/rtl8723b_dm.c
--- linux.old/3rdparty/rtl8723bs/hal/rtl8723b_dm.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/rtl8723b_dm.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,300 @@
+/******************************************************************************
+ *
+ * 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.
+ *
+ ******************************************************************************/
+/*  Description: */
+/*  This file is for 92CE/92CU dynamic mechanism only */
+
+#define _RTL8723B_DM_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+
+/*  Global var */
+
+static void dm_CheckStatistics(struct adapter *Adapter)
+{
+}
+/*  */
+/*  functions */
+/*  */
+static void Init_ODM_ComInfo_8723b(struct adapter *Adapter)
+{
+
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	PDM_ODM_T pDM_Odm = &(pHalData->odmpriv);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	u8 cut_ver, fab_ver;
+
+	/*  */
+	/*  Init Value */
+	/*  */
+	memset(pDM_Odm, 0, sizeof(*pDM_Odm));
+
+	pDM_Odm->Adapter = Adapter;
+#define ODM_CE 0x04
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_PLATFORM, ODM_CE);
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_INTERFACE, RTW_SDIO);
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_PACKAGE_TYPE, pHalData->PackageType);
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_IC_TYPE, ODM_RTL8723B);
+
+	fab_ver = ODM_TSMC;
+	cut_ver = ODM_CUT_A;
+
+	DBG_871X("%s(): fab_ver =%d cut_ver =%d\n", __func__, fab_ver, cut_ver);
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_FAB_VER, fab_ver);
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_CUT_VER, cut_ver);
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_MP_TEST_CHIP, IS_NORMAL_CHIP(pHalData->VersionID));
+
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_PATCH_ID, pHalData->CustomerID);
+	/* 	ODM_CMNINFO_BINHCT_TEST only for MP Team */
+	ODM_CmnInfoInit(pDM_Odm, ODM_CMNINFO_BWIFI_TEST, Adapter->registrypriv.wifi_spec);
+
+
+	if (pHalData->rf_type == RF_1T1R) {
+		ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_RF_TYPE, ODM_1T1R);
+	} else if (pHalData->rf_type == RF_2T2R) {
+		ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_RF_TYPE, ODM_2T2R);
+	} else if (pHalData->rf_type == RF_1T2R) {
+		ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_RF_TYPE, ODM_1T2R);
+	}
+
+	pdmpriv->InitODMFlag = ODM_RF_CALIBRATION|ODM_RF_TX_PWR_TRACK;
+
+	ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_ABILITY, pdmpriv->InitODMFlag);
+}
+
+static void Update_ODM_ComInfo_8723b(struct adapter *Adapter)
+{
+	struct mlme_ext_priv *pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
+	struct dvobj_priv *dvobj = adapter_to_dvobj(Adapter);
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(Adapter);
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	PDM_ODM_T pDM_Odm = &(pHalData->odmpriv);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	int i;
+	u8 zero = 0;
+
+	pdmpriv->InitODMFlag = 0
+		| ODM_BB_DIG
+		| ODM_BB_RA_MASK
+		| ODM_BB_DYNAMIC_TXPWR
+		| ODM_BB_FA_CNT
+		| ODM_BB_RSSI_MONITOR
+		| ODM_BB_CCK_PD
+		| ODM_BB_PWR_SAVE
+		| ODM_BB_CFO_TRACKING
+		| ODM_MAC_EDCA_TURBO
+		| ODM_RF_TX_PWR_TRACK
+		| ODM_RF_CALIBRATION
+#ifdef CONFIG_ODM_ADAPTIVITY
+		| ODM_BB_ADAPTIVITY
+#endif
+		;
+
+	/*  */
+	/*  Pointer reference */
+	/*  */
+	/* ODM_CMNINFO_MAC_PHY_MODE pHalData->MacPhyMode92D */
+	/* 	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_MAC_PHY_MODE,&(pDM_Odm->u8_temp)); */
+
+	ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_ABILITY, pdmpriv->InitODMFlag);
+
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_TX_UNI, &(dvobj->traffic_stat.tx_bytes));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_RX_UNI, &(dvobj->traffic_stat.rx_bytes));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_WM_MODE, &(pmlmeext->cur_wireless_mode));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_SEC_CHNL_OFFSET, &(pHalData->nCur40MhzPrimeSC));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_SEC_MODE, &(Adapter->securitypriv.dot11PrivacyAlgrthm));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_BW, &(pHalData->CurrentChannelBW));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_CHNL, &(pHalData->CurrentChannel));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_NET_CLOSED, &(Adapter->net_closed));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_MP_MODE, &zero);
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_BAND, &(pHalData->CurrentBandType));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_FORCED_IGI_LB, &(pHalData->u1ForcedIgiLb));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_FORCED_RATE, &(pHalData->ForcedDataRate));
+
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_SCAN, &(pmlmepriv->bScanInProcess));
+	ODM_CmnInfoHook(pDM_Odm, ODM_CMNINFO_POWER_SAVING, &(pwrctrlpriv->bpower_saving));
+
+
+	for (i = 0; i < NUM_STA; i++)
+		ODM_CmnInfoPtrArrayHook(pDM_Odm, ODM_CMNINFO_STA_STATUS, i, NULL);
+}
+
+void rtl8723b_InitHalDm(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	PDM_ODM_T pDM_Odm = &(pHalData->odmpriv);
+
+	pdmpriv->DM_Type = DM_Type_ByDriver;
+	pdmpriv->DMFlag = DYNAMIC_FUNC_DISABLE;
+
+	pdmpriv->DMFlag |= DYNAMIC_FUNC_BT;
+
+	pdmpriv->InitDMFlag = pdmpriv->DMFlag;
+
+	Update_ODM_ComInfo_8723b(Adapter);
+
+	ODM_DMInit(pDM_Odm);
+}
+
+void rtl8723b_HalDmWatchDog(struct adapter *Adapter)
+{
+	bool bFwCurrentInPSMode = false;
+	bool bFwPSAwake = true;
+	u8 hw_init_completed = false;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	hw_init_completed = Adapter->hw_init_completed;
+
+	if (hw_init_completed == false)
+		goto skip_dm;
+
+	bFwCurrentInPSMode = adapter_to_pwrctl(Adapter)->bFwCurrentInPSMode;
+	rtw_hal_get_hwreg(Adapter, HW_VAR_FWLPS_RF_ON, (u8 *)(&bFwPSAwake));
+
+	if (
+		(hw_init_completed == true) &&
+		((!bFwCurrentInPSMode) && bFwPSAwake)
+	) {
+		/*  */
+		/*  Calculate Tx/Rx statistics. */
+		/*  */
+		dm_CheckStatistics(Adapter);
+		rtw_hal_check_rxfifo_full(Adapter);
+	}
+
+	/* ODM */
+	if (hw_init_completed == true) {
+		u8 bLinked = false;
+		u8 bsta_state = false;
+		u8 bBtDisabled = true;
+
+		if (rtw_linked_check(Adapter)) {
+			bLinked = true;
+			if (check_fwstate(&Adapter->mlmepriv, WIFI_STATION_STATE))
+				bsta_state = true;
+		}
+
+		ODM_CmnInfoUpdate(&pHalData->odmpriv, ODM_CMNINFO_LINK, bLinked);
+		ODM_CmnInfoUpdate(&pHalData->odmpriv, ODM_CMNINFO_STATION_STATE, bsta_state);
+
+		/* ODM_CmnInfoUpdate(&pHalData->odmpriv , ODM_CMNINFO_RSSI_MIN, pdmpriv->MinUndecoratedPWDBForDM); */
+
+		bBtDisabled = rtw_btcoex_IsBtDisabled(Adapter);
+
+		ODM_CmnInfoUpdate(&pHalData->odmpriv, ODM_CMNINFO_BT_ENABLED, ((bBtDisabled == true)?false:true));
+
+		ODM_DMWatchdog(&pHalData->odmpriv);
+	}
+
+skip_dm:
+	return;
+}
+
+void rtl8723b_hal_dm_in_lps(struct adapter *padapter)
+{
+	u32 PWDB_rssi = 0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	DBG_871X("%s, RSSI_Min =%d\n", __func__, pDM_Odm->RSSI_Min);
+
+	/* update IGI */
+	ODM_Write_DIG(pDM_Odm, pDM_Odm->RSSI_Min);
+
+
+	/* set rssi to fw */
+	psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
+	if (psta && (psta->rssi_stat.UndecoratedSmoothedPWDB > 0)) {
+		PWDB_rssi = (psta->mac_id | (psta->rssi_stat.UndecoratedSmoothedPWDB<<16));
+
+		rtl8723b_set_rssi_cmd(padapter, (u8 *)&PWDB_rssi);
+	}
+
+}
+
+void rtl8723b_HalDmWatchDog_in_LPS(struct adapter *Adapter)
+{
+	u8 bLinked = false;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+	struct sta_priv *pstapriv = &Adapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	if (Adapter->hw_init_completed == false)
+		goto skip_lps_dm;
+
+
+	if (rtw_linked_check(Adapter))
+		bLinked = true;
+
+	ODM_CmnInfoUpdate(&pHalData->odmpriv, ODM_CMNINFO_LINK, bLinked);
+
+	if (bLinked == false)
+		goto skip_lps_dm;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_RSSI_MONITOR))
+		goto skip_lps_dm;
+
+
+	/* ODM_DMWatchdog(&pHalData->odmpriv); */
+	/* Do DIG by RSSI In LPS-32K */
+
+      /* 1 Find MIN-RSSI */
+	psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
+	if (psta == NULL)
+		goto skip_lps_dm;
+
+	pdmpriv->EntryMinUndecoratedSmoothedPWDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
+
+	DBG_871X("CurIGValue =%d, EntryMinUndecoratedSmoothedPWDB = %d\n", pDM_DigTable->CurIGValue, pdmpriv->EntryMinUndecoratedSmoothedPWDB);
+
+	if (pdmpriv->EntryMinUndecoratedSmoothedPWDB <= 0)
+		goto skip_lps_dm;
+
+	pdmpriv->MinUndecoratedPWDBForDM = pdmpriv->EntryMinUndecoratedSmoothedPWDB;
+
+	pDM_Odm->RSSI_Min = pdmpriv->MinUndecoratedPWDBForDM;
+
+	/* if (pDM_DigTable->CurIGValue != pDM_Odm->RSSI_Min) */
+	if (
+		(pDM_DigTable->CurIGValue > pDM_Odm->RSSI_Min + 5) ||
+		(pDM_DigTable->CurIGValue < pDM_Odm->RSSI_Min - 5)
+	)
+		rtw_dm_in_lps_wk_cmd(Adapter);
+
+
+skip_lps_dm:
+
+	return;
+
+}
+
+void rtl8723b_init_dm_priv(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+
+	memset(pdmpriv, 0, sizeof(struct dm_priv));
+	Init_ODM_ComInfo_8723b(Adapter);
+}
diff -Nurp linux.old/3rdparty/rtl8723bs/hal/rtl8723b_hal_init.c linux/3rdparty/rtl8723bs/hal/rtl8723b_hal_init.c
--- linux.old/3rdparty/rtl8723bs/hal/rtl8723b_hal_init.c	1970-01-01 02:00:00.000000000 +0200
+++ linux/3rdparty/rtl8723bs/hal/rtl8723b_hal_init.c	2017-01-20 22:14:45.000000000 +0200
@@ -0,0 +1,4546 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2013 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.
+ *
+ ******************************************************************************/
+#define _HAL_INIT_C_
+
+#include <linux/firmware.h>
+#include <linux/slab.h>
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+#include "hal_com_h2c.h"
+
+static void _FWDownloadEnable(struct adapter *padapter, bool enable)
+{
+	u8 tmp, count = 0;
+
+	if (enable) {
+		/*  8051 enable */
+		tmp = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+		rtw_write8(padapter, REG_SYS_FUNC_EN+1, tmp|0x04);
+
+		tmp = rtw_read8(padapter, REG_MCUFWDL);
+		rtw_write8(padapter, REG_MCUFWDL, tmp|0x01);
+
+		do {
+			tmp = rtw_read8(padapter, REG_MCUFWDL);
+			if (tmp & 0x01)
+				break;
+			rtw_write8(padapter, REG_MCUFWDL, tmp|0x01);
+			msleep(1);
+		} while (count++ < 100);
+
+		if (count > 0)
+			DBG_871X("%s: !!!!!!!!Write 0x80 Fail!: count = %d\n", __func__, count);
+
+		/*  8051 reset */
+		tmp = rtw_read8(padapter, REG_MCUFWDL+2);
+		rtw_write8(padapter, REG_MCUFWDL+2, tmp&0xf7);
+	} else {
+		/*  MCU firmware download disable. */
+		tmp = rtw_read8(padapter, REG_MCUFWDL);
+		rtw_write8(padapter, REG_MCUFWDL, tmp&0xfe);
+	}
+}
+
+static int _BlockWrite(struct adapter *padapter, void *buffer, u32 buffSize)
+{
+	int ret = _SUCCESS;
+
+	u32 blockSize_p1 = 4; /*  (Default) Phase #1 : PCI muse use 4-byte write to download FW */
+	u32 blockSize_p2 = 8; /*  Phase #2 : Use 8-byte, if Phase#1 use big size to write FW. */
+	u32 blockSize_p3 = 1; /*  Phase #3 : Use 1-byte, the remnant of FW image. */
+	u32 blockCount_p1 = 0, blockCount_p2 = 0, blockCount_p3 = 0;
+	u32 remainSize_p1 = 0, remainSize_p2 = 0;
+	u8 *bufferPtr = (u8 *)buffer;
+	u32 i = 0, offset = 0;
+
+/* 	printk("====>%s %d\n", __func__, __LINE__); */
+
+	/* 3 Phase #1 */
+	blockCount_p1 = buffSize / blockSize_p1;
+	remainSize_p1 = buffSize % blockSize_p1;
+
+	if (blockCount_p1) {
+		RT_TRACE(
+			_module_hal_init_c_,
+			_drv_notice_,
+			(
+				"_BlockWrite: [P1] buffSize(%d) blockSize_p1(%d) blockCount_p1(%d) remainSize_p1(%d)\n",
+				buffSize,
+				blockSize_p1,
+				blockCount_p1,
+				remainSize_p1
+			)
+		);
+	}
+
+	for (i = 0; i < blockCount_p1; i++) {
+		ret = rtw_write32(padapter, (FW_8723B_START_ADDRESS + i * blockSize_p1), *((u32 *)(bufferPtr + i * blockSize_p1)));
+		if (ret == _FAIL) {
+			printk("====>%s %d i:%d\n", __func__, __LINE__, i);
+			goto exit;
+		}
+	}
+
+	/* 3 Phase #2 */
+	if (remainSize_p1) {
+		offset = blockCount_p1 * blockSize_p1;
+
+		blockCount_p2 = remainSize_p1/blockSize_p2;
+		remainSize_p2 = remainSize_p1%blockSize_p2;
+
+		if (blockCount_p2) {
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_notice_,
+					(
+						"_BlockWrite: [P2] buffSize_p2(%d) blockSize_p2(%d) blockCount_p2(%d) remainSize_p2(%d)\n",
+						(buffSize-offset),
+						blockSize_p2,
+						blockCount_p2,
+						remainSize_p2
+					)
+				);
+		}
+
+	}
+
+	/* 3 Phase #3 */
+	if (remainSize_p2) {
+		offset = (blockCount_p1 * blockSize_p1) + (blockCount_p2 * blockSize_p2);
+
+		blockCount_p3 = remainSize_p2 / blockSize_p3;
+
+		RT_TRACE(_module_hal_init_c_, _drv_notice_,
+				("_BlockWrite: [P3] buffSize_p3(%d) blockSize_p3(%d) blockCount_p3(%d)\n",
+				(buffSize-offset), blockSize_p3, blockCount_p3));
+
+		for (i = 0; i < blockCount_p3; i++) {
+			ret = rtw_write8(padapter, (FW_8723B_START_ADDRESS + offset + i), *(bufferPtr + offset + i));
+
+			if (ret == _FAIL) {
+				printk("====>%s %d i:%d\n", __func__, __LINE__, i);
+				goto exit;
+			}
+		}
+	}
+exit:
+	return ret;
+}
+
+static int _PageWrite(
+	struct adapter *padapter,
+	u32 page,
+	void *buffer,
+	u32 size
+)
+{
+	u8 value8;
+	u8 u8Page = (u8) (page & 0x07);
+
+	value8 = (rtw_read8(padapter, REG_MCUFWDL+2) & 0xF8) | u8Page;
+	rtw_write8(padapter, REG_MCUFWDL+2, value8);
+
+	return _BlockWrite(padapter, buffer, size);
+}
+
+static int _WriteFW(struct adapter *padapter, void *buffer, u32 size)
+{
+	/*  Since we need dynamic decide method of dwonload fw, so we call this function to get chip version. */
+	/*  We can remove _ReadChipVersion from ReadpadapterInfo8192C later. */
+	int ret = _SUCCESS;
+	u32 pageNums, remainSize;
+	u32 page, offset;
+	u8 *bufferPtr = (u8 *)buffer;
+
+	pageNums = size / MAX_DLFW_PAGE_SIZE;
+	/* RT_ASSERT((pageNums <= 4), ("Page numbers should not greater then 4\n")); */
+	remainSize = size % MAX_DLFW_PAGE_SIZE;
+
+	for (page = 0; page < pageNums; page++) {
+		offset = page * MAX_DLFW_PAGE_SIZE;
+		ret = _PageWrite(padapter, page, bufferPtr+offset, MAX_DLFW_PAGE_SIZE);
+
+		if (ret == _FAIL) {
+			printk("====>%s %d\n", __func__, __LINE__);
+			goto exit;
+		}
+	}
+
+	if (remainSize) {
+		offset = pageNums * MAX_DLFW_PAGE_SIZE;
+		page = pageNums;
+		ret = _PageWrite(padapter, page, bufferPtr+offset, remainSize);
+
+		if (ret == _FAIL) {
+			printk("====>%s %d\n", __func__, __LINE__);
+			goto exit;
+		}
+	}
+	RT_TRACE(_module_hal_init_c_, _drv_info_, ("_WriteFW Done- for Normal chip.\n"));
+
+exit:
+	return ret;
+}
+
+void _8051Reset8723(struct adapter *padapter)
+{
+	u8 cpu_rst;
+	u8 io_rst;
+
+
+	/*  Reset 8051(WLMCU) IO wrapper */
+	/*  0x1c[8] = 0 */
+	/*  Suggested by Isaac@SD1 and Gimmy@SD1, coding by Lucas@20130624 */
+	io_rst = rtw_read8(padapter, REG_RSV_CTRL+1);
+	io_rst &= ~BIT(0);
+	rtw_write8(padapter, REG_RSV_CTRL+1, io_rst);
+
+	cpu_rst = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+	cpu_rst &= ~BIT(2);
+	rtw_write8(padapter, REG_SYS_FUNC_EN+1, cpu_rst);
+
+	/*  Enable 8051 IO wrapper */
+	/*  0x1c[8] = 1 */
+	io_rst = rtw_read8(padapter, REG_RSV_CTRL+1);
+	io_rst |= BIT(0);
+	rtw_write8(padapter, REG_RSV_CTRL+1, io_rst);
+
+	cpu_rst = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+	cpu_rst |= BIT(2);
+	rtw_write8(padapter, REG_SYS_FUNC_EN+1, cpu_rst);
+
+	DBG_8192C("%s: Finish\n", __func__);
+}
+
+static s32 polling_fwdl_chksum(
+	struct adapter *adapter, u32 min_cnt, u32 timeout_ms
+)
+{
+	s32 ret = _FAIL;
+	u32 value32;
+	unsigned long start = jiffies;
+	u32 cnt = 0;
+
+	/* polling CheckSum report */
+	do {
+		cnt++;
+		value32 = rtw_read32(adapter, REG_MCUFWDL);
+		if (value32 & FWDL_ChkSum_rpt || adapter->bSurpriseRemoved || adapter->bDriverStopped)
+			break;
+		yield();
+	} while (jiffies_to_msecs(jiffies-start) < timeout_ms || cnt < min_cnt);
+
+	if (!(value32 & FWDL_ChkSum_rpt)) {
+		goto exit;
+	}
+
+	if (g_fwdl_chksum_fail) {
+		DBG_871X("%s: fwdl test case: fwdl_chksum_fail\n", __func__);
+		g_fwdl_chksum_fail--;
+		goto exit;
+	}
+
+	ret = _SUCCESS;
+
+exit:
+	DBG_871X(
+		"%s: Checksum report %s! (%u, %dms), REG_MCUFWDL:0x%08x\n",
+		__func__,
+		(ret == _SUCCESS) ? "OK" : "Fail",
+		cnt,
+		jiffies_to_msecs(jiffies-start),
+		value32
+	);
+
+	return ret;
+}
+
+static s32 _FWFreeToGo(struct adapter *adapter, u32 min_cnt, u32 timeout_ms)
+{
+	s32 ret = _FAIL;
+	u32 value32;
+	unsigned long start = jiffies;
+	u32 cnt = 0;
+
+	value32 = rtw_read32(adapter, REG_MCUFWDL);
+	value32 |= MCUFWDL_RDY;
+	value32 &= ~WINTINI_RDY;
+	rtw_write32(adapter, REG_MCUFWDL, value32);
+
+	_8051Reset8723(adapter);
+
+	/*  polling for FW ready */
+	do {
+		cnt++;
+		value32 = rtw_read32(adapter, REG_MCUFWDL);
+		if (value32 & WINTINI_RDY || adapter->bSurpriseRemoved || adapter->bDriverStopped)
+			break;
+		yield();
+	} while (jiffies_to_msecs(jiffies - start) < timeout_ms || cnt < min_cnt);
+
+	if (!(value32 & WINTINI_RDY)) {
+		goto exit;
+	}
+
+	if (g_fwdl_wintint_rdy_fail) {
+		DBG_871X("%s: fwdl test case: wintint_rdy_fail\n", __func__);
+		g_fwdl_wintint_rdy_fail--;
+		goto exit;
+	}
+
+	ret = _SUCCESS;
+
+exit:
+	DBG_871X(
+		"%s: Polling FW ready %s! (%u, %dms), REG_MCUFWDL:0x%08x\n",
+		__func__,
+		(ret == _SUCCESS) ? "OK" : "Fail",
+		cnt,
+		jiffies_to_msecs(jiffies-start),
+		value32
+	);
+
+	return ret;
+}
+
+#define IS_FW_81xxC(padapter)	(((GET_HAL_DATA(padapter))->FirmwareSignature & 0xFFF0) == 0x88C0)
+
+void rtl8723b_FirmwareSelfReset(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	u8 u1bTmp;
+	u8 Delay = 100;
+
+	if (
+		!(IS_FW_81xxC(padapter) && ((pHalData->FirmwareVersion < 0x21) || (pHalData->FirmwareVersion == 0x21 && pHalData->FirmwareSubVersion < 0x01)))
+	) { /*  after 88C Fw v33.1 */
+		/* 0x1cf = 0x20. Inform 8051 to reset. 2009.12.25. tynli_test */
+		rtw_write8(padapter, REG_HMETFR+3, 0x20);
+
+		u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+		while (u1bTmp & BIT2) {
+			Delay--;
+			if (Delay == 0)
+				break;
+			udelay(50);
+			u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+		}
+		RT_TRACE(_module_hal_init_c_, _drv_notice_, ("-%s: 8051 reset success (%d)\n", __func__, Delay));
+
+		if (Delay == 0) {
+			RT_TRACE(_module_hal_init_c_, _drv_notice_, ("%s: Force 8051 reset!!!\n", __func__));
+			/* force firmware reset */
+			u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+			rtw_write8(padapter, REG_SYS_FUNC_EN+1, u1bTmp&(~BIT2));
+		}
+	}
+}
+
+/*  */
+/* 	Description: */
+/* 		Download 8192C firmware code. */
+/*  */
+/*  */
+s32 rtl8723b_FirmwareDownload(struct adapter *padapter, bool  bUsedWoWLANFw)
+{
+	s32 rtStatus = _SUCCESS;
+	u8 write_fw = 0;
+	unsigned long fwdl_start_time;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	struct rt_firmware *pFirmware;
+	struct rt_firmware *pBTFirmware;
+	struct rt_firmware_hdr *pFwHdr = NULL;
+	u8 *pFirmwareBuf;
+	u32 FirmwareLen;
+	const struct firmware *fw;
+	struct device *device = dvobj_to_dev(padapter->dvobj);
+	u8 *fwfilepath;
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	u8 tmp_ps;
+
+	RT_TRACE(_module_hal_init_c_, _drv_info_, ("+%s\n", __func__));
+#ifdef CONFIG_WOWLAN
+	RT_TRACE(_module_hal_init_c_, _drv_notice_, ("+%s, bUsedWoWLANFw:%d\n", __func__, bUsedWoWLANFw));
+#endif
+	pFirmware = kzalloc(sizeof(struct rt_firmware), GFP_KERNEL);
+	pBTFirmware = kzalloc(sizeof(struct rt_firmware), GFP_KERNEL);
+
+	if (!pFirmware || !pBTFirmware) {
+		rtStatus = _FAIL;
+		goto exit;
+	}
+
+	tmp_ps = rtw_read8(padapter, 0xa3);
+	tmp_ps &= 0xf8;
+	tmp_ps |= 0x02;
+	/* 1. write 0xA3[:2:0] = 3b'010 */
+	rtw_write8(padapter, 0xa3, tmp_ps);
+	/* 2. read power_state = 0xA0[1:0] */
+	tmp_ps = rtw_read8(padapter, 0xa0);
+	tmp_ps &= 0x03;
+	if (tmp_ps != 0x01) {
+		DBG_871X(FUNC_ADPT_FMT" tmp_ps =%x\n", FUNC_ADPT_ARG(padapter), tmp_ps);
+		pdbgpriv->dbg_downloadfw_pwr_state_cnt++;
+	}
+
+#ifdef CONFIG_WOWLAN
+	if (bUsedWoWLANFw)
+		fwfilepath = "rtlwifi/rtl8723bs_wowlan.bin";
+	else
+#endif /*  CONFIG_WOWLAN */
+		fwfilepath = "rtlwifi/rtl8723bs_nic.bin";
+
+	pr_info("rtl8723bs: accquire FW from file:%s\n", fwfilepath);
+
+	rtStatus = request_firmware(&fw, fwfilepath, device);
+	if (rtStatus) {
+		pr_err("Request firmware failed with error 0x%x\n", rtStatus);
+		rtStatus = _FAIL;
+		goto exit;
+	}
+
+	if (!fw) {
+		pr_err("Firmware %s not available\n", fwfilepath);
+		rtStatus = _FAIL;
+		goto exit;
+	}
+
+	if (fw->size > FW_8723B_SIZE) {
+		rtStatus = _FAIL;
+		RT_TRACE(
+			_module_hal_init_c_,
+			_drv_err_,
+			("Firmware size exceed 0x%X. Check it.\n", FW_8188E_SIZE)
+		);
+		goto exit;
+	}
+
+	pFirmware->szFwBuffer = kzalloc(fw->size, GFP_KERNEL);
+	if (!pFirmware->szFwBuffer) {
+		rtStatus = _FAIL;
+		goto exit;
+	}
+
+	memcpy(pFirmware->szFwBuffer, fw->data, fw->size);
+	pFirmware->ulFwLength = fw->size;
+	release_firmware(fw);
+	if (pFirmware->ulFwLength > FW_8723B_SIZE) {
+		rtStatus = _FAIL;
+		DBG_871X_LEVEL(_drv_emerg_, "Firmware size:%u exceed %u\n", pFirmware->ulFwLength, FW_8723B_SIZE);
+		goto exit;
+	}
+
+	pFirmwareBuf = pFirmware->szFwBuffer;
+	FirmwareLen = pFirmware->ulFwLength;
+
+	/*  To Check Fw header. Added by tynli. 2009.12.04. */
+	pFwHdr = (struct rt_firmware_hdr *)pFirmwareBuf;
+
+	pHalData->FirmwareVersion =  le16_to_cpu(pFwHdr->Version);
+	pHalData->FirmwareSubVersion = le16_to_cpu(pFwHdr->Subversion);
+	pHalData->FirmwareSignature = le16_to_cpu(pFwHdr->Signature);
+
+	DBG_871X(
+		"%s: fw_ver =%x fw_subver =%04x sig = 0x%x, Month =%02x, Date =%02x, Hour =%02x, Minute =%02x\n",
+		__func__,
+		pHalData->FirmwareVersion,
+		pHalData->FirmwareSubVersion,
+		pHalData->FirmwareSignature,
+		pFwHdr->Month,
+		pFwHdr->Date,
+		pFwHdr->Hour,
+		pFwHdr->Minute
+	);
+
+	if (IS_FW_HEADER_EXIST_8723B(pFwHdr)) {
+		DBG_871X("%s(): Shift for fw header!\n", __func__);
+		/*  Shift 32 bytes for FW header */
+		pFirmwareBuf = pFirmwareBuf + 32;
+		FirmwareLen = FirmwareLen - 32;
+	}
+
+	/*  Suggested by Filen. If 8051 is running in RAM code, driver should inform Fw to reset by itself, */
+	/*  or it will cause download Fw fail. 2010.02.01. by tynli. */
+	if (rtw_read8(padapter, REG_MCUFWDL) & RAM_DL_SEL) { /* 8051 RAM code */
+		rtw_write8(padapter, REG_MCUFWDL, 0x00);
+		rtl8723b_FirmwareSelfReset(padapter);
+	}
+
+	_FWDownloadEnable(padapter, true);
+	fwdl_start_time = jiffies;
+	while (
+		!padapter->bDriverStopped &&
+		!padapter->bSurpriseRemoved &&
+		(write_fw++ < 3 || jiffies_to_msecs(jiffies - fwdl_start_time) < 500)
+	) {
+		/* reset FWDL chksum */
+		rtw_write8(padapter, REG_MCUFWDL, rtw_read8(padapter, REG_MCUFWDL)|FWDL_ChkSum_rpt);
+
+		rtStatus = _WriteFW(padapter, pFirmwareBuf, FirmwareLen);
+		if (rtStatus != _SUCCESS)
+			continue;
+
+		rtStatus = polling_fwdl_chksum(padapter, 5, 50);
+		if (rtStatus == _SUCCESS)
+			break;
+	}
+	_FWDownloadEnable(padapter, false);
+	if (_SUCCESS != rtStatus)
+		goto fwdl_stat;
+
+	rtStatus = _FWFreeToGo(padapter, 10, 200);
+	if (_SUCCESS != rtStatus)
+		goto fwdl_stat;
+
+fwdl_stat:
+	DBG_871X(
+		"FWDL %s. write_fw:%u, %dms\n",
+		(rtStatus == _SUCCESS)?"success":"fail",
+		write_fw,
+		jiffies_to_msecs(jiffies - fwdl_start_time)
+	);
+
+exit:
+	kfree(pFirmware->szFwBuffer);
+	kfree(pFirmware);
+	kfree(pBTFirmware);
+	DBG_871X(" <=== rtl8723b_FirmwareDownload()\n");
+	return rtStatus;
+}
+
+void rtl8723b_InitializeFirmwareVars(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	/*  Init Fw LPS related. */
+	adapter_to_pwrctl(padapter)->bFwCurrentInPSMode = false;
+
+	/* Init H2C cmd. */
+	rtw_write8(padapter, REG_HMETFR, 0x0f);
+
+	/*  Init H2C counter. by tynli. 2009.12.09. */
+	pHalData->LastHMEBoxNum = 0;
+/* pHalData->H2CQueueHead = 0; */
+/* pHalData->H2CQueueTail = 0; */
+/* pHalData->H2CStopInsertQueue = false; */
+}
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+/*  */
+
+/*  */
+/*  Description: Prepare some information to Fw for WoWLAN. */
+/* (1) Download wowlan Fw. */
+/* (2) Download RSVD page packets. */
+/* (3) Enable AP offload if needed. */
+/*  */
+/*  2011.04.12 by tynli. */
+/*  */
+void SetFwRelatedForWoWLAN8723b(
+	struct adapter *padapter, u8 bHostIsGoingtoSleep
+)
+{
+	int	status = _FAIL;
+	/*  */
+	/*  1. Before WoWLAN we need to re-download WoWLAN Fw. */
+	/*  */
+	status = rtl8723b_FirmwareDownload(padapter, bHostIsGoingtoSleep);
+	if (status != _SUCCESS) {
+		DBG_871X("SetFwRelatedForWoWLAN8723b(): Re-Download Firmware failed!!\n");
+		return;
+	} else {
+		DBG_871X("SetFwRelatedForWoWLAN8723b(): Re-Download Firmware Success !!\n");
+	}
+	/*  */
+	/*  2. Re-Init the variables about Fw related setting. */
+	/*  */
+	rtl8723b_InitializeFirmwareVars(padapter);
+}
+#endif /* CONFIG_WOWLAN */
+
+static void rtl8723b_free_hal_data(struct adapter *padapter)
+{
+}
+
+/*  */
+/* 				Efuse related code */
+/*  */
+static u8 hal_EfuseSwitchToBank(
+	struct adapter *padapter, u8 bank, bool bPseudoTest
+)
+{
+	u8 bRet = false;
+	u32 value32 = 0;
+#ifdef HAL_EFUSE_MEMORY
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	PEFUSE_HAL pEfuseHal = &pHalData->EfuseHal;
+#endif
+
+
+	DBG_8192C("%s: Efuse switch bank to %d\n", __func__, bank);
+	if (bPseudoTest) {
+#ifdef HAL_EFUSE_MEMORY
+		pEfuseHal->fakeEfuseBank = bank;
+#else
+		fakeEfuseBank = bank;
+#endif
+		bRet = true;
+	} else {
+		value32 = rtw_read32(padapter, EFUSE_TEST);
+		bRet = true;
+		switch (bank) {
+		case 0:
+			value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_WIFI_SEL_0);
+			break;
+		case 1:
+			value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_BT_SEL_0);
+			break;
+		case 2:
+			value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_BT_SEL_1);
+			break;
+		case 3:
+			value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_BT_SEL_2);
+			break;
+		default:
+			value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_WIFI_SEL_0);
+			bRet = false;
+			break;
+		}
+		rtw_write32(padapter, EFUSE_TEST, value32);
+	}
+
+	return bRet;
+}
+
+static void Hal_GetEfuseDefinition(
+	struct adapter *padapter,
+	u8 efuseType,
+	u8 type,
+	void *pOut,
+	bool bPseudoTest
+)
+{
+	switch (type) {
+	case TYPE_EFUSE_MAX_SECTION:
+		{
+			u8 *pMax_section;
+			pMax_section = (u8 *)pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pMax_section = EFUSE_MAX_SECTION_8723B;
+			else
+				*pMax_section = EFUSE_BT_MAX_SECTION;
+		}
+		break;
+
+	case TYPE_EFUSE_REAL_CONTENT_LEN:
+		{
+			u16 *pu2Tmp;
+			pu2Tmp = (u16 *)pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723B;
+			else
+				*pu2Tmp = EFUSE_BT_REAL_CONTENT_LEN;
+		}
+		break;
+
+	case TYPE_AVAILABLE_EFUSE_BYTES_BANK:
+		{
+			u16 *pu2Tmp;
+			pu2Tmp = (u16 *)pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723B-EFUSE_OOB_PROTECT_BYTES);
+			else
+				*pu2Tmp = (EFUSE_BT_REAL_BANK_CONTENT_LEN-EFUSE_PROTECT_BYTES_BANK);
+		}
+		break;
+
+	case TYPE_AVAILABLE_EFUSE_BYTES_TOTAL:
+		{
+			u16 *pu2Tmp;
+			pu2Tmp = (u16 *)pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723B-EFUSE_OOB_PROTECT_BYTES);
+			else
+				*pu2Tmp = (EFUSE_BT_REAL_CONTENT_LEN-(EFUSE_PROTECT_BYTES_BANK*3));
+		}
+		break;
+
+	case TYPE_EFUSE_MAP_LEN:
+		{
+			u16 *pu2Tmp;
+			pu2Tmp = (u16 *)pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu2Tmp = EFUSE_MAX_MAP_LEN;
+			else
+				*pu2Tmp = EFUSE_BT_MAP_LEN;
+		}
+		break;
+
+	case TYPE_EFUSE_PROTECT_BYTES_BANK:
+		{
+			u8 *pu1Tmp;
+			pu1Tmp = (u8 *)pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu1Tmp = EFUSE_OOB_PROTECT_BYTES;
+			else
+				*pu1Tmp = EFUSE_PROTECT_BYTES_BANK;
+		}
+		break;
+
+	case TYPE_EFUSE_CONTENT_LEN_BANK:
+		{
+			u16 *pu2Tmp;
+			pu2Tmp = (u16 *)pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723B;
+			else
+				*pu2Tmp = EFUSE_BT_REAL_BANK_CONTENT_LEN;
+		}
+		break;
+
+	default:
+		{
+			u8 *pu1Tmp;
+			pu1Tmp = (u8 *)pOut;
+			*pu1Tmp = 0;
+		}
+		break;
+	}
+}
+
+#define VOLTAGE_V25		0x03
+#define LDOE25_SHIFT	28
+
+/*  */
+/* 	The following is for compile ok */
+/* 	That should be merged with the original in the future */
+/*  */
+#define EFUSE_ACCESS_ON_8723			0x69	/*  For RTL8723 only. */
+#define EFUSE_ACCESS_OFF_8723			0x00	/*  For RTL8723 only. */
+#define REG_EFUSE_ACCESS_8723			0x00CF	/*  Efuse access protection for RTL8723 */
+
+/*  */
+static void Hal_BT_EfusePowerSwitch(
+	struct adapter *padapter, u8 bWrite, u8 PwrState
+)
+{
+	u8 tempval;
+	if (PwrState == true) {
+		/*  enable BT power cut */
+		/*  0x6A[14] = 1 */
+		tempval = rtw_read8(padapter, 0x6B);
+		tempval |= BIT(6);
+		rtw_write8(padapter, 0x6B, tempval);
+
+		/*  Attention!! Between 0x6A[14] and 0x6A[15] setting need 100us delay */
+		/*  So don't wirte 0x6A[14]= 1 and 0x6A[15]= 0 together! */
+		msleep(1);
+		/*  disable BT output isolation */
+		/*  0x6A[15] = 0 */
+		tempval = rtw_read8(padapter, 0x6B);
+		tempval &= ~BIT(7);
+		rtw_write8(padapter, 0x6B, tempval);
+	} else {
+		/*  enable BT output isolation */
+		/*  0x6A[15] = 1 */
+		tempval = rtw_read8(padapter, 0x6B);
+		tempval |= BIT(7);
+		rtw_write8(padapter, 0x6B, tempval);
+
+		/*  Attention!! Between 0x6A[14] and 0x6A[15] setting need 100us delay */
+		/*  So don't wirte 0x6A[14]= 1 and 0x6A[15]= 0 together! */
+
+		/*  disable BT power cut */
+		/*  0x6A[14] = 1 */
+		tempval = rtw_read8(padapter, 0x6B);
+		tempval &= ~BIT(6);
+		rtw_write8(padapter, 0x6B, tempval);
+	}
+
+}
+static void Hal_EfusePowerSwitch(
+	struct adapter *padapter, u8 bWrite, u8 PwrState
+)
+{
+	u8 tempval;
+	u16 tmpV16;
+
+
+	if (PwrState == true) {
+		/*  To avoid cannot access efuse regsiters after disable/enable several times during DTM test. */
+		/*  Suggested by SD1 IsaacHsu. 2013.07.08, added by tynli. */
+		tempval = rtw_read8(padapter, SDIO_LOCAL_BASE|SDIO_REG_HSUS_CTRL);
+		if (tempval & BIT(0)) { /*  SDIO local register is suspend */
+			u8 count = 0;
+
+
+			tempval &= ~BIT(0);
+			rtw_write8(padapter, SDIO_LOCAL_BASE|SDIO_REG_HSUS_CTRL, tempval);
+
+			/*  check 0x86[1:0]= 10'2h, wait power state to leave suspend */
+			do {
+				tempval = rtw_read8(padapter, SDIO_LOCAL_BASE|SDIO_REG_HSUS_CTRL);
+				tempval &= 0x3;
+				if (tempval == 0x02)
+					break;
+
+				count++;
+				if (count >= 100)
+					break;
+
+				mdelay(10);
+			} while (1);
+
+			if (count >= 100) {
+				DBG_8192C(FUNC_ADPT_FMT ": Leave SDIO local register suspend fail! Local 0x86 =%#X\n",
+					FUNC_ADPT_ARG(padapter), tempval);
+			} else {
+				DBG_8192C(FUNC_ADPT_FMT ": Leave SDIO local register suspend OK! Local 0x86 =%#X\n",
+					FUNC_ADPT_ARG(padapter), tempval);
+			}
+		}
+
+		rtw_write8(padapter, REG_EFUSE_ACCESS_8723, EFUSE_ACCESS_ON_8723);
+
+		/*  Reset: 0x0000h[28], default valid */
+		tmpV16 =  rtw_read16(padapter, REG_SYS_FUNC_EN);
+		if (!(tmpV16 & FEN_ELDR)) {
+			tmpV16 |= FEN_ELDR;
+			rtw_write16(padapter, REG_SYS_FUNC_EN, tmpV16);
+		}
+
+		/*  Clock: Gated(0x0008h[5]) 8M(0x0008h[1]) clock from ANA, default valid */
+		tmpV16 = rtw_read16(padapter, REG_SYS_CLKR);
+		if ((!(tmpV16 & LOADER_CLK_EN))  || (!(tmpV16 & ANA8M))) {
+			tmpV16 |= (LOADER_CLK_EN | ANA8M);
+			rtw_write16(padapter, REG_SYS_CLKR, tmpV16);
+		}
+
+		if (
[cell cut: 3369467 characters, first 2000000 kept]