/******************************************************************************
 *
 * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of version 2 of the GNU General Public License as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
 *
 *
 ******************************************************************************/
#define _RTW_MLME_EXT_C_

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

static struct mlme_handler mlme_sta_tbl[] = {
        {WIFI_ASSOCREQ,         "OnAssocReq",   &OnAssocReq},
        {WIFI_ASSOCRSP,         "OnAssocRsp",   &OnAssocRsp},
        {WIFI_REASSOCREQ,       "OnReAssocReq", &OnAssocReq},
        {WIFI_REASSOCRSP,       "OnReAssocRsp", &OnAssocRsp},
        {WIFI_PROBEREQ,         "OnProbeReq",   &OnProbeReq},
        {WIFI_PROBERSP,         "OnProbeRsp",           &OnProbeRsp},

        /*----------------------------------------------------------
                                        below 2 are reserved
        -----------------------------------------------------------*/
        {0,                                     "DoReserved",           &DoReserved},
        {0,                                     "DoReserved",           &DoReserved},
        {WIFI_BEACON,           "OnBeacon",             &OnBeacon},
        {WIFI_ATIM,                     "OnATIM",               &OnAtim},
        {WIFI_DISASSOC,         "OnDisassoc",           &OnDisassoc},
        {WIFI_AUTH,                     "OnAuth",               &OnAuthClient},
        {WIFI_DEAUTH,           "OnDeAuth",             &OnDeAuth},
        {WIFI_ACTION,           "OnAction",             &OnAction},
};

static struct action_handler OnAction_tbl[] = {
        {RTW_WLAN_CATEGORY_SPECTRUM_MGMT,        "ACTION_SPECTRUM_MGMT", on_action_spct},
        {RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos},
        {RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls},
        {RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
        {RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
        {RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
        {RTW_WLAN_CATEGORY_FT, "ACTION_FT",     &DoReserved},
        {RTW_WLAN_CATEGORY_HT,  "ACTION_HT",    &OnAction_ht},
        {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved},
        {RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm},
        {RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p},
};


static u8 null_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};

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

unsigned char   WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
unsigned char   WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};

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

extern unsigned char REALTEK_96B_IE[];

/********************************************************
MCS rate definitions
*********************************************************/
unsigned char   MCS_rate_2R[16] = {0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
unsigned char   MCS_rate_1R[16] = {0xff, 0x00, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};

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

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

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

/*
 * Search the @param channel_num in given @param channel_set
 * @ch_set: the given channel set
 * @ch: the given channel number
 *
 * return the index of channel_num in channel_set, -1 if not found
 */
int rtw_ch_set_search_ch(struct rt_channel_info *ch_set, const u32 ch)
{
        int i;
        for (i = 0; ch_set[i].ChannelNum != 0; i++) {
                if (ch == ch_set[i].ChannelNum)
                        break;
        }

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

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

Following are the initialization functions for WiFi MLME

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

int init_hw_mlme_ext(struct adapter *padapter)
{
        struct  mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;

        set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
        return _SUCCESS;
}

static void init_mlme_ext_priv_value(struct adapter *padapter)
{
        struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
        unsigned char   mixed_datarate[NumRates] = {
                _1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
                _9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_,
                 _48M_RATE_, _54M_RATE_, 0xff
        };
        unsigned char   mixed_basicrate[NumRates] = {
                _1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
                _12M_RATE_, _24M_RATE_, 0xff,
        };

        ATOMIC_SET(&pmlmeext->event_seq, 0);
        pmlmeext->mgnt_seq = 0;/* reset to zero when disconnect at client mode */

        pmlmeext->cur_channel = padapter->registrypriv.channel;
        pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
        pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
        pmlmeext->oper_channel = pmlmeext->cur_channel ;
        pmlmeext->oper_bwmode = pmlmeext->cur_bwmode;
        pmlmeext->oper_ch_offset = pmlmeext->cur_ch_offset;
        pmlmeext->retry = 0;

        pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;

        memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
        memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);

        pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;

        pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
        pmlmeext->sitesurvey_res.channel_idx = 0;
        pmlmeext->sitesurvey_res.bss_cnt = 0;
        pmlmeext->scan_abort = false;

        pmlmeinfo->state = WIFI_FW_NULL_STATE;
        pmlmeinfo->reauth_count = 0;
        pmlmeinfo->reassoc_count = 0;
        pmlmeinfo->link_count = 0;
        pmlmeinfo->auth_seq = 0;
        pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
        pmlmeinfo->key_index = 0;
        pmlmeinfo->iv = 0;

        pmlmeinfo->enc_algo = _NO_PRIVACY_;
        pmlmeinfo->authModeToggle = 0;

        _rtw_memset(pmlmeinfo->chg_txt, 0, 128);

        pmlmeinfo->slotTime = SHORT_SLOT_TIME;
        pmlmeinfo->preamble_mode = PREAMBLE_AUTO;

        pmlmeinfo->dialogToken = 0;

        pmlmeext->action_public_rxseq = 0xffff;
        pmlmeext->action_public_dialog_token = 0xff;
}

static int has_channel(struct rt_channel_info *channel_set,
                                           u8 chanset_size,
                                           u8 chan) {
        int i;

        for (i = 0; i < chanset_size; i++) {
                if (channel_set[i].ChannelNum == chan)
                        return 1;
        }
        return 0;
}

static void init_channel_list(struct adapter *padapter, struct rt_channel_info *channel_set,
                                                          u8 chanset_size,
                                                          struct p2p_channels *channel_list) {
        struct p2p_oper_class_map op_class[] = {
                { IEEE80211G,  81,   1,  13,  1, BW20 },
                { IEEE80211G,  82,  14,  14,  1, BW20 },
                { -1, 0, 0, 0, 0, BW20 }
        };

        int cla, op;

        cla = 0;

        for (op = 0; op_class[op].op_class; op++) {
                u8 ch;
                struct p2p_oper_class_map *o = &op_class[op];
                struct p2p_reg_class *reg = NULL;

                for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
                        if (!has_channel(channel_set, chanset_size, ch)) {
                                continue;
                        }

                        if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
                                continue;

                        if ((0 == (padapter->registrypriv.cbw40_enable & BIT(1))) &&
                            ((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
                                continue;

                        if (reg == NULL) {
                                reg = &channel_list->reg_class[cla];
                                cla++;
                                reg->reg_class = o->op_class;
                                reg->channels = 0;
                        }
                        reg->channel[reg->channels] = ch;
                        reg->channels++;
                }
        }
        channel_list->reg_classes = cla;
}

static u8 init_channel_set(struct adapter *padapter, u8 ChannelPlan, struct rt_channel_info *channel_set)
{
        u8 index, chanset_size = 0;
        u8 b2_4GBand = false;
        u8 Index2G = 0;

        _rtw_memset(channel_set, 0, sizeof(struct rt_channel_info) * MAX_CHANNEL_NUM);

        if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
                DBG_88E("ChannelPlan ID %x error !!!!!\n", ChannelPlan);
                return chanset_size;
        }

        if (padapter->registrypriv.wireless_mode & WIRELESS_11G) {
                b2_4GBand = true;
                if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
                        Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
                else
                        Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
        }

        if (b2_4GBand) {
                for (index = 0; index < RTW_ChannelPlan2G[Index2G].Len; index++) {
                        channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];

                        if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||/* Channel 1~11 is active, and 12~14 is passive */
                            (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == ChannelPlan)) {
                                if (channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
                                        channel_set[chanset_size].ScanType = SCAN_ACTIVE;
                                else if ((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
                                        channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
                        } else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
                                   RT_CHANNEL_DOMAIN_2G_WORLD == Index2G) {/*  channel 12~13, passive scan */
                                if (channel_set[chanset_size].ChannelNum <= 11)
                                        channel_set[chanset_size].ScanType = SCAN_ACTIVE;
                                else
                                        channel_set[chanset_size].ScanType = SCAN_PASSIVE;
                        } else {
                                channel_set[chanset_size].ScanType = SCAN_ACTIVE;
                        }

                        chanset_size++;
                }
        }
        return chanset_size;
}

int     init_mlme_ext_priv(struct adapter *padapter)
{
        int     res = _SUCCESS;
        struct registry_priv *pregistrypriv = &padapter->registrypriv;
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
        struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);

        pmlmeext->padapter = padapter;

        init_mlme_ext_priv_value(padapter);
        pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;

        init_mlme_ext_timer(padapter);

#ifdef CONFIG_88EU_AP_MODE
        init_mlme_ap_info(padapter);
#endif

        pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan, pmlmeext->channel_set);
        init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);

        pmlmeext->chan_scan_time = SURVEY_TO;
        pmlmeext->mlmeext_init = true;


        pmlmeext->active_keep_alive_check = true;

        return res;
}

void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
{
        struct adapter *padapter = pmlmeext->padapter;

        if (!padapter)
                return;

        if (padapter->bDriverStopped) {
                _cancel_timer_ex(&pmlmeext->survey_timer);
                _cancel_timer_ex(&pmlmeext->link_timer);
                /* _cancel_timer_ex(&pmlmeext->ADDBA_timer); */
        }
}

static void _mgt_dispatcher(struct adapter *padapter, struct mlme_handler *ptable, union recv_frame *precv_frame)
{
        u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
        u8 *pframe = precv_frame->u.hdr.rx_data;

          if (ptable->func) {
         /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
                if (!_rtw_memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
                    !_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
                        return;
                ptable->func(padapter, precv_frame);
        }
}

void mgt_dispatcher(struct adapter *padapter, union recv_frame *precv_frame)
{
        int index;
        struct mlme_handler *ptable;
#ifdef CONFIG_88EU_AP_MODE
        struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
#endif /* CONFIG_88EU_AP_MODE */
        u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
        u8 *pframe = precv_frame->u.hdr.rx_data;
        struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));

        RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
                 ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
                  GetFrameType(pframe), GetFrameSubType(pframe)));

        if (GetFrameType(pframe) != WIFI_MGT_TYPE) {
                RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
                return;
        }

        /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
        if (!_rtw_memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
            !_rtw_memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
                return;

        ptable = mlme_sta_tbl;

        index = GetFrameSubType(pframe) >> 4;

        if (index > 13) {
                RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Currently we do not support reserved sub-fr-type=%d\n", index));
                return;
        }
        ptable += index;

        if (psta != NULL) {
                if (GetRetry(pframe)) {
                        if (precv_frame->u.hdr.attrib.seq_num == psta->RxMgmtFrameSeqNum) {
                                /* drop the duplicate management frame */
                                DBG_88E("Drop duplicate management frame with seq_num=%d.\n", precv_frame->u.hdr.attrib.seq_num);
                                return;
                        }
                }
                psta->RxMgmtFrameSeqNum = precv_frame->u.hdr.attrib.seq_num;
        }

#ifdef CONFIG_88EU_AP_MODE
        switch (GetFrameSubType(pframe)) {
        case WIFI_AUTH:
                if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
                        ptable->func = &OnAuth;
                else
                        ptable->func = &OnAuthClient;
                /* fall through */
        case WIFI_ASSOCREQ:
        case WIFI_REASSOCREQ:
                _mgt_dispatcher(padapter, ptable, precv_frame);
                break;
        case WIFI_PROBEREQ:
                if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
                        _mgt_dispatcher(padapter, ptable, precv_frame);
                else
                        _mgt_dispatcher(padapter, ptable, precv_frame);
                break;
        case WIFI_BEACON:
                _mgt_dispatcher(padapter, ptable, precv_frame);
                break;
        case WIFI_ACTION:
                _mgt_dispatcher(padapter, ptable, precv_frame);
                break;
        default:
                _mgt_dispatcher(padapter, ptable, precv_frame);
                if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
                        rtw_hostapd_mlme_rx(padapter, precv_frame);
                break;
        }
#else
        _mgt_dispatcher(padapter, ptable, precv_frame);
#endif
}

#ifdef CONFIG_88EU_P2P
static u32 p2p_listen_state_process(struct adapter *padapter, unsigned char *da)
{
        bool response = true;

        /*      do nothing if the device name is empty */
        if (!padapter->wdinfo.device_name_len)
                response = false;

        if (response)
                issue_probersp_p2p(padapter, da);

        return _SUCCESS;
}
#endif /* CONFIG_88EU_P2P */


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

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

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

unsigned int OnProbeReq(struct adapter *padapter, union recv_frame *precv_frame)
{
        unsigned int    ielen;
        unsigned char   *p;
        struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
        struct wlan_bssid_ex *cur = &(pmlmeinfo->network);
        u8 *pframe = precv_frame->u.hdr.rx_data;
        uint len = precv_frame->u.hdr.len;
        u8 is_valid_p2p_probereq = false;

#ifdef CONFIG_88EU_P2P
        struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
        u8 wifi_test_chk_rate = 1;

        if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
            !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) &&
            !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
            !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
            !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)) {
                /*      mcs_rate = 0 -> CCK 1M rate */
                /*      mcs_rate = 1 -> CCK 2M rate */
                /*      mcs_rate = 2 -> CCK 5.5M rate */
                /*      mcs_rate = 3 -> CCK 11M rate */
                /*      In the P2P mode, the driver should not support the CCK rate */

                /*      Commented by Kurt 2012/10/16 */
                /*      IOT issue: Google Nexus7 use 1M rate to send p2p_probe_req after GO nego completed and Nexus7 is client */
                if (wifi_test_chk_rate == 1) {
                        is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len);
                        if (is_valid_p2p_probereq) {
                                if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
                                        /*  FIXME */
                                        report_survey_event(padapter, precv_frame);
                                        p2p_listen_state_process(padapter,  get_sa(pframe));

                                        return _SUCCESS;
                                }

                                if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
                                        goto _continue;
                        }
                }
        }

_continue:
#endif /* CONFIG_88EU_P2P */

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

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

        p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
                        len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);

        /* check (wildcard) SSID */
        if (p != NULL) {
                if (is_valid_p2p_probereq)
                        goto _issue_probersp;

                if ((ielen != 0 && !_rtw_memcmp((void *)(p+2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength)) ||
                    (ielen == 0 && pmlmeinfo->hidden_ssid_mode))
                        return _SUCCESS;

_issue_probersp:

                if (check_fwstate(pmlmepriv, _FW_LINKED) &&
                    pmlmepriv->cur_network.join_res)
                        issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
        }
        return _SUCCESS;
}

unsigned int OnProbeRsp(struct adapter *padapter, union recv_frame *precv_frame)
{
        struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
#ifdef CONFIG_88EU_P2P
        struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
        u8 *pframe = precv_frame->u.hdr.rx_data;
#endif

#ifdef CONFIG_88EU_P2P
        if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
                if (pwdinfo->tx_prov_disc_info.benable) {
                        if (_rtw_memcmp(pwdinfo->tx_prov_disc_info.peerIFAddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
                                if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
                                        pwdinfo->tx_prov_disc_info.benable = false;
                                        issue_p2p_provision_request(padapter,
                                                                    pwdinfo->tx_prov_disc_info.ssid.Ssid,
                                                                    pwdinfo->tx_prov_disc_info.ssid.SsidLength,
                                                                    pwdinfo->tx_prov_disc_info.peerDevAddr);
                                } else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
                                        pwdinfo->tx_prov_disc_info.benable = false;
                                        issue_p2p_provision_request(padapter, NULL, 0,
                                                                    pwdinfo->tx_prov_disc_info.peerDevAddr);
                                }
                        }
                }
                return _SUCCESS;
        } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
                if (pwdinfo->nego_req_info.benable) {
                        DBG_88E("[%s] P2P State is GONEGO ING!\n", __func__);
                        if (_rtw_memcmp(pwdinfo->nego_req_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
                                pwdinfo->nego_req_info.benable = false;
                                issue_p2p_GO_request(padapter, pwdinfo->nego_req_info.peerDevAddr);
                        }
                }
        } else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) {
                if (pwdinfo->invitereq_info.benable) {
                        DBG_88E("[%s] P2P_STATE_TX_INVITE_REQ!\n", __func__);
                        if (_rtw_memcmp(pwdinfo->invitereq_info.peer_macaddr, GetAddr2Ptr(pframe), ETH_ALEN)) {
                                pwdinfo->invitereq_info.benable = false;
                                issue_p2p_invitation_request(padapter, pwdinfo->invitereq_info.peer_macaddr);
                        }
                }
        }
#endif


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

        return _SUCCESS;
}

unsigned int OnBeacon(struct adapter *padapter, union recv_frame *precv_frame)
{
        int cam_idx;
        struct sta_info *psta;
        struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
        struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
        struct sta_priv *pstapriv = &padapter->stapriv;
        u8 *pframe = precv_frame->u.hdr.rx_data;
        uint len = precv_frame->u.hdr.len;
        struct wlan_bssid_ex *pbss;
        int ret = _SUCCESS;

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

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

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

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

                        /* start auth */
                        start_clnt_auth(padapter);

                        return _SUCCESS;
                }

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

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

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

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

_END_ONBEACON_:

        return _SUCCESS;
}

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

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

        DBG_88E("+OnAuth\n");

        sa = GetAddr2Ptr(pframe);

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

        DBG_88E("auth alg=%x, seq=%X\n", algorithm, seq);

        if (auth_mode == 2 && psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
            psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
                auth_mode = 0;

        if ((algorithm > 0 && auth_mode == 0) ||        /*  rx a shared-key auth but shared not enabled */
            (algorithm == 0 && auth_mode == 1)) {       /*  rx a open-system auth but shared-key is enabled */
                DBG_88E("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n",
                        algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);

                status = _STATS_NO_SUPP_ALG_;

                goto auth_fail;
        }

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

        pstat = rtw_get_stainfo(pstapriv, sa);
        if (pstat == NULL) {
                /*  allocate a new one */
                DBG_88E("going to alloc stainfo for sa=%pM\n", sa);
                pstat = rtw_alloc_stainfo(pstapriv, sa);
                if (pstat == NULL) {
                        DBG_88E(" Exceed the upper limit of supported clients...\n");
                        status = _STATS_UNABLE_HANDLE_STA_;
                        goto auth_fail;
                }

                pstat->state = WIFI_FW_AUTH_NULL;
                pstat->auth_seq = 0;
        } else {
                _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
                if (!rtw_is_list_empty(&pstat->asoc_list)) {
                        rtw_list_delete(&pstat->asoc_list);
                        pstapriv->asoc_list_cnt--;
                }
                _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);

                if (seq == 1) {
                        /* TODO: STA re_auth and auth timeout */
                }
        }

        _enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
        if (rtw_is_list_empty(&pstat->auth_list)) {
                rtw_list_insert_tail(&pstat->auth_list, &pstapriv->auth_list);
                pstapriv->auth_list_cnt++;
        }
        _exit_critical_bh(&pstapriv->auth_list_lock, &irqL);

        if (pstat->auth_seq == 0)
                pstat->expire_to = pstapriv->auth_to;

        if ((pstat->auth_seq + 1) != seq) {
                DBG_88E("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
                        seq, pstat->auth_seq+1);
                status = _STATS_OUT_OF_AUTH_SEQ_;
                goto auth_fail;
        }

        if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2)) {
                if (seq == 1) {
                        pstat->state &= ~WIFI_FW_AUTH_NULL;
                        pstat->state |= WIFI_FW_AUTH_SUCCESS;
                        pstat->expire_to = pstapriv->assoc_to;
                        pstat->authalg = algorithm;
                } else {
                        DBG_88E("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
                                seq, pstat->auth_seq+1);
                        status = _STATS_OUT_OF_AUTH_SEQ_;
                        goto auth_fail;
                }
        } else { /*  shared system or auto authentication */
                if (seq == 1) {
                        /* prepare for the challenging txt... */

                        pstat->state &= ~WIFI_FW_AUTH_NULL;
                        pstat->state |= WIFI_FW_AUTH_STATE;
                        pstat->authalg = algorithm;
                        pstat->auth_seq = 2;
                } else if (seq == 3) {
                        /* checking for challenging txt... */
                        DBG_88E("checking for challenging txt...\n");

                        p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len,
                                        len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);

                        if ((p == NULL) || (ie_len <= 0)) {
                                DBG_88E("auth rejected because challenge failure!(1)\n");
                                status = _STATS_CHALLENGE_FAIL_;
                                goto auth_fail;
                        }

                        if (_rtw_memcmp((void *)(p + 2), pstat->chg_txt, 128)) {
                                pstat->state &= (~WIFI_FW_AUTH_STATE);
                                pstat->state |= WIFI_FW_AUTH_SUCCESS;
                                /*  challenging txt is correct... */
                                pstat->expire_to =  pstapriv->assoc_to;
                        } else {
                                DBG_88E("auth rejected because challenge failure!\n");
                                status = _STATS_CHALLENGE_FAIL_;
                                goto auth_fail;
                        }
                } else {
                        DBG_88E("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
                                seq, pstat->auth_seq+1);
                        status = _STATS_OUT_OF_AUTH_SEQ_;
                        goto auth_fail;
                }
        }

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

#ifdef CONFIG_88EU_AP_MODE
        issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
#endif

        if (pstat->state & WIFI_FW_AUTH_SUCCESS)
                pstat->auth_seq = 0;

        return _SUCCESS;

auth_fail:

        if (pstat)
                rtw_free_stainfo(padapter , pstat);

        pstat = &stat;
        _rtw_memset((char *)pstat, '\0', sizeof(stat));
        pstat->auth_seq = 2;
        memcpy(pstat->hwaddr, sa, 6);

#ifdef CONFIG_88EU_AP_MODE
        issue_auth(padapter, pstat, (unsigned short)status);
#endif

#endif
        return _FAIL;
}

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

        DBG_88E("%s\n", __func__);

        /* check A1 matches or not */
        if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
                return _SUCCESS;

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

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

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

        if (status != 0) {
                DBG_88E("clnt auth fail, status: %d\n", status);
                if (status == 13) { /*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
                        if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
                                pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
                        else
                                pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
                }

                set_link_timer(pmlmeext, 1);
                goto authclnt_fail;
        }

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

                        if (p == NULL)
                                goto authclnt_fail;

                        memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
                        pmlmeinfo->auth_seq = 3;
                        issue_auth(padapter, NULL, 0);
                        set_link_timer(pmlmeext, REAUTH_TO);

                        return _SUCCESS;
                } else {
                        /*  open system */
                        go2asoc = 1;
                }
        } else if (seq == 4) {
                if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
                        go2asoc = 1;
                else
                        goto authclnt_fail;
        } else {
                /*  this is also illegal */
                goto authclnt_fail;
        }

        if (go2asoc) {
                DBG_88E_LEVEL(_drv_info_, "auth success, start assoc\n");
                start_clnt_assoc(padapter);
                return _SUCCESS;
        }

authclnt_fail:
        return _FAIL;
}

unsigned int OnAssocReq(struct adapter *padapter, union recv_frame *precv_frame)
{
#ifdef CONFIG_88EU_AP_MODE
        unsigned long irqL;
        u16 capab_info;
        struct rtw_ieee802_11_elems elems;
        struct sta_info *pstat;
        unsigned char           reassoc, *p, *pos, *wpa_ie;
        unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
        int             i, ie_len, wpa_ie_len, left;
        unsigned char           supportRate[16];
        int                                     supportRateNum;
        unsigned short          status = _STATS_SUCCESSFUL_;
        unsigned short          frame_type, ie_offset = 0;
        struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
        struct security_priv *psecuritypriv = &padapter->securitypriv;
        struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
        struct wlan_bssid_ex *cur = &(pmlmeinfo->network);
        struct sta_priv *pstapriv = &padapter->stapriv;
        u8 *pframe = precv_frame->u.hdr.rx_data;
        uint pkt_len = precv_frame->u.hdr.len;
#ifdef CONFIG_88EU_P2P
        struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
        u8 p2p_status_code = P2P_STATUS_SUCCESS;
        u8 *p2pie;
        u32 p2pielen = 0;
#endif /* CONFIG_88EU_P2P */

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

        frame_type = GetFrameSubType(pframe);
        if (frame_type == WIFI_ASSOCREQ) {
                reassoc = 0;
                ie_offset = _ASOCREQ_IE_OFFSET_;
        } else { /*  WIFI_REASSOCREQ */
                reassoc = 1;
                ie_offset = _REASOCREQ_IE_OFFSET_;
        }


        if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
                DBG_88E("handle_assoc(reassoc=%d) - too short payload (len=%lu)"
                       "\n", reassoc, (unsigned long)pkt_len);
                return _FAIL;
        }

        pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
        if (pstat == (struct sta_info *)NULL) {
                status = _RSON_CLS2_;
                goto asoc_class2_error;
        }

        capab_info = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN);

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


        DBG_88E("%s\n", __func__);

        /*  check if this stat has been successfully authenticated/assocated */
        if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) {
                if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) {
                        status = _RSON_CLS2_;
                        goto asoc_class2_error;
                } else {
                        pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
                        pstat->state |= WIFI_FW_ASSOC_STATE;
                }
        } else {
                pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
                pstat->state |= WIFI_FW_ASSOC_STATE;
        }
        pstat->capability = capab_info;
        /* now parse all ieee802_11 ie to point to elems */
        if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
            !elems.ssid) {
                DBG_88E("STA %pM sent invalid association request\n",
                        pstat->hwaddr);
                status = _STATS_FAILURE_;
                goto OnAssocReqFail;
        }


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

        if (ie_len == 0) { /*  broadcast ssid, however it is not allowed in assocreq */
                status = _STATS_FAILURE_;
        } else {
                /*  check if ssid match */
                if (!_rtw_memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
                        status = _STATS_FAILURE_;

                if (ie_len != cur->Ssid.SsidLength)
                        status = _STATS_FAILURE_;
        }

        if (_STATS_SUCCESSFUL_ != status)
                goto OnAssocReqFail;

        /*  check if the supported rate is ok */
        p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
        if (p == NULL) {
                DBG_88E("Rx a sta assoc-req which supported rate is empty!\n");
                /*  use our own rate set as statoin used */
                /* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
                /* supportRateNum = AP_BSSRATE_LEN; */

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

                p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_ , &ie_len,
                                pkt_len - WLAN_HDR_A3_LEN - ie_offset);
                if (p !=  NULL) {
                        if (supportRateNum <= sizeof(supportRate)) {
                                memcpy(supportRate+supportRateNum, p+2, ie_len);
                                supportRateNum += ie_len;
                        }
                }
        }

        /* todo: mask supportRate between AP & STA -> move to update raid */
        /* get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0); */

        /* update station supportRate */
        pstat->bssratelen = supportRateNum;
        memcpy(pstat->bssrateset, supportRate, supportRateNum);
        UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);

        /* check RSN/WPA/WPS */
        pstat->dot8021xalg = 0;
        pstat->wpa_psk = 0;
        pstat->wpa_group_cipher = 0;
        pstat->wpa2_group_cipher = 0;
        pstat->wpa_pairwise_cipher = 0;
        pstat->wpa2_pairwise_cipher = 0;
        _rtw_memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
        if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
                int group_cipher = 0, pairwise_cipher = 0;

                wpa_ie = elems.rsn_ie;
                wpa_ie_len = elems.rsn_ie_len;

                if (rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
                        pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
                        pstat->wpa_psk |= BIT(1);

                        pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
                        pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;

                        if (!pstat->wpa2_group_cipher)
                                status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;

                        if (!pstat->wpa2_pairwise_cipher)
                                status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
                } else {
                        status = WLAN_STATUS_INVALID_IE;
                }
        } else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
                int group_cipher = 0, pairwise_cipher = 0;

                wpa_ie = elems.wpa_ie;
                wpa_ie_len = elems.wpa_ie_len;

                if (rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
                        pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
                        pstat->wpa_psk |= BIT(0);

                        pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
                        pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;

                        if (!pstat->wpa_group_cipher)
                                status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;

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

        if (_STATS_SUCCESSFUL_ != status)
                goto OnAssocReqFail;

        pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
        if (wpa_ie == NULL) {
                if (elems.wps_ie) {
                        DBG_88E("STA included WPS IE in "
                                   "(Re)Association Request - assume WPS is "
                                   "used\n");
                        pstat->flags |= WLAN_STA_WPS;
                        /* wpabuf_free(sta->wps_ie); */
                        /* sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4, */
                        /*                              elems.wps_ie_len - 4); */
                } else {
                        DBG_88E("STA did not include WPA/RSN IE "
                                   "in (Re)Association Request - possible WPS "
                                   "use\n");
                        pstat->flags |= WLAN_STA_MAYBE_WPS;
                }


                /*  AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
                /*  that the selected registrar of AP is _FLASE */
                if ((psecuritypriv->wpa_psk > 0) && (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS))) {
                        if (pmlmepriv->wps_beacon_ie) {
                                u8 selected_registrar = 0;

                                rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR , &selected_registrar, NULL);

                                if (!selected_registrar) {
                                        DBG_88E("selected_registrar is false , or AP is not ready to do WPS\n");

                                        status = _STATS_UNABLE_HANDLE_STA_;

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

                if (psecuritypriv->wpa_psk == 0) {
                        DBG_88E("STA %pM: WPA/RSN IE in association "
                        "request, but AP don't support WPA/RSN\n", pstat->hwaddr);

                        status = WLAN_STATUS_INVALID_IE;

                        goto OnAssocReqFail;
                }

                if (elems.wps_ie) {
                        DBG_88E("STA included WPS IE in "
                                   "(Re)Association Request - WPS is "
                                   "used\n");
                        pstat->flags |= WLAN_STA_WPS;
                        copy_len = 0;
                } else {
                        copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)) : (wpa_ie_len+2);
                }
                if (copy_len > 0)
                        memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
        }
        /*  check if there is WMM IE & support WWM-PS */
        pstat->flags &= ~WLAN_STA_WME;
        pstat->qos_option = 0;
        pstat->qos_info = 0;
        pstat->has_legacy_ac = true;
        pstat->uapsd_vo = 0;
        pstat->uapsd_vi = 0;
        pstat->uapsd_be = 0;
        pstat->uapsd_bk = 0;
        if (pmlmepriv->qospriv.qos_option) {
                p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
                for (;;) {
                        p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
                        if (p != NULL) {
                                if (_rtw_memcmp(p+2, WMM_IE, 6)) {
                                        pstat->flags |= WLAN_STA_WME;

                                        pstat->qos_option = 1;
                                        pstat->qos_info = *(p+8);

                                        pstat->max_sp_len = (pstat->qos_info>>5)&0x3;

                                        if ((pstat->qos_info&0xf) != 0xf)
                                                pstat->has_legacy_ac = true;
                                        else
                                                pstat->has_legacy_ac = false;

                                        if (pstat->qos_info&0xf) {
                                                if (pstat->qos_info&BIT(0))
                                                        pstat->uapsd_vo = BIT(0)|BIT(1);
                                                else
                                                        pstat->uapsd_vo = 0;

                                                if (pstat->qos_info&BIT(1))
                                                        pstat->uapsd_vi = BIT(0)|BIT(1);
                                                else
                                                        pstat->uapsd_vi = 0;

                                                if (pstat->qos_info&BIT(2))
                                                        pstat->uapsd_bk = BIT(0)|BIT(1);
                                                else
                                                        pstat->uapsd_bk = 0;

                                                if (pstat->qos_info&BIT(3))
                                                        pstat->uapsd_be = BIT(0)|BIT(1);
                                                else
                                                        pstat->uapsd_be = 0;
                                        }
                                        break;
                                }
                        } else {
                                break;
                        }
                        p = p + ie_len + 2;
                }
        }

        /* save HT capabilities in the sta object */
        _rtw_memset(&pstat->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));
        if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap)) {
                pstat->flags |= WLAN_STA_HT;

                pstat->flags |= WLAN_STA_WME;

                memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap));
        } else {
                pstat->flags &= ~WLAN_STA_HT;
        }
        if ((!pmlmepriv->htpriv.ht_option) && (pstat->flags&WLAN_STA_HT)) {
                status = _STATS_FAILURE_;
                goto OnAssocReqFail;
        }

        if ((pstat->flags & WLAN_STA_HT) &&
            ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
            (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP))) {
                DBG_88E("HT: %pM tried to "
                        "use TKIP with HT association\n", pstat->hwaddr);

                /* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
                /* goto OnAssocReqFail; */
        }

        pstat->flags |= WLAN_STA_NONERP;
        for (i = 0; i < pstat->bssratelen; i++) {
                if ((pstat->bssrateset[i] & 0x7f) > 22) {
                        pstat->flags &= ~WLAN_STA_NONERP;
                        break;
                }
        }

        if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
                pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
        else
                pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;



        if (status != _STATS_SUCCESSFUL_)
                goto OnAssocReqFail;

#ifdef CONFIG_88EU_P2P
        pstat->is_p2p_device = false;
        if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
                p2pie = rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , NULL, &p2pielen);
                if (p2pie) {
                        pstat->is_p2p_device = true;
                        p2p_status_code = (u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat);
                        if (p2p_status_code > 0) {
                                pstat->p2p_status_code = p2p_status_code;
                                status = _STATS_CAP_FAIL_;
                                goto OnAssocReqFail;
                        }
                }
        }
        pstat->p2p_status_code = p2p_status_code;
#endif /* CONFIG_88EU_P2P */

        /* TODO: identify_proprietary_vendor_ie(); */
        /*  Realtek proprietary IE */
        /*  identify if this is Broadcom sta */
        /*  identify if this is ralink sta */
        /*  Customer proprietary IE */

        /* get a unique AID */
        if (pstat->aid > 0) {
                DBG_88E("  old AID %d\n", pstat->aid);
        } else {
                for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
                        if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
                                break;

                /* if (pstat->aid > NUM_STA) { */
                if (pstat->aid > pstapriv->max_num_sta) {
                        pstat->aid = 0;

                        DBG_88E("  no room for more AIDs\n");

                        status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;

                        goto OnAssocReqFail;
                } else {
                        pstapriv->sta_aid[pstat->aid - 1] = pstat;
                        DBG_88E("allocate new AID=(%d)\n", pstat->aid);
                }
        }

        pstat->state &= (~WIFI_FW_ASSOC_STATE);
        pstat->state |= WIFI_FW_ASSOC_SUCCESS;

        _enter_critical_bh(&pstapriv->auth_list_lock, &irqL);
        if (!rtw_is_list_empty(&pstat->auth_list)) {
                rtw_list_delete(&pstat->auth_list);
                pstapriv->auth_list_cnt--;
        }
        _exit_critical_bh(&pstapriv->auth_list_lock, &irqL);

        _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
        if (rtw_is_list_empty(&pstat->asoc_list)) {
                pstat->expire_to = pstapriv->expire_to;
                rtw_list_insert_tail(&pstat->asoc_list, &pstapriv->asoc_list);
                pstapriv->asoc_list_cnt++;
        }
        _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);

        /*  now the station is qualified to join our BSS... */
        if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_ == status)) {
#ifdef CONFIG_88EU_AP_MODE
                /* 1 bss_cap_update & sta_info_update */
                bss_cap_update_on_sta_join(padapter, pstat);
                sta_info_update(padapter, pstat);

                /* issue assoc rsp before notify station join event. */
                if (frame_type == WIFI_ASSOCREQ)
                        issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
                else
                        issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);

                /* 2 - report to upper layer */
                DBG_88E("indicate_sta_join_event to upper layer - hostapd\n");
                rtw_indicate_sta_assoc_event(padapter, pstat);

                /* 3-(1) report sta add event */
                report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
#endif
        }

        return _SUCCESS;

asoc_class2_error:

#ifdef CONFIG_88EU_AP_MODE
        issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
#endif

        return _FAIL;

OnAssocReqFail:


#ifdef CONFIG_88EU_AP_MODE
        pstat->aid = 0;
        if (frame_type == WIFI_ASSOCREQ)
                issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
        else
                issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
#endif


#endif /* CONFIG_88EU_AP_MODE */

        return _FAIL;
}

unsigned int OnAssocRsp(struct adapter *padapter, union recv_frame *precv_frame)
{
        uint i;
        int res;
        unsigned short  status;
        struct ndis_802_11_var_ie *pIE;
        struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
        struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
        /* struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); */
        u8 *pframe = precv_frame->u.hdr.rx_data;
        uint pkt_len = precv_frame->u.hdr.len;

        DBG_88E("%s\n", __func__);

        /* check A1 matches or not */
        if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
                return _SUCCESS;

        if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
                return _SUCCESS;

        if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
                return _SUCCESS;

        _cancel_timer_ex(&pmlmeext->link_timer);

        /* status */
        status = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 2));
        if (status > 0) {
                DBG_88E("assoc reject, status code: %d\n", status);
                pmlmeinfo->state = WIFI_FW_NULL_STATE;
                res = -4;
                goto report_assoc_result;
        }

        /* get capabilities */
        pmlmeinfo->capability = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));

        /* set slot time */
        pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10)) ? 9 : 20;

        /* AID */
        pmlmeinfo->aid = (int)(le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
        res = pmlmeinfo->aid;

        /* following are moved to join event callback function */
        /* to handle HT, WMM, rate adaptive, update MAC reg */
        /* for not to handle the synchronous IO in the tasklet */
        for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;) {
                pIE = (struct ndis_802_11_var_ie *)(pframe + i);

                switch (pIE->ElementID) {
                case _VENDOR_SPECIFIC_IE_:
                        if (_rtw_memcmp(pIE->data, WMM_PARA_OUI, 6))    /* WMM */
                                WMM_param_handler(padapter, pIE);
                        break;
                case _HT_CAPABILITY_IE_:        /* HT caps */
                        HT_caps_handler(padapter, pIE);
                        break;
                case _HT_EXTRA_INFO_IE_:        /* HT info */
                        HT_info_handler(padapter, pIE);
                        break;
                case _ERPINFO_IE_:
                        ERP_IE_handler(padapter, pIE);
                default:
                        break;
                }

                i += (pIE->Length + 2);
        }

        pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
        pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;

        /* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
        UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);

report_assoc_result:
        if (res > 0) {
                rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
        } else {
                rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
        }

        report_join_res(padapter, res);

        return _SUCCESS;
}

unsigned int OnDeAuth(struct adapter *padapter, union recv_frame *precv_frame)
{
        unsigned short  reason;
        struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
        struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
        u8 *pframe = precv_frame->u.hdr.rx_data;
#ifdef CONFIG_88EU_P2P
        struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
#endif /* CONFIG_88EU_P2P */

        /* check A3 */
        if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
                return _SUCCESS;

#ifdef CONFIG_88EU_P2P
        if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
                _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
                _set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
        }
#endif /* CONFIG_88EU_P2P */

        reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));

        DBG_88E("%s Reason code(%d)\n", __func__, reason);

#ifdef CONFIG_88EU_AP_MODE
        if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
                unsigned long irqL;
                struct sta_info *psta;
                struct sta_priv *pstapriv = &padapter->stapriv;

                DBG_88E_LEVEL(_drv_always_, "ap recv deauth reason code(%d) sta:%pM\n",
                              reason, GetAddr2Ptr(pframe));

                psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
                if (psta) {
                        u8 updated = 0;

                        _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
                        if (!rtw_is_list_empty(&psta->asoc_list)) {
                                rtw_list_delete(&psta->asoc_list);
                                pstapriv->asoc_list_cnt--;
                                updated = ap_free_sta(padapter, psta, false, reason);
                        }
                        _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);

                        associated_clients_update(padapter, updated);
                }


                return _SUCCESS;
        } else
#endif
        {
                DBG_88E_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM\n",
                              reason, GetAddr3Ptr(pframe));

                receive_disconnect(padapter, GetAddr3Ptr(pframe) , reason);
        }
        pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
        return _SUCCESS;
}

unsigned int OnDisassoc(struct adapter *padapter, union recv_frame *precv_frame)
{
        u16 reason;
        struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
        struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
        u8 *pframe = precv_frame->u.hdr.rx_data;
#ifdef CONFIG_88EU_P2P
        struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
#endif /* CONFIG_88EU_P2P */

        /* check A3 */
        if (!(_rtw_memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN)))
                return _SUCCESS;

#ifdef CONFIG_88EU_P2P
        if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
                _cancel_timer_ex(&pwdinfo->reset_ch_sitesurvey);
                _set_timer(&pwdinfo->reset_ch_sitesurvey, 10);
        }
#endif /* CONFIG_88EU_P2P */

        reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));

        DBG_88E("%s Reason code(%d)\n", __func__, reason);

#ifdef CONFIG_88EU_AP_MODE
        if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
                unsigned long irqL;
                struct sta_info *psta;
                struct sta_priv *pstapriv = &padapter->stapriv;

                /* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */
                /* rtw_free_stainfo(padapter, psta); */
                /* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL); */

                DBG_88E_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
                              reason, GetAddr2Ptr(pframe));

                psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
                if (psta) {
                        u8 updated = 0;

                        _enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
                        if (!rtw_is_list_empty(&psta->asoc_list)) {
                                rtw_list_delete(&psta->asoc_list);
                                pstapriv->asoc_list_cnt--;
                                updated = ap_free_sta(padapter, psta, false, reason);
                        }
                        _exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);

                        associated_clients_update(padapter, updated);
                }

                return _SUCCESS;
        } else
#endif
        {
                DBG_88E_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
                              reason, GetAddr3Ptr(pframe));

                receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
        }
        pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
        return _SUCCESS;
}

unsigned int OnAtim(struct adapter *padapter, union recv_frame *precv_frame)
{
        DBG_88E("%s\n", __func__);
        return _SUCCESS;
}

unsigned int on_action_spct(struct adapter *padapter, union recv_frame *precv_frame)
{
        unsigned int ret = _FAIL;
        struct sta_info *psta = NULL;
        struct sta_priv *pstapriv = &padapter->stapriv;
        u8 *pframe = precv_frame->u.hdr.rx_data;
        u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
        u8 category;
        u8 action;

        DBG_88E(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));

        psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));

        if (!psta)
                goto exit;

        category = frame_body[0];
        if (category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
                goto exit;

        action = frame_body[1];
        switch (action) {
        case RTW_WLAN_ACTION_SPCT_MSR_REQ:
        case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
        case RTW_WLAN_ACTION_SPCT_TPC_REQ:
        case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
                break;
        case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
                break;
        default:
                break;
        }

exit:
        return ret;
}

unsigned int OnAction_qos(struct adapter *padapter, union recv_frame *precv_frame)
{
        return _SUCCESS;
}

unsigned int OnAction_dls(struct adapter *padapter, union recv_frame *precv_frame)
{
        return _SUCCESS;
}

unsigned int OnAction_back(struct adapter *padapter, union recv_frame *precv_frame)
{
        u8 *addr;
        struct sta_info *psta = NULL;
        struct recv_reorder_ctrl *preorder_ctrl;
        unsigned char           *frame_body;
        unsigned char           category, action;
        unsigned short  tid, status, reason_code = 0;
        struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
        struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
        u8 *pframe = precv_frame->u.hdr.rx_data;
        struct sta_priv *pstapriv = &padapter->stapriv;
        /* check RA matches or not */
        if (!_rtw_memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))/* for if1, sta/ap mode */
                return _SUCCESS;

        DBG_88E("%s\n", __func__);

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

        addr = GetAddr2Ptr(pframe);
        psta = rtw_get_stainfo(pstapriv, addr);

        if (psta == NULL)
                return _SUCCESS;

        frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));

        category = frame_body[0];
        if (category == RTW_WLAN_CATEGORY_BACK) { /*  representing Block Ack */
                if (!pmlmeinfo->HT_enable)
                        return _SUCCESS;
                action = frame_body[1];
                DBG_88E("%s, action=%d\n", __func__, action);
                switch (action) {
                case RTW_WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
                        memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
                        process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), addr);

                        if (pmlmeinfo->bAcceptAddbaReq)
                                issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0);
                        else
                                issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);/* reject ADDBA Req */
                        break;
                case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
                        status = RTW_GET_LE16(&frame_body[3]);
                        tid = ((frame_body[5] >> 2) & 0x7);
                        if (status == 0) {      /* successful */
                                DBG_88E("agg_enable for TID=%d\n", tid);
                                psta->htpriv.agg_enable_bitmap |= 1 << tid;
                                psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
                        } else {
                                psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
                        }
                        break;
                case RTW_WLAN_ACTION_DELBA: /* DELBA */
                        if ((frame_body[3] & BIT(3)) == 0) {
                                psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
                                psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
                                reason_code = RTW_GET_LE16(&frame_body[4]);
                        } else if ((frame_body[3] & BIT(3)) == BIT(3)) {
                                tid = (frame_body[3] >> 4) & 0x0F;
                                preorder_ctrl =  &psta->recvreorder_ctrl[tid];
                                preorder_ctrl->enable = false;
                                preorder_ctrl->indicate_seq = 0xffff;
                        }
                        DBG_88E("%s(): DELBA: %x(%x)\n", __func__, pmlmeinfo->agg_enable_bitmap, reason_code);
                        /* todo: how to notify the host while receiving DELETE BA */
                        break;
                default:
                        break;
                }
        }
        return _SUCCESS;
}

#ifdef CONFIG_88EU_P2P

static int get_reg_classes_full_count(struct p2p_channels *channel_list)
{
        int cnt = 0;
        int i;

        for (i = 0; i < channel_list->reg_classes; i++) {
                cnt += channel_list->reg_class[i].channels;
        }

        return cnt;
}

void issue_p2p_GO_request(struct adapter *padapter, u8 *raddr)
{
        unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
        u8 action = P2P_PUB_ACTION_ACTION;
        __be32 p2poui = cpu_to_be32(P2POUI);
        u8 oui_subtype = P2P_GO_NEGO_REQ;
        u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
        u8 wpsielen = 0, p2pielen = 0;
        u16 len_channellist_attr = 0;
        struct xmit_frame *pmgntframe;
        struct pkt_attrib *pattrib;
        unsigned char *pframe;
        struct rtw_ieee80211_hdr        *pwlanhdr;
        unsigned short *fctrl;
        struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
        struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
        struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (pmgntframe == NULL)
                return;

        DBG_88E("[%s] In\n", __func__);
        /* update attribute */
        pattrib = &pmgntframe->attrib;
        update_mgntframe_attrib(padapter, pattrib);

        _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);

        pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
        pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;

        fctrl = &(pwlanhdr->frame_ctl);
        *(fctrl) = 0;

        memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
        memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
        memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);

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

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

        pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
        pwdinfo->negotiation_dialog_token = 1;  /*      Initialize the dialog value */
        pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &(pattrib->pktlen));



        /*      WPS Section */
        wpsielen = 0;
        /*      WPS OUI */
        *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
        wpsielen += 4;

        /*      WPS version */
        /*      Type: */
        *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
        wpsielen += 2;

        /*      Length: */
        *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
        wpsielen += 2;

        /*      Value: */
        wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */

        /*      Device Password ID */
        /*      Type: */
        *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
        wpsielen += 2;

        /*      Length: */
        *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
        wpsielen += 2;

        /*      Value: */

        if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN)
                *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
        else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN)
                *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
        else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC)
                *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);

        wpsielen += 2;

        pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);


        /*      P2P IE Section. */

        /*      P2P OUI */
        p2pielen = 0;
        p2pie[p2pielen++] = 0x50;
        p2pie[p2pielen++] = 0x6F;
        p2pie[p2pielen++] = 0x9A;
        p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */

        /*      Commented by Albert 20110306 */
        /*      According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes */
        /*      1. P2P Capability */
        /*      2. Group Owner Intent */
        /*      3. Configuration Timeout */
        /*      4. Listen Channel */
        /*      5. Extended Listen Timing */
        /*      6. Intended P2P Interface Address */
        /*      7. Channel List */
        /*      8. P2P Device Info */
        /*      9. Operating Channel */


        /*      P2P Capability */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
        p2pielen += 2;

        /*      Value: */
        /*      Device Capability Bitmap, 1 byte */
        p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;

        /*      Group Capability Bitmap, 1 byte */
        if (pwdinfo->persistent_supported)
                p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
        else
                p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;

        /*      Group Owner Intent */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
        p2pielen += 2;

        /*      Value: */
        /*      Todo the tie breaker bit. */
        p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));

        /*      Configuration Timeout */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
        p2pielen += 2;

        /*      Value: */
        p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P GO */
        p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P Client */

        /*      Listen Channel */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
        p2pielen += 2;

        /*      Value: */
        /*      Country String */
        p2pie[p2pielen++] = 'X';
        p2pie[p2pielen++] = 'X';

        /*      The third byte should be set to 0x04. */
        /*      Described in the "Operating Channel Attribute" section. */
        p2pie[p2pielen++] = 0x04;

        /*      Operating Class */
        p2pie[p2pielen++] = 0x51;       /*      Copy from SD7 */

        /*      Channel Number */
        p2pie[p2pielen++] = pwdinfo->listen_channel;    /*      listening channel number */


        /*      Extended Listen Timing ATTR */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0004);
        p2pielen += 2;

        /*      Value: */
        /*      Availability Period */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
        p2pielen += 2;

        /*      Availability Interval */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0xFFFF);
        p2pielen += 2;


        /*      Intended P2P Interface Address */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
        p2pielen += 2;

        /*      Value: */
        memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
        p2pielen += ETH_ALEN;


        /*      Channel List */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_CH_LIST;

        /*  Length: */
        /*  Country String(3) */
        /*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
        /*  + number of channels in all classes */
        len_channellist_attr = 3
           + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
           + get_reg_classes_full_count(&pmlmeext->channel_list);

        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
        p2pielen += 2;

        /*      Value: */
        /*      Country String */
        p2pie[p2pielen++] = 'X';
        p2pie[p2pielen++] = 'X';

        /*      The third byte should be set to 0x04. */
        /*      Described in the "Operating Channel Attribute" section. */
        p2pie[p2pielen++] = 0x04;

        /*      Channel Entry List */

        {
                int i, j;
                for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
                        /*      Operating Class */
                        p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;

                        /*      Number of Channels */
                        p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;

                        /*      Channel List */
                        for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
                                p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
                        }
                }
        }

        /*      Device Info */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;

        /*      Length: */
        /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
        /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
        p2pielen += 2;

        /*      Value: */
        /*      P2P Device Address */
        memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
        p2pielen += ETH_ALEN;

        /*      Config Method */
        /*      This field should be big endian. Noted by P2P specification. */

        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);

        p2pielen += 2;

        /*      Primary Device Type */
        /*      Category ID */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
        p2pielen += 2;

        /*      OUI */
        *(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
        p2pielen += 4;

        /*      Sub Category ID */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
        p2pielen += 2;

        /*      Number of Secondary Device Types */
        p2pie[p2pielen++] = 0x00;       /*      No Secondary Device Type List */

        /*      Device Name */
        /*      Type: */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
        p2pielen += 2;

        /*      Length: */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
        p2pielen += 2;

        /*      Value: */
        memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len);
        p2pielen += pwdinfo->device_name_len;


        /*      Operating Channel */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
        p2pielen += 2;

        /*      Value: */
        /*      Country String */
        p2pie[p2pielen++] = 'X';
        p2pie[p2pielen++] = 'X';

        /*      The third byte should be set to 0x04. */
        /*      Described in the "Operating Channel Attribute" section. */
        p2pie[p2pielen++] = 0x04;

        /*      Operating Class */
        p2pie[p2pielen++] = 0x51;

        /*      Channel Number */
        p2pie[p2pielen++] = pwdinfo->operating_channel; /*      operating channel number */

        pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);

        pattrib->last_txcmdsz = pattrib->pktlen;

        dump_mgntframe(padapter, pmgntframe);

        return;
}

static void issue_p2p_GO_response(struct adapter *padapter, u8 *raddr, u8 *frame_body, uint len, u8 result)
{
        unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
        u8 action = P2P_PUB_ACTION_ACTION;
        __be32                  p2poui = cpu_to_be32(P2POUI);
        u8 oui_subtype = P2P_GO_NEGO_RESP;
        u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
        u8 p2pielen = 0;
        uint                    wpsielen = 0;
        u16 wps_devicepassword_id = 0x0000;
        __be16                  be_tmp;
        uint                    wps_devicepassword_id_len = 0;
        u16 len_channellist_attr = 0;

        struct xmit_frame                       *pmgntframe;
        struct pkt_attrib                       *pattrib;
        unsigned char                                   *pframe;
        struct rtw_ieee80211_hdr        *pwlanhdr;
        unsigned short                          *fctrl;
        struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
        struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
        struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (pmgntframe == NULL)
                return;

        DBG_88E("[%s] In, result=%d\n", __func__,  result);
        /* update attribute */
        pattrib = &pmgntframe->attrib;
        update_mgntframe_attrib(padapter, pattrib);

        _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);

        pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
        pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;

        fctrl = &(pwlanhdr->frame_ctl);
        *(fctrl) = 0;

        memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
        memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
        memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);

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

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

        pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
        pwdinfo->negotiation_dialog_token = frame_body[7];      /*      The Dialog Token of provisioning discovery request frame. */
        pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));

        /*      Commented by Albert 20110328 */
        /*      Try to get the device password ID from the WPS IE of group negotiation request frame */
        /*      WiFi Direct test plan 5.1.15 */
        rtw_get_wps_ie(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
        rtw_get_wps_attr_content(wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, (u8 *)&be_tmp, &wps_devicepassword_id_len);
        wps_devicepassword_id = be16_to_cpu(be_tmp);

        _rtw_memset(wpsie, 0x00, 255);
        wpsielen = 0;

        /*      WPS Section */
        wpsielen = 0;
        /*      WPS OUI */
        *(__be32 *)(wpsie) = cpu_to_be32(WPSOUI);
        wpsielen += 4;

        /*      WPS version */
        /*      Type: */
        *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
        wpsielen += 2;

        /*      Length: */
        *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
        wpsielen += 2;

        /*      Value: */
        wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */

        /*      Device Password ID */
        /*      Type: */
        *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
        wpsielen += 2;

        /*      Length: */
        *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
        wpsielen += 2;

        /*      Value: */
        if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
                *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
        else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
                *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
        else
                *(__be16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
        wpsielen += 2;

        /*      Commented by Kurt 20120113 */
        /*      If some device wants to do p2p handshake without sending prov_disc_req */
        /*      We have to get peer_req_cm from here. */
        if (_rtw_memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) {
                if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
                        memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
                else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
                        memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
                else
                        memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
        }

        pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *)wpsie, &pattrib->pktlen);


        /*      P2P IE Section. */

        /*      P2P OUI */
        p2pielen = 0;
        p2pie[p2pielen++] = 0x50;
        p2pie[p2pielen++] = 0x6F;
        p2pie[p2pielen++] = 0x9A;
        p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */

        /*      Commented by Albert 20100908 */
        /*      According to the P2P Specification, the group negoitation response frame should contain 9 P2P attributes */
        /*      1. Status */
        /*      2. P2P Capability */
        /*      3. Group Owner Intent */
        /*      4. Configuration Timeout */
        /*      5. Operating Channel */
        /*      6. Intended P2P Interface Address */
        /*      7. Channel List */
        /*      8. Device Info */
        /*      9. Group ID     (Only GO) */


        /*      ToDo: */

        /*      P2P Status */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_STATUS;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
        p2pielen += 2;

        /*      Value: */
        p2pie[p2pielen++] = result;

        /*      P2P Capability */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
        p2pielen += 2;

        /*      Value: */
        /*      Device Capability Bitmap, 1 byte */

        if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
                /*      Commented by Albert 2011/03/08 */
                /*      According to the P2P specification */
                /*      if the sending device will be client, the P2P Capability should be reserved of group negotation response frame */
                p2pie[p2pielen++] = 0;
        } else {
                /*      Be group owner or meet the error case */
                p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
        }

        /*      Group Capability Bitmap, 1 byte */
        if (pwdinfo->persistent_supported) {
                p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
        } else {
                p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
        }

        /*      Group Owner Intent */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
        p2pielen += 2;

        /*      Value: */
        if (pwdinfo->peer_intent & 0x01) {
                /*      Peer's tie breaker bit is 1, our tie breaker bit should be 0 */
                p2pie[p2pielen++] = (pwdinfo->intent << 1);
        } else {
                /*      Peer's tie breaker bit is 0, our tie breaker bit should be 1 */
                p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
        }


        /*      Configuration Timeout */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
        p2pielen += 2;

        /*      Value: */
        p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P GO */
        p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P Client */

        /*      Operating Channel */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
        p2pielen += 2;

        /*      Value: */
        /*      Country String */
        p2pie[p2pielen++] = 'X';
        p2pie[p2pielen++] = 'X';

        /*      The third byte should be set to 0x04. */
        /*      Described in the "Operating Channel Attribute" section. */
        p2pie[p2pielen++] = 0x04;

        /*      Operating Class */
        p2pie[p2pielen++] = 0x51;

        /*      Channel Number */
        p2pie[p2pielen++] = pwdinfo->operating_channel; /*      operating channel number */

        /*      Intended P2P Interface Address */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
        p2pielen += 2;

        /*      Value: */
        memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
        p2pielen += ETH_ALEN;

        /*      Channel List */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_CH_LIST;

        /*  Country String(3) */
        /*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
        /*  + number of channels in all classes */
        len_channellist_attr = 3
           + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
           + get_reg_classes_full_count(&pmlmeext->channel_list);

        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);

        p2pielen += 2;

        /*      Value: */
        /*      Country String */
        p2pie[p2pielen++] = 'X';
        p2pie[p2pielen++] = 'X';

        /*      The third byte should be set to 0x04. */
        /*      Described in the "Operating Channel Attribute" section. */
        p2pie[p2pielen++] = 0x04;

        /*      Channel Entry List */

        {
                int i, j;
                for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
                        /*      Operating Class */
                        p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;

                        /*      Number of Channels */
                        p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;

                        /*      Channel List */
                        for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
                                p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
                        }
                }
        }

        /*      Device Info */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;

        /*      Length: */
        /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
        /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
        p2pielen += 2;

        /*      Value: */
        /*      P2P Device Address */
        memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
        p2pielen += ETH_ALEN;

        /*      Config Method */
        /*      This field should be big endian. Noted by P2P specification. */

        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);

        p2pielen += 2;

        /*      Primary Device Type */
        /*      Category ID */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
        p2pielen += 2;

        /*      OUI */
        *(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
        p2pielen += 4;

        /*      Sub Category ID */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
        p2pielen += 2;

        /*      Number of Secondary Device Types */
        p2pie[p2pielen++] = 0x00;       /*      No Secondary Device Type List */

        /*      Device Name */
        /*      Type: */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
        p2pielen += 2;

        /*      Length: */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
        p2pielen += 2;

        /*      Value: */
        memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len);
        p2pielen += pwdinfo->device_name_len;

        if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
                /*      Group ID Attribute */
                /*      Type: */
                p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;

                /*      Length: */
                *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
                p2pielen += 2;

                /*      Value: */
                /*      p2P Device Address */
                memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN);
                p2pielen += ETH_ALEN;

                /*      SSID */
                memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
                p2pielen += pwdinfo->nego_ssidlen;
        }

        pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);

        pattrib->last_txcmdsz = pattrib->pktlen;

        dump_mgntframe(padapter, pmgntframe);
        return;
}

static void issue_p2p_GO_confirm(struct adapter *padapter, u8 *raddr, u8 result)
{
        unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
        u8 action = P2P_PUB_ACTION_ACTION;
        __be32                  p2poui = cpu_to_be32(P2POUI);
        u8 oui_subtype = P2P_GO_NEGO_CONF;
        u8 p2pie[255] = { 0x00 };
        u8 p2pielen = 0;

        struct xmit_frame                       *pmgntframe;
        struct pkt_attrib                       *pattrib;
        unsigned char                                   *pframe;
        struct rtw_ieee80211_hdr        *pwlanhdr;
        unsigned short                          *fctrl;
        struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
        struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
        struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (pmgntframe == NULL)
                return;

        DBG_88E("[%s] In\n", __func__);
        /* update attribute */
        pattrib = &pmgntframe->attrib;
        update_mgntframe_attrib(padapter, pattrib);

        _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);

        pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
        pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;

        fctrl = &(pwlanhdr->frame_ctl);
        *(fctrl) = 0;

        memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
        memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
        memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);

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

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

        pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));



        /*      P2P IE Section. */

        /*      P2P OUI */
        p2pielen = 0;
        p2pie[p2pielen++] = 0x50;
        p2pie[p2pielen++] = 0x6F;
        p2pie[p2pielen++] = 0x9A;
        p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */

        /*      Commented by Albert 20110306 */
        /*      According to the P2P Specification, the group negoitation request frame should contain 5 P2P attributes */
        /*      1. Status */
        /*      2. P2P Capability */
        /*      3. Operating Channel */
        /*      4. Channel List */
        /*      5. Group ID     (if this WiFi is GO) */

        /*      P2P Status */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_STATUS;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
        p2pielen += 2;

        /*      Value: */
        p2pie[p2pielen++] = result;

        /*      P2P Capability */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
        p2pielen += 2;

        /*      Value: */
        /*      Device Capability Bitmap, 1 byte */
        p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;

        /*      Group Capability Bitmap, 1 byte */
        if (pwdinfo->persistent_supported)
                p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
        else
                p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;

        /*      Operating Channel */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
        p2pielen += 2;

        /*      Value: */
        /*      Country String */
        p2pie[p2pielen++] = 'X';
        p2pie[p2pielen++] = 'X';

        /*      The third byte should be set to 0x04. */
        /*      Described in the "Operating Channel Attribute" section. */
        p2pie[p2pielen++] = 0x04;


        if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
                /*      Operating Class */
                p2pie[p2pielen++] = 0x51;
                p2pie[p2pielen++] = pwdinfo->peer_operating_ch;
        } else {
                /*      Operating Class */
                p2pie[p2pielen++] = 0x51;

                /*      Channel Number */
                p2pie[p2pielen++] = pwdinfo->operating_channel;         /*      Use the listen channel as the operating channel */
        }


        /*      Channel List */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_CH_LIST;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(pwdinfo->channel_list_attr_len);
        p2pielen += 2;

        /*      Value: */
        memcpy(p2pie + p2pielen, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len);
        p2pielen += pwdinfo->channel_list_attr_len;

        if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
                /*      Group ID Attribute */
                /*      Type: */
                p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;

                /*      Length: */
                *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
                p2pielen += 2;

                /*      Value: */
                /*      p2P Device Address */
                memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN);
                p2pielen += ETH_ALEN;

                /*      SSID */
                memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
                p2pielen += pwdinfo->nego_ssidlen;
        }
        pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);
        pattrib->last_txcmdsz = pattrib->pktlen;
        dump_mgntframe(padapter, pmgntframe);
        return;
}

void issue_p2p_invitation_request(struct adapter *padapter, u8 *raddr)
{
        unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
        u8 action = P2P_PUB_ACTION_ACTION;
        __be32                  p2poui = cpu_to_be32(P2POUI);
        u8 oui_subtype = P2P_INVIT_REQ;
        u8 p2pie[255] = { 0x00 };
        u8 p2pielen = 0;
        u8 dialogToken = 3;
        u16 len_channellist_attr = 0;
        struct xmit_frame                       *pmgntframe;
        struct pkt_attrib                       *pattrib;
        unsigned char                                   *pframe;
        struct rtw_ieee80211_hdr        *pwlanhdr;
        unsigned short                          *fctrl;
        struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
        struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
        struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (pmgntframe == NULL)
                return;

        /* update attribute */
        pattrib = &pmgntframe->attrib;
        update_mgntframe_attrib(padapter, pattrib);

        _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);

        pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
        pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;

        fctrl = &(pwlanhdr->frame_ctl);
        *(fctrl) = 0;

        memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
        memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
        memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);

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

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

        pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&(p2poui), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
        pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));

        /*      P2P IE Section. */

        /*      P2P OUI */
        p2pielen = 0;
        p2pie[p2pielen++] = 0x50;
        p2pie[p2pielen++] = 0x6F;
        p2pie[p2pielen++] = 0x9A;
        p2pie[p2pielen++] = 0x09;       /*      WFA P2P v1.0 */

        /*      Commented by Albert 20101011 */
        /*      According to the P2P Specification, the P2P Invitation request frame should contain 7 P2P attributes */
        /*      1. Configuration Timeout */
        /*      2. Invitation Flags */
        /*      3. Operating Channel    (Only GO) */
        /*      4. P2P Group BSSID      (Should be included if I am the GO) */
        /*      5. Channel List */
        /*      6. P2P Group ID */
        /*      7. P2P Device Info */

        /*      Configuration Timeout */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0002);
        p2pielen += 2;

        /*      Value: */
        p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P GO */
        p2pie[p2pielen++] = 200;        /*      2 seconds needed to be the P2P Client */

        /*      Invitation Flags */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_INVITATION_FLAGS;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0001);
        p2pielen += 2;

        /*      Value: */
        p2pie[p2pielen++] = P2P_INVITATION_FLAGS_PERSISTENT;


        /*      Operating Channel */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(0x0005);
        p2pielen += 2;

        /*      Value: */
        /*      Country String */
        p2pie[p2pielen++] = 'X';
        p2pie[p2pielen++] = 'X';

        /*      The third byte should be set to 0x04. */
        /*      Described in the "Operating Channel Attribute" section. */
        p2pie[p2pielen++] = 0x04;

        /*      Operating Class */
        p2pie[p2pielen++] = 0x51;

        /*      Channel Number */
        p2pie[p2pielen++] = pwdinfo->invitereq_info.operating_ch;       /*      operating channel number */

        if (_rtw_memcmp(myid(&padapter->eeprompriv), pwdinfo->invitereq_info.go_bssid, ETH_ALEN)) {
                /*      P2P Group BSSID */
                /*      Type: */
                p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;

                /*      Length: */
                *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
                p2pielen += 2;

                /*      Value: */
                /*      P2P Device Address for GO */
                memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
                p2pielen += ETH_ALEN;
        }

        /*      Channel List */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_CH_LIST;


        /*      Length: */
        /*  Country String(3) */
        /*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
        /*  + number of channels in all classes */
        len_channellist_attr = 3
           + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
           + get_reg_classes_full_count(&pmlmeext->channel_list);

        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);

        p2pielen += 2;

        /*      Value: */
        /*      Country String */
        p2pie[p2pielen++] = 'X';
        p2pie[p2pielen++] = 'X';

        /*      The third byte should be set to 0x04. */
        /*      Described in the "Operating Channel Attribute" section. */
        p2pie[p2pielen++] = 0x04;

        /*      Channel Entry List */
        {
                int i, j;
                for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
                        /*      Operating Class */
                        p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;

                        /*      Number of Channels */
                        p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;

                        /*      Channel List */
                        for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
                                p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
                        }
                }
        }


        /*      P2P Group ID */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;

        /*      Length: */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(6 + pwdinfo->invitereq_info.ssidlen);
        p2pielen += 2;

        /*      Value: */
        /*      P2P Device Address for GO */
        memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
        p2pielen += ETH_ALEN;

        /*      SSID */
        memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid, pwdinfo->invitereq_info.ssidlen);
        p2pielen += pwdinfo->invitereq_info.ssidlen;


        /*      Device Info */
        /*      Type: */
        p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;

        /*      Length: */
        /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
        /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
        *(__le16 *)(p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
        p2pielen += 2;

        /*      Value: */
        /*      P2P Device Address */
        memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
        p2pielen += ETH_ALEN;

        /*      Config Method */
        /*      This field should be big endian. Noted by P2P specification. */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
        p2pielen += 2;

        /*      Primary Device Type */
        /*      Category ID */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
        p2pielen += 2;

        /*      OUI */
        *(__be32 *)(p2pie + p2pielen) = cpu_to_be32(WPSOUI);
        p2pielen  += 4;

        /*      Sub Category ID */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
        p2pielen += 2;

        /*      Number of Secondary Device Types */
        p2pie[p2pielen++] = 0x00;       /*      No Secondary Device Type List */

        /*      Device Name */
        /*      Type: */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
        p2pielen += 2;

        /*      Length: */
        *(__be16 *)(p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
        p2pielen += 2;

        /*      Value: */
        memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
        p2pielen += pwdinfo->device_name_len;

        pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *)p2pie, &pattrib->pktlen);

        pattrib->last_txcmdsz = pattrib->pktlen;

        dump_mgntframe(padapter, pmgntframe);

        return;
}

void issue_p2p_invitation_response(struct adapter *padapter, u8 *raddr, u8 dialogToken, u8 status_code)
{
        unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
        u8 action = P2P_PUB_ACTION_ACTION;
        __be32                  p2poui = cpu_to_be32(P2POUI);
        u8 oui_subtype = P2P_INVIT_RESP;
        u8 p2pie[255] = { 0x00 };
        u8 p2pielen = 0;
        u16 len_channellist_attr = 0;
        struct xmit_frame                       *pmgntframe;
        struct pkt_attrib                       *pattrib;
        unsigned char                                   *pframe;
        struct rtw_ieee80211_hdr        *pwlanhdr;
        unsigned short                          *fctrl;
        struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
        struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
        struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);

        pmgntframe = alloc_mgtxmitframe(pxmitpriv);
        if (pmgntframe == NULL)
                return;

        /* update attribute */
        pattrib = &pmgntframe->attrib;
        update_mgntframe_attrib(padapter, pattrib);

        _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);

        pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
        pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;