// SPDX-License-Identifier: GPL-2.0
/******************************************************************************
 *
 * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
 *
 ******************************************************************************/
#define _RTW_MLME_EXT_C_

#include <linux/ieee80211.h>
#include <linux/etherdevice.h>
#include <asm/unaligned.h>

#include <osdep_service.h>
#include <drv_types.h>
#include <wifi.h>
#include <rtw_mlme_ext.h>
#include <wlan_bssdef.h>
#include <mlme_osdep.h>
#include <recv_osdep.h>

static u8 null_addr[ETH_ALEN] = {};

/* OUI definitions for the vendor specific IE */
const u8 RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
const u8 WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
static const u8 WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
static const u8 P2P_OUI[] = {0x50, 0x6F, 0x9A, 0x09};

static const u8 WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};

const u8 WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
const u8 RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};

/* MCS rate definitions */
const u8 MCS_rate_1R[16] = {
        0xff, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

/* ChannelPlan definitions */
static struct rt_channel_plan_2g RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
        {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},              /*  0x00, RT_CHANNEL_DOMAIN_2G_WORLD , Passive scan CH 12, 13 */
        {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},              /*  0x01, RT_CHANNEL_DOMAIN_2G_ETSI1 */
        {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11},                      /*  0x02, RT_CHANNEL_DOMAIN_2G_FCC1 */
        {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14},  /*  0x03, RT_CHANNEL_DOMAIN_2G_MIKK1 */
        {{10, 11, 12, 13}, 4},                                  /*  0x04, RT_CHANNEL_DOMAIN_2G_ETSI2 */
        {{}, 0},                                                                        /*  0x05, RT_CHANNEL_DOMAIN_2G_NULL */
};

static struct rt_channel_plan_map RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
        /*  0x00 ~ 0x1F , Old Define ===== */
        {0x02}, /* 0x00, RT_CHANNEL_DOMAIN_FCC */
        {0x02}, /* 0x01, RT_CHANNEL_DOMAIN_IC */
        {0x01}, /* 0x02, RT_CHANNEL_DOMAIN_ETSI */
        {0x01}, /* 0x03, RT_CHANNEL_DOMAIN_SPAIN */
        {0x01}, /* 0x04, RT_CHANNEL_DOMAIN_FRANCE */
        {0x03}, /* 0x05, RT_CHANNEL_DOMAIN_MKK */
        {0x03}, /* 0x06, RT_CHANNEL_DOMAIN_MKK1 */
        {0x01}, /* 0x07, RT_CHANNEL_DOMAIN_ISRAEL */
        {0x03}, /* 0x08, RT_CHANNEL_DOMAIN_TELEC */
        {0x03}, /* 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN */
        {0x00}, /* 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13 */
        {0x02}, /* 0x0B, RT_CHANNEL_DOMAIN_TAIWAN */
        {0x01}, /* 0x0C, RT_CHANNEL_DOMAIN_CHINA */
        {0x02}, /* 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO */
        {0x02}, /* 0x0E, RT_CHANNEL_DOMAIN_KOREA */
        {0x02}, /* 0x0F, RT_CHANNEL_DOMAIN_TURKEY */
        {0x01}, /* 0x10, RT_CHANNEL_DOMAIN_JAPAN */
        {0x02}, /* 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS */
        {0x01}, /* 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
        {0x00}, /* 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G */
        {0x02}, /* 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS */
        {0x00}, /* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */
        {0x00}, /* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */
        {0x03}, /* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
        {0x05}, /* 0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS */
        {0x02}, /* 0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS */
        {0x00}, /* 0x1A, */
        {0x00}, /* 0x1B, */
        {0x00}, /* 0x1C, */
        {0x00}, /* 0x1D, */
        {0x00}, /* 0x1E, */
        {0x05}, /* 0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G */
        /*  0x20 ~ 0x7F , New Define ===== */
        {0x00}, /* 0x20, RT_CHANNEL_DOMAIN_WORLD_NULL */
        {0x01}, /* 0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL */
        {0x02}, /* 0x22, RT_CHANNEL_DOMAIN_FCC1_NULL */
        {0x03}, /* 0x23, RT_CHANNEL_DOMAIN_MKK1_NULL */
        {0x04}, /* 0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL */
        {0x02}, /* 0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1 */
        {0x00}, /* 0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1 */
        {0x03}, /* 0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1 */
        {0x00}, /* 0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1 */
        {0x00}, /* 0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2 */
        {0x00}, /* 0x2A, */
        {0x00}, /* 0x2B, */
        {0x00}, /* 0x2C, */
        {0x00}, /* 0x2D, */
        {0x00}, /* 0x2E, */
        {0x00}, /* 0x2F, */
        {0x00}, /* 0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3 */
        {0x00}, /* 0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4 */
        {0x00}, /* 0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5 */
        {0x00}, /* 0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6 */
        {0x02}, /* 0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7 */
        {0x00}, /* 0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2 */
        {0x00}, /* 0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3 */
        {0x03}, /* 0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2 */
        {0x03}, /* 0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3 */
        {0x02}, /* 0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1 */
        {0x00}, /* 0x3A, */
        {0x00}, /* 0x3B, */
        {0x00}, /* 0x3C, */
        {0x00}, /* 0x3D, */
        {0x00}, /* 0x3E, */
        {0x00}, /* 0x3F, */
        {0x02}, /* 0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2 */
        {0x03}, /* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G */
};

static const struct rt_channel_plan_map RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {
        0x03
}; /* use the combination for max channel numbers */

/*
 * Search the @param channel_num in given @param channel_set
 * @ch_set: the given channel set
 * @ch: the given channel number
 *
 * return the index of channel_num in channel_set, -1 if not found
 */
int rtw_ch_set_search_ch(struct rt_channel_info *ch_set, const u32 ch)
{
        int i;

        for (i = 0; ch_set[i].ChannelNum != 0; i++) {
                if (ch == ch_set[i].ChannelNum)
                        break;
        }

        if (i >= ch_set[i].ChannelNum)
                return -1;
        return i;
}

struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
{
        struct xmit_frame *pmgntframe;
        struct xmit_buf *pxmitbuf;

        pmgntframe = rtw_alloc_xmitframe(pxmitpriv);
        if (!pmgntframe)
                return NULL;

        pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv);
        if (!pxmitbuf) {
                rtw_free_xmitframe(pxmitpriv, pmgntframe);
                return NULL;
        }
        pmgntframe->frame_tag = MGNT_FRAMETAG;
        pmgntframe->pxmitbuf = pxmitbuf;
        pmgntframe->buf_addr = pxmitbuf->pbuf;
        pxmitbuf->priv_data = pmgntframe;
        return pmgntframe;
}

/****************************************************************************

Following are some TX functions for WiFi MLME

*****************************************************************************/

void update_mgnt_tx_rate(struct adapter *padapter, u8 rate)
{
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;

        pmlmeext->tx_rate = rate;
}

void update_mgntframe_attrib(struct adapter *padapter, struct pkt_attrib *pattrib)
{
        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->cur_wireless_mode & WIRELESS_11B)
                pattrib->raid = 6;/* b mode */
        else
                pattrib->raid = 5;/* a/g mode */

        pattrib->encrypt = _NO_PRIVACY_;
        pattrib->bswenc = false;

        pattrib->qos_en = false;
        pattrib->ht_en = false;
        pattrib->bwmode = HT_CHANNEL_WIDTH_20;
        pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
        pattrib->sgi = false;

        pattrib->seqnum = pmlmeext->mgnt_seq;

        pattrib->retry_ctrl = true;
}

static void dump_mgntframe(struct adapter *padapter,
                           struct xmit_frame *pmgntframe)
{
        if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
                return;

        rtw_hal_mgnt_xmit(padapter, pmgntframe);
}

static s32 dump_mgntframe_and_wait(struct adapter *padapter,
                                   struct xmit_frame *pmgntframe,
                                   int timeout_ms)
{
        s32 ret = _FAIL;
        struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
        struct submit_ctx sctx;

        if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
                return ret;

        rtw_sctx_init(&sctx, timeout_ms);
        pxmitbuf->sctx = &sctx;

        ret = rtw_hal_mgnt_xmit(padapter, pmgntframe);

        if (ret == _SUCCESS)
                ret = rtw_sctx_wait(&sctx);

        return ret;
}

static s32 dump_mgntframe_and_wait_ack(struct adapter *padapter,
                                       struct xmit_frame *pmgntframe)
{
        s32 ret = _FAIL;
        u32 timeout_ms = 500;/*   500ms */
        struct xmit_priv *pxmitpriv = &padapter->xmitpriv;

        if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
                return -1;

        if (mutex_lock_interruptible(&pxmitpriv->ack_tx_mutex))
                return _FAIL;
        pxmitpriv->ack_tx = true;

        pmgntframe->ack_report = 1;
        if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS)
                ret = rtw_ack_tx_wait(pxmitpriv, timeout_ms);

        pxmitpriv->ack_tx = false;
        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;
        uint ssid_len_ori;
        int len_diff = 0;

        ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);

        if (ssid_ie && ssid_len_ori > 0) {
                switch (hidden_ssid_mode) {
                case 1: {
                        u8 *next_ie = ssid_ie + 2 + ssid_len_ori;
                        u32 remain_len = 0;

                        remain_len = ies_len - (next_ie - ies);

                        ssid_ie[1] = 0;
                        memcpy(ssid_ie + 2, next_ie, remain_len);
                        len_diff -= ssid_len_ori;

                        break;
                }
                case 2:
                        memset(&ssid_ie[2], 0, ssid_len_ori);
                        break;
                default:
                        break;
                }
        }

        return len_diff;
}

static 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;

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (!pmgntframe)
                return;
#if defined(CONFIG_88EU_AP_MODE)
        spin_lock_bh(&pmlmepriv->bcn_update_lock);
#endif

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

        eth_broadcast_addr(pwlanhdr->addr1);
        ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
        ether_addr_copy(pwlanhdr->addr3, cur_network->MacAddress);

        SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
        /* pmlmeext->mgnt_seq++; */
        SetFrameSubType(pframe, IEEE80211_STYPE_BEACON);

        pframe += sizeof(struct ieee80211_hdr_3addr);
        pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);

        if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
                int len_diff;
                u8 *wps_ie;
                uint wps_ielen;
                u8 sr = 0;

                memcpy(pframe, cur_network->ies, cur_network->ie_length);
                len_diff = update_hidden_ssid(
                        pframe + _BEACON_IE_OFFSET_
                        , cur_network->ie_length - _BEACON_IE_OFFSET_
                        , pmlmeinfo->hidden_ssid_mode
                        );
                pframe += (cur_network->ie_length + len_diff);
                pattrib->pktlen += (cur_network->ie_length + len_diff);
                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, WLAN_EID_SSID, cur_network->ssid.ssid_length, cur_network->ssid.ssid, &pattrib->pktlen);

        /*  supported rates... */
        rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
        pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, min_t(unsigned int, rate_len, 8), cur_network->SupportedRates, &pattrib->pktlen);

        /*  DS parameter set */
        pframe = rtw_set_ie(pframe, WLAN_EID_DS_PARAMS, 1, (unsigned char *)&cur_network->Configuration.DSConfig, &pattrib->pktlen);

        {
                u8 erpinfo = 0;
                u32 ATIMWindow;
                /*  IBSS Parameter Set... */
                ATIMWindow = 0;
                pframe = rtw_set_ie(pframe, WLAN_EID_IBSS_PARAMS, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);

                /* ERP IE */
                pframe = rtw_set_ie(pframe, WLAN_EID_ERP_INFO, 1, &erpinfo, &pattrib->pktlen);
        }

        /*  EXTERNDED SUPPORTED RATE */
        if (rate_len > 8)
                pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
        /* todo:HT for adhoc */
_issue_bcn:

#if defined(CONFIG_88EU_AP_MODE)
        pmlmepriv->update_bcn = false;

        spin_unlock_bh(&pmlmepriv->bcn_update_lock);
#endif

        if ((pattrib->pktlen + TXDESC_SIZE) > 512)
                return;

        pattrib->last_txcmdsz = pattrib->pktlen;

        if (timeout_ms > 0)
                dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
        else
                dump_mgntframe(padapter, pmgntframe);
}

static void issue_probersp(struct adapter *padapter, unsigned char *da)
{
        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;
#if defined(CONFIG_88EU_AP_MODE)
        u8 *pwps_ie;
        uint wps_ielen;
        struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
#endif
        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;

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (!pmgntframe)
                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;
        ether_addr_copy(pwlanhdr->addr1, da);
        ether_addr_copy(pwlanhdr->addr2, mac);
        ether_addr_copy(pwlanhdr->addr3, bssid);

        SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
        pmlmeext->mgnt_seq++;
        SetFrameSubType(fctrl, IEEE80211_STYPE_PROBE_RESP);

        pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
        pattrib->pktlen = pattrib->hdrlen;
        pframe += pattrib->hdrlen;

        if (cur_network->ie_length > MAX_IE_SZ)
                return;

#if defined(CONFIG_88EU_AP_MODE)
        if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
                pwps_ie = rtw_get_wps_ie(cur_network->ies + _FIXED_IE_LENGTH_, cur_network->ie_length - _FIXED_IE_LENGTH_, NULL, &wps_ielen);

                /* inerset & update wps_probe_resp_ie */
                if (pmlmepriv->wps_probe_resp_ie && 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->ie_length - 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->ie_length);
                        pframe += cur_network->ie_length;
                        pattrib->pktlen += cur_network->ie_length;
                }
        } else
#endif
        {
                /* timestamp will be inserted by hardware */
                pframe += 8;
                pattrib->pktlen += 8;

                /*  beacon interval: 2 bytes */

                memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->ies)), 2);

                pframe += 2;
                pattrib->pktlen += 2;

                /*  capability info: 2 bytes */

                memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->ies)), 2);

                pframe += 2;
                pattrib->pktlen += 2;

                /* below for ad-hoc mode */

                /*  SSID */
                pframe = rtw_set_ie(pframe, WLAN_EID_SSID, cur_network->ssid.ssid_length, cur_network->ssid.ssid, &pattrib->pktlen);

                /*  supported rates... */
                rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
                pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, min_t(unsigned int, rate_len, 8), cur_network->SupportedRates, &pattrib->pktlen);

                /*  DS parameter set */
                pframe = rtw_set_ie(pframe, WLAN_EID_DS_PARAMS, 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, WLAN_EID_IBSS_PARAMS, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);

                        /* ERP IE */
                        pframe = rtw_set_ie(pframe, WLAN_EID_ERP_INFO, 1, &erpinfo, &pattrib->pktlen);
                }

                /*  EXTERNDED SUPPORTED RATE */
                if (rate_len > 8)
                        pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
                /* todo:HT for adhoc */
        }

        pattrib->last_txcmdsz = pattrib->pktlen;

        dump_mgntframe(padapter, pmgntframe);
}

static int issue_probereq(struct adapter *padapter,
                          struct ndis_802_11_ssid *pssid, u8 *da,
                          bool 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;

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (!pmgntframe)
                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 */
                ether_addr_copy(pwlanhdr->addr1, da);
                ether_addr_copy(pwlanhdr->addr3, da);
        } else {
                /*      broadcast probe request frame */
                eth_broadcast_addr(pwlanhdr->addr1);
                eth_broadcast_addr(pwlanhdr->addr3);
        }

        ether_addr_copy(pwlanhdr->addr2, mac);

        SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
        pmlmeext->mgnt_seq++;
        SetFrameSubType(pframe, IEEE80211_STYPE_PROBE_REQ);

        pframe += sizeof(struct ieee80211_hdr_3addr);
        pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);

        if (pssid)
                pframe = rtw_set_ie(pframe, WLAN_EID_SSID, pssid->ssid_length, pssid->ssid, &pattrib->pktlen);
        else
                pframe = rtw_set_ie(pframe, WLAN_EID_SSID, 0, NULL, &pattrib->pktlen);

        get_rate_set(padapter, bssrate, &bssrate_len);

        if (bssrate_len > 8) {
                pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, 8, bssrate, &pattrib->pktlen);
                pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, bssrate_len - 8, bssrate + 8, &pattrib->pktlen);
        } else {
                pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, bssrate_len, bssrate, &pattrib->pktlen);
        }

        /* add wps_ie for wps2.0 */
        if (pmlmepriv->wps_probe_req_ie_len > 0 && pmlmepriv->wps_probe_req_ie) {
                memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
                pframe += pmlmepriv->wps_probe_req_ie_len;
                pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
        }

        pattrib->last_txcmdsz = pattrib->pktlen;

        if (wait_ack) {
                ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
        } else {
                dump_mgntframe(padapter, pmgntframe);
                ret = _SUCCESS;
        }

exit:
        return ret;
}

static int issue_probereq_ex(struct adapter *padapter,
                             struct ndis_802_11_ssid *pssid, u8 *da,
                             int try_cnt, int wait_ms)
{
        int ret;
        int i = 0;

        do {
                ret = issue_probereq(padapter, pssid, da, wait_ms > 0);

                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;
                goto exit;
        }
exit:
        return ret;
}

/*  if psta == NULL, indicate we are station(client) now... */
static 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;
        u16 val16;
#ifdef CONFIG_88EU_AP_MODE
        __le16 le_val16;
#endif
        int use_shared_key = 0;
        struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
        struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (!pmgntframe)
                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, IEEE80211_STYPE_AUTH);

        pframe += sizeof(struct ieee80211_hdr_3addr);
        pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);

        if (psta) {/*  for AP mode */
#ifdef CONFIG_88EU_AP_MODE

                ether_addr_copy(pwlanhdr->addr1, psta->hwaddr);
                ether_addr_copy(pwlanhdr->addr2,
                                myid(&padapter->eeprompriv));
                ether_addr_copy(pwlanhdr->addr3,
                                myid(&padapter->eeprompriv));

                /*  setting auth algo number */
                val16 = (u16)psta->authalg;

                if (status != WLAN_STATUS_SUCCESS)
                        val16 = 0;

                if (val16) {
                        le_val16 = cpu_to_le16(val16);
                        use_shared_key = 1;
                } else {
                        le_val16 = 0;
                }

                pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, &le_val16,
                                          &pattrib->pktlen);

                /*  setting auth seq number */
                val16 = (u16)psta->auth_seq;
                le_val16 = cpu_to_le16(val16);
                pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, &le_val16,
                                          &pattrib->pktlen);

                /*  setting status code... */
                val16 = status;
                le_val16 = cpu_to_le16(val16);
                pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_,
                                          &le_val16, &pattrib->pktlen);

                /*  added challenging text... */
                if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1))
                        pframe = rtw_set_ie(pframe, WLAN_EID_CHALLENGE, 128, psta->chg_txt, &pattrib->pktlen);
#endif
        } else {
                __le32 le_tmp32;
                __le16 le_tmp16;

                ether_addr_copy(pwlanhdr->addr1, pnetwork->MacAddress);
                ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
                ether_addr_copy(pwlanhdr->addr3, pnetwork->MacAddress);

                /*  setting auth algo number */
                val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) ? 1 : 0;/*  0:OPEN System, 1:Shared key */
                if (val16)
                        use_shared_key = 1;

                /* setting IV for auth seq #3 */
                if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
                        val32 = (pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30);
                        le_tmp32 = cpu_to_le32(val32);
                        pframe = rtw_set_fixed_ie(pframe, 4, &le_tmp32,
                                                  &pattrib->pktlen);

                        pattrib->iv_len = 4;
                }

                le_tmp16 = cpu_to_le16(val16);
                pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, &le_tmp16,
                                          &pattrib->pktlen);

                /*  setting auth seq number */
                val16 = pmlmeinfo->auth_seq;
                le_tmp16 = cpu_to_le16(val16);
                pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, &le_tmp16,
                                          &pattrib->pktlen);

                /*  setting status code... */
                le_tmp16 = cpu_to_le16(status);
                pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, &le_tmp16,
                                          &pattrib->pktlen);

                /*  then checking to see if sending challenging text... */
                if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
                        pframe = rtw_set_ie(pframe, WLAN_EID_CHALLENGE, 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, pmgntframe);
        dump_mgntframe(padapter, pmgntframe);
}

#ifdef CONFIG_88EU_AP_MODE
static 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, leval;

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (!pmgntframe)
                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;

        ether_addr_copy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr);
        ether_addr_copy((void *)GetAddr2Ptr(pwlanhdr),
                        myid(&padapter->eeprompriv));
        ether_addr_copy((void *)GetAddr3Ptr(pwlanhdr), pnetwork->MacAddress);

        SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
        pmlmeext->mgnt_seq++;
        if ((pkt_type == IEEE80211_STYPE_ASSOC_RESP) || (pkt_type == IEEE80211_STYPE_REASSOC_RESP))
                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_, &val, &pattrib->pktlen);

        lestatus = cpu_to_le16(status);
        pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, &lestatus,
                                  &pattrib->pktlen);

        leval = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
        pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_, &leval, &pattrib->pktlen);

        if (pstat->bssratelen <= 8) {
                pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, pstat->bssratelen, pstat->bssrateset, &pattrib->pktlen);
        } else {
                pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, 8, pstat->bssrateset, &pattrib->pktlen);
                pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, pstat->bssratelen - 8, pstat->bssrateset + 8, &pattrib->pktlen);
        }

        if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) {
                uint ie_len = 0;

                /* FILL HT CAP INFO IE */
                pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, WLAN_EID_HT_CAPABILITY, &ie_len, (pnetwork->ie_length - _BEACON_IE_OFFSET_));
                if (pbuf && ie_len > 0) {
                        memcpy(pframe, pbuf, ie_len + 2);
                        pframe += (ie_len + 2);
                        pattrib->pktlen += (ie_len + 2);
                }

                /* FILL HT ADD INFO IE */
                pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, WLAN_EID_HT_OPERATION, &ie_len, (pnetwork->ie_length - _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, WLAN_EID_VENDOR_SPECIFIC, &ie_len, (pnetwork->ie_length - _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 || ie_len == 0)
                                break;
                }
        }

        if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
                pframe = rtw_set_ie(pframe, WLAN_EID_VENDOR_SPECIFIC, 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);
}
#endif /* CONFIG_88EU_AP_MODE */

static void issue_assocreq(struct adapter *padapter)
{
        int ret = _FAIL;
        struct xmit_frame *pmgntframe;
        struct pkt_attrib *pattrib;
        unsigned char *pframe, *p;
        struct ieee80211_hdr *pwlanhdr;
        __le16 *fctrl;
        unsigned int i, j, ie_len, index = 0;
        unsigned char bssrate[NumRates], sta_bssrate[NumRates];
        struct ndis_802_11_var_ie *pIE;
        struct registry_priv *pregpriv = &padapter->registrypriv;
        struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
        struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
        int bssrate_len = 0, sta_bssrate_len = 0;
        struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (!pmgntframe)
                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;
        ether_addr_copy(pwlanhdr->addr1, pnetwork->MacAddress);
        ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
        ether_addr_copy(pwlanhdr->addr3, pnetwork->MacAddress);

        SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
        pmlmeext->mgnt_seq++;
        SetFrameSubType(pframe, IEEE80211_STYPE_ASSOC_REQ);

        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 */
        put_unaligned_le16(3, pframe);
        pframe += 2;
        pattrib->pktlen += 2;

        /* SSID */
        pframe = rtw_set_ie(pframe, WLAN_EID_SSID,  pmlmeinfo->network.ssid.ssid_length, pmlmeinfo->network.ssid.ssid, &pattrib->pktlen);

        /* supported rate & extended supported rate */

        /*  Check if the AP's supported rates are also supported by STA. */
        get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);

        if (pmlmeext->cur_channel == 14)/*  for JAPAN, channel 14 can only uses B Mode(CCK) */
                sta_bssrate_len = 4;

        for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
                if (pmlmeinfo->network.SupportedRates[i] == 0)
                        break;
        }

        for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
                if (pmlmeinfo->network.SupportedRates[i] == 0)
                        break;

                /*  Check if the AP's supported rates are also supported by STA. */
                for (j = 0; j < sta_bssrate_len; j++) {
                        /*  Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP */
                        if ((pmlmeinfo->network.SupportedRates[i] | IEEE80211_BASIC_RATE_MASK)
                                        == (sta_bssrate[j] | IEEE80211_BASIC_RATE_MASK))
                                break;
                }

                if (j != sta_bssrate_len)
                        /*  the rate is supported by STA */
                        bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
        }

        bssrate_len = index;

        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, WLAN_EID_SUPP_RATES, 8, bssrate, &pattrib->pktlen);
                pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, bssrate_len - 8, bssrate + 8, &pattrib->pktlen);
        } else {
                pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, bssrate_len, bssrate, &pattrib->pktlen);
        }

        /* RSN */
        p = rtw_get_ie((pmlmeinfo->network.ies + sizeof(struct ndis_802_11_fixed_ie)), WLAN_EID_RSN, &ie_len, (pmlmeinfo->network.ie_length - sizeof(struct ndis_802_11_fixed_ie)));
        if (p)
                pframe = rtw_set_ie(pframe, WLAN_EID_RSN, ie_len, p + 2, &pattrib->pktlen);

        /* HT caps */
        if (padapter->mlmepriv.htpriv.ht_option) {
                p = rtw_get_ie((pmlmeinfo->network.ies + sizeof(struct ndis_802_11_fixed_ie)), WLAN_EID_HT_CAPABILITY, &ie_len, (pmlmeinfo->network.ie_length - sizeof(struct ndis_802_11_fixed_ie)));
                if (p && !is_ap_in_tkip(padapter)) {
                        memcpy(&pmlmeinfo->HT_caps, p + 2, sizeof(struct ieee80211_ht_cap));

                        /* to disable 40M Hz support while gd_bw_40MHz_en = 0 */
                        if (pregpriv->cbw40_enable == 0)
                                pmlmeinfo->HT_caps.cap_info &= cpu_to_le16(~(BIT(6) | BIT(1)));
                        else
                                pmlmeinfo->HT_caps.cap_info |= cpu_to_le16(BIT(1));

                        /* todo: disable SM power save mode */
                        pmlmeinfo->HT_caps.cap_info |= cpu_to_le16(0x000c);

                        if (pregpriv->rx_stbc)
                                pmlmeinfo->HT_caps.cap_info |= cpu_to_le16(0x0100);/* RX STBC One spatial stream */
                        memcpy((u8 *)&pmlmeinfo->HT_caps.mcs, MCS_rate_1R, 16);
                        pframe = rtw_set_ie(pframe, WLAN_EID_HT_CAPABILITY, ie_len, (u8 *)(&pmlmeinfo->HT_caps), &pattrib->pktlen);
                }
        }

        /* vendor specific IE, such as WPA, WMM, WPS */
        for (i = sizeof(struct ndis_802_11_fixed_ie); i < pmlmeinfo->network.ie_length; i += (pIE->Length + 2)) {
                pIE = (struct ndis_802_11_var_ie *)(pmlmeinfo->network.ies + i);

                switch (pIE->ElementID) {
                case WLAN_EID_VENDOR_SPECIFIC:
                        if ((!memcmp(pIE->data, RTW_WPA_OUI, 4)) ||
                            (!memcmp(pIE->data, WMM_OUI, 4)) ||
                            (!memcmp(pIE->data, WPS_OUI, 4))) {
                                if (!padapter->registrypriv.wifi_spec) {
                                        /* Commented by Kurt 20110629 */
                                        /* In some older APs, WPS handshake */
                                        /* would be fail if we append vender extensions information to AP */
                                        if (!memcmp(pIE->data, WPS_OUI, 4))
                                                pIE->Length = 14;
                                }
                                pframe = rtw_set_ie(pframe, WLAN_EID_VENDOR_SPECIFIC, pIE->Length, pIE->data, &pattrib->pktlen);
                        }
                        break;
                default:
                        break;
                }
        }

        if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
                pframe = rtw_set_ie(pframe, WLAN_EID_VENDOR_SPECIFIC, 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);
}

/* when wait_ack is true, this function should be called at process context */
static int _issue_nulldata(struct adapter *padapter, unsigned char *da,
                           unsigned int power_mode, bool 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;
        struct wlan_bssid_ex *pnetwork;

        if (!padapter)
                goto exit;

        pxmitpriv = &padapter->xmitpriv;
        pmlmeext = &padapter->mlmeextpriv;
        pmlmeinfo = &pmlmeext->mlmext_info;
        pnetwork = &pmlmeinfo->network;

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (!pmgntframe)
                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);

        ether_addr_copy(pwlanhdr->addr1, da);
        ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
        ether_addr_copy(pwlanhdr->addr3, pnetwork->MacAddress);

        SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
        pmlmeext->mgnt_seq++;
        SetFrameSubType(pframe, IEEE80211_FTYPE_DATA | IEEE80211_STYPE_NULLFUNC);

        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;
}

/* when wait_ms > 0 , this function should 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 wlan_bssid_ex *pnetwork = &pmlmeinfo->network;

        /* da == NULL, assume it's null data for sta to ap*/
        if (!da)
                da = pnetwork->MacAddress;

        do {
                ret = _issue_nulldata(padapter, da, power_mode, wait_ms > 0);

                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;
                goto exit;
        }
exit:
        return ret;
}

/* when wait_ack is true, this function should be called at process context */
static int _issue_qos_nulldata(struct adapter *padapter, unsigned char *da,
                               u16 tid, bool wait_ack)
{
        int ret = _FAIL;
        struct xmit_frame *pmgntframe;
        struct pkt_attrib *pattrib;
        unsigned char *pframe;
        struct ieee80211_hdr *pwlanhdr;
        __le16 *fctrl;
        unsigned short *qc;
        struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
        struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (!pmgntframe)
                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);

        ether_addr_copy(pwlanhdr->addr1, da);
        ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
        ether_addr_copy(pwlanhdr->addr3, pnetwork->MacAddress);

        SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
        pmlmeext->mgnt_seq++;
        SetFrameSubType(pframe, IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_NULLFUNC);

        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 should 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;
        struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;

        /* da == NULL, assume it's null data for sta to ap*/
        if (!da)
                da = pnetwork->MacAddress;

        do {
                ret = _issue_qos_nulldata(padapter, da, tid, wait_ms > 0);

                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;
                goto exit;
        }
exit:
        return ret;
}

static int _issue_deauth(struct adapter *padapter, unsigned char *da,
                         unsigned short reason, bool 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;
        struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
        int ret = _FAIL;
        __le16 le_tmp;

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (!pmgntframe)
                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;

        ether_addr_copy(pwlanhdr->addr1, da);
        ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
        ether_addr_copy(pwlanhdr->addr3, pnetwork->MacAddress);

        SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
        pmlmeext->mgnt_seq++;
        SetFrameSubType(pframe, IEEE80211_STYPE_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_, &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)
{
        return _issue_deauth(padapter, da, reason, false);
}

static 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);

                i++;

                if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
                        break;

                if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
                        mdelay(wait_ms);
        } while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));

        if (ret != _FAIL) {
                ret = _SUCCESS;
                goto exit;
        }
exit:
        return ret;
}

static void issue_action_BA(struct adapter *padapter, unsigned char *raddr,
                            unsigned char action, unsigned short status)
{
        u8 category = RTW_WLAN_CATEGORY_BACK;
        u16 start_seq;
        u16 BA_para_set;
        u16 reason_code;
        u16 BA_timeout_value;
        __le16 le_tmp;
        u16 BA_starting_seqctrl = 0;
        enum ht_cap_ampdu_factor max_rx_ampdu_factor;
        struct xmit_frame *pmgntframe;
        struct pkt_attrib *pattrib;
        u8 *pframe;
        struct ieee80211_hdr *pwlanhdr;
        __le16 *fctrl;
        struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
        struct sta_info *psta;
        struct sta_priv *pstapriv = &padapter->stapriv;
        struct registry_priv *pregpriv = &padapter->registrypriv;
        struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (!pmgntframe)
                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;

        ether_addr_copy(pwlanhdr->addr1, raddr);
        ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
        ether_addr_copy(pwlanhdr->addr3, pnetwork->MacAddress);

        SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
        pmlmeext->mgnt_seq++;
        SetFrameSubType(pframe, IEEE80211_STYPE_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);

                        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, &(le_tmp),
                                                  &pattrib->pktlen);

                        BA_timeout_value = 5000;/*  5ms */
                        le_tmp = cpu_to_le16(BA_timeout_value);
                        pframe = rtw_set_fixed_ie(pframe, 2, &(le_tmp),
                                                  &pattrib->pktlen);

                        psta = rtw_get_stainfo(pstapriv, raddr);
                        if (psta) {
                                start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07] & 0xfff) + 1;

                                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, &(le_tmp),
                                                  &pattrib->pktlen);
                        break;
                case 1: /* ADDBA rsp */
                {
                        struct ADDBA_request *ADDBA_req = &pmlmeinfo->ADDBA_req;

                        pframe = rtw_set_fixed_ie(pframe, 1,
                                                  &ADDBA_req->dialog_token,
                                                  &pattrib->pktlen);
                        pframe = rtw_set_fixed_ie(pframe, 2, &status,
                                                  &pattrib->pktlen);

                        BA_para_set = le16_to_cpu(ADDBA_req->BA_para_set) &
                                      0x3f;
                        rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
                        switch (max_rx_ampdu_factor) {
                        case MAX_AMPDU_FACTOR_64K:
                                BA_para_set |= 0x1000; /* 64 buffer size */
                                break;
                        case MAX_AMPDU_FACTOR_32K:
                                BA_para_set |= 0x0800; /* 32 buffer size */
                                break;
                        case MAX_AMPDU_FACTOR_16K:
                                BA_para_set |= 0x0400; /* 16 buffer size */
                                break;
                        case MAX_AMPDU_FACTOR_8K:
                                BA_para_set |= 0x0200; /* 8 buffer size */
                                break;
                        default:
                                BA_para_set |= 0x1000; /* 64 buffer size */
                                break;
                        }

                        if (pregpriv->ampdu_amsdu == 0)/* disabled */
                                BA_para_set = BA_para_set & ~BIT(0);
                        else if (pregpriv->ampdu_amsdu == 1)/* enabled */
                                BA_para_set = BA_para_set | BIT(0);
                        le_tmp = cpu_to_le16(BA_para_set);

                        pframe = rtw_set_fixed_ie(pframe, 2, &(le_tmp),
                                                  &pattrib->pktlen);
                        pframe = rtw_set_fixed_ie(pframe, 2,
                                                  &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, &(le_tmp),
                                                  &pattrib->pktlen);

                        reason_code = 37;/* Requested from peer STA as it does not want to use the mechanism */
                        le_tmp = cpu_to_le16(reason_code);
                        pframe = rtw_set_fixed_ie(pframe, 2, &(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];
        struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;

        if ((pmlmepriv->num_FortyMHzIntolerant == 0) || (pmlmepriv->num_sta_no_ht == 0))
                return;

        if (pmlmeinfo->bwmode_updated)
                return;

        category = RTW_WLAN_CATEGORY_PUBLIC;
        action = ACT_PUBLIC_BSSCOEXIST;

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (!pmgntframe)
                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;

        ether_addr_copy(pwlanhdr->addr1, cur_network->MacAddress);
        ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
        ether_addr_copy(pwlanhdr->addr3, cur_network->MacAddress);

        SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
        pmlmeext->mgnt_seq++;
        SetFrameSubType(pframe, IEEE80211_STYPE_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, WLAN_EID_BSS_COEX_2040,  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);
                list_for_each(plist, phead) {
                        uint len;
                        u8 *p;
                        struct wlan_bssid_ex *pbss_network;

                        pnetwork = list_entry(plist, struct wlan_network,
                                              list);

                        pbss_network = &pnetwork->network;

                        p = rtw_get_ie(pbss_network->ies + _FIXED_IE_LENGTH_, WLAN_EID_HT_CAPABILITY, &len, pbss_network->ie_length - _FIXED_IE_LENGTH_);
                        if (!p || len == 0) { /* non-HT */
                                if (pbss_network->Configuration.DSConfig <= 0)
                                        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, WLAN_EID_BSS_INTOLERANT_CHL_REPORT, 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 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)
                return _SUCCESS;

        if (initiator == 0) { /*  recipient */
                for (tid = 0; tid < MAXTID; tid++) {
                        if (psta->recvreorder_ctrl[tid].enable) {
                                issue_action_BA(padapter, addr, WLAN_ACTION_DELBA, (((tid << 1) | initiator) & 0x1F));
                                psta->recvreorder_ctrl[tid].enable = false;
                                psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
                        }
                }
        } else if (initiator == 1) { /*  originator */
                for (tid = 0; tid < MAXTID; tid++) {
                        if (psta->htpriv.agg_enable_bitmap & BIT(tid)) {
                                issue_action_BA(padapter, addr, 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;

        rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, 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 && !bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
        } while (!bxmitok && issue < 100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);

        if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
                return _FAIL;
        if (!bxmitok)
                return _FAIL;

        return _SUCCESS;
}

/****************************************************************************

Following are some utility functions for WiFi MLME

*****************************************************************************/

static void site_survey(struct adapter *padapter)
{
        unsigned char survey_channel = 0, val8;
        enum rt_scan_type ScanType = SCAN_PASSIVE;
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
        u32 initialgain = 0;
        struct rtw_ieee80211_channel *ch;

        if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
                ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
                survey_channel = ch->hw_value;
                ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
        }

        if (survey_channel != 0) {
                /* PAUSE 4-AC Queue when site_survey */
                /* rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
                /* val8 |= 0x0f; */
                /* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
                if (pmlmeext->sitesurvey_res.channel_idx == 0)
                        set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
                else
                        SelectChannel(padapter, survey_channel);

                if (ScanType == SCAN_ACTIVE) { /* obey the channel plan setting... */
                        int i;

                        for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
                                if (pmlmeext->sitesurvey_res.ssid[i].ssid_length) {
                                        /* todo: to issue two probe req??? */
                                        issue_probereq(padapter,
                                                       &pmlmeext->sitesurvey_res.ssid[i],
                                                       NULL, false);
                                        /* msleep(SURVEY_TO>>1); */
                                        issue_probereq(padapter,
                                                       &pmlmeext->sitesurvey_res.ssid[i],
                                                       NULL, false);
                                }
                        }

                        if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
                                /* todo: to issue two probe req??? */
                                issue_probereq(padapter, NULL, NULL, false);
                                /* msleep(SURVEY_TO>>1); */
                                issue_probereq(padapter, NULL, NULL, false);
                        }

                        if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
                                /* todo: to issue two probe req??? */
                                issue_probereq(padapter, NULL, NULL, false);
                                /* msleep(SURVEY_TO>>1); */
                                issue_probereq(padapter, NULL, NULL, false);
                        }
                }

                set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
        } else {
                /*  20100721:Interrupt scan operation here. */
                /*  For SW antenna diversity before link, it needs to switch to another antenna and scan again. */
                /*  It compares the scan result and select better one to do connection. */
                if (rtw_hal_antdiv_before_linked(padapter)) {
                        pmlmeext->sitesurvey_res.bss_cnt = 0;
                        pmlmeext->sitesurvey_res.channel_idx = -1;
                        pmlmeext->chan_scan_time = SURVEY_TO / 2;
                        set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
                        return;
                }

                pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;

                /* switch back to the original channel */

                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))
                        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);
        }
}

/* collect bss info from Beacon and Probe request/response frames. */
static u8 collect_bss_info(struct adapter *padapter,
                           struct recv_frame *precv_frame,
                           struct wlan_bssid_ex *bssid)
{
        int i;
        u32 len;
        u8 *p;
        u16 val16, subtype;
        u8 *pframe = precv_frame->pkt->data;
        u32 packet_len = precv_frame->pkt->len;
        u8 ie_offset;
        struct registry_priv *pregistrypriv = &padapter->registrypriv;
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;

        len = packet_len - sizeof(struct ieee80211_hdr_3addr);

        if (len > MAX_IE_SZ)
                return _FAIL;

        memset(bssid, 0, sizeof(struct wlan_bssid_ex));

        subtype = GetFrameSubType(pframe);

        if (subtype == IEEE80211_STYPE_BEACON) {
                bssid->Reserved[0] = 1;
                ie_offset = _BEACON_IE_OFFSET_;
        } else {
                /*  FIXME : more type */
                if (subtype == IEEE80211_STYPE_PROBE_REQ) {
                        ie_offset = _PROBEREQ_IE_OFFSET_;
                        bssid->Reserved[0] = 2;
                } else if (subtype == IEEE80211_STYPE_PROBE_RESP) {
                        ie_offset = _PROBERSP_IE_OFFSET_;
                        bssid->Reserved[0] = 3;
                } else {
                        bssid->Reserved[0] = 0;
                        ie_offset = _FIXED_IE_LENGTH_;
                }
        }

        bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;

        /* below is to copy the information element */
        bssid->ie_length = len;
        memcpy(bssid->ies, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->ie_length);

        /* get the signal strength in dBM.raw data */
        bssid->Rssi = precv_frame->attrib.phy_info.recvpower;
        bssid->PhyInfo.SignalQuality = precv_frame->attrib.phy_info.SignalQuality;/* in percentage */
        bssid->PhyInfo.SignalStrength = precv_frame->attrib.phy_info.SignalStrength;/* in percentage */
        rtw_hal_get_def_var(padapter, HAL_DEF_CURRENT_ANTENNA,  &bssid->PhyInfo.Optimum_antenna);

        /*  checking SSID */
        p = rtw_get_ie(bssid->ies + ie_offset, WLAN_EID_SSID, &len, bssid->ie_length - ie_offset);
        if (!p)
                return _FAIL;

        if (len) {
                if (len > NDIS_802_11_LENGTH_SSID)
                        return _FAIL;
                memcpy(bssid->ssid.ssid, (p + 2), len);
                bssid->ssid.ssid_length = len;
        } else {
                bssid->ssid.ssid_length = 0;
        }

        memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);

        /* checking rate info... */
        i = 0;
        p = rtw_get_ie(bssid->ies + ie_offset, WLAN_EID_SUPP_RATES, &len, bssid->ie_length - ie_offset);
        if (p) {
                if (len > NDIS_802_11_LENGTH_RATES_EX)
                        return _FAIL;
                memcpy(bssid->SupportedRates, (p + 2), len);
                i = len;
        }

        p = rtw_get_ie(bssid->ies + ie_offset, WLAN_EID_EXT_SUPP_RATES, &len, bssid->ie_length - ie_offset);
        if (p) {
                if (len > (NDIS_802_11_LENGTH_RATES_EX - i))
                        return _FAIL;
                memcpy(bssid->SupportedRates + i, (p + 2), len);
        }

        /* todo: */
        bssid->NetworkTypeInUse = Ndis802_11OFDM24;

        if (bssid->ie_length < 12)
                return _FAIL;

        /*  Checking for DSConfig */
        p = rtw_get_ie(bssid->ies + ie_offset, WLAN_EID_DS_PARAMS, &len, bssid->ie_length - 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, WLAN_EID_HT_OPERATION, &len, bssid->ie_length - ie_offset);
                if (p) {

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

                        bssid->Configuration.DSConfig = HT_info->primary_channel;
                } else { /*  use current channel */
                        bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter);
                }
        }

        if (subtype == IEEE80211_STYPE_PROBE_REQ) {
                /*  FIXME */
                bssid->InfrastructureMode = Ndis802_11Infrastructure;
                ether_addr_copy(bssid->MacAddress, GetAddr2Ptr(pframe));
                bssid->Privacy = 1;
                return _SUCCESS;
        }

        bssid->Configuration.BeaconPeriod =
                get_unaligned_le16(rtw_get_beacon_interval_from_ie(bssid->ies));

        val16 = rtw_get_capability(bssid);

        if (val16 & BIT(0)) {
                bssid->InfrastructureMode = Ndis802_11Infrastructure;
                ether_addr_copy(bssid->MacAddress, GetAddr2Ptr(pframe));
        } else {
                bssid->InfrastructureMode = Ndis802_11IBSS;
                ether_addr_copy(bssid->MacAddress, GetAddr3Ptr(pframe));
        }

        if (val16 & BIT(4))
                bssid->Privacy = 1;
        else
                bssid->Privacy = 0;

        bssid->Configuration.ATIMWindow = 0;

        /* 20/40 BSS Coexistence check */
        if ((pregistrypriv->wifi_spec == 1) && (!pmlmeinfo->bwmode_updated)) {
                struct mlme_priv *pmlmepriv = &padapter->mlmepriv;

                p = rtw_get_ie(bssid->ies + ie_offset, WLAN_EID_HT_CAPABILITY, &len, bssid->ie_length - ie_offset);
                if (p && len > 0) {
                        struct ieee80211_ht_cap *pHT_caps =
                                (struct ieee80211_ht_cap *)(p + 2);

                        if (le16_to_cpu(pHT_caps->cap_info) & BIT(14))
                                pmlmepriv->num_FortyMHzIntolerant++;
                } else {
                        pmlmepriv->num_sta_no_ht++;
                }
        }

        /*  mark bss info receiving from nearby channel as SignalQuality 101 */
        if (bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
                bssid->PhyInfo.SignalQuality = 101;
        return _SUCCESS;
}

static 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 = &pmlmeinfo->network;

        pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
        pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);

        /* update wireless mode */
        update_wireless_mode(padapter);

        /* update capability */
        caps = rtw_get_capability(pnetwork);
        update_capinfo(padapter, caps);
        if (caps & WLAN_CAPABILITY_IBSS) {/* adhoc master */
                val8 = 0xcf;
                rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));

                /* switch channel */
                /* SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */
                set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);

                beacon_timing_control(padapter);

                /* set msr to WIFI_FW_ADHOC_STATE */
                pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
                Set_MSR(padapter, (pmlmeinfo->state & 0x3));

                /* issue beacon */
                if (send_beacon(padapter) == _FAIL) {
                        report_join_res(padapter, -1);
                        pmlmeinfo->state = WIFI_FW_NULL_STATE;
                } else {
                        rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
                        join_type = 0;
                        rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));

                        report_join_res(padapter, 1);
                        pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
                }
        } else {
                return;
        }
}

static 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 = &pmlmeinfo->network;
        int beacon_timeout;

        pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
        pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);

        /* update wireless mode */
        update_wireless_mode(padapter);

        /* update capability */
        caps = rtw_get_capability(pnetwork);
        update_capinfo(padapter, caps);
        if (caps & WLAN_CAPABILITY_ESS) {
                Set_MSR(padapter, WIFI_FW_STATION_STATE);

                val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf;

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

                /* switch channel */
                set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);

                /* here wait for receiving the beacon to start auth */
                /* and enable a timer */
                beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
                set_link_timer(pmlmeext, beacon_timeout);
                mod_timer(&padapter->mlmepriv.assoc_timer, jiffies +
                          msecs_to_jiffies((REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO * REASSOC_LIMIT) + beacon_timeout));

                pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
        } else if (caps & WLAN_CAPABILITY_IBSS) { /* adhoc client */
                Set_MSR(padapter, WIFI_FW_ADHOC_STATE);

                val8 = 0xcf;
                rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));

                /* switch channel */
                set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);

                beacon_timing_control(padapter);

                pmlmeinfo->state = WIFI_FW_ADHOC_STATE;

                report_join_res(padapter, 1);
        } else {
                return;
        }
}

static 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;

        /*  Because of AP's not receiving deauth before */
        /*  AP may: 1)not response auth or 2)deauth us after link is complete */
        /*  issue deauth before issuing auth to deal with the situation */
        /*      Commented by Albert 2012/07/21 */
        /*      For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it. */
        issue_deauth(padapter, (&pmlmeinfo->network)->MacAddress, WLAN_REASON_DEAUTH_LEAVING);

        issue_auth(padapter, NULL, 0);

        set_link_timer(pmlmeext, REAUTH_TO);
}

static 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);
}

static 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;
        struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;

        /* check A3 */
        if (memcmp(MacAddr, pnetwork->MacAddress, ETH_ALEN))
                return _SUCCESS;

        if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
                if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
                        pmlmeinfo->state = WIFI_FW_NULL_STATE;
                        report_del_sta_event(padapter, MacAddr, reason);
                } else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE) {
                        pmlmeinfo->state = WIFI_FW_NULL_STATE;
                        report_join_res(padapter, -2);
                }
        }
        return _SUCCESS;
}

static void process_80211d(struct adapter *padapter, struct wlan_bssid_ex *bssid)
{
        struct registry_priv *pregistrypriv;
        struct mlme_ext_priv *pmlmeext;
        struct rt_channel_info *chplan_new;
        u8 channel;
        u8 i;

        pregistrypriv = &padapter->registrypriv;
        pmlmeext = &padapter->mlmeextpriv;

        /*  Adjust channel plan by AP Country IE */
        if (pregistrypriv->enable80211d &&
            (!pmlmeext->update_channel_plan_by_ap_done)) {
                u8 *ie, *p;
                u32 len;
                struct rt_channel_plan chplan_ap;
                struct rt_channel_info chplan_sta[MAX_CHANNEL_NUM];
                u8 country[4];
                u8 fcn; /*  first channel number */
                u8 noc; /*  number of channel */
                u8 j, k;

                ie = rtw_get_ie(bssid->ies + _FIXED_IE_LENGTH_, WLAN_EID_COUNTRY, &len, bssid->ie_length - _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;
                i = 0;
                while ((ie - p) >= 3) {
                        fcn = *(p++);
                        noc = *(p++);
                        p++;

                        for (j = 0; j < noc; j++) {
                                channel = fcn + j;

                                chplan_ap.Channel[i++] = channel;
                        }
                }
                chplan_ap.Len = i;

                memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));

                memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
                chplan_new = pmlmeext->channel_set;

                i = 0;
                j = 0;
                k = 0;
                if (pregistrypriv->wireless_mode & WIRELESS_11G) {
                        do {
                                if ((i == MAX_CHANNEL_NUM) ||
                                    (chplan_sta[i].ChannelNum == 0) ||
                                    (chplan_sta[i].ChannelNum > 14))
                                        break;

                                if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
                                        break;

                                if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
                                        chplan_new[k].ChannelNum = chplan_ap.Channel[j];
                                        chplan_new[k].ScanType = SCAN_ACTIVE;
                                        i++;
                                        j++;
                                        k++;
                                } else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
                                        chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
                                        chplan_new[k].ScanType = SCAN_PASSIVE;
                                        i++;
                                        k++;
                                } else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
                                        chplan_new[k].ChannelNum = chplan_ap.Channel[j];
                                        chplan_new[k].ScanType = SCAN_ACTIVE;
                                        j++;
                                        k++;
                                }
                        } while (1);

                        /*  change AP not support channel to Passive scan */
                        while ((i < MAX_CHANNEL_NUM) &&
                               (chplan_sta[i].ChannelNum != 0) &&
                               (chplan_sta[i].ChannelNum <= 14)) {
                                chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
                                chplan_new[k].ScanType = SCAN_PASSIVE;
                                i++;
                                k++;
                        }

                        /*  add channel AP supported */
                        while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
                                chplan_new[k].ChannelNum = chplan_ap.Channel[j];
                                chplan_new[k].ScanType = SCAN_ACTIVE;
                                j++;
                                k++;
                        }
                } else {
                        /*  keep original STA 2.4G channel plan */
                        while ((i < MAX_CHANNEL_NUM) &&
                               (chplan_sta[i].ChannelNum != 0) &&
                               (chplan_sta[i].ChannelNum <= 14)) {
                                chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
                                chplan_new[k].ScanType = chplan_sta[i].ScanType;
                                i++;
                                k++;
                        }

                        /*  skip AP 2.4G channel plan */
                        while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14))
                                j++;
                }

                pmlmeext->update_channel_plan_by_ap_done = 1;
        }

        /*  If channel is used by AP, set channel scan type to active */
        channel = bssid->Configuration.DSConfig;
        chplan_new = pmlmeext->channel_set;
        i = 0;
        while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0)) {
                if (chplan_new[i].ChannelNum == channel) {
                        if (chplan_new[i].ScanType == SCAN_PASSIVE)
                                chplan_new[i].ScanType = SCAN_ACTIVE;
                        break;
                }
                i++;
        }
}

/****************************************************************************

Following are the callback functions for each subtype of the management frames

*****************************************************************************/

static unsigned int OnProbeReq(struct adapter *padapter,
                               struct 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->pkt->data;
        uint len = precv_frame->pkt->len;

        if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
                return _SUCCESS;

        if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
            !check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE))
                return _SUCCESS;

        p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, WLAN_EID_SSID, &ielen,
                       len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);

        /* check (wildcard) SSID */
        if (p) {
                if ((ielen != 0 && memcmp((void *)(p + 2), (void *)cur->ssid.ssid, cur->ssid.ssid_length)) ||
                    (ielen == 0 && pmlmeinfo->hidden_ssid_mode))
                        return _SUCCESS;

                if (check_fwstate(pmlmepriv, _FW_LINKED) &&
                    pmlmepriv->cur_network.join_res)
                        issue_probersp(padapter, ieee80211_get_SA((struct ieee80211_hdr *)pframe));
        }
        return _SUCCESS;
}

static unsigned int OnProbeRsp(struct adapter *padapter,
                               struct 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;
}

static unsigned int OnBeacon(struct adapter *padapter,
                             struct 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->pkt->data;
        uint len = precv_frame->pkt->len;
        struct wlan_bssid_ex *pbss;
        int ret = _SUCCESS;
        struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;

        if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
                report_survey_event(padapter, precv_frame);
                return _SUCCESS;
        }

        if (!memcmp(GetAddr3Ptr(pframe), pnetwork->MacAddress, ETH_ALEN)) {
                if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
                        /* we should update current network before auth, or some IE is wrong */
                        pbss = (struct wlan_bssid_ex *)rtw_malloc(sizeof(struct wlan_bssid_ex));
                        if (pbss) {
                                if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
                                        update_network(&pmlmepriv->cur_network.network, pbss, padapter, true);
                                        rtw_get_bcn_info(&pmlmepriv->cur_network);
                                }
                                kfree(pbss);
                        }

                        /* check the vendor of the assoc AP */
                        pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe + sizeof(struct ieee80211_hdr_3addr), len - sizeof(struct ieee80211_hdr_3addr));

                        /* update TSF Value */
                        update_TSF(pmlmeext, pframe, len);

                        /* start auth */
                        start_clnt_auth(padapter);

                        return _SUCCESS;
                }

                if (((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
                        psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
                        if (psta) {
                                ret = rtw_check_bcn_info(padapter, pframe, len);
                                if (!ret) {
                                        receive_disconnect(padapter, pmlmeinfo->network.MacAddress, 65535);
                                        return _SUCCESS;
                                }
                                /* update WMM, ERP in the beacon */
                                /* todo: the timer is used instead of the number of the beacon received */
                                if ((sta_rx_pkts(psta) & 0xf) == 0)
                                        update_beacon_info(padapter, pframe, len, psta);
                        }
                } else if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
                        psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
                        if (psta) {
                                /* update WMM, ERP in the beacon */
                                /* todo: the timer is used instead of the number of the beacon received */
                                if ((sta_rx_pkts(psta) & 0xf) == 0)
                                        update_beacon_info(padapter, pframe, len, psta);
                        } else {
                                /* allocate a new CAM entry for IBSS station */
                                cam_idx = allocate_fw_sta_entry(padapter);
                                if (cam_idx == NUM_STA)
                                        goto _END_ONBEACON_;

                                /* get supported rate */
                                if (update_sta_support_rate(padapter, (pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_), (len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_), cam_idx) == _FAIL) {
                                        pmlmeinfo->FW_sta_info[cam_idx].status = 0;
                                        goto _END_ONBEACON_;
                                }

                                /* update TSF Value */
                                update_TSF(pmlmeext, pframe, len);

                                /* report sta add event */
                                report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
                        }
                }
        }

_END_ONBEACON_:

        return _SUCCESS;
}

#ifdef CONFIG_88EU_AP_MODE
static unsigned int OnAuth(struct adapter *padapter,
                           struct recv_frame *precv_frame)
{
        unsigned int auth_mode, ie_len;
        u16 seq;
        unsigned char *sa, *p;
        u16 algorithm;
        int status;
        static struct sta_info stat;
        struct sta_info *pstat = NULL;
        struct sta_priv *pstapriv = &padapter->stapriv;
        struct security_priv *psecuritypriv = &padapter->securitypriv;
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
        u8 *pframe = precv_frame->pkt->data;
        uint len = precv_frame->pkt->len;

        if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
                return _FAIL;

        sa = GetAddr2Ptr(pframe);

        auth_mode = psecuritypriv->dot11AuthAlgrthm;
        seq = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + 2));
        algorithm = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN));

        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 */
                status = WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG;

                goto auth_fail;
        }

        if (!rtw_access_ctrl(padapter, sa)) {
                status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
                goto auth_fail;
        }

        pstat = rtw_get_stainfo(pstapriv, sa);
        if (!pstat) {
                /*  allocate a new one */
                pstat = rtw_alloc_stainfo(pstapriv, sa);
                if (!pstat) {
                        status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
                        goto auth_fail;
                }

                pstat->state = WIFI_FW_AUTH_NULL;
                pstat->auth_seq = 0;
        } else {
                spin_lock_bh(&pstapriv->asoc_list_lock);
                if (!list_empty(&pstat->asoc_list)) {
                        list_del_init(&pstat->asoc_list);
                        pstapriv->asoc_list_cnt--;
                }
                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) {
                status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION;
                goto auth_fail;
        }

        if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2)) {
                if (seq == 1) {
                        pstat->state &= ~WIFI_FW_AUTH_NULL;
                        pstat->state |= WIFI_FW_AUTH_SUCCESS;
                        pstat->expire_to = pstapriv->assoc_to;
                        pstat->authalg = algorithm;
                } else {
                        status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION;
                        goto auth_fail;
                }
        } else { /*  shared system or auto authentication */
                if (seq == 1) {
                        /* prepare for the challenging txt... */

                        pstat->state &= ~WIFI_FW_AUTH_NULL;
                        pstat->state |= WIFI_FW_AUTH_STATE;
                        pstat->authalg = algorithm;
                        pstat->auth_seq = 2;
                } else if (seq == 3) {
                        /* checking for challenging txt... */
                        p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_, WLAN_EID_CHALLENGE, &ie_len,
                                       len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);

                        if (!p || ie_len <= 0) {
                                status = WLAN_STATUS_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 {
                                status = WLAN_STATUS_CHALLENGE_FAIL;
                                goto auth_fail;
                        }
                } else {
                        status = WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION;
                        goto auth_fail;
                }
        }

        /*  Now, we are going to issue_auth... */
        pstat->auth_seq = seq + 1;

        issue_auth(padapter, pstat, (unsigned short)(WLAN_STATUS_SUCCESS));

        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;
}
#endif /* CONFIG_88EU_AP_MODE */

static unsigned int OnAuthClient(struct adapter *padapter,
                                 struct recv_frame *precv_frame)
{
        unsigned int seq, len, status, offset;
        unsigned char *p;
        unsigned int go2asoc = 0;
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
        u8 *pframe = precv_frame->pkt->data;
        uint pkt_len = precv_frame->pkt->len;

        /* check A1 matches or not */
        if (memcmp(myid(&padapter->eeprompriv), ieee80211_get_DA((struct ieee80211_hdr *)pframe), ETH_ALEN))
                return _SUCCESS;

        if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
                return _SUCCESS;

        offset = (GetPrivacy(pframe)) ? 4 : 0;

        seq     = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 2));
        status  = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 4));

        if (status != 0) {
                if (status == 13) { /*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
                        if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
                                pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
                        else
                                pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
                }

                set_link_timer(pmlmeext, 1);
                goto authclnt_fail;
        }

        if (seq == 2) {
                if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
                        /*  legendary shared system */
                        p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, WLAN_EID_CHALLENGE, &len,
                                       pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);

                        if (!p)
                                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;
                }
                /*  open system */
                go2asoc = 1;
        } else if (seq == 4) {
                if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
                        go2asoc = 1;
                else
                        goto authclnt_fail;
        } else {
                /*  this is also illegal */
                goto authclnt_fail;
        }

        if (go2asoc) {
                start_clnt_assoc(padapter);
                return _SUCCESS;
        }
authclnt_fail:
        return _FAIL;
}

static unsigned int OnAssocReq(struct adapter *padapter,
                               struct recv_frame *precv_frame)
{
#ifdef CONFIG_88EU_AP_MODE
        u16 capab_info;
        struct rtw_ieee802_11_elems elems;
        struct sta_info *pstat;
        unsigned char *p, *pos, *wpa_ie;
        unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
        int i, wpa_ie_len, left;
        unsigned char supportRate[16];
        int supportRateNum;
        unsigned short status = WLAN_STATUS_SUCCESS;
        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->pkt->data;
        uint ie_len, pkt_len = precv_frame->pkt->len;

        if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
                return _FAIL;

        frame_type = GetFrameSubType(pframe);
        if (frame_type == IEEE80211_STYPE_ASSOC_REQ)
                ie_offset = _ASOCREQ_IE_OFFSET_;
        else /*  IEEE80211_STYPE_REASSOC_REQ */
                ie_offset = _REASOCREQ_IE_OFFSET_;

        if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset)
                return _FAIL;

        pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
        if (!pstat) {
                status = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA;
                goto asoc_class2_error;
        }

        capab_info = get_unaligned_le16(pframe + WLAN_HDR_A3_LEN);

        left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
        pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);

        /*  check if this stat has been successfully authenticated/assocated */
        if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) {
                if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) {
                        status = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA;
                        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) {
                status = WLAN_STATUS_UNSPECIFIED_FAILURE;
                goto OnAssocReqFail;
        }

        /*  now we should check all the fields... */
        /*  checking SSID */
        p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, WLAN_EID_SSID, &ie_len,
                       pkt_len - WLAN_HDR_A3_LEN - ie_offset);

        if (!p || ie_len == 0) {
                /*  broadcast ssid, however it is not allowed in assocreq */
                status = WLAN_STATUS_UNSPECIFIED_FAILURE;
                goto OnAssocReqFail;
        } else {
                /*  check if ssid match */
                if (memcmp((void *)(p + 2), cur->ssid.ssid, cur->ssid.ssid_length))
                        status = WLAN_STATUS_UNSPECIFIED_FAILURE;

                if (ie_len != cur->ssid.ssid_length)
                        status = WLAN_STATUS_UNSPECIFIED_FAILURE;
        }

        if (status != WLAN_STATUS_SUCCESS)
                goto OnAssocReqFail;

        /*  check if the supported rate is ok */
        p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, WLAN_EID_SUPP_RATES, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
        if (!p) {
                /*  use our own rate set as statoin used */
                /* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
                /* supportRateNum = AP_BSSRATE_LEN; */

                status = WLAN_STATUS_UNSPECIFIED_FAILURE;
                goto OnAssocReqFail;
        } else {
                memcpy(supportRate, p + 2, ie_len);
                supportRateNum = ie_len;

                p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, WLAN_EID_EXT_SUPP_RATES, &ie_len,
                               pkt_len - WLAN_HDR_A3_LEN - ie_offset);
                if (p) {
                        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_INVALID_GROUP_CIPHER;

                        if (!pstat->wpa2_pairwise_cipher)
                                status = WLAN_STATUS_INVALID_PAIRWISE_CIPHER;
                } 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_INVALID_GROUP_CIPHER;

                        if (!pstat->wpa_pairwise_cipher)
                                status = WLAN_STATUS_INVALID_PAIRWISE_CIPHER;
                } else {
                        status = WLAN_STATUS_INVALID_IE;
                }
        } else {
                wpa_ie = NULL;
                wpa_ie_len = 0;
        }

        if (status != WLAN_STATUS_SUCCESS)
                goto OnAssocReqFail;

        pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
        if (!wpa_ie) {
                if (elems.wps_ie) {
                        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 {
                        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) {
                                        status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;

                                        goto OnAssocReqFail;
                                }
                        }
                }
        } else {
                int copy_len;

                if (psecuritypriv->wpa_psk == 0) {
                        status = WLAN_STATUS_INVALID_IE;

                        goto OnAssocReqFail;
                }

                if (elems.wps_ie) {
                        pstat->flags |= WLAN_STA_WPS;
                        copy_len = 0;
                } else {
                        copy_len = min_t(int, wpa_ie_len + 2, sizeof(pstat->wpa_ie));
                }
                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, WLAN_EID_VENDOR_SPECIFIC, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
                        if (p) {
                                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;