core/rtw_mlme_ext.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  *
  19  ******************************************************************************/
  20 #define _RTW_MLME_EXT_C_
  21
  22 #include <drv_conf.h>
  23 #include <osdep_service.h>
  24 #include <drv_types.h>
  25 #include <wifi.h>
  26 #include <rtw_mlme_ext.h>
  27 #include <wlan_bssdef.h>
  28 #include <mlme_osdep.h>
  29 #include <recv_osdep.h>
  30 #include <linux/ieee80211.h>
  31
  32 #ifdef CONFIG_BT_COEXIST
  33 #include <rtl8723a_hal.h>
  34 #endif
  35
  36 struct mlme_handler mlme_sta_tbl[]={
  37         {WIFI_ASSOCREQ,         "OnAssocReq",   &OnAssocReq},
  38         {WIFI_ASSOCRSP,         "OnAssocRsp",   &OnAssocRsp},
  39         {WIFI_REASSOCREQ,       "OnReAssocReq", &OnAssocReq},
  40         {WIFI_REASSOCRSP,       "OnReAssocRsp", &OnAssocRsp},
  41         {WIFI_PROBEREQ,         "OnProbeReq",   &OnProbeReq},
  42         {WIFI_PROBERSP,         "OnProbeRsp",           &OnProbeRsp},
  43
  44         /*----------------------------------------------------------
  45                                         below 2 are reserved
  46         -----------------------------------------------------------*/
  47         {0,                                     "DoReserved",           &DoReserved},
  48         {0,                                     "DoReserved",           &DoReserved},
  49         {WIFI_BEACON,           "OnBeacon",             &OnBeacon},
  50         {WIFI_ATIM,                     "OnATIM",               &OnAtim},
  51         {WIFI_DISASSOC,         "OnDisassoc",           &OnDisassoc},
  52         {WIFI_AUTH,                     "OnAuth",               &OnAuthClient},
  53         {WIFI_DEAUTH,           "OnDeAuth",             &OnDeAuth},
  54         {WIFI_ACTION,           "OnAction",             &OnAction},
  55 };
  56
  57 #ifdef _CONFIG_NATIVEAP_MLME_
  58 struct mlme_handler mlme_ap_tbl[]={
  59         {WIFI_ASSOCREQ,         "OnAssocReq",   &OnAssocReq},
  60         {WIFI_ASSOCRSP,         "OnAssocRsp",   &OnAssocRsp},
  61         {WIFI_REASSOCREQ,       "OnReAssocReq", &OnAssocReq},
  62         {WIFI_REASSOCRSP,       "OnReAssocRsp", &OnAssocRsp},
  63         {WIFI_PROBEREQ,         "OnProbeReq",   &OnProbeReq},
  64         {WIFI_PROBERSP,         "OnProbeRsp",           &OnProbeRsp},
  65
  66         /*----------------------------------------------------------
  67                                         below 2 are reserved
  68         -----------------------------------------------------------*/
  69         {0,                                     "DoReserved",           &DoReserved},
  70         {0,                                     "DoReserved",           &DoReserved},
  71         {WIFI_BEACON,           "OnBeacon",             &OnBeacon},
  72         {WIFI_ATIM,                     "OnATIM",               &OnAtim},
  73         {WIFI_DISASSOC,         "OnDisassoc",           &OnDisassoc},
  74         {WIFI_AUTH,                     "OnAuth",               &OnAuth},
  75         {WIFI_DEAUTH,           "OnDeAuth",             &OnDeAuth},
  76         {WIFI_ACTION,           "OnAction",             &OnAction},
  77 };
  78 #endif
  79
  80 struct action_handler OnAction_tbl[]={
  81         {RTW_WLAN_CATEGORY_SPECTRUM_MGMT,        "ACTION_SPECTRUM_MGMT", on_action_spct},
  82         {RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos},
  83         {RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls},
  84         {RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
  85         {RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
  86         {RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
  87         {RTW_WLAN_CATEGORY_FT, "ACTION_FT",     &DoReserved},
  88         {RTW_WLAN_CATEGORY_HT,  "ACTION_HT",    &OnAction_ht},
  89         {RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved},
  90         {RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm},
  91         {RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p},
  92 };
  93
  94 u8      null_addr[ETH_ALEN]= {0,0,0,0,0,0};
  95
  96 /**************************************************
  97 OUI definitions for the vendor specific IE
  98 ***************************************************/
  99 unsigned char   RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
 100 unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
 101 unsigned char   WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
 102 unsigned char   P2P_OUI[] = {0x50,0x6F,0x9A,0x09};
 103 unsigned char   WFD_OUI[] = {0x50,0x6F,0x9A,0x0A};
 104
 105 unsigned char   WMM_INFO_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
 106 unsigned char   WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
 107
 108 unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
 109 unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
 110
 111 extern unsigned char REALTEK_96B_IE[];
 112
 113 /********************************************************
 114 MCS rate definitions
 115 *********************************************************/
 116 #ifdef CONFIG_DISABLE_MCS13TO15
 117 unsigned char   MCS_rate_2R_MCS13TO15_OFF[16] = {0xff, 0x1f, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 118 unsigned char   MCS_rate_2R[16] = {0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 119 #else /* CONFIG_DISABLE_MCS13TO15 */
 120 unsigned char   MCS_rate_2R[16] = {0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 121 #endif /* CONFIG_DISABLE_MCS13TO15 */
 122 unsigned char   MCS_rate_1R[16] = {0xff, 0x00, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 123
 124 /********************************************************
 125 ChannelPlan definitions
 126 *********************************************************/
 127
 128 static RT_CHANNEL_PLAN_2G       RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
 129         {{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 */
 130         {{1,2,3,4,5,6,7,8,9,10,11,12,13},13},           /*  0x01, RT_CHANNEL_DOMAIN_2G_ETSI1 */
 131         {{1,2,3,4,5,6,7,8,9,10,11},11},                 /*  0x02, RT_CHANNEL_DOMAIN_2G_FCC1 */
 132         {{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},        /*  0x03, RT_CHANNEL_DOMAIN_2G_MIKK1 */
 133         {{10,11,12,13},4},                                      /*  0x04, RT_CHANNEL_DOMAIN_2G_ETSI2 */
 134         {{},0},                                                                 /*  0x05, RT_CHANNEL_DOMAIN_2G_NULL */
 135 };
 136
 137 static RT_CHANNEL_PLAN_5G       RTW_ChannelPlan5G[RT_CHANNEL_DOMAIN_5G_MAX] = {
 138         {{},0},                                                                                                                                                                 /*  0x00, RT_CHANNEL_DOMAIN_5G_NULL */
 139         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},19},                                             /*  0x01, RT_CHANNEL_DOMAIN_5G_ETSI1 */
 140         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140,149,153,157,161,165},24}, /*  0x02, RT_CHANNEL_DOMAIN_5G_ETSI2 */
 141         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,149,153,157,161,165},22},                 /*  0x03, RT_CHANNEL_DOMAIN_5G_ETSI3 */
 142         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140,149,153,157,161,165},24}, /*  0x04, RT_CHANNEL_DOMAIN_5G_FCC1 */
 143         {{36,40,44,48,149,153,157,161,165},9},                                                                                                          /*  0x05, RT_CHANNEL_DOMAIN_5G_FCC2 */
 144         {{36,40,44,48,52,56,60,64,149,153,157,161,165},13},                                                                                     /*  0x06, RT_CHANNEL_DOMAIN_5G_FCC3 */
 145         {{36,40,44,48,52,56,60,64,149,153,157,161},12},                                                                                         /*  0x07, RT_CHANNEL_DOMAIN_5G_FCC4 */
 146         {{149,153,157,161,165},5},                                                                                                                                      /*  0x08, RT_CHANNEL_DOMAIN_5G_FCC5 */
 147         {{36,40,44,48,52,56,60,64},8},                                                                                                                          /*  0x09, RT_CHANNEL_DOMAIN_5G_FCC6 */
 148         {{36,40,44,48,52,56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},20},                                 /*  0x0A, RT_CHANNEL_DOMAIN_5G_FCC7_IC1 */
 149         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,149,153,157,161,165},20},                                 /*  0x0B, RT_CHANNEL_DOMAIN_5G_KCC1 */
 150         {{36,40,44,48,52,56,60,64,100,104,108,112,116,120,124,128,132,136,140},19},                                             /*  0x0C, RT_CHANNEL_DOMAIN_5G_MKK1 */
 151         {{36,40,44,48,52,56,60,64},8},                                                                                                                          /*  0x0D, RT_CHANNEL_DOMAIN_5G_MKK2 */
 152         {{100,104,108,112,116,120,124,128,132,136,140},11},                                                                                     /*  0x0E, RT_CHANNEL_DOMAIN_5G_MKK3 */
 153         {{56,60,64,100,104,108,112,116,136,140,149,153,157,161,165},15},                                                                /*  0x0F, RT_CHANNEL_DOMAIN_5G_NCC1 */
 154         {{56,60,64,149,153,157,161,165},8},                                                                                                                     /*  0x10, RT_CHANNEL_DOMAIN_5G_NCC2 */
 155
 156         /*  Driver self defined for old channel plan Compatible ,Remember to modify if have new channel plan definition ===== */
 157         {{36,40,44,48,52,56,60,64,100,104,108,112,116,132,136,140,149,153,157,161,165},21},                             /*  0x11, RT_CHANNEL_DOMAIN_5G_FCC */
 158         {{36,40,44,48},4},                                                                                                                                                      /*  0x12, RT_CHANNEL_DOMAIN_5G_JAPAN_NO_DFS */
 159         {{36,40,44,48,149,153,157,161},8},                                                                                                                              /*  0x13, RT_CHANNEL_DOMAIN_5G_FCC4_NO_DFS */
 160 };
 161
 162 static RT_CHANNEL_PLAN_MAP      RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
 163         /*  0x00 ~ 0x1F , Old Define ===== */
 164         {0x02,0x11},    /* 0x00, RT_CHANNEL_DOMAIN_FCC */
 165         {0x02,0x0A},    /* 0x01, RT_CHANNEL_DOMAIN_IC */
 166         {0x01,0x01},    /* 0x02, RT_CHANNEL_DOMAIN_ETSI */
 167         {0x01,0x00},    /* 0x03, RT_CHANNEL_DOMAIN_SPAIN */
 168         {0x01,0x00},    /* 0x04, RT_CHANNEL_DOMAIN_FRANCE */
 169         {0x03,0x00},    /* 0x05, RT_CHANNEL_DOMAIN_MKK */
 170         {0x03,0x00},    /* 0x06, RT_CHANNEL_DOMAIN_MKK1 */
 171         {0x01,0x09},    /* 0x07, RT_CHANNEL_DOMAIN_ISRAEL */
 172         {0x03,0x09},    /* 0x08, RT_CHANNEL_DOMAIN_TELEC */
 173         {0x03,0x00},    /* 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN */
 174         {0x00,0x00},    /* 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13 */
 175         {0x02,0x0F},    /* 0x0B, RT_CHANNEL_DOMAIN_TAIWAN */
 176         {0x01,0x08},    /* 0x0C, RT_CHANNEL_DOMAIN_CHINA */
 177         {0x02,0x06},    /* 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO */
 178         {0x02,0x0B},    /* 0x0E, RT_CHANNEL_DOMAIN_KOREA */
 179         {0x02,0x09},    /* 0x0F, RT_CHANNEL_DOMAIN_TURKEY */
 180         {0x01,0x01},    /* 0x10, RT_CHANNEL_DOMAIN_JAPAN */
 181         {0x02,0x05},    /* 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS */
 182         {0x01,0x12},    /* 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
 183         {0x00,0x04},    /* 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G */
 184         {0x02,0x10},    /* 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS */
 185         {0x00,0x12},    /* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */
 186         {0x00,0x13},    /* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */
 187         {0x03,0x12},    /* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
 188         {0x05,0x08},    /* 0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS */
 189         {0x02,0x08},    /* 0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS */
 190         {0x00,0x00},    /* 0x1A, */
 191         {0x00,0x00},    /* 0x1B, */
 192         {0x00,0x00},    /* 0x1C, */
 193         {0x00,0x00},    /* 0x1D, */
 194         {0x00,0x00},    /* 0x1E, */
 195         {0x05,0x04},    /* 0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G */
 196         /*  0x20 ~ 0x7F ,New Define ===== */
 197         {0x00,0x00},    /* 0x20, RT_CHANNEL_DOMAIN_WORLD_NULL */
 198         {0x01,0x00},    /* 0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL */
 199         {0x02,0x00},    /* 0x22, RT_CHANNEL_DOMAIN_FCC1_NULL */
 200         {0x03,0x00},    /* 0x23, RT_CHANNEL_DOMAIN_MKK1_NULL */
 201         {0x04,0x00},    /* 0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL */
 202         {0x02,0x04},    /* 0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1 */
 203         {0x00,0x01},    /* 0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1 */
 204         {0x03,0x0C},    /* 0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1 */
 205         {0x00,0x0B},    /* 0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1 */
 206         {0x00,0x05},    /* 0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2 */
 207         {0x00,0x00},    /* 0x2A, */
 208         {0x00,0x00},    /* 0x2B, */
 209         {0x00,0x00},    /* 0x2C, */
 210         {0x00,0x00},    /* 0x2D, */
 211         {0x00,0x00},    /* 0x2E, */
 212         {0x00,0x00},    /* 0x2F, */
 213         {0x00,0x06},    /* 0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3 */
 214         {0x00,0x07},    /* 0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4 */
 215         {0x00,0x08},    /* 0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5 */
 216         {0x00,0x09},    /* 0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6 */
 217         {0x02,0x0A},    /* 0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7 */
 218         {0x00,0x02},    /* 0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2 */
 219         {0x00,0x03},    /* 0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3 */
 220         {0x03,0x0D},    /* 0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2 */
 221         {0x03,0x0E},    /* 0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3 */
 222         {0x02,0x0F},    /* 0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1 */
 223         {0x00,0x00},    /* 0x3A, */
 224         {0x00,0x00},    /* 0x3B, */
 225         {0x00,0x00},    /* 0x3C, */
 226         {0x00,0x00},    /* 0x3D, */
 227         {0x00,0x00},    /* 0x3E, */
 228         {0x00,0x00},    /* 0x3F, */
 229         {0x02,0x10},    /* 0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2 */
 230         {0x03,0x00},    /* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G */
 231 };
 232
 233 static RT_CHANNEL_PLAN_MAP      RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03,0x02}; /* use the conbination for max channel numbers */
 234
 235 /*
 236  * Search the @param channel_num in given @param channel_set
 237  * @ch_set: the given channel set
 238  * @ch: the given channel number
 239  *
 240  * return the index of channel_num in channel_set, -1 if not found
 241  */
 242 int rtw_ch_set_search_ch(RT_CHANNEL_INFO *ch_set, const u32 ch)
 243 {
 244         int i;
 245         for(i=0;ch_set[i].ChannelNum!=0;i++){
 246                 if(ch == ch_set[i].ChannelNum)
 247                         break;
 248         }
 249
 250         if(i >= ch_set[i].ChannelNum)
 251                 return -1;
 252         return i;
 253 }
 254
 255 /****************************************************************************
 256
 257 Following are the initialization functions for WiFi MLME
 258
 259 *****************************************************************************/
 260
 261 int init_hw_mlme_ext(_adapter *padapter)
 262 {
 263         struct  mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 264
 265         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 266         return _SUCCESS;
 267 }
 268
 269 static void init_mlme_ext_priv_value(_adapter* padapter)
 270 {
 271         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 272         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 273
 274 #ifdef CONFIG_TDLS
 275         u8 i;
 276 #endif
 277
 278         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};
 279         unsigned char   mixed_basicrate[NumRates] ={_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_, _12M_RATE_, _24M_RATE_, 0xff,};
 280
 281         ATOMIC_SET(&pmlmeext->event_seq, 0);
 282         pmlmeext->mgnt_seq = 0;/* reset to zero when disconnect at client mode */
 283
 284         pmlmeext->cur_channel = padapter->registrypriv.channel;
 285         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
 286         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 287
 288         pmlmeext->retry = 0;
 289
 290         pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
 291
 292         /* memcpy(pmlmeext->channel_set, DefaultChannelPlan[padapter->mlmepriv.ChannelPlan].Channel, DefaultChannelPlan[padapter->mlmepriv.ChannelPlan].Len); */
 293         /* memcpy(pmlmeext->channel_set, default_channel_set, MAX_CHANNEL_NUM); */
 294         memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
 295         memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
 296
 297         if(pmlmeext->cur_channel > 14)
 298                 pmlmeext->tx_rate = IEEE80211_OFDM_RATE_6MB;
 299         else
 300                 pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
 301
 302         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
 303         pmlmeext->sitesurvey_res.channel_idx = 0;
 304         pmlmeext->sitesurvey_res.bss_cnt = 0;
 305         pmlmeext->scan_abort = _FALSE;
 306
 307         pmlmeinfo->state = WIFI_FW_NULL_STATE;
 308         pmlmeinfo->reauth_count = 0;
 309         pmlmeinfo->reassoc_count = 0;
 310         pmlmeinfo->link_count = 0;
 311         pmlmeinfo->auth_seq = 0;
 312         pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
 313         pmlmeinfo->key_index = 0;
 314         pmlmeinfo->iv = 0;
 315
 316         pmlmeinfo->enc_algo = _NO_PRIVACY_;
 317         pmlmeinfo->authModeToggle = 0;
 318
 319         memset(pmlmeinfo->chg_txt, 0, 128);
 320
 321         pmlmeinfo->slotTime = SHORT_SLOT_TIME;
 322         pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
 323
 324         pmlmeinfo->dialogToken = 0;
 325
 326         pmlmeext->action_public_rxseq = 0xffff;
 327         pmlmeext->action_public_dialog_token = 0xff;
 328 }
 329
 330 static int has_channel(RT_CHANNEL_INFO *channel_set,
 331                                            u8 chanset_size,
 332                                            u8 chan) {
 333         int i;
 334
 335         for (i = 0; i < chanset_size; i++) {
 336                 if (channel_set[i].ChannelNum == chan) {
 337                         return 1;
 338                 }
 339         }
 340
 341         return 0;
 342 }
 343
 344 static void init_channel_list(_adapter *padapter, RT_CHANNEL_INFO *channel_set,
 345                                                           u8 chanset_size,
 346                                                           struct p2p_channels *channel_list) {
 347
 348         struct p2p_oper_class_map op_class[] = {
 349                 { IEEE80211G,  81,   1,  13,  1, BW20 },
 350                 { IEEE80211G,  82,  14,  14,  1, BW20 },
 351                 { IEEE80211A, 115,  36,  48,  4, BW20 },
 352                 { IEEE80211A, 116,  36,  44,  8, BW40PLUS },
 353                 { IEEE80211A, 117,  40,  48,  8, BW40MINUS },
 354                 { IEEE80211A, 124, 149, 161,  4, BW20 },
 355                 { IEEE80211A, 125, 149, 169,  4, BW20 },
 356                 { IEEE80211A, 126, 149, 157,  8, BW40PLUS },
 357                 { IEEE80211A, 127, 153, 161,  8, BW40MINUS },
 358                 { -1, 0, 0, 0, 0, BW20 }
 359         };
 360
 361         int cla, op;
 362
 363         cla = 0;
 364
 365         for (op = 0; op_class[op].op_class; op++) {
 366                 u8 ch;
 367                 struct p2p_oper_class_map *o = &op_class[op];
 368                 struct p2p_reg_class *reg = NULL;
 369
 370                 for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
 371                         if (!has_channel(channel_set, chanset_size, ch)) {
 372                                 continue;
 373                         }
 374
 375                         if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
 376                                 continue;
 377
 378                         if ((0 == (padapter->registrypriv.cbw40_enable & BIT(1))) &&
 379                                 ((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
 380                                 continue;
 381
 382                         if (reg == NULL) {
 383                                 reg = &channel_list->reg_class[cla];
 384                                 cla++;
 385                                 reg->reg_class = o->op_class;
 386                                 reg->channels = 0;
 387                         }
 388                         reg->channel[reg->channels] = ch;
 389                         reg->channels++;
 390                 }
 391         }
 392         channel_list->reg_classes = cla;
 393 }
 394
 395 static u8 init_channel_set(_adapter* padapter, u8 ChannelPlan, RT_CHANNEL_INFO *channel_set)
 396 {
 397         u8      index,chanset_size = 0;
 398         u8      b5GBand = _FALSE, b2_4GBand = _FALSE;
 399         u8      Index2G = 0, Index5G=0;
 400
 401         memset(channel_set, 0, sizeof(RT_CHANNEL_INFO)*MAX_CHANNEL_NUM);
 402
 403         if(ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE)
 404         {
 405                 DBG_8723A("ChannelPlan ID %x error !!!!!\n",ChannelPlan);
 406                 return chanset_size;
 407         }
 408
 409         if(padapter->registrypriv.wireless_mode & WIRELESS_11G)
 410         {
 411                 b2_4GBand = _TRUE;
 412                 if(RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
 413                         Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
 414                 else
 415                         Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
 416         }
 417
 418         if(padapter->registrypriv.wireless_mode & WIRELESS_11A)
 419         {
 420                 b5GBand = _TRUE;
 421                 if(RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
 422                         Index5G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index5G;
 423                 else
 424                         Index5G = RTW_ChannelPlanMap[ChannelPlan].Index5G;
 425         }
 426
 427         if(b2_4GBand)
 428         {
 429                 for(index=0;index<RTW_ChannelPlan2G[Index2G].Len;index++)
 430                 {
 431                         channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
 432
 433                         if(     (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||/* Channel 1~11 is active, and 12~14 is passive */
 434                                 (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == ChannelPlan)     )
 435                         {
 436                                 if(channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
 437                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 438                                 else if((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
 439                                         channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
 440                         }
 441                         else if(RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
 442                                 RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan ||
 443                                 RT_CHANNEL_DOMAIN_2G_WORLD == Index2G)/*  channel 12~13, passive scan */
 444                         {
 445                                 if(channel_set[chanset_size].ChannelNum <= 11)
 446                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 447                                 else
 448                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 449                         }
 450                         else
 451                         {
 452                                 channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 453                         }
 454
 455                         chanset_size++;
 456                 }
 457         }
 458
 459         if(b5GBand)
 460         {
 461                 for(index=0;index<RTW_ChannelPlan5G[Index5G].Len;index++)
 462                 {
 463 #ifdef CONFIG_DFS
 464                         channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
 465                         if ( channel_set[chanset_size].ChannelNum <= 48
 466                                 || channel_set[chanset_size].ChannelNum >= 149 )
 467                         {
 468                                 if(RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)/* passive scan for all 5G channels */
 469                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 470                                 else
 471                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 472                         }
 473                         else
 474                         {
 475                                 channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 476                         }
 477                         chanset_size++;
 478 #else /* CONFIG_DFS */
 479                         if ( RTW_ChannelPlan5G[Index5G].Channel[index] <= 48
 480                                 || RTW_ChannelPlan5G[Index5G].Channel[index] >= 149 ) {
 481                                 channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
 482                                 if(RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)/* passive scan for all 5G channels */
 483                                         channel_set[chanset_size].ScanType = SCAN_PASSIVE;
 484                                 else
 485                                         channel_set[chanset_size].ScanType = SCAN_ACTIVE;
 486                                 DBG_8723A("%s(): channel_set[%d].ChannelNum = %d\n", __FUNCTION__, chanset_size, channel_set[chanset_size].ChannelNum);
 487                                 chanset_size++;
 488                         }
 489 #endif /* CONFIG_DFS */
 490                 }
 491         }
 492
 493         return chanset_size;
 494 }
 495
 496 int     init_mlme_ext_priv(_adapter* padapter)
 497 {
 498         int     res = _SUCCESS;
 499         struct registry_priv* pregistrypriv = &padapter->registrypriv;
 500         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 501         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
 502         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 503
 504         /*  We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */
 505         /* memset((u8 *)pmlmeext, 0, sizeof(struct mlme_ext_priv)); */
 506
 507         pmlmeext->padapter = padapter;
 508
 509         /* fill_fwpriv(padapter, &(pmlmeext->fwpriv)); */
 510
 511         init_mlme_ext_priv_value(padapter);
 512         pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
 513
 514         init_mlme_ext_timer(padapter);
 515
 516 #ifdef CONFIG_AP_MODE
 517         init_mlme_ap_info(padapter);
 518 #endif
 519
 520         pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan,pmlmeext->channel_set);
 521         init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
 522
 523         pmlmeext->chan_scan_time = SURVEY_TO;
 524         pmlmeext->mlmeext_init = _TRUE;
 525
 526 #ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
 527         pmlmeext->active_keep_alive_check = _TRUE;
 528 #endif
 529
 530 #ifdef DBG_FIXED_CHAN
 531         pmlmeext->fixed_chan = 0xFF;
 532 #endif
 533
 534         return res;
 535 }
 536
 537 void free_mlme_ext_priv (struct mlme_ext_priv *pmlmeext)
 538 {
 539         _adapter *padapter = pmlmeext->padapter;
 540
 541         if (!padapter)
 542                 return;
 543
 544         if (padapter->bDriverStopped == _TRUE)
 545         {
 546                 _cancel_timer_ex(&pmlmeext->survey_timer);
 547                 _cancel_timer_ex(&pmlmeext->link_timer);
 548                 /* _cancel_timer_ex(&pmlmeext->ADDBA_timer); */
 549         }
 550 }
 551
 552 static u8 cmp_pkt_chnl_diff(_adapter *padapter,u8* pframe,uint packet_len)
 553 {       /*  if the channel is same, return 0. else return channel differential */
 554         uint len;
 555         u8 channel;
 556         u8 *p;
 557         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_, _DSSET_IE_, &len, packet_len - _BEACON_IE_OFFSET_);
 558         if (p)
 559         {
 560                 channel = *(p + 2);
 561                 if(padapter->mlmeextpriv.cur_channel >= channel)
 562                 {
 563                         return (padapter->mlmeextpriv.cur_channel - channel);
 564                 }
 565                 else
 566                 {
 567                         return (channel-padapter->mlmeextpriv.cur_channel);
 568                 }
 569         }
 570         else
 571         {
 572                 return 0;
 573         }
 574 }
 575
 576 static void _mgt_dispatcher(_adapter *padapter, struct mlme_handler *ptable, union recv_frame *precv_frame)
 577 {
 578         u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
 579         u8 *pframe = precv_frame->u.hdr.rx_data;
 580
 581           if(ptable->func)
 582         {
 583          /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
 584                 if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv),
 585                            ETH_ALEN) &&
 586                     memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
 587                 {
 588                         return;
 589                 }
 590
 591                 ptable->func(padapter, precv_frame);
 592         }
 593 }
 594
 595 void mgt_dispatcher(_adapter *padapter, union recv_frame *precv_frame)
 596 {
 597         int index;
 598         struct mlme_handler *ptable;
 599 #ifdef CONFIG_AP_MODE
 600         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 601 #endif /* CONFIG_AP_MODE */
 602         u8 bc_addr[ETH_ALEN] = {0xff,0xff,0xff,0xff,0xff,0xff};
 603         u8 *pframe = precv_frame->u.hdr.rx_data;
 604         struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));
 605
 606         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 607                  ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
 608                   GetFrameType(pframe), GetFrameSubType(pframe)));
 609
 610         if (GetFrameType(pframe) != WIFI_MGT_TYPE)
 611         {
 612                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
 613                 return;
 614         }
 615
 616         /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
 617         if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv),
 618                    ETH_ALEN) &&
 619             memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
 620         {
 621                 return;
 622         }
 623
 624         ptable = mlme_sta_tbl;
 625
 626         index = GetFrameSubType(pframe) >> 4;
 627
 628 #ifdef CONFIG_TDLS
 629         if((index << 4)==WIFI_ACTION){
 630                 /* category==public (4), action==TDLS_DISCOVERY_RESPONSE */
 631                 if(*(pframe+24)==0x04 && *(pframe+25)==TDLS_DISCOVERY_RESPONSE){
 632                         DBG_8723A("recv tdls discovery response frame\n");
 633                         On_TDLS_Dis_Rsp(padapter, precv_frame);
 634                 }
 635         }
 636 #endif /* CONFIG_TDLS */
 637
 638         if (index > 13)
 639         {
 640                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Currently we do not support reserved sub-fr-type=%d\n", index));
 641                 return;
 642         }
 643         ptable += index;
 644
 645 #if 1
 646         if (psta != NULL)
 647         {
 648                 if (GetRetry(pframe))
 649                 {
 650                         if (precv_frame->u.hdr.attrib.seq_num == psta->RxMgmtFrameSeqNum)
 651                         {
 652                                 /* drop the duplicate management frame */
 653                                 DBG_8723A("Drop duplicate management frame with seq_num = %d.\n", precv_frame->u.hdr.attrib.seq_num);
 654                                 return;
 655                         }
 656                 }
 657                 psta->RxMgmtFrameSeqNum = precv_frame->u.hdr.attrib.seq_num;
 658         }
 659 #else
 660
 661         if(GetRetry(pframe))
 662         {
 663                 /* RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("drop due to decache!\n")); */
 664                 /* return; */
 665         }
 666 #endif
 667
 668 #ifdef CONFIG_AP_MODE
 669         switch (GetFrameSubType(pframe))
 670         {
 671                 case WIFI_AUTH:
 672                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 673                                 ptable->func = &OnAuth;
 674                         else
 675                                 ptable->func = &OnAuthClient;
 676                         /* pass through */
 677                 case WIFI_ASSOCREQ:
 678                 case WIFI_REASSOCREQ:
 679                         _mgt_dispatcher(padapter, ptable, precv_frame);
 680 #ifdef CONFIG_HOSTAPD_MLME
 681                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 682                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 683 #endif
 684                         break;
 685                 case WIFI_PROBEREQ:
 686                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 687                         {
 688 #ifdef CONFIG_HOSTAPD_MLME
 689                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 690 #else
 691                                 _mgt_dispatcher(padapter, ptable, precv_frame);
 692 #endif
 693                         }
 694                         else
 695                                 _mgt_dispatcher(padapter, ptable, precv_frame);
 696                         break;
 697                 case WIFI_BEACON:
 698                         _mgt_dispatcher(padapter, ptable, precv_frame);
 699                         break;
 700                 case WIFI_ACTION:
 701                         /* if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE) */
 702                         _mgt_dispatcher(padapter, ptable, precv_frame);
 703                         break;
 704                 default:
 705                         _mgt_dispatcher(padapter, ptable, precv_frame);
 706                         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
 707                                 rtw_hostapd_mlme_rx(padapter, precv_frame);
 708                         break;
 709         }
 710 #else
 711
 712         _mgt_dispatcher(padapter, ptable, precv_frame);
 713
 714 #endif
 715 }
 716
 717 #ifdef CONFIG_P2P
 718 u32 p2p_listen_state_process(_adapter *padapter, unsigned char *da)
 719 {
 720         bool response = _TRUE;
 721
 722 #ifdef CONFIG_IOCTL_CFG80211
 723         if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled == _FALSE
 724                 || padapter->mlmepriv.wps_probe_resp_ie == NULL
 725                 || padapter->mlmepriv.p2p_probe_resp_ie == NULL
 726         )
 727         {
 728                 DBG_8723A("DON'T issue_probersp_p2p: p2p_enabled:%d, wps_probe_resp_ie:%p, p2p_probe_resp_ie:%p\n",
 729                         wdev_to_priv(padapter->rtw_wdev)->p2p_enabled,
 730                         padapter->mlmepriv.wps_probe_resp_ie,
 731                         padapter->mlmepriv.p2p_probe_resp_ie);
 732                 response = _FALSE;
 733         }
 734 #else
 735
 736         /*      do nothing if the device name is empty */
 737         if ( !padapter->wdinfo.device_name_len )
 738         {
 739                 response        = _FALSE;
 740         }
 741 #endif
 742
 743         if (response == _TRUE)
 744                 issue_probersp_p2p( padapter, da);
 745
 746         return _SUCCESS;
 747 }
 748 #endif /* CONFIG_P2P */
 749
 750 /****************************************************************************
 751
 752 Following are the callback functions for each subtype of the management frames
 753
 754 *****************************************************************************/
 755
 756 unsigned int OnProbeReq(_adapter *padapter, union recv_frame *precv_frame)
 757 {
 758         unsigned int    ielen;
 759         unsigned char   *p;
 760         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 761         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 762         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 763         WLAN_BSSID_EX   *cur = &(pmlmeinfo->network);
 764         u8 *pframe = precv_frame->u.hdr.rx_data;
 765         uint len = precv_frame->u.hdr.len;
 766         u8 is_valid_p2p_probereq = _FALSE;
 767
 768 #ifdef CONFIG_P2P
 769         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
 770         struct rx_pkt_attrib    *pattrib = &precv_frame->u.hdr.attrib;
 771         u8 wifi_test_chk_rate = 1;
 772
 773         if (    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
 774                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) &&
 775                 !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
 776                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
 777                 !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)
 778            )
 779         {
 780                 /*      Commented by Albert 2011/03/17 */
 781                 /*      mcs_rate = 0 -> CCK 1M rate */
 782                 /*      mcs_rate = 1 -> CCK 2M rate */
 783                 /*      mcs_rate = 2 -> CCK 5.5M rate */
 784                 /*      mcs_rate = 3 -> CCK 11M rate */
 785                 /*      In the P2P mode, the driver should not support the CCK rate */
 786
 787                 /*      Commented by Kurt 2012/10/16 */
 788                 /*      IOT issue: Google Nexus7 use 1M rate to send p2p_probe_req after GO nego completed and Nexus7 is client */
 789 #ifdef CONFIG_WIFI_TEST
 790                 if ( pattrib->mcs_rate <= 3 )
 791                 {
 792                         wifi_test_chk_rate = 0;
 793                 }
 794 #endif /* CONFIG_WIFI_TEST */
 795
 796                 if( wifi_test_chk_rate == 1 )
 797                 {
 798                         if((is_valid_p2p_probereq = process_probe_req_p2p_ie(pwdinfo, pframe, len)) == _TRUE)
 799                         {
 800                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE))
 801                                 {
 802 #ifndef CONFIG_IOCTL_CFG80211
 803 /*  FIXME */
 804                                         report_survey_event(padapter, precv_frame);
 805 #endif
 806                                         p2p_listen_state_process( padapter,  get_sa(pframe));
 807
 808                                         return _SUCCESS;
 809                                 }
 810
 811                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
 812                                 {
 813                                         goto _continue;
 814                                 }
 815                         }
 816                 }
 817         }
 818
 819 _continue:
 820 #endif /* CONFIG_P2P */
 821
 822         if(check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 823         {
 824                 return _SUCCESS;
 825         }
 826
 827         if(check_fwstate(pmlmepriv, _FW_LINKED) == _FALSE &&
 828                 check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE)==_FALSE)
 829         {
 830                 return _SUCCESS;
 831         }
 832
 833         /* DBG_8723A("+OnProbeReq\n"); */
 834
 835 #ifdef CONFIG_CONCURRENT_MODE
 836         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
 837                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
 838         {
 839                 /* don't process probe req */
 840                 return _SUCCESS;
 841         }
 842 #endif
 843
 844         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
 845                         len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
 846
 847         /* check (wildcard) SSID */
 848         if (p != NULL)
 849         {
 850                 if(is_valid_p2p_probereq == _TRUE)
 851                 {
 852                         goto _issue_probersp;
 853                 }
 854
 855                 if ((ielen != 0 &&
 856                       memcmp((void *)(p+2), cur->Ssid.Ssid,
 857                              cur->Ssid.SsidLength)) ||
 858                     (ielen == 0 && pmlmeinfo->hidden_ssid_mode))
 859                 {
 860                         return _SUCCESS;
 861                 }
 862
 863 _issue_probersp:
 864
 865                 if(check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE &&
 866                         pmlmepriv->cur_network.join_res == _TRUE)
 867                 {
 868                         /* DBG_8723A("+issue_probersp during ap mode\n"); */
 869                         issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
 870                 }
 871
 872         }
 873
 874         return _SUCCESS;
 875 }
 876
 877 unsigned int OnProbeRsp(_adapter *padapter, union recv_frame *precv_frame)
 878 {
 879         struct sta_info         *psta;
 880         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 881         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 882         struct sta_priv         *pstapriv = &padapter->stapriv;
 883         u8      *pframe = precv_frame->u.hdr.rx_data;
 884 #ifdef CONFIG_P2P
 885         struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
 886 #endif
 887
 888 #ifdef CONFIG_P2P
 889         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
 890         {
 891                 if ( _TRUE == pwdinfo->tx_prov_disc_info.benable )
 892                 {
 893                         if (!memcmp(pwdinfo->tx_prov_disc_info.peerIFAddr,
 894                                     GetAddr2Ptr(pframe), ETH_ALEN))
 895                         {
 896                                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT))
 897                                 {
 898                                         pwdinfo->tx_prov_disc_info.benable = _FALSE;
 899                                         issue_p2p_provision_request( padapter,
 900                                                                                                 pwdinfo->tx_prov_disc_info.ssid.Ssid,
 901                                                                                                 pwdinfo->tx_prov_disc_info.ssid.SsidLength,
 902                                                                                                 pwdinfo->tx_prov_disc_info.peerDevAddr );
 903                                 }
 904                                 else if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
 905                                 {
 906                                         pwdinfo->tx_prov_disc_info.benable = _FALSE;
 907                                         issue_p2p_provision_request( padapter,
 908                                                                                                 NULL,
 909                                                                                                 0,
 910                                                                                                 pwdinfo->tx_prov_disc_info.peerDevAddr );
 911                                 }
 912                         }
 913                 }
 914                 return _SUCCESS;
 915         }
 916         else if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
 917         {
 918                 if ( _TRUE == pwdinfo->nego_req_info.benable )
 919                 {
 920                         DBG_8723A( "[%s] P2P State is GONEGO ING!\n", __FUNCTION__ );
 921                         if (!memcmp(pwdinfo->nego_req_info.peerDevAddr,
 922                                     GetAddr2Ptr(pframe), ETH_ALEN))
 923                         {
 924                                 pwdinfo->nego_req_info.benable = _FALSE;
 925                                 issue_p2p_GO_request( padapter, pwdinfo->nego_req_info.peerDevAddr);
 926                         }
 927                 }
 928         }
 929         else if( rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ ) )
 930         {
 931                 if ( _TRUE == pwdinfo->invitereq_info.benable )
 932                 {
 933                         DBG_8723A( "[%s] P2P_STATE_TX_INVITE_REQ!\n", __FUNCTION__ );
 934                         if (!memcmp( pwdinfo->invitereq_info.peer_macaddr,
 935                                      GetAddr2Ptr(pframe), ETH_ALEN))
 936                         {
 937                                 pwdinfo->invitereq_info.benable = _FALSE;
 938                                 issue_p2p_invitation_request( padapter, pwdinfo->invitereq_info.peer_macaddr );
 939                         }
 940                 }
 941         }
 942 #endif
 943
 944         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
 945         {
 946                 report_survey_event(padapter, precv_frame);
 947 #ifdef CONFIG_CONCURRENT_MODE
 948                 report_survey_event(padapter->pbuddy_adapter, precv_frame);
 949 #endif
 950 #ifdef CONFIG_DUALMAC_CONCURRENT
 951                 dc_report_survey_event(padapter, precv_frame);
 952 #endif
 953                 return _SUCCESS;
 954         }
 955
 956         return _SUCCESS;
 957 }
 958
 959 unsigned int OnBeacon(_adapter *padapter, union recv_frame *precv_frame)
 960 {
 961         int cam_idx;
 962         struct sta_info *psta;
 963         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 964         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 965         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 966         struct sta_priv *pstapriv = &padapter->stapriv;
 967         u8 *pframe = precv_frame->u.hdr.rx_data;
 968         uint len = precv_frame->u.hdr.len;
 969         WLAN_BSSID_EX *pbss;
 970         int ret = _SUCCESS;
 971         u8 *p = NULL;
 972         u32 ielen = 0;
 973
 974 #ifdef CONFIG_ATTEMPT_TO_FIX_AP_BEACON_ERROR
 975         p = rtw_get_ie(pframe + sizeof(struct rtw_ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ielen, precv_frame->u.hdr.len -sizeof(struct rtw_ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_);
 976         if ((p != NULL) && (ielen > 0))
 977         {
 978                 if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D))
 979                 {
 980                         /* Invalid value 0x2D is detected in Extended Supported Rates (ESR) IE. Try to fix the IE length to avoid failed Beacon parsing. */
 981                         DBG_8723A("[WIFIDBG] Error in ESR IE is detected in Beacon of BSSID:"MAC_FMT". Fix the length of ESR IE to avoid failed Beacon parsing.\n", MAC_ARG(GetAddr3Ptr(pframe)));
 982                         *(p + 1) = ielen - 1;
 983                 }
 984         }
 985 #endif
 986
 987         if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS)
 988         {
 989                 report_survey_event(padapter, precv_frame);
 990 #ifdef CONFIG_CONCURRENT_MODE
 991                 report_survey_event(padapter->pbuddy_adapter, precv_frame);
 992 #endif
 993
 994 #ifdef CONFIG_DUALMAC_CONCURRENT
 995                 dc_report_survey_event(padapter, precv_frame);
 996 #endif
 997
 998                 return _SUCCESS;
 999         }
1000
1001         if (!memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network),
1002                     ETH_ALEN)) {
1003                 if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
1004                 {
1005                         /* we should update current network before auth, or some IE is wrong */
1006                         pbss = (WLAN_BSSID_EX*)rtw_malloc(sizeof(WLAN_BSSID_EX));
1007                         if (pbss) {
1008                                 if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
1009                                         update_network(&(pmlmepriv->cur_network.network), pbss, padapter, _TRUE);
1010                                         rtw_get_bcn_info(&(pmlmepriv->cur_network));
1011                                 }
1012                                 rtw_mfree((u8*)pbss, sizeof(WLAN_BSSID_EX));
1013                         }
1014
1015                         /* check the vendor of the assoc AP */
1016                         pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct rtw_ieee80211_hdr_3addr), len-sizeof(struct rtw_ieee80211_hdr_3addr));
1017
1018                         /* update TSF Value */
1019                         update_TSF(pmlmeext, pframe, len);
1020
1021                         /* start auth */
1022                         start_clnt_auth(padapter);
1023
1024                         return _SUCCESS;
1025                 }
1026
1027                 if(((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
1028                 {
1029                         if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
1030                         {
1031                                 #ifdef CONFIG_PATCH_JOIN_WRONG_CHANNEL
1032                                 /* Merge from 8712 FW code */
1033                                 if (cmp_pkt_chnl_diff(padapter,pframe,len) != 0)
1034                                 {            /*  join wrong channel, deauth and reconnect */
1035                                         issue_deauth(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
1036
1037                                         report_del_sta_event(padapter,(&(pmlmeinfo->network))->MacAddress, WLAN_REASON_JOIN_WRONG_CHANNEL);
1038                                         pmlmeinfo->state &= (~WIFI_FW_ASSOC_SUCCESS);
1039                                         return _SUCCESS;
1040                                 }
1041                                 #endif /* CONFIG_PATCH_JOIN_WRONG_CHANNEL */
1042
1043                                 ret = rtw_check_bcn_info(padapter, pframe, len);
1044                                 if (!ret) {
1045                                                 DBG_8723A_LEVEL(_drv_always_, "ap has changed, disconnect now\n ");
1046                                                 receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 65535);
1047                                                 return _SUCCESS;
1048                                 }
1049                                 /* update WMM, ERP in the beacon */
1050                                 /* todo: the timer is used instead of the number of the beacon received */
1051                                 if ((sta_rx_pkts(psta) & 0xf) == 0)
1052                                 {
1053                                         /* DBG_8723A("update_bcn_info\n"); */
1054                                         update_beacon_info(padapter, pframe, len, psta);
1055                                 }
1056
1057 #ifdef CONFIG_DFS
1058                                 process_csa_ie(padapter, pframe, len);  /* channel switch announcement */
1059 #endif /* CONFIG_DFS */
1060
1061 #ifdef CONFIG_P2P_PS
1062                                 process_p2p_ps_ie(padapter, (pframe + WLAN_HDR_A3_LEN), (len - WLAN_HDR_A3_LEN));
1063 #endif /* CONFIG_P2P_PS */
1064
1065                         }
1066                 }
1067                 else if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
1068                 {
1069                         if ((psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe))) != NULL)
1070                         {
1071                                 /* update WMM, ERP in the beacon */
1072                                 /* todo: the timer is used instead of the number of the beacon received */
1073                                 if ((sta_rx_pkts(psta) & 0xf) == 0)
1074                                 {
1075                                         /* DBG_8723A("update_bcn_info\n"); */
1076                                         update_beacon_info(padapter, pframe, len, psta);
1077                                 }
1078
1079                         }
1080                         else
1081                         {
1082                                 /* allocate a new CAM entry for IBSS station */
1083                                 if ((cam_idx = allocate_fw_sta_entry(padapter)) == NUM_STA)
1084                                 {
1085                                         goto _END_ONBEACON_;
1086                                 }
1087
1088                                 /* get supported rate */
1089                                 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)
1090                                 {
1091                                         pmlmeinfo->FW_sta_info[cam_idx].status = 0;
1092                                         goto _END_ONBEACON_;
1093                                 }
1094
1095                                 /* update TSF Value */
1096                                 update_TSF(pmlmeext, pframe, len);
1097
1098                                 /* report sta add event */
1099                                 report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
1100                         }
1101                 }
1102         }
1103
1104 _END_ONBEACON_:
1105
1106         return _SUCCESS;
1107 }
1108
1109 unsigned int OnAuth(_adapter *padapter, union recv_frame *precv_frame)
1110 {
1111 #ifdef CONFIG_AP_MODE
1112         _irqL irqL;
1113         unsigned int    auth_mode, seq, ie_len;
1114         unsigned char   *sa, *p;
1115         u16     algorithm;
1116         int     status;
1117         static struct sta_info stat;
1118         struct  sta_info        *pstat=NULL;
1119         struct  sta_priv *pstapriv = &padapter->stapriv;
1120         struct security_priv *psecuritypriv = &padapter->securitypriv;
1121         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1122         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1123         u8 *pframe = precv_frame->u.hdr.rx_data;
1124         uint len = precv_frame->u.hdr.len;
1125
1126 #ifdef CONFIG_CONCURRENT_MODE
1127         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
1128                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
1129         {
1130                 /* don't process auth request; */
1131                 return _SUCCESS;
1132         }
1133 #endif /* CONFIG_CONCURRENT_MODE */
1134
1135         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
1136                 return _FAIL;
1137
1138         DBG_8723A("+OnAuth\n");
1139
1140         sa = GetAddr2Ptr(pframe);
1141
1142         auth_mode = psecuritypriv->dot11AuthAlgrthm;
1143         seq = cpu_to_le16(*(u16*)((unsigned long)pframe + WLAN_HDR_A3_LEN + 2));
1144         algorithm = cpu_to_le16(*(u16*)((unsigned long)pframe + WLAN_HDR_A3_LEN));
1145
1146         DBG_8723A("auth alg=%x, seq=%X\n", algorithm, seq);
1147
1148         if (auth_mode == 2 &&
1149                         psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
1150                         psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
1151                 auth_mode = 0;
1152
1153         if ((algorithm > 0 && auth_mode == 0) ||        /*  rx a shared-key auth but shared not enabled */
1154                 (algorithm == 0 && auth_mode == 1) )    /*  rx a open-system auth but shared-key is enabled */
1155         {
1156                 DBG_8723A("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n",
1157                         algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
1158
1159                 status = _STATS_NO_SUPP_ALG_;
1160
1161                 goto auth_fail;
1162         }
1163
1164         if(rtw_access_ctrl(padapter, sa) == _FALSE)
1165         {
1166                 status = _STATS_UNABLE_HANDLE_STA_;
1167                 goto auth_fail;
1168         }
1169
1170         pstat = rtw_get_stainfo(pstapriv, sa);
1171         if (pstat == NULL)
1172         {
1173                 /*  allocate a new one */
1174                 DBG_8723A("going to alloc stainfo for sa="MAC_FMT"\n",  MAC_ARG(sa));
1175                 pstat = rtw_alloc_stainfo(pstapriv, sa);
1176                 if (pstat == NULL)
1177                 {
1178                         DBG_8723A(" Exceed the upper limit of supported clients...\n");
1179                         status = _STATS_UNABLE_HANDLE_STA_;
1180                         goto auth_fail;
1181                 }
1182
1183                 pstat->state = WIFI_FW_AUTH_NULL;
1184                 pstat->auth_seq = 0;
1185
1186                 /* pstat->flags = 0; */
1187                 /* pstat->capability = 0; */
1188         }
1189         else
1190         {
1191                 spin_lock_bh(&pstapriv->asoc_list_lock);
1192                 if(rtw_is_list_empty(&pstat->asoc_list)==_FALSE)
1193                 {
1194                         rtw_list_delete(&pstat->asoc_list);
1195                         pstapriv->asoc_list_cnt--;
1196                         if (pstat->expire_to > 0)
1197                         {
1198                                 /* TODO: STA re_auth within expire_to */
1199                         }
1200                 }
1201                 spin_unlock_bh(&pstapriv->asoc_list_lock);
1202
1203                 if (seq==1) {
1204                         /* TODO: STA re_auth and auth timeout */
1205                 }
1206         }
1207
1208         spin_lock_bh(&pstapriv->auth_list_lock);
1209         if (rtw_is_list_empty(&pstat->auth_list))
1210         {
1211                 rtw_list_insert_tail(&pstat->auth_list, &pstapriv->auth_list);
1212                 pstapriv->auth_list_cnt++;
1213         }
1214         spin_unlock_bh(&pstapriv->auth_list_lock);
1215
1216         if (pstat->auth_seq == 0)
1217                 pstat->expire_to = pstapriv->auth_to;
1218
1219         if ((pstat->auth_seq + 1) != seq)
1220         {
1221                 DBG_8723A("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1222                         seq, pstat->auth_seq+1);
1223                 status = _STATS_OUT_OF_AUTH_SEQ_;
1224                 goto auth_fail;
1225         }
1226
1227         if (algorithm==0 && (auth_mode == 0 || auth_mode == 2))
1228         {
1229                 if (seq == 1)
1230                 {
1231                         pstat->state &= ~WIFI_FW_AUTH_NULL;
1232                         pstat->state |= WIFI_FW_AUTH_SUCCESS;
1233                         pstat->expire_to = pstapriv->assoc_to;
1234                         pstat->authalg = algorithm;
1235                 }
1236                 else
1237                 {
1238                         DBG_8723A("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1239                                 seq, pstat->auth_seq+1);
1240                         status = _STATS_OUT_OF_AUTH_SEQ_;
1241                         goto auth_fail;
1242                 }
1243         }
1244         else /*  shared system or auto authentication */
1245         {
1246                 if (seq == 1)
1247                 {
1248                         /* prepare for the challenging txt... */
1249                         pstat->state &= ~WIFI_FW_AUTH_NULL;
1250                         pstat->state |= WIFI_FW_AUTH_STATE;
1251                         pstat->authalg = algorithm;
1252                         pstat->auth_seq = 2;
1253                 }
1254                 else if (seq == 3)
1255                 {
1256                         /* checking for challenging txt... */
1257                         DBG_8723A("checking for challenging txt...\n");
1258
1259                         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len,
1260                                         len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
1261
1262                         if((p==NULL) || (ie_len<=0))
1263                         {
1264                                 DBG_8723A("auth rejected because challenge failure!(1)\n");
1265                                 status = _STATS_CHALLENGE_FAIL_;
1266                                 goto auth_fail;
1267                         }
1268
1269                         if (!memcmp((void *)(p + 2), pstat->chg_txt, 128))
1270                         {
1271                                 pstat->state &= (~WIFI_FW_AUTH_STATE);
1272                                 pstat->state |= WIFI_FW_AUTH_SUCCESS;
1273                                 /*  challenging txt is correct... */
1274                                 pstat->expire_to =  pstapriv->assoc_to;
1275                         }
1276                         else
1277                         {
1278                                 DBG_8723A("auth rejected because challenge failure!\n");
1279                                 status = _STATS_CHALLENGE_FAIL_;
1280                                 goto auth_fail;
1281                         }
1282                 }
1283                 else
1284                 {
1285                         DBG_8723A("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
1286                                 seq, pstat->auth_seq+1);
1287                         status = _STATS_OUT_OF_AUTH_SEQ_;
1288                         goto auth_fail;
1289                 }
1290         }
1291
1292         /*  Now, we are going to issue_auth... */
1293         pstat->auth_seq = seq + 1;
1294
1295 #ifdef CONFIG_NATIVEAP_MLME
1296         issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
1297 #endif
1298
1299         if (pstat->state & WIFI_FW_AUTH_SUCCESS)
1300                 pstat->auth_seq = 0;
1301
1302         return _SUCCESS;
1303
1304 auth_fail:
1305
1306         if(pstat)
1307                 rtw_free_stainfo(padapter , pstat);
1308
1309         pstat = &stat;
1310         memset((char *)pstat, '\0', sizeof(stat));
1311         pstat->auth_seq = 2;
1312         memcpy(pstat->hwaddr, sa, 6);
1313
1314 #ifdef CONFIG_NATIVEAP_MLME
1315         issue_auth(padapter, pstat, (unsigned short)status);
1316 #endif
1317
1318 #endif
1319         return _FAIL;
1320 }
1321
1322 unsigned int OnAuthClient(_adapter *padapter, union recv_frame *precv_frame)
1323 {
1324         unsigned int    seq, len, status, algthm, offset;
1325         unsigned char   *p;
1326         unsigned int    go2asoc = 0;
1327         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1328         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1329         u8 *pframe = precv_frame->u.hdr.rx_data;
1330         uint pkt_len = precv_frame->u.hdr.len;
1331
1332         DBG_8723A("%s\n", __FUNCTION__);
1333
1334         /* check A1 matches or not */
1335         if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
1336                 return _SUCCESS;
1337
1338         if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
1339                 return _SUCCESS;
1340
1341         offset = (GetPrivacy(pframe))? 4: 0;
1342
1343         algthm  = le16_to_cpu(*(unsigned short *)((unsigned long)pframe + WLAN_HDR_A3_LEN + offset));
1344         seq     = le16_to_cpu(*(unsigned short *)((unsigned long)pframe + WLAN_HDR_A3_LEN + offset + 2));
1345         status  = le16_to_cpu(*(unsigned short *)((unsigned long)pframe + WLAN_HDR_A3_LEN + offset + 4));
1346
1347         if (status != 0)
1348         {
1349                 DBG_8723A("clnt auth fail, status: %d\n", status);
1350                 if(status == 13)/*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
1351                 {
1352                         if(pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1353                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
1354                         else
1355                                 pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
1356                         /* pmlmeinfo->reauth_count = 0; */
1357                 }
1358
1359                 set_link_timer(pmlmeext, 1);
1360                 goto authclnt_fail;
1361         }
1362
1363         if (seq == 2)
1364         {
1365                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1366                 {
1367                          /*  legendary shared system */
1368                         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
1369                                 pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
1370
1371                         if (p == NULL)
1372                         {
1373                                 /* DBG_8723A("marc: no challenge text?\n"); */
1374                                 goto authclnt_fail;
1375                         }
1376
1377                         memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
1378                         pmlmeinfo->auth_seq = 3;
1379                         issue_auth(padapter, NULL, 0);
1380                         set_link_timer(pmlmeext, REAUTH_TO);
1381
1382                         return _SUCCESS;
1383                 }
1384                 else
1385                 {
1386                         /*  open system */
1387                         go2asoc = 1;
1388                 }
1389         }
1390         else if (seq == 4)
1391         {
1392                 if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
1393                 {
1394                         go2asoc = 1;
1395                 }
1396                 else
1397                 {
1398                         goto authclnt_fail;
1399                 }
1400         }
1401         else
1402         {
1403                 /*  this is also illegal */
1404                 /* DBG_8723A("marc: clnt auth failed due to illegal seq=%x\n", seq); */
1405                 goto authclnt_fail;
1406         }
1407
1408         if (go2asoc)
1409         {
1410                 DBG_8723A_LEVEL(_drv_always_, "auth success, start assoc\n");
1411                 start_clnt_assoc(padapter);
1412                 return _SUCCESS;
1413         }
1414
1415 authclnt_fail:
1416
1417         /* pmlmeinfo->state &= ~(WIFI_FW_AUTH_STATE); */
1418
1419         return _FAIL;
1420 }
1421
1422 unsigned int OnAssocReq(_adapter *padapter, union recv_frame *precv_frame)
1423 {
1424 #ifdef CONFIG_AP_MODE
1425         _irqL irqL;
1426         u16 capab_info, listen_interval;
1427         struct rtw_ieee802_11_elems elems;
1428         struct sta_info *pstat;
1429         unsigned char           reassoc, *p, *pos, *wpa_ie;
1430         unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
1431         int             i, ie_len, wpa_ie_len, left;
1432         unsigned char           supportRate[16];
1433         int                                     supportRateNum;
1434         unsigned short          status = _STATS_SUCCESSFUL_;
1435         unsigned short          frame_type, ie_offset=0;
1436         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1437         struct security_priv *psecuritypriv = &padapter->securitypriv;
1438         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
1439         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1440         WLAN_BSSID_EX   *cur = &(pmlmeinfo->network);
1441         struct sta_priv *pstapriv = &padapter->stapriv;
1442         u8 *pframe = precv_frame->u.hdr.rx_data;
1443         uint pkt_len = precv_frame->u.hdr.len;
1444 #ifdef CONFIG_P2P
1445         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
1446         u8 p2p_status_code = P2P_STATUS_SUCCESS;
1447         u8 *p2pie;
1448         u32 p2pielen = 0;
1449 #ifdef CONFIG_WFD
1450         u8      wfd_ie[ 128 ] = { 0x00 };
1451         u32     wfd_ielen = 0;
1452 #endif /*  CONFIG_WFD */
1453 #endif /* CONFIG_P2P */
1454
1455 #ifdef CONFIG_CONCURRENT_MODE
1456         if(((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
1457                 check_buddy_fwstate(padapter, _FW_UNDER_LINKING|_FW_UNDER_SURVEY))
1458         {
1459                 /* don't process assoc request; */
1460                 return _SUCCESS;
1461         }
1462 #endif /* CONFIG_CONCURRENT_MODE */
1463
1464         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
1465                 return _FAIL;
1466
1467         frame_type = GetFrameSubType(pframe);
1468         if (frame_type == WIFI_ASSOCREQ)
1469         {
1470                 reassoc = 0;
1471                 ie_offset = _ASOCREQ_IE_OFFSET_;
1472         }
1473         else /*  WIFI_REASSOCREQ */
1474         {
1475                 reassoc = 1;
1476                 ie_offset = _REASOCREQ_IE_OFFSET_;
1477         }
1478
1479         if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
1480                 DBG_8723A("handle_assoc(reassoc=%d) - too short payload (len=%lu)"
1481                        "\n", reassoc, (unsigned long)pkt_len);
1482                 return _FAIL;
1483         }
1484
1485         pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
1486         if (pstat == (struct sta_info *)NULL)
1487         {
1488                 status = _RSON_CLS2_;
1489                 goto asoc_class2_error;
1490         }
1491
1492         capab_info = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN);
1493         /* capab_info = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN)); */
1494         /* listen_interval = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN+2)); */
1495         listen_interval = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN+2);
1496
1497         left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
1498         pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
1499
1500         DBG_8723A("%s\n", __FUNCTION__);
1501
1502         /*  check if this stat has been successfully authenticated/assocated */
1503         if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS))
1504         {
1505                 if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS))
1506                 {
1507                         status = _RSON_CLS2_;
1508                         goto asoc_class2_error;
1509                 }
1510                 else
1511                 {
1512                         pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
1513                         pstat->state |= WIFI_FW_ASSOC_STATE;
1514                 }
1515         }
1516         else
1517         {
1518                 pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
1519                 pstat->state |= WIFI_FW_ASSOC_STATE;
1520         }
1521
1522         pstat->capability = capab_info;
1523
1524         /* now parse all ieee802_11 ie to point to elems */
1525         if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
1526             !elems.ssid) {
1527                 DBG_8723A("STA " MAC_FMT " sent invalid association request\n",
1528                        MAC_ARG(pstat->hwaddr));
1529                 status = _STATS_FAILURE_;
1530                 goto OnAssocReqFail;
1531         }
1532
1533         /*  now we should check all the fields... */
1534         /*  checking SSID */
1535         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
1536                 pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1537         if (p == NULL)
1538         {
1539                 status = _STATS_FAILURE_;
1540         }
1541
1542         if (ie_len == 0) /*  broadcast ssid, however it is not allowed in assocreq */
1543                 status = _STATS_FAILURE_;
1544         else
1545         {
1546                 /*  check if ssid match */
1547                 if (memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
1548                         status = _STATS_FAILURE_;
1549
1550                 if (ie_len != cur->Ssid.SsidLength)
1551                         status = _STATS_FAILURE_;
1552         }
1553
1554         if(_STATS_SUCCESSFUL_ != status)
1555                 goto OnAssocReqFail;
1556
1557         /*  check if the supported rate is ok */
1558         p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1559         if (p == NULL) {
1560                 DBG_8723A("Rx a sta assoc-req which supported rate is empty!\n");
1561                 /*  use our own rate set as statoin used */
1562                 /* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
1563                 /* supportRateNum = AP_BSSRATE_LEN; */
1564
1565                 status = _STATS_FAILURE_;
1566                 goto OnAssocReqFail;
1567         }
1568         else {
1569                 memcpy(supportRate, p+2, ie_len);
1570                 supportRateNum = ie_len;
1571
1572                 p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_ , &ie_len,
1573                                 pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1574                 if (p !=  NULL) {
1575
1576                         if(supportRateNum<=sizeof(supportRate))
1577                         {
1578                                 memcpy(supportRate+supportRateNum, p+2, ie_len);
1579                                 supportRateNum += ie_len;
1580                         }
1581                 }
1582         }
1583
1584         /* todo: mask supportRate between AP & STA -> move to update raid */
1585         /* get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0); */
1586
1587         /* update station supportRate */
1588         pstat->bssratelen = supportRateNum;
1589         memcpy(pstat->bssrateset, supportRate, supportRateNum);
1590         UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
1591
1592         /* check RSN/WPA/WPS */
1593         pstat->dot8021xalg = 0;
1594         pstat->wpa_psk = 0;
1595         pstat->wpa_group_cipher = 0;
1596         pstat->wpa2_group_cipher = 0;
1597         pstat->wpa_pairwise_cipher = 0;
1598         pstat->wpa2_pairwise_cipher = 0;
1599         memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
1600         if((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
1601
1602                 int group_cipher=0, pairwise_cipher=0;
1603
1604                 wpa_ie = elems.rsn_ie;
1605                 wpa_ie_len = elems.rsn_ie_len;
1606
1607                 if(rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
1608                         pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
1609                         pstat->wpa_psk |= BIT(1);
1610
1611                         pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
1612                         pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
1613
1614                         if(!pstat->wpa2_group_cipher)
1615                                 status = WLAN_REASON_INVALID_GROUP_CIPHER;
1616
1617                         if(!pstat->wpa2_pairwise_cipher)
1618                                 status = WLAN_REASON_INVALID_PAIRWISE_CIPHER;
1619                 } else {
1620                         status = WLAN_STATUS_INVALID_IE;
1621                 }
1622
1623         } else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
1624
1625                 int group_cipher=0, pairwise_cipher=0;
1626
1627                 wpa_ie = elems.wpa_ie;
1628                 wpa_ie_len = elems.wpa_ie_len;
1629
1630                 if(rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
1631                         pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
1632                         pstat->wpa_psk |= BIT(0);
1633
1634                         pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
1635                         pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
1636
1637                         if(!pstat->wpa_group_cipher)
1638                                 status = WLAN_STATUS_INVALID_GROUP_CIPHER;
1639
1640                         if(!pstat->wpa_pairwise_cipher)
1641                                 status = WLAN_STATUS_INVALID_PAIRWISE_CIPHER;
1642
1643                 } else {
1644                         status = WLAN_STATUS_INVALID_IE;
1645                 }
1646
1647         } else {
1648                 wpa_ie = NULL;
1649                 wpa_ie_len = 0;
1650         }
1651
1652         if(_STATS_SUCCESSFUL_ != status)
1653                 goto OnAssocReqFail;
1654
1655         pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
1656         if(wpa_ie == NULL) {
1657                 if (elems.wps_ie) {
1658                         DBG_8723A("STA included WPS IE in "
1659                                    "(Re)Association Request - assume WPS is "
1660                                    "used\n");
1661                         pstat->flags |= WLAN_STA_WPS;
1662                 } else {
1663                         DBG_8723A("STA did not include WPA/RSN IE "
1664                                    "in (Re)Association Request - possible WPS "
1665                                    "use\n");
1666                         pstat->flags |= WLAN_STA_MAYBE_WPS;
1667                 }
1668
1669                 /*  AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
1670                 /*  that the selected registrar of AP is _FLASE */
1671                 if ((psecuritypriv->wpa_psk > 0) &&
1672                     (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS))) {
1673                         if(pmlmepriv->wps_beacon_ie) {
1674                                 u8 selected_registrar = 0;
1675
1676                                 rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len,
1677                                                          WPS_ATTR_SELECTED_REGISTRAR , &selected_registrar, NULL);
1678
1679                                 if(!selected_registrar) {
1680                                         DBG_8723A("selected_registrar is _FALSE , or AP is not ready to do WPS\n");
1681
1682                                         status = _STATS_UNABLE_HANDLE_STA_;
1683
1684                                         goto OnAssocReqFail;
1685                                 }
1686                         }
1687                 }
1688         } else {
1689                 int copy_len;
1690
1691                 if(psecuritypriv->wpa_psk == 0) {
1692                         DBG_8723A("STA " MAC_FMT ": WPA/RSN IE in association "
1693                         "request, but AP don't support WPA/RSN\n", MAC_ARG(pstat->hwaddr));
1694
1695                         status = WLAN_STATUS_INVALID_IE;
1696
1697                         goto OnAssocReqFail;
1698                 }
1699
1700                 if (elems.wps_ie) {
1701                         DBG_8723A("STA included WPS IE in "
1702                                    "(Re)Association Request - WPS is "
1703                                    "used\n");
1704                         pstat->flags |= WLAN_STA_WPS;
1705                         copy_len=0;
1706                 } else {
1707                         copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)):(wpa_ie_len+2);
1708                 }
1709
1710                 if(copy_len>0)
1711                         memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
1712
1713         }
1714
1715         /*  check if there is WMM IE & support WWM-PS */
1716         pstat->flags &= ~WLAN_STA_WME;
1717         pstat->qos_option = 0;
1718         pstat->qos_info = 0;
1719         pstat->has_legacy_ac = _TRUE;
1720         pstat->uapsd_vo = 0;
1721         pstat->uapsd_vi = 0;
1722         pstat->uapsd_be = 0;
1723         pstat->uapsd_bk = 0;
1724         if (pmlmepriv->qospriv.qos_option)
1725         {
1726                 p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
1727                 for (;;)
1728                 {
1729                         p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
1730                         if (p != NULL) {
1731                                 if (!memcmp(p+2, WMM_IE, 6)) {
1732
1733                                         pstat->flags |= WLAN_STA_WME;
1734
1735                                         pstat->qos_option = 1;
1736                                         pstat->qos_info = *(p+8);
1737
1738                                         pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
1739
1740                                         if((pstat->qos_info&0xf) !=0xf)
1741                                                 pstat->has_legacy_ac = _TRUE;
1742                                         else
1743                                                 pstat->has_legacy_ac = _FALSE;
1744
1745                                         if(pstat->qos_info&0xf)
1746                                         {
1747                                                 if(pstat->qos_info&BIT(0))
1748                                                         pstat->uapsd_vo = BIT(0)|BIT(1);
1749                                                 else
1750                                                         pstat->uapsd_vo = 0;
1751
1752                                                 if(pstat->qos_info&BIT(1))
1753                                                         pstat->uapsd_vi = BIT(0)|BIT(1);
1754                                                 else
1755                                                         pstat->uapsd_vi = 0;
1756
1757                                                 if(pstat->qos_info&BIT(2))
1758                                                         pstat->uapsd_bk = BIT(0)|BIT(1);
1759                                                 else
1760                                                         pstat->uapsd_bk = 0;
1761
1762                                                 if(pstat->qos_info&BIT(3))
1763                                                         pstat->uapsd_be = BIT(0)|BIT(1);
1764                                                 else
1765                                                         pstat->uapsd_be = 0;
1766
1767                                         }
1768
1769                                         break;
1770                                 }
1771                         }
1772                         else {
1773                                 break;
1774                         }
1775                         p = p + ie_len + 2;
1776                 }
1777         }
1778
1779 #ifdef CONFIG_80211N_HT
1780         /* save HT capabilities in the sta object */
1781         memset(&pstat->htpriv.ht_cap, 0, sizeof(struct ieee80211_ht_cap));
1782         if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct ieee80211_ht_cap))
1783         {
1784                 pstat->flags |= WLAN_STA_HT;
1785
1786                 pstat->flags |= WLAN_STA_WME;
1787
1788                 memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct ieee80211_ht_cap));
1789
1790         } else
1791                 pstat->flags &= ~WLAN_STA_HT;
1792
1793         if((pmlmepriv->htpriv.ht_option == _FALSE) && (pstat->flags&WLAN_STA_HT))
1794         {
1795                 status = _STATS_FAILURE_;
1796                 goto OnAssocReqFail;
1797         }
1798
1799         if ((pstat->flags & WLAN_STA_HT) &&
1800                     ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
1801                       (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP)))
1802         {
1803                 DBG_8723A("HT: " MAC_FMT " tried to "
1804                                    "use TKIP with HT association\n", MAC_ARG(pstat->hwaddr));
1805
1806                 /* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
1807                 /* goto OnAssocReqFail; */
1808         }
1809 #endif /* CONFIG_80211N_HT */
1810
1811        /*  */
1812         pstat->flags |= WLAN_STA_NONERP;
1813         for (i = 0; i < pstat->bssratelen; i++) {
1814                 if ((pstat->bssrateset[i] & 0x7f) > 22) {
1815                         pstat->flags &= ~WLAN_STA_NONERP;
1816                         break;
1817                 }
1818         }
1819
1820         if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
1821                 pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
1822         else
1823                 pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
1824
1825         if (status != _STATS_SUCCESSFUL_)
1826                 goto OnAssocReqFail;
1827
1828 #ifdef CONFIG_P2P
1829         pstat->is_p2p_device = _FALSE;
1830         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
1831         {
1832                 if( (p2pie=rtw_get_p2p_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , NULL, &p2pielen)))
1833                 {
1834                         pstat->is_p2p_device = _TRUE;
1835                         if((p2p_status_code=(u8)process_assoc_req_p2p_ie(pwdinfo, pframe, pkt_len, pstat))>0)
1836                         {
1837                                 pstat->p2p_status_code = p2p_status_code;
1838                                 status = _STATS_CAP_FAIL_;
1839                                 goto OnAssocReqFail;
1840                         }
1841                 }
1842 #ifdef CONFIG_WFD
1843                 if(rtw_get_wfd_ie(pframe + WLAN_HDR_A3_LEN + ie_offset , pkt_len - WLAN_HDR_A3_LEN - ie_offset , wfd_ie, &wfd_ielen ))
1844                 {
1845                         u8      attr_content[ 10 ] = { 0x00 };
1846                         u32     attr_contentlen = 0;
1847
1848                         DBG_8723A( "[%s] WFD IE Found!!\n", __FUNCTION__ );
1849                         rtw_get_wfd_attr_content( wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
1850                         if ( attr_contentlen )
1851                         {
1852                                 pwdinfo->wfd_info->peer_rtsp_ctrlport = RTW_GET_BE16( attr_content + 2 );
1853                                 DBG_8723A( "[%s] Peer PORT NUM = %d\n", __FUNCTION__, pwdinfo->wfd_info->peer_rtsp_ctrlport );
1854                         }
1855                 }
1856 #endif
1857         }
1858         pstat->p2p_status_code = p2p_status_code;
1859 #endif /* CONFIG_P2P */
1860
1861         /* TODO: identify_proprietary_vendor_ie(); */
1862         /*  Realtek proprietary IE */
1863         /*  identify if this is Broadcom sta */
1864         /*  identify if this is ralink sta */
1865         /*  Customer proprietary IE */
1866
1867         /* get a unique AID */
1868         if (pstat->aid > 0) {
1869                 DBG_8723A("  old AID %d\n", pstat->aid);
1870         } else {
1871                 for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
1872                         if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
1873                                 break;
1874
1875                 if (pstat->aid > NUM_STA)
1876                         pstat->aid = NUM_STA;
1877                 if (pstat->aid > pstapriv->max_num_sta) {
1878
1879                         pstat->aid = 0;
1880
1881                         DBG_8723A("  no room for more AIDs\n");
1882
1883                         status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
1884
1885                         goto OnAssocReqFail;
1886
1887                 } else {
1888                         pstapriv->sta_aid[pstat->aid - 1] = pstat;
1889                         DBG_8723A("allocate new AID = (%d)\n", pstat->aid);
1890                 }
1891         }
1892
1893         pstat->state &= (~WIFI_FW_ASSOC_STATE);
1894         pstat->state |= WIFI_FW_ASSOC_SUCCESS;
1895
1896         spin_lock_bh(&pstapriv->auth_list_lock);
1897         if (!rtw_is_list_empty(&pstat->auth_list))
1898         {
1899                 rtw_list_delete(&pstat->auth_list);
1900                 pstapriv->auth_list_cnt--;
1901         }
1902         spin_unlock_bh(&pstapriv->auth_list_lock);
1903
1904         spin_lock_bh(&pstapriv->asoc_list_lock);
1905         if (rtw_is_list_empty(&pstat->asoc_list))
1906         {
1907                 pstat->expire_to = pstapriv->expire_to;
1908                 rtw_list_insert_tail(&pstat->asoc_list, &pstapriv->asoc_list);
1909                 pstapriv->asoc_list_cnt++;
1910         }
1911         spin_unlock_bh(&pstapriv->asoc_list_lock);
1912
1913         /*  now the station is qualified to join our BSS... */
1914         if(pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_==status))
1915         {
1916 #ifdef CONFIG_NATIVEAP_MLME
1917                 /* 1 bss_cap_update & sta_info_update */
1918                 bss_cap_update_on_sta_join(padapter, pstat);
1919                 sta_info_update(padapter, pstat);
1920
1921                 /* issue assoc rsp before notify station join event. */
1922                 if (frame_type == WIFI_ASSOCREQ)
1923                         issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
1924                 else
1925                         issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
1926
1927                 /* 2 - report to upper layer */
1928                 DBG_8723A("indicate_sta_join_event to upper layer - hostapd\n");
1929                 #ifdef CONFIG_IOCTL_CFG80211
1930                 if (1) {
1931                         #ifdef COMPAT_KERNEL_RELEASE
1932                         rtw_cfg80211_indicate_sta_assoc(padapter, pframe, pkt_len);
1933                         #elif (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER)
1934                         rtw_cfg80211_indicate_sta_assoc(padapter, pframe, pkt_len);
1935                         #else /* LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER) */
1936                         spin_lock_bh(&pstat->lock);
1937                         if(pstat->passoc_req)
1938                         {
1939                                 rtw_mfree(pstat->passoc_req, pstat->assoc_req_len);
1940                                 pstat->passoc_req = NULL;
1941                                 pstat->assoc_req_len = 0;
1942                         }
1943
1944                         pstat->passoc_req =  rtw_zmalloc(pkt_len);
1945                         if(pstat->passoc_req)
1946                         {
1947                                 memcpy(pstat->passoc_req, pframe, pkt_len);
1948                                 pstat->assoc_req_len = pkt_len;
1949                         }
1950                         spin_unlock_bh(&pstat->lock);
1951                         #endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37)) && !defined(CONFIG_CFG80211_FORCE_COMPATIBLE_2_6_37_UNDER) */
1952                 }
1953                 else
1954                 #endif /* CONFIG_IOCTL_CFG80211 */
1955                 {
1956                         rtw_indicate_sta_assoc_event(padapter, pstat);
1957                 }
1958
1959                 /* 3-(1) report sta add event */
1960                 report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
1961 #endif
1962         }
1963
1964         return _SUCCESS;
1965
1966 asoc_class2_error:
1967
1968 #ifdef CONFIG_NATIVEAP_MLME
1969         issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
1970 #endif
1971
1972         return _FAIL;
1973
1974 OnAssocReqFail:
1975
1976 #ifdef CONFIG_NATIVEAP_MLME
1977         pstat->aid = 0;
1978         if (frame_type == WIFI_ASSOCREQ)
1979                 issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
1980         else
1981                 issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
1982 #endif
1983
1984 #endif /* CONFIG_AP_MODE */
1985
1986         return _FAIL;
1987 }
1988
1989 unsigned int OnAssocRsp(_adapter *padapter, union recv_frame *precv_frame)
1990 {
1991         uint i;
1992         int res;
1993         unsigned short  status;
1994         PNDIS_802_11_VARIABLE_IEs       pIE;
1995         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1996         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1997         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1998         /* WLAN_BSSID_EX                        *cur_network = &(pmlmeinfo->network); */
1999         u8 *pframe = precv_frame->u.hdr.rx_data;
2000         uint pkt_len = precv_frame->u.hdr.len;
2001         PNDIS_802_11_VARIABLE_IEs       pWapiIE = NULL;
2002
2003         DBG_8723A("%s\n", __FUNCTION__);
2004
2005         /* check A1 matches or not */
2006         if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
2007                 return _SUCCESS;
2008
2009         if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
2010                 return _SUCCESS;
2011
2012         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
2013                 return _SUCCESS;
2014
2015         _cancel_timer_ex(&pmlmeext->link_timer);
2016
2017         /* status */
2018         if ((status = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 2))) > 0)
2019         {
2020                 DBG_8723A("assoc reject, status code: %d\n", status);
2021                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
2022                 res = -4;
2023                 goto report_assoc_result;
2024         }
2025
2026         /* get capabilities */
2027         pmlmeinfo->capability = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2028
2029         /* set slot time */
2030         pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10))? 9: 20;
2031
2032         /* AID */
2033         res = pmlmeinfo->aid = (int)(le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
2034
2035         /* following are moved to join event callback function */
2036         /* to handle HT, WMM, rate adaptive, update MAC reg */
2037         /* for not to handle the synchronous IO in the tasklet */
2038         for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;)
2039         {
2040                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pframe + i);
2041
2042                 switch (pIE->ElementID)
2043                 {
2044                         case _VENDOR_SPECIFIC_IE_:
2045                                 if (!memcmp(pIE->data, WMM_PARA_OUI, 6))/* WMM */
2046                                 {
2047                                         WMM_param_handler(padapter, pIE);
2048                                 }
2049 #if defined(CONFIG_P2P) && defined(CONFIG_WFD)
2050                                 else if (!memcmp(pIE->data, WFD_OUI, 4))/* WFD */
2051                                 {
2052                                         DBG_8723A( "[%s] Found WFD IE\n", __FUNCTION__ );
2053                                         WFD_info_handler( padapter, pIE );
2054                                 }
2055 #endif
2056                                 break;
2057
2058 #ifdef CONFIG_WAPI_SUPPORT
2059                         case _WAPI_IE_:
2060                                 pWapiIE = pIE;
2061                                 break;
2062 #endif
2063
2064                         case _HT_CAPABILITY_IE_:        /* HT caps */
2065                                 HT_caps_handler(padapter, pIE);
2066                                 break;
2067
2068                         case _HT_EXTRA_INFO_IE_:        /* HT info */
2069                                 HT_info_handler(padapter, pIE);
2070                                 break;
2071
2072                         case _ERPINFO_IE_:
2073                                 ERP_IE_handler(padapter, pIE);
2074
2075                         default:
2076                                 break;
2077                 }
2078
2079                 i += (pIE->Length + 2);
2080         }
2081
2082 #ifdef CONFIG_WAPI_SUPPORT
2083         rtw_wapi_on_assoc_ok(padapter, pIE);
2084 #endif
2085
2086         pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
2087         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
2088
2089         /* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
2090         UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);
2091
2092 report_assoc_result:
2093         if (res > 0) {
2094                 rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
2095         } else {
2096                 rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
2097         }
2098
2099         report_join_res(padapter, res);
2100
2101         return _SUCCESS;
2102 }
2103
2104 unsigned int OnDeAuth(_adapter *padapter, union recv_frame *precv_frame)
2105 {
2106         unsigned short  reason;
2107         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2108         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2109         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2110         u8 *pframe = precv_frame->u.hdr.rx_data;
2111 #ifdef CONFIG_P2P
2112         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
2113 #endif /* CONFIG_P2P */
2114
2115         /* check A3 */
2116         if (memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network),
2117                    ETH_ALEN))
2118                 return _SUCCESS;
2119
2120 #ifdef CONFIG_P2P
2121         if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
2122         {
2123                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
2124                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
2125         }
2126 #endif /* CONFIG_P2P */
2127
2128         reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2129
2130         DBG_8723A("%s Reason code(%d)\n", __FUNCTION__,reason);
2131
2132 #ifdef CONFIG_AP_MODE
2133         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
2134         {
2135                 _irqL irqL;
2136                 struct sta_info *psta;
2137                 struct sta_priv *pstapriv = &padapter->stapriv;
2138
2139                 /* spin_lock_bh(&(pstapriv->sta_hash_lock)); */
2140                 /* rtw_free_stainfo(padapter, psta); */
2141                 /* spin_unlock_bh(&(pstapriv->sta_hash_lock)); */
2142
2143                 DBG_8723A_LEVEL(_drv_always_, "ap recv deauth reason code(%d) sta:%pM\n",
2144                                 reason, GetAddr2Ptr(pframe));
2145
2146                 psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2147                 if(psta)
2148                 {
2149                         u8 updated;
2150
2151                         spin_lock_bh(&pstapriv->asoc_list_lock);
2152                         if(rtw_is_list_empty(&psta->asoc_list)==_FALSE)
2153                         {
2154                                 rtw_list_delete(&psta->asoc_list);
2155                                 pstapriv->asoc_list_cnt--;
2156                                 updated = ap_free_sta(padapter, psta, _FALSE, reason);
2157
2158                         }
2159                         spin_unlock_bh(&pstapriv->asoc_list_lock);
2160
2161                         associated_clients_update(padapter, updated);
2162                 }
2163
2164                 return _SUCCESS;
2165         }
2166         else
2167 #endif
2168         {
2169                 DBG_8723A_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM\n",
2170                                 reason, GetAddr3Ptr(pframe));
2171
2172                 receive_disconnect(padapter, GetAddr3Ptr(pframe) ,reason);
2173         }
2174         pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE;
2175         return _SUCCESS;
2176 }
2177
2178 unsigned int OnDisassoc(_adapter *padapter, union recv_frame *precv_frame)
2179 {
2180         unsigned short  reason;
2181         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2182         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2183         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2184         u8 *pframe = precv_frame->u.hdr.rx_data;
2185 #ifdef CONFIG_P2P
2186         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
2187 #endif /* CONFIG_P2P */
2188
2189         /* check A3 */
2190         if (memcmp(GetAddr3Ptr(pframe), get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
2191                 return _SUCCESS;
2192
2193 #ifdef CONFIG_P2P
2194         if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
2195         {
2196                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
2197                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
2198         }
2199 #endif /* CONFIG_P2P */
2200
2201         reason = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN));
2202
2203         DBG_8723A("%s Reason code(%d)\n", __FUNCTION__,reason);
2204
2205 #ifdef CONFIG_AP_MODE
2206         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
2207         {
2208                 _irqL irqL;
2209                 struct sta_info *psta;
2210                 struct sta_priv *pstapriv = &padapter->stapriv;
2211
2212                 /* spin_lock_bh(&(pstapriv->sta_hash_lock)); */
2213                 /* rtw_free_stainfo(padapter, psta); */
2214                 /* spin_unlock_bh(&(pstapriv->sta_hash_lock)); */
2215
2216                 DBG_8723A_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
2217                                 reason, GetAddr2Ptr(pframe));
2218
2219                 psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2220                 if(psta)
2221                 {
2222                         u8 updated;
2223
2224                         spin_lock_bh(&pstapriv->asoc_list_lock);
2225                         if(rtw_is_list_empty(&psta->asoc_list)==_FALSE)
2226                         {
2227                                 rtw_list_delete(&psta->asoc_list);
2228                                 pstapriv->asoc_list_cnt--;
2229                                 updated = ap_free_sta(padapter, psta, _FALSE, reason);
2230
2231                         }
2232                         spin_unlock_bh(&pstapriv->asoc_list_lock);
2233
2234                         associated_clients_update(padapter, updated);
2235                 }
2236
2237                 return _SUCCESS;
2238         }
2239         else
2240 #endif
2241         {
2242                 DBG_8723A_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
2243                                 reason, GetAddr3Ptr(pframe));
2244
2245                 receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
2246         }
2247         pmlmepriv->LinkDetectInfo.bBusyTraffic = _FALSE;
2248         return _SUCCESS;
2249 }
2250
2251 unsigned int OnAtim(_adapter *padapter, union recv_frame *precv_frame)
2252 {
2253         DBG_8723A("%s\n", __FUNCTION__);
2254         return _SUCCESS;
2255 }
2256
2257 unsigned int on_action_spct_ch_switch(_adapter *padapter, struct sta_info *psta, u8 *ies, uint ies_len)
2258 {
2259         unsigned int ret = _FAIL;
2260         struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
2261         struct mlme_ext_info    *pmlmeinfo = &(mlmeext->mlmext_info);
2262
2263         if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
2264                 ret = _SUCCESS;
2265                 goto exit;
2266         }
2267
2268         if ((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) {
2269
2270                 int ch_switch_mode = -1, ch = -1, ch_switch_cnt = -1;
2271                 int ch_offset = -1;
2272                 u8 bwmode;
2273                 struct ieee80211_info_element *ie;
2274
2275                 DBG_8723A(FUNC_NDEV_FMT" from "MAC_FMT"\n",
2276                         FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(psta->hwaddr));
2277
2278                 for_each_ie(ie, ies, ies_len) {
2279                         if (ie->id == WLAN_EID_CHANNEL_SWITCH) {
2280                                 ch_switch_mode = ie->data[0];
2281                                 ch = ie->data[1];
2282                                 ch_switch_cnt = ie->data[2];
2283                                 DBG_8723A("ch_switch_mode:%d, ch:%d, ch_switch_cnt:%d\n",
2284                                         ch_switch_mode, ch, ch_switch_cnt);
2285                         }
2286                         else if (ie->id == WLAN_EID_SECONDARY_CHANNEL_OFFSET) {
2287                                 ch_offset = secondary_ch_offset_to_hal_ch_offset(ie->data[0]);
2288                                 DBG_8723A("ch_offset:%d\n", ch_offset);
2289                         }
2290                 }
2291
2292                 if (ch == -1)
2293                         return _SUCCESS;
2294
2295                 if (ch_offset == -1)
2296                         bwmode = mlmeext->cur_bwmode;
2297                 else
2298                         bwmode = (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) ?
2299                                 HT_CHANNEL_WIDTH_20 : HT_CHANNEL_WIDTH_40;
2300
2301                 ch_offset = (ch_offset == -1) ? mlmeext->cur_ch_offset : ch_offset;
2302
2303                 /* todo:
2304                  * 1. the decision of channel switching
2305                  * 2. things after channel switching
2306                  */
2307
2308                 ret = rtw_set_ch_cmd(padapter, ch, bwmode, ch_offset, _TRUE);
2309         }
2310
2311 exit:
2312         return ret;
2313 }
2314
2315 unsigned int on_action_spct(_adapter *padapter, union recv_frame *precv_frame)
2316 {
2317         unsigned int ret = _FAIL;
2318         struct sta_info *psta = NULL;
2319         struct sta_priv *pstapriv = &padapter->stapriv;
2320         u8 *pframe = precv_frame->u.hdr.rx_data;
2321         uint frame_len = precv_frame->u.hdr.len;
2322         u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
2323         u8 category;
2324         u8 action;
2325
2326         DBG_8723A(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
2327
2328         psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
2329
2330         if (!psta)
2331                 goto exit;
2332
2333         category = frame_body[0];
2334         if(category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
2335                 goto exit;
2336
2337         action = frame_body[1];
2338         switch (action) {
2339         case RTW_WLAN_ACTION_SPCT_MSR_REQ:
2340         case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
2341         case RTW_WLAN_ACTION_SPCT_TPC_REQ:
2342         case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
2343                 break;
2344         case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
2345                 #ifdef CONFIG_SPCT_CH_SWITCH
2346                 ret = on_action_spct_ch_switch(padapter, psta, &frame_body[2],
2347                         frame_len-(frame_body-pframe)-2);
2348                 #endif
2349                 break;
2350         default:
2351                 break;
2352         }
2353
2354 exit:
2355         return ret;
2356 }
2357
2358 unsigned int OnAction_qos(_adapter *padapter, union recv_frame *precv_frame)
2359 {
2360         return _SUCCESS;
2361 }
2362
2363 unsigned int OnAction_dls(_adapter *padapter, union recv_frame *precv_frame)
2364 {
2365         return _SUCCESS;
2366 }
2367
2368 unsigned int OnAction_back(_adapter *padapter, union recv_frame *precv_frame)
2369 {
2370         u8 *addr;
2371         struct sta_info *psta=NULL;
2372         struct recv_reorder_ctrl *preorder_ctrl;
2373         unsigned char           *frame_body;
2374         unsigned char           category, action;
2375         unsigned short  tid, status, reason_code = 0;
2376         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2377         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2378         u8 *pframe = precv_frame->u.hdr.rx_data;
2379         struct sta_priv *pstapriv = &padapter->stapriv;
2380 #ifdef CONFIG_80211N_HT
2381         /* check RA matches or not */
2382         if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))/* for if1, sta/ap mode */
2383                 return _SUCCESS;
2384
2385         DBG_8723A("%s\n", __FUNCTION__);
2386
2387         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
2388                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
2389                         return _SUCCESS;
2390
2391         addr = GetAddr2Ptr(pframe);
2392         psta = rtw_get_stainfo(pstapriv, addr);
2393
2394         if(psta==NULL)
2395                 return _SUCCESS;
2396
2397         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
2398
2399         category = frame_body[0];
2400         if (category == RTW_WLAN_CATEGORY_BACK)/*  representing Block Ack */
2401         {
2402 #ifdef CONFIG_TDLS
2403                 if((psta->tdls_sta_state & TDLS_LINKED_STATE) &&
2404                         (psta->htpriv.ht_option==_TRUE) &&
2405                         (psta->htpriv.ampdu_enable==_TRUE) )
2406                 {
2407                         /* do nothing; just don't want to return _SUCCESS; */
2408                 } else
2409 #endif /* CONFIG_TDLS */
2410                 if (!pmlmeinfo->HT_enable)
2411                         return _SUCCESS;
2412                 action = frame_body[1];
2413                 DBG_8723A("%s, action=%d\n", __FUNCTION__, action);
2414                 switch (action) {
2415                 case RTW_WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
2416                         memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
2417                         /* process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), GetAddr3Ptr(pframe)); */
2418                         process_addba_req(padapter, (u8*)&(pmlmeinfo->ADDBA_req), addr);
2419
2420                         if(pmlmeinfo->bAcceptAddbaReq == _TRUE)
2421                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0);
2422                         else
2423                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);/* reject ADDBA Req */
2424                         break;
2425                 case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
2426                         status = RTW_GET_LE16(&frame_body[3]);
2427                         tid = ((frame_body[5] >> 2) & 0x7);
2428                         if (status == 0) {      /* successful */
2429                                 DBG_8723A("agg_enable for TID=%d\n", tid);
2430                                 psta->htpriv.agg_enable_bitmap |= 1 << tid;
2431                                 psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
2432                         } else {
2433                                 psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
2434                         }
2435
2436                         break;
2437                 case RTW_WLAN_ACTION_DELBA: /* DELBA */
2438                         if ((frame_body[3] & BIT(3)) == 0) {
2439                                 psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
2440                                 psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
2441
2442                                 /* reason_code = frame_body[4] | (frame_body[5] << 8); */
2443                                 reason_code = RTW_GET_LE16(&frame_body[4]);
2444                         } else if((frame_body[3] & BIT(3)) == BIT(3)) {
2445                                 tid = (frame_body[3] >> 4) & 0x0F;
2446
2447                                 preorder_ctrl =  &psta->recvreorder_ctrl[tid];
2448                                 preorder_ctrl->enable = _FALSE;
2449                                 preorder_ctrl->indicate_seq = 0xffff;
2450                                 #ifdef DBG_RX_SEQ
2451                                 DBG_8723A("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
2452                                         preorder_ctrl->indicate_seq);
2453                                 #endif
2454                         }
2455
2456                         DBG_8723A("%s(): DELBA: %x(%x)\n", __FUNCTION__,pmlmeinfo->agg_enable_bitmap, reason_code);
2457                         /* todo: how to notify the host while receiving DELETE BA */
2458                         break;
2459                 default:
2460                         break;
2461                 }
2462         }
2463 #endif /* CONFIG_80211N_HT */
2464         return _SUCCESS;
2465 }
2466
2467 #ifdef CONFIG_P2P
2468
2469 static int get_reg_classes_full_count(struct p2p_channels channel_list) {
2470         int cnt = 0;
2471         int i;
2472
2473         for (i = 0; i < channel_list.reg_classes; i++) {
2474                 cnt += channel_list.reg_class[i].channels;
2475         }
2476
2477         return cnt;
2478 }
2479
2480 static void get_channel_cnt_24g_5gl_5gh(  struct mlme_ext_priv *pmlmeext, u8* p24g_cnt, u8* p5gl_cnt, u8* p5gh_cnt )
2481 {
2482         int     i = 0;
2483
2484         *p24g_cnt = 0;
2485         *p5gl_cnt = 0;
2486         *p5gh_cnt = 0;
2487
2488         for( i = 0; i < pmlmeext->max_chan_nums; i++ )
2489         {
2490                 if ( pmlmeext->channel_set[ i ].ChannelNum <= 14 )
2491                 {
2492                         (*p24g_cnt)++;
2493                 }
2494                 else if ( ( pmlmeext->channel_set[ i ].ChannelNum > 14 ) && ( pmlmeext->channel_set[ i ].ChannelNum <= 48 ) )
2495                 {
2496                         /*      Just include the channel 36, 40, 44, 48 channels for 5G low */
2497                         (*p5gl_cnt)++;
2498                 }
2499                 else if ( ( pmlmeext->channel_set[ i ].ChannelNum >= 149 ) && ( pmlmeext->channel_set[ i ].ChannelNum <= 161 ) )
2500                 {
2501                         /*      Just include the channel 149, 153, 157, 161 channels for 5G high */
2502                         (*p5gh_cnt)++;
2503                 }
2504         }
2505 }
2506
2507 void issue_p2p_GO_request(_adapter *padapter, u8* raddr)
2508 {
2509
2510         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
2511         u8                      action = P2P_PUB_ACTION_ACTION;
2512         u32                     p2poui = cpu_to_be32(P2POUI);
2513         u8                      oui_subtype = P2P_GO_NEGO_REQ;
2514         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
2515         u8                      wpsielen = 0, p2pielen = 0, i;
2516         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
2517         u16                     len_channellist_attr = 0;
2518 #ifdef CONFIG_WFD
2519         u32                                     wfdielen = 0;
2520 #endif /* CONFIG_WFD */
2521
2522         struct xmit_frame                       *pmgntframe;
2523         struct pkt_attrib                       *pattrib;
2524         unsigned char                                   *pframe;
2525         struct rtw_ieee80211_hdr        *pwlanhdr;
2526         unsigned short                          *fctrl;
2527         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
2528         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
2529         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2530         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
2531
2532         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
2533         {
2534                 return;
2535         }
2536
2537         DBG_8723A( "[%s] In\n", __FUNCTION__ );
2538         /* update attribute */
2539         pattrib = &pmgntframe->attrib;
2540         update_mgntframe_attrib(padapter, pattrib);
2541
2542         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2543
2544         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2545         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2546
2547         fctrl = &(pwlanhdr->frame_ctl);
2548         *(fctrl) = 0;
2549
2550         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
2551         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
2552         memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
2553
2554         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
2555         pmlmeext->mgnt_seq++;
2556         SetFrameSubType(pframe, WIFI_ACTION);
2557
2558         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
2559         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
2560
2561         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
2562         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
2563         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
2564         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
2565         pwdinfo->negotiation_dialog_token = 1;  /*      Initialize the dialog value */
2566         pframe = rtw_set_fixed_ie(pframe, 1, &pwdinfo->negotiation_dialog_token, &(pattrib->pktlen));
2567
2568         /*      WPS Section */
2569         wpsielen = 0;
2570         /*      WPS OUI */
2571         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
2572         wpsielen += 4;
2573
2574         /*      WPS version */
2575         /*      Type: */
2576         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
2577         wpsielen += 2;
2578
2579         /*      Length: */
2580         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
2581         wpsielen += 2;
2582
2583         /*      Value: */
2584         wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */
2585
2586         /*      Device Password ID */
2587         /*      Type: */
2588         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
2589         wpsielen += 2;
2590
2591         /*      Length: */
2592         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
2593         wpsielen += 2;
2594
2595         /*      Value: */
2596
2597         if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN )
2598         {
2599                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_USER_SPEC );
2600         }
2601         else if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN )
2602         {
2603                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );
2604         }
2605         else if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC )
2606         {
2607                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_PBC );
2608         }
2609
2610         wpsielen += 2;
2611
2612         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
2613
2614         /*      P2P IE Section. */
2615
2616         /*      P2P OUI */
2617         p2pielen = 0;
2618         p2pie[ p2pielen++ ] = 0x50;
2619         p2pie[ p2pielen++ ] = 0x6F;
2620         p2pie[ p2pielen++ ] = 0x9A;
2621         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
2622
2623         /*      Commented by Albert 20110306 */
2624         /*      According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes */
2625         /*      1. P2P Capability */
2626         /*      2. Group Owner Intent */
2627         /*      3. Configuration Timeout */
2628         /*      4. Listen Channel */
2629         /*      5. Extended Listen Timing */
2630         /*      6. Intended P2P Interface Address */
2631         /*      7. Channel List */
2632         /*      8. P2P Device Info */
2633         /*      9. Operating Channel */
2634
2635         /*      P2P Capability */
2636         /*      Type: */
2637         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
2638
2639         /*      Length: */
2640         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
2641         p2pielen += 2;
2642
2643         /*      Value: */
2644         /*      Device Capability Bitmap, 1 byte */
2645         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
2646
2647         /*      Group Capability Bitmap, 1 byte */
2648         if ( pwdinfo->persistent_supported )
2649         {
2650                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
2651         }
2652         else
2653         {
2654                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
2655         }
2656
2657         /*      Group Owner Intent */
2658         /*      Type: */
2659         p2pie[ p2pielen++ ] = P2P_ATTR_GO_INTENT;
2660
2661         /*      Length: */
2662         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
2663         p2pielen += 2;
2664
2665         /*      Value: */
2666         /*      Todo the tie breaker bit. */
2667         p2pie[ p2pielen++ ] = ( ( pwdinfo->intent << 1 ) | BIT(0) );
2668
2669         /*      Configuration Timeout */
2670         /*      Type: */
2671         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
2672
2673         /*      Length: */
2674         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
2675         p2pielen += 2;
2676
2677         /*      Value: */
2678         p2pie[ p2pielen++ ] = 200;      /*      2 seconds needed to be the P2P GO */
2679         p2pie[ p2pielen++ ] = 200;      /*      2 seconds needed to be the P2P Client */
2680
2681         /*      Listen Channel */
2682         /*      Type: */
2683         p2pie[ p2pielen++ ] = P2P_ATTR_LISTEN_CH;
2684
2685         /*      Length: */
2686         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
2687         p2pielen += 2;
2688
2689         /*      Value: */
2690         /*      Country String */
2691         p2pie[ p2pielen++ ] = 'X';
2692         p2pie[ p2pielen++ ] = 'X';
2693
2694         /*      The third byte should be set to 0x04. */
2695         /*      Described in the "Operating Channel Attribute" section. */
2696         p2pie[ p2pielen++ ] = 0x04;
2697
2698         /*      Operating Class */
2699         p2pie[ p2pielen++ ] = 0x51;     /*      Copy from SD7 */
2700
2701         /*      Channel Number */
2702         p2pie[ p2pielen++ ] = pwdinfo->listen_channel;  /*      listening channel number */
2703
2704         /*      Extended Listen Timing ATTR */
2705         /*      Type: */
2706         p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
2707
2708         /*      Length: */
2709         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
2710         p2pielen += 2;
2711
2712         /*      Value: */
2713         /*      Availability Period */
2714         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
2715         p2pielen += 2;
2716
2717         /*      Availability Interval */
2718         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
2719         p2pielen += 2;
2720
2721         /*      Intended P2P Interface Address */
2722         /*      Type: */
2723         p2pie[ p2pielen++ ] = P2P_ATTR_INTENTED_IF_ADDR;
2724
2725         /*      Length: */
2726         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
2727         p2pielen += 2;
2728
2729         /*      Value: */
2730         memcpy(p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
2731         p2pielen += ETH_ALEN;
2732
2733         /*      Channel List */
2734         /*      Type: */
2735         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
2736
2737         /*  Length: */
2738         /*  Country String(3) */
2739         /*  + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */
2740         /*  + number of channels in all classes */
2741         len_channellist_attr = 3
2742            + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
2743            + get_reg_classes_full_count(pmlmeext->channel_list);
2744
2745 #ifdef CONFIG_CONCURRENT_MODE
2746         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
2747         {
2748                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
2749         }
2750         else
2751         {
2752                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
2753         }
2754 #else
2755
2756         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
2757
2758 #endif
2759         p2pielen += 2;
2760
2761         /*      Value: */
2762         /*      Country String */
2763         p2pie[ p2pielen++ ] = 'X';
2764         p2pie[ p2pielen++ ] = 'X';
2765
2766         /*      The third byte should be set to 0x04. */
2767         /*      Described in the "Operating Channel Attribute" section. */
2768         p2pie[ p2pielen++ ] = 0x04;
2769
2770         /*      Channel Entry List */
2771
2772 #ifdef CONFIG_CONCURRENT_MODE
2773         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
2774         {
2775                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
2776                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
2777
2778                 /*      Operating Class */
2779                 if ( pbuddy_mlmeext->cur_channel > 14 )
2780                 {
2781                         if ( pbuddy_mlmeext->cur_channel >= 149 )
2782                         {
2783                                 p2pie[ p2pielen++ ] = 0x7c;
2784                         }
2785                         else
2786                         {
2787                                 p2pie[ p2pielen++ ] = 0x73;
2788                         }
2789                 }
2790                 else
2791                 {
2792                         p2pie[ p2pielen++ ] = 0x51;
2793                 }
2794
2795                 /*      Number of Channels */
2796                 /*      Just support 1 channel and this channel is AP's channel */
2797                 p2pie[ p2pielen++ ] = 1;
2798
2799                 /*      Channel List */
2800                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
2801         }
2802         else
2803         {
2804                 int i,j;
2805                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
2806                         /*      Operating Class */
2807                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
2808
2809                         /*      Number of Channels */
2810                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
2811
2812                         /*      Channel List */
2813                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
2814                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
2815                         }
2816                 }
2817         }
2818 #else /*  CONFIG_CONCURRENT_MODE */
2819         {
2820                 int i,j;
2821                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
2822                         /*      Operating Class */
2823                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
2824
2825                         /*      Number of Channels */
2826                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
2827
2828                         /*      Channel List */
2829                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
2830                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
2831                         }
2832                 }
2833         }
2834 #endif /*  CONFIG_CONCURRENT_MODE */
2835
2836         /*      Device Info */
2837         /*      Type: */
2838         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
2839
2840         /*      Length: */
2841         /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
2842         /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
2843         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
2844         p2pielen += 2;
2845
2846         /*      Value: */
2847         /*      P2P Device Address */
2848         memcpy(p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
2849         p2pielen += ETH_ALEN;
2850
2851         /*      Config Method */
2852         /*      This field should be big endian. Noted by P2P specification. */
2853
2854         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
2855
2856         p2pielen += 2;
2857
2858         /*      Primary Device Type */
2859         /*      Category ID */
2860         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
2861         p2pielen += 2;
2862
2863         /*      OUI */
2864         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
2865         p2pielen += 4;
2866
2867         /*      Sub Category ID */
2868         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
2869         p2pielen += 2;
2870
2871         /*      Number of Secondary Device Types */
2872         p2pie[ p2pielen++ ] = 0x00;     /*      No Secondary Device Type List */
2873
2874         /*      Device Name */
2875         /*      Type: */
2876         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
2877         p2pielen += 2;
2878
2879         /*      Length: */
2880         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
2881         p2pielen += 2;
2882
2883         /*      Value: */
2884         memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len );
2885         p2pielen += pwdinfo->device_name_len;
2886
2887         /*      Operating Channel */
2888         /*      Type: */
2889         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
2890
2891         /*      Length: */
2892         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
2893         p2pielen += 2;
2894
2895         /*      Value: */
2896         /*      Country String */
2897         p2pie[ p2pielen++ ] = 'X';
2898         p2pie[ p2pielen++ ] = 'X';
2899
2900         /*      The third byte should be set to 0x04. */
2901         /*      Described in the "Operating Channel Attribute" section. */
2902         p2pie[ p2pielen++ ] = 0x04;
2903
2904         /*      Operating Class */
2905         if ( pwdinfo->operating_channel <= 14 )
2906         {
2907                 /*      Operating Class */
2908                 p2pie[ p2pielen++ ] = 0x51;
2909         }
2910         else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
2911         {
2912                 /*      Operating Class */
2913                 p2pie[ p2pielen++ ] = 0x73;
2914         }
2915         else
2916         {
2917                 /*      Operating Class */
2918                 p2pie[ p2pielen++ ] = 0x7c;
2919         }
2920
2921         /*      Channel Number */
2922         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       /*      operating channel number */
2923
2924         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
2925
2926 #ifdef CONFIG_WFD
2927         wfdielen = build_nego_req_wfd_ie(pwdinfo, pframe);
2928         pframe += wfdielen;
2929         pattrib->pktlen += wfdielen;
2930 #endif /* CONFIG_WFD */
2931
2932         pattrib->last_txcmdsz = pattrib->pktlen;
2933
2934         dump_mgntframe(padapter, pmgntframe);
2935
2936         return;
2937 }
2938
2939 void issue_p2p_GO_response(_adapter *padapter, u8* raddr, u8* frame_body,uint len, u8 result)
2940 {
2941
2942         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
2943         u8                      action = P2P_PUB_ACTION_ACTION;
2944         u32                     p2poui = cpu_to_be32(P2POUI);
2945         u8                      oui_subtype = P2P_GO_NEGO_RESP;
2946         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
2947         u8                      p2pielen = 0, i;
2948         uint                    wpsielen = 0;
2949         u16                     wps_devicepassword_id = 0x0000;
2950         uint                    wps_devicepassword_id_len = 0;
2951         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh;
2952         u16                     len_channellist_attr = 0;
2953
2954         struct xmit_frame                       *pmgntframe;
2955         struct pkt_attrib                       *pattrib;
2956         unsigned char                                   *pframe;
2957         struct rtw_ieee80211_hdr        *pwlanhdr;
2958         unsigned short                          *fctrl;
2959         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
2960         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
2961         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2962         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
2963
2964 #ifdef CONFIG_WFD
2965         u32                                     wfdielen = 0;
2966 #endif /* CONFIG_WFD */
2967
2968         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
2969         {
2970                 return;
2971         }
2972
2973         DBG_8723A( "[%s] In, result = %d\n", __FUNCTION__,  result );
2974         /* update attribute */
2975         pattrib = &pmgntframe->attrib;
2976         update_mgntframe_attrib(padapter, pattrib);
2977
2978         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2979
2980         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2981         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2982
2983         fctrl = &(pwlanhdr->frame_ctl);
2984         *(fctrl) = 0;
2985
2986         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
2987         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
2988         memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
2989
2990         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
2991         pmlmeext->mgnt_seq++;
2992         SetFrameSubType(pframe, WIFI_ACTION);
2993
2994         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
2995         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
2996
2997         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
2998         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
2999         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
3000         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3001         pwdinfo->negotiation_dialog_token = frame_body[7];      /*      The Dialog Token of provisioning discovery request frame. */
3002         pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
3003
3004         /*      Commented by Albert 20110328 */
3005         /*      Try to get the device password ID from the WPS IE of group negotiation request frame */
3006         /*      WiFi Direct test plan 5.1.15 */
3007         rtw_get_wps_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
3008         rtw_get_wps_attr_content( wpsie, wpsielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len);
3009         wps_devicepassword_id = be16_to_cpu( wps_devicepassword_id );
3010
3011         memset( wpsie, 0x00, 255 );
3012         wpsielen = 0;
3013
3014         /*      WPS Section */
3015         wpsielen = 0;
3016         /*      WPS OUI */
3017         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
3018         wpsielen += 4;
3019
3020         /*      WPS version */
3021         /*      Type: */
3022         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
3023         wpsielen += 2;
3024
3025         /*      Length: */
3026         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
3027         wpsielen += 2;
3028
3029         /*      Value: */
3030         wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */
3031
3032         /*      Device Password ID */
3033         /*      Type: */
3034         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
3035         wpsielen += 2;
3036
3037         /*      Length: */
3038         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
3039         wpsielen += 2;
3040
3041         /*      Value: */
3042         if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
3043         {
3044                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );
3045         }
3046         else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
3047         {
3048                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_USER_SPEC );
3049         }
3050         else
3051         {
3052                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_PBC );
3053         }
3054         wpsielen += 2;
3055
3056         /*      Commented by Kurt 20120113 */
3057         /*      If some device wants to do p2p handshake without sending prov_disc_req */
3058         /*      We have to get peer_req_cm from here. */
3059         if (!memcmp( pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3) )
3060         {
3061                 if ( wps_devicepassword_id == WPS_DPID_USER_SPEC )
3062                 {
3063                         memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3 );
3064                 }
3065                 else if ( wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC )
3066                 {
3067                         memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3 );
3068                 }
3069                 else
3070                 {
3071                         memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3 );
3072                 }
3073         }
3074
3075         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
3076
3077         /*      P2P IE Section. */
3078
3079         /*      P2P OUI */
3080         p2pielen = 0;
3081         p2pie[ p2pielen++ ] = 0x50;
3082         p2pie[ p2pielen++ ] = 0x6F;
3083         p2pie[ p2pielen++ ] = 0x9A;
3084         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
3085
3086         /*      Commented by Albert 20100908 */
3087         /*      According to the P2P Specification, the group negoitation response frame should contain 9 P2P attributes */
3088         /*      1. Status */
3089         /*      2. P2P Capability */
3090         /*      3. Group Owner Intent */
3091         /*      4. Configuration Timeout */
3092         /*      5. Operating Channel */
3093         /*      6. Intended P2P Interface Address */
3094         /*      7. Channel List */
3095         /*      8. Device Info */
3096         /*      9. Group ID     ( Only GO ) */
3097
3098         /*      ToDo: */
3099
3100         /*      P2P Status */
3101         /*      Type: */
3102         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
3103
3104         /*      Length: */
3105         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3106         p2pielen += 2;
3107
3108         /*      Value: */
3109         p2pie[ p2pielen++ ] = result;
3110
3111         /*      P2P Capability */
3112         /*      Type: */
3113         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
3114
3115         /*      Length: */
3116         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3117         p2pielen += 2;
3118
3119         /*      Value: */
3120         /*      Device Capability Bitmap, 1 byte */
3121
3122         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
3123         {
3124                 /*      Commented by Albert 2011/03/08 */
3125                 /*      According to the P2P specification */
3126                 /*      if the sending device will be client, the P2P Capability should be reserved of group negotation response frame */
3127                 p2pie[ p2pielen++ ] = 0;
3128         }
3129         else
3130         {
3131                 /*      Be group owner or meet the error case */
3132                 p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
3133         }
3134
3135         /*      Group Capability Bitmap, 1 byte */
3136         if ( pwdinfo->persistent_supported )
3137         {
3138                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
3139         }
3140         else
3141         {
3142                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
3143         }
3144
3145         /*      Group Owner Intent */
3146         /*      Type: */
3147         p2pie[ p2pielen++ ] = P2P_ATTR_GO_INTENT;
3148
3149         /*      Length: */
3150         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3151         p2pielen += 2;
3152
3153         /*      Value: */
3154         if ( pwdinfo->peer_intent & 0x01 )
3155         {
3156                 /*      Peer's tie breaker bit is 1, our tie breaker bit should be 0 */
3157                 p2pie[ p2pielen++ ] = ( pwdinfo->intent << 1 );
3158         }
3159         else
3160         {
3161                 /*      Peer's tie breaker bit is 0, our tie breaker bit should be 1 */
3162                 p2pie[ p2pielen++ ] = ( ( pwdinfo->intent << 1 ) | BIT(0) );
3163         }
3164
3165         /*      Configuration Timeout */
3166         /*      Type: */
3167         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
3168
3169         /*      Length: */
3170         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3171         p2pielen += 2;
3172
3173         /*      Value: */
3174         p2pie[ p2pielen++ ] = 200;      /*      2 seconds needed to be the P2P GO */
3175         p2pie[ p2pielen++ ] = 200;      /*      2 seconds needed to be the P2P Client */
3176
3177         /*      Operating Channel */
3178         /*      Type: */
3179         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
3180
3181         /*      Length: */
3182         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
3183         p2pielen += 2;
3184
3185         /*      Value: */
3186         /*      Country String */
3187         p2pie[ p2pielen++ ] = 'X';
3188         p2pie[ p2pielen++ ] = 'X';
3189
3190         /*      The third byte should be set to 0x04. */
3191         /*      Described in the "Operating Channel Attribute" section. */
3192         p2pie[ p2pielen++ ] = 0x04;
3193
3194         /*      Operating Class */
3195         if ( pwdinfo->operating_channel <= 14 )
3196         {
3197                 /*      Operating Class */
3198                 p2pie[ p2pielen++ ] = 0x51;
3199         }
3200         else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
3201         {
3202                 /*      Operating Class */
3203                 p2pie[ p2pielen++ ] = 0x73;
3204         }
3205         else
3206         {
3207                 /*      Operating Class */
3208                 p2pie[ p2pielen++ ] = 0x7c;
3209         }
3210
3211         /*      Channel Number */
3212         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       /*      operating channel number */
3213
3214         /*      Intended P2P Interface Address */
3215         /*      Type: */
3216         p2pie[ p2pielen++ ] = P2P_ATTR_INTENTED_IF_ADDR;
3217
3218         /*      Length: */
3219         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
3220         p2pielen += 2;
3221
3222         /*      Value: */
3223         memcpy(p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
3224         p2pielen += ETH_ALEN;
3225
3226         /*      Channel List */
3227         /*      Type: */
3228         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
3229
3230         /*  Country String(3) */
3231         /*  + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */
3232         /*  + number of channels in all classes */
3233         len_channellist_attr = 3
3234            + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
3235            + get_reg_classes_full_count(pmlmeext->channel_list);
3236
3237 #ifdef CONFIG_CONCURRENT_MODE
3238         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3239         {
3240                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
3241         }
3242         else
3243         {
3244                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3245         }
3246 #else
3247
3248         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3249
3250  #endif
3251         p2pielen += 2;
3252
3253         /*      Value: */
3254         /*      Country String */
3255         p2pie[ p2pielen++ ] = 'X';
3256         p2pie[ p2pielen++ ] = 'X';
3257
3258         /*      The third byte should be set to 0x04. */
3259         /*      Described in the "Operating Channel Attribute" section. */
3260         p2pie[ p2pielen++ ] = 0x04;
3261
3262         /*      Channel Entry List */
3263
3264 #ifdef CONFIG_CONCURRENT_MODE
3265         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3266         {
3267                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
3268                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3269
3270                 /*      Operating Class */
3271                 if ( pbuddy_mlmeext->cur_channel > 14 )
3272                 {
3273                         if ( pbuddy_mlmeext->cur_channel >= 149 )
3274                         {
3275                                 p2pie[ p2pielen++ ] = 0x7c;
3276                         }
3277                         else
3278                         {
3279                                 p2pie[ p2pielen++ ] = 0x73;
3280                         }
3281                 }
3282                 else
3283                 {
3284                         p2pie[ p2pielen++ ] = 0x51;
3285                 }
3286
3287                 /*      Number of Channels */
3288                 /*      Just support 1 channel and this channel is AP's channel */
3289                 p2pie[ p2pielen++ ] = 1;
3290
3291                 /*      Channel List */
3292                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
3293         }
3294         else
3295         {
3296                 int i, j;
3297                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3298                         /*      Operating Class */
3299                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3300
3301                         /*      Number of Channels */
3302                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3303
3304                         /*      Channel List */
3305                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3306                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3307                         }
3308                 }
3309         }
3310 #else /*  CONFIG_CONCURRENT_MODE */
3311         {
3312                 int i, j;
3313                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3314                         /*      Operating Class */
3315                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3316
3317                         /*      Number of Channels */
3318                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3319
3320                         /*      Channel List */
3321                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3322                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3323                         }
3324                 }
3325         }
3326 #endif /*  CONFIG_CONCURRENT_MODE */
3327
3328         /*      Device Info */
3329         /*      Type: */
3330         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
3331
3332         /*      Length: */
3333         /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
3334         /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
3335         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
3336         p2pielen += 2;
3337
3338         /*      Value: */
3339         /*      P2P Device Address */
3340         memcpy(p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
3341         p2pielen += ETH_ALEN;
3342
3343         /*      Config Method */
3344         /*      This field should be big endian. Noted by P2P specification. */
3345
3346         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
3347
3348         p2pielen += 2;
3349
3350         /*      Primary Device Type */
3351         /*      Category ID */
3352         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
3353         p2pielen += 2;
3354
3355         /*      OUI */
3356         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
3357         p2pielen += 4;
3358
3359         /*      Sub Category ID */
3360         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
3361         p2pielen += 2;
3362
3363         /*      Number of Secondary Device Types */
3364         p2pie[ p2pielen++ ] = 0x00;     /*      No Secondary Device Type List */
3365
3366         /*      Device Name */
3367         /*      Type: */
3368         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
3369         p2pielen += 2;
3370
3371         /*      Length: */
3372         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
3373         p2pielen += 2;
3374
3375         /*      Value: */
3376         memcpy(p2pie + p2pielen, pwdinfo->device_name , pwdinfo->device_name_len );
3377         p2pielen += pwdinfo->device_name_len;
3378
3379         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
3380         {
3381                 /*      Group ID Attribute */
3382                 /*      Type: */
3383                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
3384
3385                 /*      Length: */
3386                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + pwdinfo->nego_ssidlen );
3387                 p2pielen += 2;
3388
3389                 /*      Value: */
3390                 /*      p2P Device Address */
3391                 memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN );
3392                 p2pielen += ETH_ALEN;
3393
3394                 /*      SSID */
3395                 memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
3396                 p2pielen += pwdinfo->nego_ssidlen;
3397
3398         }
3399
3400         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
3401
3402 #ifdef CONFIG_WFD
3403         wfdielen = build_nego_resp_wfd_ie(pwdinfo, pframe);
3404         pframe += wfdielen;
3405         pattrib->pktlen += wfdielen;
3406 #endif /* CONFIG_WFD */
3407
3408         pattrib->last_txcmdsz = pattrib->pktlen;
3409
3410         dump_mgntframe(padapter, pmgntframe);
3411
3412         return;
3413 }
3414
3415 void issue_p2p_GO_confirm(_adapter *padapter, u8* raddr, u8 result)
3416 {
3417
3418         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3419         u8                      action = P2P_PUB_ACTION_ACTION;
3420         u32                     p2poui = cpu_to_be32(P2POUI);
3421         u8                      oui_subtype = P2P_GO_NEGO_CONF;
3422         u8                      wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
3423         u8                      wpsielen = 0, p2pielen = 0;
3424
3425         struct xmit_frame                       *pmgntframe;
3426         struct pkt_attrib                       *pattrib;
3427         unsigned char                                   *pframe;
3428         struct rtw_ieee80211_hdr        *pwlanhdr;
3429         unsigned short                          *fctrl;
3430         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3431         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3432         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3433         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
3434 #ifdef CONFIG_WFD
3435         u32                                     wfdielen = 0;
3436 #endif /* CONFIG_WFD */
3437
3438         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
3439         {
3440                 return;
3441         }
3442
3443         DBG_8723A( "[%s] In\n", __FUNCTION__ );
3444         /* update attribute */
3445         pattrib = &pmgntframe->attrib;
3446         update_mgntframe_attrib(padapter, pattrib);
3447
3448         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3449
3450         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3451         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3452
3453         fctrl = &(pwlanhdr->frame_ctl);
3454         *(fctrl) = 0;
3455
3456         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
3457         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
3458         memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
3459
3460         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3461         pmlmeext->mgnt_seq++;
3462         SetFrameSubType(pframe, WIFI_ACTION);
3463
3464         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
3465         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3466
3467         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
3468         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
3469         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
3470         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3471         pframe = rtw_set_fixed_ie(pframe, 1, &(pwdinfo->negotiation_dialog_token), &(pattrib->pktlen));
3472
3473         /*      P2P IE Section. */
3474
3475         /*      P2P OUI */
3476         p2pielen = 0;
3477         p2pie[ p2pielen++ ] = 0x50;
3478         p2pie[ p2pielen++ ] = 0x6F;
3479         p2pie[ p2pielen++ ] = 0x9A;
3480         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
3481
3482         /*      Commented by Albert 20110306 */
3483         /*      According to the P2P Specification, the group negoitation request frame should contain 5 P2P attributes */
3484         /*      1. Status */
3485         /*      2. P2P Capability */
3486         /*      3. Operating Channel */
3487         /*      4. Channel List */
3488         /*      5. Group ID     ( if this WiFi is GO ) */
3489
3490         /*      P2P Status */
3491         /*      Type: */
3492         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
3493
3494         /*      Length: */
3495         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3496         p2pielen += 2;
3497
3498         /*      Value: */
3499         p2pie[ p2pielen++ ] = result;
3500
3501         /*      P2P Capability */
3502         /*      Type: */
3503         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
3504
3505         /*      Length: */
3506         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3507         p2pielen += 2;
3508
3509         /*      Value: */
3510         /*      Device Capability Bitmap, 1 byte */
3511         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
3512
3513         /*      Group Capability Bitmap, 1 byte */
3514         if ( pwdinfo->persistent_supported )
3515         {
3516                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
3517         }
3518         else
3519         {
3520                 p2pie[ p2pielen++ ] = P2P_GRPCAP_CROSS_CONN;
3521         }
3522
3523         /*      Operating Channel */
3524         /*      Type: */
3525         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
3526
3527         /*      Length: */
3528         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
3529         p2pielen += 2;
3530
3531         /*      Value: */
3532         /*      Country String */
3533         p2pie[ p2pielen++ ] = 'X';
3534         p2pie[ p2pielen++ ] = 'X';
3535
3536         /*      The third byte should be set to 0x04. */
3537         /*      Described in the "Operating Channel Attribute" section. */
3538         p2pie[ p2pielen++ ] = 0x04;
3539
3540         if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) )
3541         {
3542                 if ( pwdinfo->peer_operating_ch <= 14 )
3543                 {
3544                         /*      Operating Class */
3545                         p2pie[ p2pielen++ ] = 0x51;
3546                 }
3547                 else if ( ( pwdinfo->peer_operating_ch >= 36 ) && ( pwdinfo->peer_operating_ch <= 48 ) )
3548                 {
3549                         /*      Operating Class */
3550                         p2pie[ p2pielen++ ] = 0x73;
3551                 }
3552                 else
3553                 {
3554                         /*      Operating Class */
3555                         p2pie[ p2pielen++ ] = 0x7c;
3556                 }
3557
3558                 p2pie[ p2pielen++ ] = pwdinfo->peer_operating_ch;
3559         }
3560         else
3561         {
3562                 if ( pwdinfo->operating_channel <= 14 )
3563                 {
3564                         /*      Operating Class */
3565                         p2pie[ p2pielen++ ] = 0x51;
3566                 }
3567                 else if ( ( pwdinfo->operating_channel >= 36 ) && ( pwdinfo->operating_channel <= 48 ) )
3568                 {
3569                         /*      Operating Class */
3570                         p2pie[ p2pielen++ ] = 0x73;
3571                 }
3572                 else
3573                 {
3574                         /*      Operating Class */
3575                         p2pie[ p2pielen++ ] = 0x7c;
3576                 }
3577
3578                 /*      Channel Number */
3579                 p2pie[ p2pielen++ ] = pwdinfo->operating_channel;               /*      Use the listen channel as the operating channel */
3580         }
3581
3582         /*      Channel List */
3583         /*      Type: */
3584         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
3585
3586         /*      Length: */
3587         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( pwdinfo->channel_list_attr_len );
3588         p2pielen += 2;
3589
3590         /*      Value: */
3591         memcpy(p2pie + p2pielen, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len );
3592         p2pielen += pwdinfo->channel_list_attr_len;
3593
3594         if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
3595         {
3596                 /*      Group ID Attribute */
3597                 /*      Type: */
3598                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
3599
3600                 /*      Length: */
3601                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + pwdinfo->nego_ssidlen );
3602                 p2pielen += 2;
3603
3604                 /*      Value: */
3605                 /*      p2P Device Address */
3606                 memcpy(p2pie + p2pielen , pwdinfo->device_addr, ETH_ALEN );
3607                 p2pielen += ETH_ALEN;
3608
3609                 /*      SSID */
3610                 memcpy(p2pie + p2pielen, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen );
3611                 p2pielen += pwdinfo->nego_ssidlen;
3612         }
3613
3614         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
3615
3616 #ifdef CONFIG_WFD
3617         wfdielen = build_nego_confirm_wfd_ie(pwdinfo, pframe);
3618         pframe += wfdielen;
3619         pattrib->pktlen += wfdielen;
3620 #endif /* CONFIG_WFD */
3621
3622         pattrib->last_txcmdsz = pattrib->pktlen;
3623
3624         dump_mgntframe(padapter, pmgntframe);
3625
3626         return;
3627 }
3628
3629 void issue_p2p_invitation_request(_adapter *padapter, u8* raddr )
3630 {
3631
3632         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3633         u8                      action = P2P_PUB_ACTION_ACTION;
3634         u32                     p2poui = cpu_to_be32(P2POUI);
3635         u8                      oui_subtype = P2P_INVIT_REQ;
3636         u8                      p2pie[ 255 ] = { 0x00 };
3637         u8                      p2pielen = 0, i;
3638         u8                      dialogToken = 3;
3639         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
3640         u16                     len_channellist_attr = 0;
3641 #ifdef CONFIG_WFD
3642         u32                                     wfdielen = 0;
3643 #endif /* CONFIG_WFD */
3644 #ifdef CONFIG_CONCURRENT_MODE
3645         _adapter                                *pbuddy_adapter = padapter->pbuddy_adapter;
3646         struct wifidirect_info  *pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
3647         struct mlme_priv                *pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
3648         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3649 #endif
3650
3651         struct xmit_frame                       *pmgntframe;
3652         struct pkt_attrib                       *pattrib;
3653         unsigned char                                   *pframe;
3654         struct rtw_ieee80211_hdr        *pwlanhdr;
3655         unsigned short                          *fctrl;
3656         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3657         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3658         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3659         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
3660
3661         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
3662         {
3663                 return;
3664         }
3665
3666         /* update attribute */
3667         pattrib = &pmgntframe->attrib;
3668         update_mgntframe_attrib(padapter, pattrib);
3669
3670         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
3671
3672         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
3673         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
3674
3675         fctrl = &(pwlanhdr->frame_ctl);
3676         *(fctrl) = 0;
3677
3678         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
3679         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
3680         memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
3681
3682         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
3683         pmlmeext->mgnt_seq++;
3684         SetFrameSubType(pframe, WIFI_ACTION);
3685
3686         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
3687         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
3688
3689         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
3690         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
3691         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
3692         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
3693         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
3694
3695         /*      P2P IE Section. */
3696
3697         /*      P2P OUI */
3698         p2pielen = 0;
3699         p2pie[ p2pielen++ ] = 0x50;
3700         p2pie[ p2pielen++ ] = 0x6F;
3701         p2pie[ p2pielen++ ] = 0x9A;
3702         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
3703
3704         /*      Commented by Albert 20101011 */
3705         /*      According to the P2P Specification, the P2P Invitation request frame should contain 7 P2P attributes */
3706         /*      1. Configuration Timeout */
3707         /*      2. Invitation Flags */
3708         /*      3. Operating Channel    ( Only GO ) */
3709         /*      4. P2P Group BSSID      ( Should be included if I am the GO ) */
3710         /*      5. Channel List */
3711         /*      6. P2P Group ID */
3712         /*      7. P2P Device Info */
3713
3714         /*      Configuration Timeout */
3715         /*      Type: */
3716         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
3717
3718         /*      Length: */
3719         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
3720         p2pielen += 2;
3721
3722         /*      Value: */
3723         p2pie[ p2pielen++ ] = 200;      /*      2 seconds needed to be the P2P GO */
3724         p2pie[ p2pielen++ ] = 200;      /*      2 seconds needed to be the P2P Client */
3725
3726         /*      Invitation Flags */
3727         /*      Type: */
3728         p2pie[ p2pielen++ ] = P2P_ATTR_INVITATION_FLAGS;
3729
3730         /*      Length: */
3731         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
3732         p2pielen += 2;
3733
3734         /*      Value: */
3735         p2pie[ p2pielen++ ] = P2P_INVITATION_FLAGS_PERSISTENT;
3736
3737         /*      Operating Channel */
3738         /*      Type: */
3739         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
3740
3741         /*      Length: */
3742         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
3743         p2pielen += 2;
3744
3745         /*      Value: */
3746         /*      Country String */
3747         p2pie[ p2pielen++ ] = 'X';
3748         p2pie[ p2pielen++ ] = 'X';
3749
3750         /*      The third byte should be set to 0x04. */
3751         /*      Described in the "Operating Channel Attribute" section. */
3752         p2pie[ p2pielen++ ] = 0x04;
3753
3754         /*      Operating Class */
3755         if ( pwdinfo->invitereq_info.operating_ch <= 14 )
3756                 p2pie[ p2pielen++ ] = 0x51;
3757         else if ( ( pwdinfo->invitereq_info.operating_ch >= 36 ) && ( pwdinfo->invitereq_info.operating_ch <= 48 ) )
3758                 p2pie[ p2pielen++ ] = 0x73;
3759         else
3760                 p2pie[ p2pielen++ ] = 0x7c;
3761
3762         /*      Channel Number */
3763         p2pie[ p2pielen++ ] = pwdinfo->invitereq_info.operating_ch;     /*      operating channel number */
3764
3765         if (!memcmp(myid(&padapter->eeprompriv),
3766                     pwdinfo->invitereq_info.go_bssid, ETH_ALEN))
3767         {
3768                 /*      P2P Group BSSID */
3769                 /*      Type: */
3770                 p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_BSSID;
3771
3772                 /*      Length: */
3773                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
3774                 p2pielen += 2;
3775
3776                 /*      Value: */
3777                 /*      P2P Device Address for GO */
3778                 memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN );
3779                 p2pielen += ETH_ALEN;
3780         }
3781
3782         /*      Channel List */
3783         /*      Type: */
3784         p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
3785
3786         /*      Length: */
3787         /*  Country String(3) */
3788         /*  + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */
3789         /*  + number of channels in all classes */
3790         len_channellist_attr = 3
3791            + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
3792            + get_reg_classes_full_count(pmlmeext->channel_list);
3793
3794 #ifdef CONFIG_CONCURRENT_MODE
3795         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3796         {
3797                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
3798         }
3799         else
3800         {
3801                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3802         }
3803 #else
3804
3805         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
3806
3807  #endif
3808         p2pielen += 2;
3809
3810         /*      Value: */
3811         /*      Country String */
3812         p2pie[ p2pielen++ ] = 'X';
3813         p2pie[ p2pielen++ ] = 'X';
3814
3815         /*      The third byte should be set to 0x04. */
3816         /*      Described in the "Operating Channel Attribute" section. */
3817         p2pie[ p2pielen++ ] = 0x04;
3818
3819         /*      Channel Entry List */
3820 #ifdef CONFIG_CONCURRENT_MODE
3821         if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
3822         {
3823                 _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
3824                 struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3825
3826                 /*      Operating Class */
3827                 if ( pbuddy_mlmeext->cur_channel > 14 )
3828                 {
3829                         if ( pbuddy_mlmeext->cur_channel >= 149 )
3830                         {
3831                                 p2pie[ p2pielen++ ] = 0x7c;
3832                         }
3833                         else
3834                         {
3835                                 p2pie[ p2pielen++ ] = 0x73;
3836                         }
3837                 }
3838                 else
3839                 {
3840                         p2pie[ p2pielen++ ] = 0x51;
3841                 }
3842
3843                 /*      Number of Channels */
3844                 /*      Just support 1 channel and this channel is AP's channel */
3845                 p2pie[ p2pielen++ ] = 1;
3846
3847                 /*      Channel List */
3848                 p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
3849         }
3850         else
3851         {
3852                 int i, j;
3853                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3854                         /*      Operating Class */
3855                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3856
3857                         /*      Number of Channels */
3858                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3859
3860                         /*      Channel List */
3861                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3862                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3863                         }
3864                 }
3865         }
3866 #else /*  CONFIG_CONCURRENT_MODE */
3867         {
3868                 int i, j;
3869                 for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
3870                         /*      Operating Class */
3871                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
3872
3873                         /*      Number of Channels */
3874                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
3875
3876                         /*      Channel List */
3877                         for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
3878                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
3879                         }
3880                 }
3881         }
3882 #endif /*  CONFIG_CONCURRENT_MODE */
3883
3884         /*      P2P Group ID */
3885         /*      Type: */
3886         p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
3887
3888         /*      Length: */
3889         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 6 + pwdinfo->invitereq_info.ssidlen );
3890         p2pielen += 2;
3891
3892         /*      Value: */
3893         /*      P2P Device Address for GO */
3894         memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN );
3895         p2pielen += ETH_ALEN;
3896
3897         /*      SSID */
3898         memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid, pwdinfo->invitereq_info.ssidlen );
3899         p2pielen += pwdinfo->invitereq_info.ssidlen;
3900
3901         /*      Device Info */
3902         /*      Type: */
3903         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
3904
3905         /*      Length: */
3906         /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
3907         /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
3908         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
3909         p2pielen += 2;
3910
3911         /*      Value: */
3912         /*      P2P Device Address */
3913         memcpy(p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
3914         p2pielen += ETH_ALEN;
3915
3916         /*      Config Method */
3917         /*      This field should be big endian. Noted by P2P specification. */
3918         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
3919         p2pielen += 2;
3920
3921         /*      Primary Device Type */
3922         /*      Category ID */
3923         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
3924         p2pielen += 2;
3925
3926         /*      OUI */
3927         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
3928         p2pielen += 4;
3929
3930         /*      Sub Category ID */
3931         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
3932         p2pielen += 2;
3933
3934         /*      Number of Secondary Device Types */
3935         p2pie[ p2pielen++ ] = 0x00;     /*      No Secondary Device Type List */
3936
3937         /*      Device Name */
3938         /*      Type: */
3939         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
3940         p2pielen += 2;
3941
3942         /*      Length: */
3943         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
3944         p2pielen += 2;
3945
3946         /*      Value: */
3947         memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
3948         p2pielen += pwdinfo->device_name_len;
3949
3950         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
3951
3952 #ifdef CONFIG_WFD
3953         wfdielen = build_invitation_req_wfd_ie(pwdinfo, pframe);
3954         pframe += wfdielen;
3955         pattrib->pktlen += wfdielen;
3956 #endif /* CONFIG_WFD */
3957
3958         pattrib->last_txcmdsz = pattrib->pktlen;
3959
3960         dump_mgntframe(padapter, pmgntframe);
3961
3962         return;
3963 }
3964
3965 void issue_p2p_invitation_response(_adapter *padapter, u8* raddr, u8 dialogToken, u8 status_code)
3966 {
3967
3968         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
3969         u8                      action = P2P_PUB_ACTION_ACTION;
3970         u32                     p2poui = cpu_to_be32(P2POUI);
3971         u8                      oui_subtype = P2P_INVIT_RESP;
3972         u8                      p2pie[ 255 ] = { 0x00 };
3973         u8                      p2pielen = 0, i;
3974         u8                      channel_cnt_24g = 0, channel_cnt_5gl = 0, channel_cnt_5gh = 0;
3975         u16                     len_channellist_attr = 0;
3976 #ifdef CONFIG_CONCURRENT_MODE
3977         _adapter                                *pbuddy_adapter = padapter->pbuddy_adapter;
3978         struct wifidirect_info  *pbuddy_wdinfo = &pbuddy_adapter->wdinfo;
3979         struct mlme_priv                *pbuddy_mlmepriv = &pbuddy_adapter->mlmepriv;
3980         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
3981 #endif
3982 #ifdef CONFIG_WFD
3983         u32                                     wfdielen = 0;
3984 #endif /* CONFIG_WFD */
3985
3986         struct xmit_frame                       *pmgntframe;
3987         struct pkt_attrib                       *pattrib;
3988         unsigned char                                   *pframe;
3989         struct rtw_ieee80211_hdr        *pwlanhdr;
3990         unsigned short                          *fctrl;
3991         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
3992         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3993         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
3994         struct wifidirect_info  *pwdinfo = &( padapter->wdinfo);
3995
3996         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
3997         {
3998                 return;
3999         }
4000
4001         /* update attribute */
4002         pattrib = &pmgntframe->attrib;
4003         update_mgntframe_attrib(padapter, pattrib);
4004
4005         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4006
4007         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4008         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4009
4010         fctrl = &(pwlanhdr->frame_ctl);
4011         *(fctrl) = 0;
4012
4013         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
4014         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
4015         memcpy(pwlanhdr->addr3, raddr,  ETH_ALEN);
4016
4017         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4018         pmlmeext->mgnt_seq++;
4019         SetFrameSubType(pframe, WIFI_ACTION);
4020
4021         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4022         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4023
4024         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4025         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4026         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4027         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4028         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
4029
4030         /*      P2P IE Section. */
4031
4032         /*      P2P OUI */
4033         p2pielen = 0;
4034         p2pie[ p2pielen++ ] = 0x50;
4035         p2pie[ p2pielen++ ] = 0x6F;
4036         p2pie[ p2pielen++ ] = 0x9A;
4037         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
4038
4039         /*      Commented by Albert 20101005 */
4040         /*      According to the P2P Specification, the P2P Invitation response frame should contain 5 P2P attributes */
4041         /*      1. Status */
4042         /*      2. Configuration Timeout */
4043         /*      3. Operating Channel    ( Only GO ) */
4044         /*      4. P2P Group BSSID      ( Only GO ) */
4045         /*      5. Channel List */
4046
4047         /*      P2P Status */
4048         /*      Type: */
4049         p2pie[ p2pielen++ ] = P2P_ATTR_STATUS;
4050
4051         /*      Length: */
4052         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0001 );
4053         p2pielen += 2;
4054
4055         /*      Value: */
4056         /*      When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE. */
4057         /*      Sent the event receiving the P2P Invitation Req frame to DMP UI. */
4058         /*      DMP had to compare the MAC address to find out the profile. */
4059         /*      So, the WiFi driver will send the P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB. */
4060         /*      If the UI found the corresponding profile, the WiFi driver sends the P2P Invitation Req */
4061         /*      to NB to rebuild the persistent group. */
4062         p2pie[ p2pielen++ ] = status_code;
4063
4064         /*      Configuration Timeout */
4065         /*      Type: */
4066         p2pie[ p2pielen++ ] = P2P_ATTR_CONF_TIMEOUT;
4067
4068         /*      Length: */
4069         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
4070         p2pielen += 2;
4071
4072         /*      Value: */
4073         p2pie[ p2pielen++ ] = 200;      /*      2 seconds needed to be the P2P GO */
4074         p2pie[ p2pielen++ ] = 200;      /*      2 seconds needed to be the P2P Client */
4075
4076         if( status_code == P2P_STATUS_SUCCESS )
4077         {
4078                 if( rtw_p2p_chk_role( pwdinfo, P2P_ROLE_GO ) )
4079                 {
4080                         /*      The P2P Invitation request frame asks this Wi-Fi device to be the P2P GO */
4081                         /*      In this case, the P2P Invitation response frame should carry the two more P2P attributes. */
4082                         /*      First one is operating channel attribute. */
4083                         /*      Second one is P2P Group BSSID attribute. */
4084
4085                         /*      Operating Channel */
4086                         /*      Type: */
4087                         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4088
4089                         /*      Length: */
4090                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4091                         p2pielen += 2;
4092
4093                         /*      Value: */
4094                         /*      Country String */
4095                         p2pie[ p2pielen++ ] = 'X';
4096                         p2pie[ p2pielen++ ] = 'X';
4097
4098                         /*      The third byte should be set to 0x04. */
4099                         /*      Described in the "Operating Channel Attribute" section. */
4100                         p2pie[ p2pielen++ ] = 0x04;
4101
4102                         /*      Operating Class */
4103                         p2pie[ p2pielen++ ] = 0x51;     /*      Copy from SD7 */
4104
4105                         /*      Channel Number */
4106                         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       /*      operating channel number */
4107
4108                         /*      P2P Group BSSID */
4109                         /*      Type: */
4110                         p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_BSSID;
4111
4112                         /*      Length: */
4113                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN );
4114                         p2pielen += 2;
4115
4116                         /*      Value: */
4117                         /*      P2P Device Address for GO */
4118                         memcpy(p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
4119                         p2pielen += ETH_ALEN;
4120
4121                 }
4122
4123                 /*      Channel List */
4124                 /*      Type: */
4125                 p2pie[ p2pielen++ ] = P2P_ATTR_CH_LIST;
4126
4127                 /*      Length: */
4128                 /*  Country String(3) */
4129                 /*  + ( Operating Class (1) + Number of Channels(1) ) * Operation Classes (?) */
4130                 /*  + number of channels in all classes */
4131                 len_channellist_attr = 3
4132                         + (1 + 1) * (u16)pmlmeext->channel_list.reg_classes
4133                         + get_reg_classes_full_count(pmlmeext->channel_list);
4134
4135 #ifdef CONFIG_CONCURRENT_MODE
4136                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4137                 {
4138                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 5 + 1 );
4139                 }
4140                 else
4141                 {
4142                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4143                 }
4144 #else
4145
4146                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( len_channellist_attr );
4147
4148 #endif
4149                 p2pielen += 2;
4150
4151                 /*      Value: */
4152                 /*      Country String */
4153                 p2pie[ p2pielen++ ] = 'X';
4154                 p2pie[ p2pielen++ ] = 'X';
4155
4156                 /*      The third byte should be set to 0x04. */
4157                 /*      Described in the "Operating Channel Attribute" section. */
4158                 p2pie[ p2pielen++ ] = 0x04;
4159
4160                 /*      Channel Entry List */
4161 #ifdef CONFIG_CONCURRENT_MODE
4162                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
4163                 {
4164                         _adapter *pbuddy_adapter = padapter->pbuddy_adapter;
4165                         struct mlme_ext_priv    *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
4166
4167                         /*      Operating Class */
4168                         if ( pbuddy_mlmeext->cur_channel > 14 )
4169                         {
4170                                 if ( pbuddy_mlmeext->cur_channel >= 149 )
4171                                 {
4172                                         p2pie[ p2pielen++ ] = 0x7c;
4173                                 }
4174                                 else
4175                                 {
4176                                         p2pie[ p2pielen++ ] = 0x73;
4177                                 }
4178                         }
4179                         else
4180                         {
4181                                 p2pie[ p2pielen++ ] = 0x51;
4182                         }
4183
4184                         /*      Number of Channels */
4185                         /*      Just support 1 channel and this channel is AP's channel */
4186                         p2pie[ p2pielen++ ] = 1;
4187
4188                         /*      Channel List */
4189                         p2pie[ p2pielen++ ] = pbuddy_mlmeext->cur_channel;
4190                 }
4191                 else
4192                 {
4193                         int i, j;
4194                         for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4195                                 /*      Operating Class */
4196                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4197
4198                                 /*      Number of Channels */
4199                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4200
4201                                 /*      Channel List */
4202                                 for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4203                                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4204                                 }
4205                         }
4206                 }
4207 #else /*  CONFIG_CONCURRENT_MODE */
4208                 {
4209                         int i, j;
4210                         for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
4211                                 /*      Operating Class */
4212                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
4213
4214                                 /*      Number of Channels */
4215                                 p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
4216
4217                                 /*      Channel List */
4218                                 for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
4219                                         p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
4220                                 }
4221                         }
4222                 }
4223 #endif /*  CONFIG_CONCURRENT_MODE */
4224         }
4225
4226         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
4227
4228 #ifdef CONFIG_WFD
4229         wfdielen = build_invitation_resp_wfd_ie(pwdinfo, pframe);
4230         pframe += wfdielen;
4231         pattrib->pktlen += wfdielen;
4232 #endif /* CONFIG_WFD */
4233
4234         pattrib->last_txcmdsz = pattrib->pktlen;
4235
4236         dump_mgntframe(padapter, pmgntframe);
4237
4238         return;
4239 }
4240
4241 void issue_p2p_provision_request(_adapter *padapter, u8* pssid, u8 ussidlen, u8* pdev_raddr )
4242 {
4243         unsigned char category = RTW_WLAN_CATEGORY_PUBLIC;
4244         u8                      action = P2P_PUB_ACTION_ACTION;
4245         u8                      dialogToken = 1;
4246         u32                     p2poui = cpu_to_be32(P2POUI);
4247         u8                      oui_subtype = P2P_PROVISION_DISC_REQ;
4248         u8                      wpsie[ 100 ] = { 0x00 };
4249         u8                      wpsielen = 0;
4250         u32                     p2pielen = 0;
4251 #ifdef CONFIG_WFD
4252         u32                                     wfdielen = 0;
4253 #endif /* CONFIG_WFD */
4254
4255         struct xmit_frame                       *pmgntframe;
4256         struct pkt_attrib                       *pattrib;
4257         unsigned char                                   *pframe;
4258         struct rtw_ieee80211_hdr        *pwlanhdr;
4259         unsigned short                          *fctrl;
4260         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
4261         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4262         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4263         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
4264
4265         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4266         {
4267                 return;
4268         }
4269
4270         DBG_8723A( "[%s] In\n", __FUNCTION__ );
4271         /* update attribute */
4272         pattrib = &pmgntframe->attrib;
4273         update_mgntframe_attrib(padapter, pattrib);
4274
4275         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4276
4277         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4278         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4279
4280         fctrl = &(pwlanhdr->frame_ctl);
4281         *(fctrl) = 0;
4282
4283         memcpy(pwlanhdr->addr1, pdev_raddr, ETH_ALEN);
4284         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
4285         memcpy(pwlanhdr->addr3, pdev_raddr, ETH_ALEN);
4286
4287         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4288         pmlmeext->mgnt_seq++;
4289         SetFrameSubType(pframe, WIFI_ACTION);
4290
4291         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
4292         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4293
4294         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
4295         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
4296         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
4297         pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
4298         pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
4299
4300         p2pielen = build_prov_disc_request_p2p_ie( pwdinfo, pframe, pssid, ussidlen, pdev_raddr );
4301
4302         pframe += p2pielen;
4303         pattrib->pktlen += p2pielen;
4304
4305         wpsielen = 0;
4306         /*      WPS OUI */
4307         *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
4308         wpsielen += 4;
4309
4310         /*      WPS version */
4311         /*      Type: */
4312         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
4313         wpsielen += 2;
4314
4315         /*      Length: */
4316         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
4317         wpsielen += 2;
4318
4319         /*      Value: */
4320         wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */
4321
4322         /*      Config Method */
4323         /*      Type: */
4324         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
4325         wpsielen += 2;
4326
4327         /*      Length: */
4328         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
4329         wpsielen += 2;
4330
4331         /*      Value: */
4332         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->tx_prov_disc_info.wps_config_method_request );
4333         wpsielen += 2;
4334
4335         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
4336
4337 #ifdef CONFIG_WFD
4338         wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe);
4339         pframe += wfdielen;
4340         pattrib->pktlen += wfdielen;
4341 #endif /* CONFIG_WFD */
4342
4343         pattrib->last_txcmdsz = pattrib->pktlen;
4344
4345         dump_mgntframe(padapter, pmgntframe);
4346
4347         return;
4348 }
4349
4350 u8 is_matched_in_profilelist( u8* peermacaddr, struct profile_info* profileinfo )
4351 {
4352         u8 i, match_result = 0;
4353
4354         DBG_8723A( "[%s] peermac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
4355                     peermacaddr[0], peermacaddr[1],peermacaddr[2],peermacaddr[3],peermacaddr[4],peermacaddr[5]);
4356
4357         for( i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++ )
4358         {
4359                DBG_8723A( "[%s] profileinfo_mac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
4360                             profileinfo->peermac[0], profileinfo->peermac[1],profileinfo->peermac[2],profileinfo->peermac[3],profileinfo->peermac[4],profileinfo->peermac[5]);
4361                 if (!memcmp(peermacaddr, profileinfo->peermac, ETH_ALEN))
4362                 {
4363                         match_result = 1;
4364                         DBG_8723A( "[%s] Match!\n", __FUNCTION__ );
4365                         break;
4366                 }
4367         }
4368
4369         return (match_result );
4370 }
4371
4372 void issue_probersp_p2p(_adapter *padapter, unsigned char *da)
4373 {
4374         struct xmit_frame                       *pmgntframe;
4375         struct pkt_attrib                       *pattrib;
4376         unsigned char                                   *pframe;
4377         struct rtw_ieee80211_hdr        *pwlanhdr;
4378         unsigned short                          *fctrl;
4379         unsigned char                                   *mac;
4380         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
4381         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4382         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4383         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
4384         /* WLAN_BSSID_EX                        *cur_network = &(pmlmeinfo->network); */
4385         u16                                     beacon_interval = 100;
4386         u16                                     capInfo = 0;
4387         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
4388         u8                                      wpsie[255] = { 0x00 };
4389         u32                                     wpsielen = 0, p2pielen = 0;
4390 #ifdef CONFIG_WFD
4391         u32                                     wfdielen = 0;
4392 #endif /* CONFIG_WFD */
4393 #ifdef CONFIG_IOCTL_CFG80211
4394         struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
4395         struct ieee80211_channel *ieee_ch = &pcfg80211_wdinfo->remain_on_ch_channel;
4396         u8 listen_channel = (u8) ieee80211_frequency_to_channel(ieee_ch->center_freq);
4397 #endif /* CONFIG_IOCTL_CFG80211 */
4398 #ifdef CONFIG_INTEL_WIDI
4399         u8 zero_array_check[L2SDTA_SERVICE_VE_LEN] = { 0x00 };
4400 #endif /* CONFIG_INTEL_WIDI */
4401
4402         /* DBG_8723A("%s\n", __FUNCTION__); */
4403
4404         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4405         {
4406                 return;
4407         }
4408
4409         /* update attribute */
4410         pattrib = &pmgntframe->attrib;
4411         update_mgntframe_attrib(padapter, pattrib);
4412
4413         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4414
4415         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4416         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4417
4418         mac = myid(&(padapter->eeprompriv));
4419
4420         fctrl = &(pwlanhdr->frame_ctl);
4421         *(fctrl) = 0;
4422         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
4423         memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
4424
4425         /*      Use the device address for BSSID field. */
4426         memcpy(pwlanhdr->addr3, mac, ETH_ALEN);
4427
4428         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4429         pmlmeext->mgnt_seq++;
4430         SetFrameSubType(fctrl, WIFI_PROBERSP);
4431
4432         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
4433         pattrib->pktlen = pattrib->hdrlen;
4434         pframe += pattrib->hdrlen;
4435
4436         /* timestamp will be inserted by hardware */
4437         pframe += 8;
4438         pattrib->pktlen += 8;
4439
4440         /*  beacon interval: 2 bytes */
4441         memcpy(pframe, (unsigned char *) &beacon_interval, 2);
4442         pframe += 2;
4443         pattrib->pktlen += 2;
4444
4445         /*      capability info: 2 bytes */
4446         /*      ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of WiFi Direct Spec) */
4447         capInfo |= cap_ShortPremble;
4448         capInfo |= cap_ShortSlot;
4449
4450         memcpy(pframe, (unsigned char *) &capInfo, 2);
4451         pframe += 2;
4452         pattrib->pktlen += 2;
4453
4454         /*  SSID */
4455         pframe = rtw_set_ie(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid, &pattrib->pktlen);
4456
4457         /*  supported rates... */
4458         /*      Use the OFDM rate in the P2P probe response frame. ( 6(B), 9(B), 12, 18, 24, 36, 48, 54 ) */
4459         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
4460
4461         /*  DS parameter set */
4462 #ifdef CONFIG_IOCTL_CFG80211
4463         if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled && listen_channel !=0 )
4464         {
4465                 pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&listen_channel, &pattrib->pktlen);
4466         }
4467         else
4468 #endif /* CONFIG_IOCTL_CFG80211 */
4469         {
4470                 pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&pwdinfo->listen_channel, &pattrib->pktlen);
4471         }
4472
4473 #ifdef CONFIG_IOCTL_CFG80211
4474         if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
4475         {
4476                 if( pmlmepriv->wps_probe_resp_ie != NULL && pmlmepriv->p2p_probe_resp_ie != NULL )
4477                 {
4478                         /* WPS IE */
4479                         memcpy(pframe, pmlmepriv->wps_probe_resp_ie, pmlmepriv->wps_probe_resp_ie_len);
4480                         pattrib->pktlen += pmlmepriv->wps_probe_resp_ie_len;
4481                         pframe += pmlmepriv->wps_probe_resp_ie_len;
4482
4483                         /* P2P IE */
4484                         memcpy(pframe, pmlmepriv->p2p_probe_resp_ie, pmlmepriv->p2p_probe_resp_ie_len);
4485                         pattrib->pktlen += pmlmepriv->p2p_probe_resp_ie_len;
4486                         pframe += pmlmepriv->p2p_probe_resp_ie_len;
4487                 }
4488         }
4489         else
4490 #endif /* CONFIG_IOCTL_CFG80211 */
4491         {
4492
4493                 /*      Todo: WPS IE */
4494                 /*      Noted by Albert 20100907 */
4495                 /*      According to the WPS specification, all the WPS attribute is presented by Big Endian. */
4496
4497                 wpsielen = 0;
4498                 /*      WPS OUI */
4499                 *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
4500                 wpsielen += 4;
4501
4502                 /*      WPS version */
4503                 /*      Type: */
4504                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
4505                 wpsielen += 2;
4506
4507                 /*      Length: */
4508                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
4509                 wpsielen += 2;
4510
4511                 /*      Value: */
4512                 wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */
4513
4514 #ifdef CONFIG_INTEL_WIDI
4515                 /*      Commented by Kurt */
4516                 /*      Appended WiDi info. only if we did issued_probereq_widi(), and then we saved ven. ext. in pmlmepriv->sa_ext. */
4517                 if (memcmp(pmlmepriv->sa_ext, zero_array_check,
4518                            L2SDTA_SERVICE_VE_LEN))
4519                 {
4520                         /* Sec dev type */
4521                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SEC_DEV_TYPE_LIST );
4522                         wpsielen += 2;
4523
4524                         /*      Length: */
4525                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
4526                         wpsielen += 2;
4527
4528                         /*      Value: */
4529                         /*      Category ID */
4530                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_DISPLAYS );
4531                         wpsielen += 2;
4532
4533                         /*      OUI */
4534                         *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( INTEL_DEV_TYPE_OUI );
4535                         wpsielen += 4;
4536
4537                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_WIDI_CONSUMER_SINK );
4538                         wpsielen += 2;
4539
4540                         /*      Vendor Extension */
4541                         memcpy(wpsie + wpsielen, pmlmepriv->sa_ext, L2SDTA_SERVICE_VE_LEN );
4542                         wpsielen += L2SDTA_SERVICE_VE_LEN;
4543                 }
4544 #endif /* CONFIG_INTEL_WIDI */
4545
4546                 /*      WiFi Simple Config State */
4547                 /*      Type: */
4548                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SIMPLE_CONF_STATE );
4549                 wpsielen += 2;
4550
4551                 /*      Length: */
4552                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
4553                 wpsielen += 2;
4554
4555                 /*      Value: */
4556                 wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG;   /*      Not Configured. */
4557
4558                 /*      Response Type */
4559                 /*      Type: */
4560                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_RESP_TYPE );
4561                 wpsielen += 2;
4562
4563                 /*      Length: */
4564                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
4565                 wpsielen += 2;
4566
4567                 /*      Value: */
4568                 wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
4569
4570                 /*      UUID-E */
4571                 /*      Type: */
4572                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_UUID_E );
4573                 wpsielen += 2;
4574
4575                 /*      Length: */
4576                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0010 );
4577                 wpsielen += 2;
4578
4579                 /*      Value: */
4580                 memcpy(wpsie + wpsielen, myid( &padapter->eeprompriv ), ETH_ALEN );
4581                 wpsielen += 0x10;
4582
4583                 /*      Manufacturer */
4584                 /*      Type: */
4585                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MANUFACTURER );
4586                 wpsielen += 2;
4587
4588                 /*      Length: */
4589                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0007 );
4590                 wpsielen += 2;
4591
4592                 /*      Value: */
4593                 memcpy(wpsie + wpsielen, "Realtek", 7 );
4594                 wpsielen += 7;
4595
4596                 /*      Model Name */
4597                 /*      Type: */
4598                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MODEL_NAME );
4599                 wpsielen += 2;
4600
4601                 /*      Length: */
4602                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0006 );
4603                 wpsielen += 2;
4604
4605                 /*      Value: */
4606                 memcpy(wpsie + wpsielen, "8192CU", 6 );
4607                 wpsielen += 6;
4608
4609                 /*      Model Number */
4610                 /*      Type: */
4611                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_MODEL_NUMBER );
4612                 wpsielen += 2;
4613
4614                 /*      Length: */
4615                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
4616                 wpsielen += 2;
4617
4618                 /*      Value: */
4619                 wpsie[ wpsielen++ ] = 0x31;             /*      character 1 */
4620
4621                 /*      Serial Number */
4622                 /*      Type: */
4623                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_SERIAL_NUMBER );
4624                 wpsielen += 2;
4625
4626                 /*      Length: */
4627                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( ETH_ALEN );
4628                 wpsielen += 2;
4629
4630                 /*      Value: */
4631                 memcpy(wpsie + wpsielen, "123456" , ETH_ALEN );
4632                 wpsielen += ETH_ALEN;
4633
4634                 /*      Primary Device Type */
4635                 /*      Type: */
4636                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
4637                 wpsielen += 2;
4638
4639                 /*      Length: */
4640                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
4641                 wpsielen += 2;
4642
4643                 /*      Value: */
4644                 /*      Category ID */
4645                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
4646                 wpsielen += 2;
4647
4648                 /*      OUI */
4649                 *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( WPSOUI );
4650                 wpsielen += 4;
4651
4652                 /*      Sub Category ID */
4653                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
4654                 wpsielen += 2;
4655
4656                 /*      Device Name */
4657                 /*      Type: */
4658                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
4659                 wpsielen += 2;
4660
4661                 /*      Length: */
4662                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->device_name_len );
4663                 wpsielen += 2;
4664
4665                 /*      Value: */
4666                 if (pwdinfo->device_name_len)
4667                 {
4668                         memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len );
4669                         wpsielen += pwdinfo->device_name_len;
4670                 }
4671
4672                 /*      Config Method */
4673                 /*      Type: */
4674                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
4675                 wpsielen += 2;
4676
4677                 /*      Length: */
4678                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
4679                 wpsielen += 2;
4680
4681                 /*      Value: */
4682                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
4683                 wpsielen += 2;
4684
4685                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
4686
4687                 p2pielen = build_probe_resp_p2p_ie(pwdinfo, pframe);
4688                 pframe += p2pielen;
4689                 pattrib->pktlen += p2pielen;
4690         }
4691
4692 #ifdef CONFIG_WFD
4693 #ifdef CONFIG_IOCTL_CFG80211
4694         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
4695 #endif /* CONFIG_IOCTL_CFG80211 */
4696         {
4697                 wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
4698                 pframe += wfdielen;
4699                 pattrib->pktlen += wfdielen;
4700         }
4701 #ifdef CONFIG_IOCTL_CFG80211
4702         else if (pmlmepriv->wfd_probe_resp_ie != NULL && pmlmepriv->wfd_probe_resp_ie_len>0)
4703         {
4704                 /* WFD IE */
4705                 memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, pmlmepriv->wfd_probe_resp_ie_len);
4706                 pattrib->pktlen += pmlmepriv->wfd_probe_resp_ie_len;
4707                 pframe += pmlmepriv->wfd_probe_resp_ie_len;
4708         }
4709 #endif /* CONFIG_IOCTL_CFG80211 */
4710 #endif /* CONFIG_WFD */
4711
4712         pattrib->last_txcmdsz = pattrib->pktlen;
4713
4714         dump_mgntframe(padapter, pmgntframe);
4715
4716         return;
4717 }
4718
4719 int _issue_probereq_p2p(_adapter *padapter, u8 *da, int wait_ack)
4720 {
4721         int ret = _FAIL;
4722         struct xmit_frame               *pmgntframe;
4723         struct pkt_attrib               *pattrib;
4724         unsigned char                   *pframe;
4725         struct rtw_ieee80211_hdr        *pwlanhdr;
4726         unsigned short          *fctrl;
4727         unsigned char                   *mac;
4728         unsigned char                   bssrate[NumRates];
4729         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
4730         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
4731         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
4732         int     bssrate_len = 0;
4733         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
4734         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
4735         u8                                      wpsie[255] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
4736         u16                                     wpsielen = 0, p2pielen = 0;
4737 #ifdef CONFIG_WFD
4738         u32                                     wfdielen = 0;
4739 #endif /* CONFIG_WFD */
4740
4741         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
4742
4743         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
4744         {
4745                 goto exit;
4746         }
4747
4748         /* update attribute */
4749         pattrib = &pmgntframe->attrib;
4750         update_mgntframe_attrib(padapter, pattrib);
4751
4752         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
4753
4754         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
4755         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
4756
4757         mac = myid(&(padapter->eeprompriv));
4758
4759         fctrl = &(pwlanhdr->frame_ctl);
4760         *(fctrl) = 0;
4761
4762         if (da) {
4763                 memcpy(pwlanhdr->addr1, da, ETH_ALEN);
4764                 memcpy(pwlanhdr->addr3, da, ETH_ALEN);
4765         } else {
4766                 if ( ( pwdinfo->p2p_info.scan_op_ch_only ) || ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) )
4767                 {
4768                         /*      This two flags will be set when this is only the P2P client mode. */
4769                         memcpy(pwlanhdr->addr1, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
4770                         memcpy(pwlanhdr->addr3, pwdinfo->p2p_peer_interface_addr, ETH_ALEN);
4771                 }
4772                 else
4773                 {
4774                         /*      broadcast probe request frame */
4775                         memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
4776                         memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
4777                 }
4778         }
4779         memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
4780
4781         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
4782         pmlmeext->mgnt_seq++;
4783         SetFrameSubType(pframe, WIFI_PROBEREQ);
4784
4785         pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
4786         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
4787
4788         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
4789         {
4790                 pframe = rtw_set_ie(pframe, _SSID_IE_, pwdinfo->tx_prov_disc_info.ssid.SsidLength, pwdinfo->tx_prov_disc_info.ssid.Ssid, &(pattrib->pktlen));
4791         }
4792         else
4793         {
4794                 pframe = rtw_set_ie(pframe, _SSID_IE_, P2P_WILDCARD_SSID_LEN, pwdinfo->p2p_wildcard_ssid, &(pattrib->pktlen));
4795         }
4796         /*      Use the OFDM rate in the P2P probe request frame. ( 6(B), 9(B), 12(B), 24(B), 36, 48, 54 ) */
4797         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pwdinfo->support_rate, &pattrib->pktlen);
4798
4799 #ifdef CONFIG_IOCTL_CFG80211
4800         if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
4801         {
4802                 if( pmlmepriv->wps_probe_req_ie != NULL && pmlmepriv->p2p_probe_req_ie != NULL )
4803                 {
4804                         /* WPS IE */
4805                         memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
4806                         pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
4807                         pframe += pmlmepriv->wps_probe_req_ie_len;
4808
4809                         /* P2P IE */
4810                         memcpy(pframe, pmlmepriv->p2p_probe_req_ie, pmlmepriv->p2p_probe_req_ie_len);
4811                         pattrib->pktlen += pmlmepriv->p2p_probe_req_ie_len;
4812                         pframe += pmlmepriv->p2p_probe_req_ie_len;
4813                 }
4814         }
4815         else
4816 #endif /* CONFIG_IOCTL_CFG80211 */
4817         {
4818
4819                 /*      WPS IE */
4820                 /*      Noted by Albert 20110221 */
4821                 /*      According to the WPS specification, all the WPS attribute is presented by Big Endian. */
4822
4823                 wpsielen = 0;
4824                 /*      WPS OUI */
4825                 *(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
4826                 wpsielen += 4;
4827
4828                 /*      WPS version */
4829                 /*      Type: */
4830                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
4831                 wpsielen += 2;
4832
4833                 /*      Length: */
4834                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
4835                 wpsielen += 2;
4836
4837                 /*      Value: */
4838                 wpsie[wpsielen++] = WPS_VERSION_1;      /*      Version 1.0 */
4839
4840                 if( pmlmepriv->wps_probe_req_ie == NULL )
4841                 {
4842                         /*      UUID-E */
4843                         /*      Type: */
4844                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_UUID_E );
4845                         wpsielen += 2;
4846
4847                         /*      Length: */
4848                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0010 );
4849                         wpsielen += 2;
4850
4851                         /*      Value: */
4852                         memcpy(wpsie + wpsielen, myid( &padapter->eeprompriv ), ETH_ALEN );
4853                         wpsielen += 0x10;
4854
4855                         /*      Config Method */
4856                         /*      Type: */
4857                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
4858                         wpsielen += 2;
4859
4860                         /*      Length: */
4861                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
4862                         wpsielen += 2;
4863
4864                         /*      Value: */
4865                         *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
4866                         wpsielen += 2;
4867                 }
4868
4869                 /*      Device Name */
4870                 /*      Type: */
4871                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
4872                 wpsielen += 2;
4873
4874                 /*      Length: */
4875                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( pwdinfo->device_name_len );
4876                 wpsielen += 2;
4877
4878                 /*      Value: */
4879                 memcpy(wpsie + wpsielen, pwdinfo->device_name, pwdinfo->device_name_len );
4880                 wpsielen += pwdinfo->device_name_len;
4881
4882                 /*      Primary Device Type */
4883                 /*      Type: */
4884                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
4885                 wpsielen += 2;
4886
4887                 /*      Length: */
4888                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0008 );
4889                 wpsielen += 2;
4890
4891                 /*      Value: */
4892                 /*      Category ID */
4893                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_CID_RTK_WIDI );
4894                 wpsielen += 2;
4895
4896                 /*      OUI */
4897                 *(u32*) ( wpsie + wpsielen ) = cpu_to_be32( WPSOUI );
4898                 wpsielen += 4;
4899
4900                 /*      Sub Category ID */
4901                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_PDT_SCID_RTK_DMP );
4902                 wpsielen += 2;
4903
4904                 /*      Device Password ID */
4905                 /*      Type: */
4906                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_DEVICE_PWID );
4907                 wpsielen += 2;
4908
4909                 /*      Length: */
4910                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
4911                 wpsielen += 2;
4912
4913                 /*      Value: */
4914                 *(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_DPID_REGISTRAR_SPEC );  /*      Registrar-specified */
4915                 wpsielen += 2;
4916
4917                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
4918
4919                 /*      P2P OUI */
4920                 p2pielen = 0;
4921                 p2pie[ p2pielen++ ] = 0x50;
4922                 p2pie[ p2pielen++ ] = 0x6F;
4923                 p2pie[ p2pielen++ ] = 0x9A;
4924                 p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
4925
4926                 /*      Commented by Albert 20110221 */
4927                 /*      According to the P2P Specification, the probe request frame should contain 5 P2P attributes */
4928                 /*      1. P2P Capability */
4929                 /*      2. P2P Device ID if this probe request wants to find the specific P2P device */
4930                 /*      3. Listen Channel */
4931                 /*      4. Extended Listen Timing */
4932                 /*      5. Operating Channel if this WiFi is working as the group owner now */
4933
4934                 /*      P2P Capability */
4935                 /*      Type: */
4936                 p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
4937
4938                 /*      Length: */
4939                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
4940                 p2pielen += 2;
4941
4942                 /*      Value: */
4943                 /*      Device Capability Bitmap, 1 byte */
4944                 p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
4945
4946                 /*      Group Capability Bitmap, 1 byte */
4947                 if ( pwdinfo->persistent_supported )
4948                         p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
4949                 else
4950                         p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
4951
4952                 /*      Listen Channel */
4953                 /*      Type: */
4954                 p2pie[ p2pielen++ ] = P2P_ATTR_LISTEN_CH;
4955
4956                 /*      Length: */
4957                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
4958                 p2pielen += 2;
4959
4960                 /*      Value: */
4961                 /*      Country String */
4962                 p2pie[ p2pielen++ ] = 'X';
4963                 p2pie[ p2pielen++ ] = 'X';
4964
4965                 /*      The third byte should be set to 0x04. */
4966                 /*      Described in the "Operating Channel Attribute" section. */
4967                 p2pie[ p2pielen++ ] = 0x04;
4968
4969                 /*      Operating Class */
4970                 p2pie[ p2pielen++ ] = 0x51;     /*      Copy from SD7 */
4971
4972                 /*      Channel Number */
4973                 p2pie[ p2pielen++ ] = pwdinfo->listen_channel;  /*      listen channel */
4974
4975                 /*      Extended Listen Timing */
4976                 /*      Type: */
4977                 p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
4978
4979                 /*      Length: */
4980                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
4981                 p2pielen += 2;
4982
4983                 /*      Value: */
4984                 /*      Availability Period */
4985                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
4986                 p2pielen += 2;
4987
4988                 /*      Availability Interval */
4989                 *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
4990                 p2pielen += 2;
4991
4992                 if ( rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) )
4993                 {
4994                         /*      Operating Channel (if this WiFi is working as the group owner now) */
4995                         /*      Type: */
4996                         p2pie[ p2pielen++ ] = P2P_ATTR_OPERATING_CH;
4997
4998                         /*      Length: */
4999                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0005 );
5000                         p2pielen += 2;
5001
5002                         /*      Value: */
5003                         /*      Country String */
5004                         p2pie[ p2pielen++ ] = 'X';
5005                         p2pie[ p2pielen++ ] = 'X';
5006
5007                         /*      The third byte should be set to 0x04. */
5008                         /*      Described in the "Operating Channel Attribute" section. */
5009                         p2pie[ p2pielen++ ] = 0x04;
5010
5011                         /*      Operating Class */
5012                         p2pie[ p2pielen++ ] = 0x51;     /*      Copy from SD7 */
5013
5014                         /*      Channel Number */
5015                         p2pie[ p2pielen++ ] = pwdinfo->operating_channel;       /*      operating channel number */
5016
5017                 }
5018
5019                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
5020
5021                 if( pmlmepriv->wps_probe_req_ie != NULL )
5022                 {
5023                         /* WPS IE */
5024                         memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
5025                         pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
5026                         pframe += pmlmepriv->wps_probe_req_ie_len;
5027                 }
5028         }
5029
5030 #ifdef CONFIG_WFD
5031 #ifdef CONFIG_IOCTL_CFG80211
5032         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
5033 #endif
5034         {
5035                 wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe);
5036                 pframe += wfdielen;
5037                 pattrib->pktlen += wfdielen;
5038         }
5039 #ifdef CONFIG_IOCTL_CFG80211
5040         else if (pmlmepriv->wfd_probe_req_ie != NULL && pmlmepriv->wfd_probe_req_ie_len>0)
5041         {
5042                 /* WFD IE */
5043                 memcpy(pframe, pmlmepriv->wfd_probe_req_ie, pmlmepriv->wfd_probe_req_ie_len);
5044                 pattrib->pktlen += pmlmepriv->wfd_probe_req_ie_len;
5045                 pframe += pmlmepriv->wfd_probe_req_ie_len;
5046         }
5047 #endif /* CONFIG_IOCTL_CFG80211 */
5048 #endif /* CONFIG_WFD */
5049
5050         pattrib->last_txcmdsz = pattrib->pktlen;
5051
5052         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
5053
5054         if (wait_ack) {
5055                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
5056         } else {
5057                 dump_mgntframe(padapter, pmgntframe);
5058                 ret = _SUCCESS;
5059         }
5060
5061 exit:
5062         return ret;
5063 }
5064
5065 inline void issue_probereq_p2p(_adapter *adapter, u8 *da)
5066 {
5067         _issue_probereq_p2p(adapter, da, _FALSE);
5068 }
5069
5070 int issue_probereq_p2p_ex(_adapter *adapter, u8 *da, int try_cnt, int wait_ms)
5071 {
5072         int ret;
5073         int i = 0;
5074         u32 start = rtw_get_current_time();
5075
5076         do
5077         {
5078                 ret = _issue_probereq_p2p(adapter, da, wait_ms>0?_TRUE:_FALSE);
5079
5080                 i++;
5081
5082                 if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
5083                         break;
5084
5085                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
5086                         rtw_msleep_os(wait_ms);
5087
5088         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
5089
5090         if (ret != _FAIL) {
5091                 ret = _SUCCESS;
5092                 #ifndef DBG_XMIT_ACK
5093                 goto exit;
5094                 #endif
5095         }
5096
5097         if (try_cnt && wait_ms) {
5098                 if (da)
5099                         DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
5100                                 FUNC_ADPT_ARG(adapter), MAC_ARG(da), rtw_get_oper_ch(adapter),
5101                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
5102                 else
5103                         DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
5104                                 FUNC_ADPT_ARG(adapter), rtw_get_oper_ch(adapter),
5105                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
5106         }
5107 exit:
5108         return ret;
5109 }
5110
5111 #endif /* CONFIG_P2P */
5112
5113 s32 rtw_action_public_decache(union recv_frame *recv_frame, s32 token)
5114 {
5115         _adapter *adapter = recv_frame->u.hdr.adapter;
5116         struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
5117         u8 *frame = recv_frame->u.hdr.rx_data;
5118         u16 seq_ctrl = ( (recv_frame->u.hdr.attrib.seq_num&0xffff) << 4) |
5119                 (recv_frame->u.hdr.attrib.frag_num & 0xf);
5120
5121         if (GetRetry(frame)) {
5122                 if (token >= 0) {
5123                         if ((seq_ctrl == mlmeext->action_public_rxseq)
5124                                 && (token == mlmeext->action_public_dialog_token))
5125                         {
5126                                 DBG_8723A(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x, token:%d\n",
5127                                         FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
5128                                 return _FAIL;
5129                         }
5130                 } else {
5131                         if (seq_ctrl == mlmeext->action_public_rxseq) {
5132                                 DBG_8723A(FUNC_ADPT_FMT" seq_ctrl=0x%x, rxseq=0x%x\n",
5133                                         FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq);
5134                                 return _FAIL;
5135                         }
5136                 }
5137         }
5138
5139         mlmeext->action_public_rxseq = seq_ctrl;
5140
5141         if (token >= 0)
5142                 mlmeext->action_public_dialog_token = token;
5143
5144         return _SUCCESS;
5145 }
5146
5147 unsigned int on_action_public_p2p(union recv_frame *precv_frame)
5148 {
5149         _adapter *padapter = precv_frame->u.hdr.adapter;
5150         u8 *pframe = precv_frame->u.hdr.rx_data;
5151         uint len = precv_frame->u.hdr.len;
5152         u8 *frame_body;
5153         u8 dialogToken=0;
5154 #ifdef CONFIG_P2P
5155         u8 *p2p_ie;
5156         u32     p2p_ielen, wps_ielen;
5157         struct  wifidirect_info *pwdinfo = &( padapter->wdinfo );
5158         u8      result = P2P_STATUS_SUCCESS;
5159         u8      empty_addr[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
5160 #endif /* CONFIG_P2P */
5161
5162         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
5163
5164         dialogToken = frame_body[7];
5165
5166         if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
5167                 return _FAIL;
5168
5169 #ifdef CONFIG_P2P
5170         _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
5171 #ifdef CONFIG_IOCTL_CFG80211
5172         if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
5173         {
5174                 rtw_cfg80211_rx_p2p_action_public(padapter, pframe, len);
5175         }
5176         else
5177 #endif /* CONFIG_IOCTL_CFG80211 */
5178         {
5179                 /*      Do nothing if the driver doesn't enable the P2P function. */
5180                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
5181                         return _SUCCESS;
5182
5183                 len -= sizeof(struct rtw_ieee80211_hdr_3addr);
5184
5185                 switch( frame_body[ 6 ] )/* OUI Subtype */
5186                 {
5187                         case P2P_GO_NEGO_REQ:
5188                         {
5189                                 DBG_8723A( "[%s] Got GO Nego Req Frame\n", __FUNCTION__);
5190                                 memset( &pwdinfo->groupid_info, 0x00, sizeof( struct group_id_info ) );
5191
5192                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
5193                                 {
5194                                         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
5195                                 }
5196
5197                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
5198                                 {
5199                                         /*      Commented by Albert 20110526 */
5200                                         /*      In this case, this means the previous nego fail doesn't be reset yet. */
5201                                         _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
5202                                         /*      Restore the previous p2p state */
5203                                         rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
5204                                         DBG_8723A( "[%s] Restore the previous p2p state to %d\n", __FUNCTION__, rtw_p2p_state(pwdinfo) );
5205                                 }
5206 #ifdef CONFIG_CONCURRENT_MODE
5207                                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
5208                                 {
5209                                         _cancel_timer_ex( &pwdinfo->ap_p2p_switch_timer );
5210                                 }
5211 #endif /*  CONFIG_CONCURRENT_MODE */
5212
5213                                 /*      Commented by Kurt 20110902 */
5214                                 /* Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
5215                                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
5216                                         rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
5217
5218                                 /*      Commented by Kurt 20120113 */
5219                                 /*      Get peer_dev_addr here if peer doesn't issue prov_disc frame. */
5220                                 if (!memcmp(pwdinfo->rx_prov_disc_info.peerDevAddr, empty_addr, ETH_ALEN))
5221                                         memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
5222
5223                                 result = process_p2p_group_negotation_req( pwdinfo, frame_body, len );
5224                                 issue_p2p_GO_response( padapter, GetAddr2Ptr(pframe), frame_body, len, result );
5225 #ifdef CONFIG_INTEL_WIDI
5226                                 if( (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) && (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_WFD_CONNECTION) )
5227                                 {
5228                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
5229                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL);
5230                                 }
5231 #endif /* CONFIG_INTEL_WIDI */
5232
5233                                 /*      Commented by Albert 20110718 */
5234                                 /*      No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer. */
5235 #ifdef CONFIG_CONCURRENT_MODE
5236                                 /*      Commented by Albert 20120107 */
5237                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
5238 #else /*  CONFIG_CONCURRENT_MODE */
5239                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
5240 #endif /*  CONFIG_CONCURRENT_MODE */
5241                                 break;
5242                         }
5243                         case P2P_GO_NEGO_RESP:
5244                         {
5245                                 DBG_8723A( "[%s] Got GO Nego Resp Frame\n", __FUNCTION__);
5246
5247                                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
5248                                 {
5249                                         /*      Commented by Albert 20110425 */
5250                                         /*      The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function. */
5251                                         _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
5252                                         pwdinfo->nego_req_info.benable = _FALSE;
5253                                         result = process_p2p_group_negotation_resp( pwdinfo, frame_body, len);
5254                                         issue_p2p_GO_confirm( pwdinfo->padapter, GetAddr2Ptr(pframe), result);
5255                                         if ( P2P_STATUS_SUCCESS == result )
5256                                         {
5257                                                 if ( rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT )
5258                                                 {
5259                                                         pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
5260                                                         pwdinfo->p2p_info.scan_op_ch_only = 1;
5261                                                         _set_timer( &pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH );
5262                                                 }
5263                                         }
5264
5265                                         /*      Reset the dialog token for group negotiation frames. */
5266                                         pwdinfo->negotiation_dialog_token = 1;
5267
5268                                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
5269                                         {
5270                                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
5271                                         }
5272                                 }
5273                                 else
5274                                 {
5275                                         DBG_8723A( "[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __FUNCTION__);
5276                                 }
5277
5278                                 break;
5279                         }
5280                         case P2P_GO_NEGO_CONF:
5281                         {
5282                                 DBG_8723A( "[%s] Got GO Nego Confirm Frame\n", __FUNCTION__);
5283                                 result = process_p2p_group_negotation_confirm( pwdinfo, frame_body, len);
5284                                 if ( P2P_STATUS_SUCCESS == result )
5285                                 {
5286                                         if ( rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT )
5287                                         {
5288                                                 pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
5289                                                 pwdinfo->p2p_info.scan_op_ch_only = 1;
5290                                                 _set_timer( &pwdinfo->reset_ch_sitesurvey2, P2P_RESET_SCAN_CH );
5291                                         }
5292                                 }
5293                                 break;
5294                         }
5295                         case P2P_INVIT_REQ:
5296                         {
5297                                 /*      Added by Albert 2010/10/05 */
5298                                 /*      Received the P2P Invite Request frame. */
5299
5300                                 DBG_8723A( "[%s] Got invite request frame!\n", __FUNCTION__ );
5301                                 if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
5302                                 {
5303                                         /*      Parse the necessary information from the P2P Invitation Request frame. */
5304                                         /*      For example: The MAC address of sending this P2P Invitation Request frame. */
5305                                         u32     attr_contentlen = 0;
5306                                         u8      status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
5307                                         struct group_id_info group_id;
5308                                         u8      invitation_flag = 0;
5309
5310                                         rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen);
5311                                         if ( attr_contentlen )
5312                                         {
5313
5314                                                 rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen);
5315                                                 /*      Commented by Albert 20120510 */
5316                                                 /*      Copy to the pwdinfo->p2p_peer_interface_addr. */
5317                                                 /*      So that the WFD UI ( or Sigma ) can get the peer interface address by using the following command. */
5318                                                 /*      #> iwpriv wlan0 p2p_get peer_ifa */
5319                                                 /*      After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant. */
5320
5321                                                 if ( attr_contentlen )
5322                                                 {
5323                                                         DBG_8723A( "[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __FUNCTION__,
5324                                                                         pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1],
5325                                                                         pwdinfo->p2p_peer_interface_addr[2], pwdinfo->p2p_peer_interface_addr[3],
5326                                                                         pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5] );
5327                                                 }
5328
5329                                                 if ( invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT )
5330                                                 {
5331                                                         /*      Re-invoke the persistent group. */
5332
5333                                                         memset( &group_id, 0x00, sizeof( struct group_id_info ) );
5334                                                         rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, ( u8* ) &group_id, &attr_contentlen);
5335                                                         if ( attr_contentlen )
5336                                                         {
5337                                                                 if (!memcmp(group_id.go_device_addr, myid( &padapter->eeprompriv), ETH_ALEN))
5338                                                                 {
5339                                                                         /*      The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO. */
5340                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO );
5341                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_GO );
5342                                                                         status_code = P2P_STATUS_SUCCESS;
5343                                                                 }
5344                                                                 else
5345                                                                 {
5346                                                                         /*      The p2p device sending this p2p invitation request wants to be the persistent GO. */
5347                                                                         if ( is_matched_in_profilelist( pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[ 0 ] ) )
5348                                                                         {
5349                                                                                 u8 operatingch_info[5] = { 0x00 };
5350                                                                                 if ( rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen) )
5351                                                                                 {
5352                                                                                         if( rtw_ch_set_search_ch(padapter->mlmeextpriv.channel_set, (u32)operatingch_info[4] ) )
5353                                                                                         {
5354                                                                                                 /*      The operating channel is acceptable for this device. */
5355                                                                                                 pwdinfo->rx_invitereq_info.operation_ch[0]= operatingch_info[4];
5356                                                                                                 pwdinfo->rx_invitereq_info.scan_op_ch_only = 1;
5357                                                                                                 _set_timer( &pwdinfo->reset_ch_sitesurvey, P2P_RESET_SCAN_CH );
5358                                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH );
5359                                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
5360                                                                                                 status_code = P2P_STATUS_SUCCESS;
5361                                                                                                 }
5362                                                                                         else
5363                                                                                         {
5364                                                                                                 /*      The operating channel isn't supported by this device. */
5365                                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
5366                                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_DEVICE );
5367                                                                                                 status_code = P2P_STATUS_FAIL_NO_COMMON_CH;
5368                                                                                                 _set_timer( &pwdinfo->restore_p2p_state_timer, 3000 );
5369                                                                                         }
5370                                                                                 }
5371                                                                                 else
5372                                                                                 {
5373                                                                                         /*      Commented by Albert 20121130 */
5374                                                                                         /*      Intel will use the different P2P IE to store the operating channel information */
5375                                                                                         /*      Workaround for Intel WiDi 3.5 */
5376                                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH );
5377                                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
5378                                                                                         status_code = P2P_STATUS_SUCCESS;
5379                                                                                 }
5380                                                                         }
5381                                                                         else
5382                                                                         {
5383                                                                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
5384                                                                                 #ifdef CONFIG_INTEL_WIDI
5385                                                                                 memcpy(pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN );
5386                                                                                 rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
5387                                                                                 #endif /* CONFIG_INTEL_WIDI */
5388
5389                                                                                 status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
5390                                                                         }
5391                                                                 }
5392                                                         }
5393                                                         else
5394                                                         {
5395                                                                 DBG_8723A( "[%s] P2P Group ID Attribute NOT FOUND!\n", __FUNCTION__ );
5396                                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
5397                                                         }
5398                                                 }
5399                                                 else
5400                                                 {
5401                                                         /*      Received the invitation to join a P2P group. */
5402
5403                                                         memset( &group_id, 0x00, sizeof( struct group_id_info ) );
5404                                                         rtw_get_p2p_attr_content( p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, ( u8* ) &group_id, &attr_contentlen);
5405                                                         if ( attr_contentlen )
5406                                                         {
5407                                                                 if (!memcmp(group_id.go_device_addr, myid( &padapter->eeprompriv), ETH_ALEN))
5408                                                                 {
5409                                                                         /*      In this case, the GO can't be myself. */
5410                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH );
5411                                                                         status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
5412                                                                 }
5413                                                                 else
5414                                                                 {
5415                                                                         /*      The p2p device sending this p2p invitation request wants to join an existing P2P group */
5416                                                                         /*      Commented by Albert 2012/06/28 */
5417                                                                         /*      In this case, this Wi-Fi device should use the iwpriv command to get the peer device address. */
5418                                                                         /*      The peer device address should be the destination address for the provisioning discovery request. */
5419                                                                         /*      Then, this Wi-Fi device should use the iwpriv command to get the peer interface address. */
5420                                                                         /*      The peer interface address should be the address for WPS mac address */
5421                                                                         memcpy(pwdinfo->p2p_peer_device_addr, group_id.go_device_addr , ETH_ALEN );
5422                                                                         rtw_p2p_set_role( pwdinfo, P2P_ROLE_CLIENT );
5423                                                                         rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN );
5424                                                                         status_code = P2P_STATUS_SUCCESS;
5425                                                                 }
5426                                                         }
5427                                                         else
5428                                                         {
5429                                                                 DBG_8723A( "[%s] P2P Group ID Attribute NOT FOUND!\n", __FUNCTION__ );
5430                                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
5431                                                         }
5432                                                 }
5433                                         }
5434                                         else
5435                                         {
5436                                                 DBG_8723A( "[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __FUNCTION__ );
5437                                                 status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
5438                                         }
5439
5440                                         DBG_8723A( "[%s] status_code = %d\n", __FUNCTION__, status_code );
5441
5442                                         pwdinfo->inviteresp_info.token = frame_body[ 7 ];
5443                                         issue_p2p_invitation_response( padapter, GetAddr2Ptr(pframe), pwdinfo->inviteresp_info.token, status_code );
5444                                 }
5445 #ifdef CONFIG_INTEL_WIDI
5446                                 if( (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) && (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_WFD_CONNECTION) )
5447                                 {
5448                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
5449                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL);
5450                                 }
5451 #endif /* CONFIG_INTEL_WIDI */
5452                                 break;
5453                         }
5454                         case P2P_INVIT_RESP:
5455                         {
5456                                 u8      attr_content = 0x00;
5457                                 u32     attr_contentlen = 0;
5458
5459                                 DBG_8723A( "[%s] Got invite response frame!\n", __FUNCTION__ );
5460                                 _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
5461                                 if ( (p2p_ie=rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)) )
5462                                 {
5463                                         rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
5464
5465                                         if ( attr_contentlen == 1 )
5466                                         {
5467                                                 DBG_8723A( "[%s] Status = %d\n", __FUNCTION__, attr_content );
5468                                                 pwdinfo->invitereq_info.benable = _FALSE;
5469
5470                                                 if ( attr_content == P2P_STATUS_SUCCESS )
5471                                                 {
5472                                                         if (!memcmp(pwdinfo->invitereq_info.go_bssid, myid( &padapter->eeprompriv), ETH_ALEN))
5473                                                         {
5474                                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO );
5475                                                         }
5476                                                         else
5477                                                         {
5478                                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
5479                                                         }
5480                                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_OK );
5481                                                 }
5482                                                 else
5483                                                 {
5484                                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
5485                                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
5486                                                 }
5487                                         }
5488                                         else
5489                                         {
5490                                                 rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
5491                                                 rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
5492                                         }
5493                                 }
5494                                 else
5495                                 {
5496                                         rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
5497                                         rtw_p2p_set_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL );
5498                                 }
5499
5500                                 if ( rtw_p2p_chk_state( pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL ) )
5501                                 {
5502                                         _set_timer( &pwdinfo->restore_p2p_state_timer, 5000 );
5503                                 }
5504                                 break;
5505                         }
5506                         case P2P_DEVDISC_REQ:
5507
5508                                 process_p2p_devdisc_req(pwdinfo, pframe, len);
5509
5510                                 break;
5511
5512                         case P2P_DEVDISC_RESP:
5513
5514                                 process_p2p_devdisc_resp(pwdinfo, pframe, len);
5515
5516                                 break;
5517
5518                         case P2P_PROVISION_DISC_REQ:
5519                                 DBG_8723A( "[%s] Got Provisioning Discovery Request Frame\n", __FUNCTION__ );
5520                                 process_p2p_provdisc_req(pwdinfo, pframe, len);
5521                                 memcpy(pwdinfo->rx_prov_disc_info.peerDevAddr, GetAddr2Ptr(pframe), ETH_ALEN);
5522
5523                                 /* 20110902 Kurt */
5524                                 /* Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
5525                                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
5526                                         rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
5527
5528                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ);
5529                                 _set_timer( &pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT );
5530 #ifdef CONFIG_INTEL_WIDI
5531                                 if( (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_LISTEN) && (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_WFD_CONNECTION) )
5532                                 {
5533                                         padapter->mlmepriv.widi_state = INTEL_WIDI_STATE_WFD_CONNECTION;
5534                                         intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_STOP_WK, NULL);
5535                                 }
5536 #endif /* CONFIG_INTEL_WIDI */
5537                                 break;
5538
5539                         case P2P_PROVISION_DISC_RESP:
5540                                 /*      Commented by Albert 20110707 */
5541                                 /*      Should we check the pwdinfo->tx_prov_disc_info.bsent flag here?? */
5542                                 DBG_8723A( "[%s] Got Provisioning Discovery Response Frame\n", __FUNCTION__ );
5543                                 /*      Commented by Albert 20110426 */
5544                                 /*      The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function. */
5545                                 _cancel_timer_ex( &pwdinfo->restore_p2p_state_timer );
5546                                 rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP);
5547                                 process_p2p_provdisc_resp(pwdinfo, pframe);
5548                                 _set_timer( &pwdinfo->restore_p2p_state_timer, P2P_PROVISION_TIMEOUT );
5549                                 break;
5550
5551                 }
5552         }
5553 #endif /* CONFIG_P2P */
5554
5555         return _SUCCESS;
5556 }
5557
5558 unsigned int on_action_public_vendor(union recv_frame *precv_frame)
5559 {
5560         unsigned int ret = _FAIL;
5561         u8 *pframe = precv_frame->u.hdr.rx_data;
5562         uint frame_len = precv_frame->u.hdr.len;
5563         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
5564
5565         if (!memcmp(frame_body + 2, P2P_OUI, 4)) {
5566                 ret = on_action_public_p2p(precv_frame);
5567         }
5568
5569         return ret;
5570 }
5571
5572 unsigned int on_action_public_default(union recv_frame *precv_frame, u8 action)
5573 {
5574         unsigned int ret = _FAIL;
5575         u8 *pframe = precv_frame->u.hdr.rx_data;
5576         uint frame_len = precv_frame->u.hdr.len;
5577         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
5578         u8 token;
5579         _adapter *adapter = precv_frame->u.hdr.adapter;
5580         int cnt = 0;
5581         char msg[64];
5582
5583         token = frame_body[2];
5584
5585         if (rtw_action_public_decache(precv_frame, token) == _FAIL)
5586                 goto exit;
5587
5588         #ifdef CONFIG_IOCTL_CFG80211
5589         cnt += sprintf((msg+cnt), "%s(token:%u)", action_public_str(action), token);
5590         rtw_cfg80211_rx_action(adapter, pframe, frame_len, msg);
5591         #endif
5592
5593         ret = _SUCCESS;
5594
5595 exit:
5596         return ret;
5597 }
5598
5599 unsigned int on_action_public(_adapter *padapter, union recv_frame *precv_frame)
5600 {
5601         unsigned int ret = _FAIL;
5602         u8 *pframe = precv_frame->u.hdr.rx_data;
5603         uint frame_len = precv_frame->u.hdr.len;
5604         u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
5605         u8 category, action;
5606
5607         /* check RA matches or not */
5608         if (memcmp(myid(&(padapter->eeprompriv)),
5609                    GetAddr1Ptr(pframe), ETH_ALEN))
5610                 goto exit;
5611
5612         category = frame_body[0];
5613         if(category != RTW_WLAN_CATEGORY_PUBLIC)
5614                 goto exit;
5615
5616         action = frame_body[1];
5617         switch (action) {
5618         case ACT_PUBLIC_VENDOR:
5619                 ret = on_action_public_vendor(precv_frame);
5620                 break;
5621         default:
5622                 ret = on_action_public_default(precv_frame, action);
5623                 break;
5624         }
5625
5626 exit:
5627         return ret;
5628 }
5629
5630 unsigned int OnAction_ht(_adapter *padapter, union recv_frame *precv_frame)
5631 {
5632         return _SUCCESS;
5633 }
5634
5635 unsigned int OnAction_wmm(_adapter *padapter, union recv_frame *precv_frame)
5636 {
5637         return _SUCCESS;
5638 }
5639
5640 unsigned int OnAction_p2p(_adapter *padapter, union recv_frame *precv_frame)
5641 {
5642 #ifdef CONFIG_P2P
5643         u8 *frame_body;
5644         u8 category, OUI_Subtype, dialogToken=0;
5645         u8 *pframe = precv_frame->u.hdr.rx_data;
5646         uint len = precv_frame->u.hdr.len;
5647         struct  wifidirect_info *pwdinfo = &( padapter->wdinfo );
5648
5649         DBG_8723A("%s\n", __FUNCTION__);
5650
5651         /* check RA matches or not */
5652         if (memcmp(myid(&(padapter->eeprompriv)),
5653                    GetAddr1Ptr(pframe), ETH_ALEN))/* for if1, sta/ap mode */
5654                 return _SUCCESS;
5655
5656         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
5657
5658         category = frame_body[0];
5659         if(category != RTW_WLAN_CATEGORY_P2P)
5660                 return _SUCCESS;
5661
5662         if ( cpu_to_be32( *( ( u32* ) ( frame_body + 1 ) ) ) != P2POUI )
5663                 return _SUCCESS;
5664
5665 #ifdef CONFIG_IOCTL_CFG80211
5666         if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
5667         {
5668                 rtw_cfg80211_rx_action_p2p(padapter, pframe, len);
5669                 return _SUCCESS;
5670         }
5671         else
5672 #endif /* CONFIG_IOCTL_CFG80211 */
5673         {
5674                 len -= sizeof(struct rtw_ieee80211_hdr_3addr);
5675                 OUI_Subtype = frame_body[5];
5676                 dialogToken = frame_body[6];
5677
5678                 switch(OUI_Subtype)
5679                 {
5680                         case P2P_NOTICE_OF_ABSENCE:
5681
5682                                 break;
5683
5684                         case P2P_PRESENCE_REQUEST:
5685
5686                                 process_p2p_presence_req(pwdinfo, pframe, len);
5687
5688                                 break;
5689
5690                         case P2P_PRESENCE_RESPONSE:
5691
5692                                 break;
5693
5694                         case P2P_GO_DISC_REQUEST:
5695
5696                                 break;
5697
5698                         default:
5699                                 break;
5700
5701                 }
5702         }
5703 #endif /* CONFIG_P2P */
5704
5705         return _SUCCESS;
5706 }
5707
5708 unsigned int OnAction(_adapter *padapter, union recv_frame *precv_frame)
5709 {
5710         int i;
5711         unsigned char   category;
5712         struct action_handler *ptable;
5713         unsigned char   *frame_body;
5714         u8 *pframe = precv_frame->u.hdr.rx_data;
5715
5716         frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
5717
5718         category = frame_body[0];
5719
5720         for(i = 0; i < sizeof(OnAction_tbl)/sizeof(struct action_handler); i++)
5721         {
5722                 ptable = &OnAction_tbl[i];
5723
5724                 if(category == ptable->num)
5725                         ptable->func(padapter, precv_frame);
5726
5727         }
5728
5729         return _SUCCESS;
5730 }
5731
5732 unsigned int DoReserved(_adapter *padapter, union recv_frame *precv_frame)
5733 {
5734
5735         /* DBG_8723A("rcvd mgt frame(%x, %x)\n", (GetFrameSubType(pframe) >> 4), *(unsigned int *)GetAddr1Ptr(pframe)); */
5736         return _SUCCESS;
5737 }
5738
5739 struct xmit_frame *_alloc_mgtxmitframe(struct xmit_priv *pxmitpriv, bool once)
5740 {
5741         struct xmit_frame *pmgntframe;
5742         struct xmit_buf *pxmitbuf;
5743
5744         if (once)
5745                 pmgntframe = rtw_alloc_xmitframe_once(pxmitpriv);
5746         else
5747                 pmgntframe = rtw_alloc_xmitframe_ext(pxmitpriv);
5748
5749         if (pmgntframe == NULL) {
5750                 DBG_8723A(FUNC_ADPT_FMT" alloc xmitframe fail, once:%d\n", FUNC_ADPT_ARG(pxmitpriv->adapter), once);
5751                 goto exit;
5752         }
5753
5754         if ((pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv)) == NULL) {
5755                 DBG_8723A(FUNC_ADPT_FMT" alloc xmitbuf fail\n", FUNC_ADPT_ARG(pxmitpriv->adapter));
5756                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
5757                 pmgntframe = NULL;
5758                 goto exit;
5759         }
5760
5761         pmgntframe->frame_tag = MGNT_FRAMETAG;
5762         pmgntframe->pxmitbuf = pxmitbuf;
5763         pmgntframe->buf_addr = pxmitbuf->pbuf;
5764         pxmitbuf->priv_data = pmgntframe;
5765
5766 exit:
5767         return pmgntframe;
5768 }
5769
5770 inline struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
5771 {
5772         return _alloc_mgtxmitframe(pxmitpriv, _FALSE);
5773 }
5774
5775 inline struct xmit_frame *alloc_mgtxmitframe_once(struct xmit_priv *pxmitpriv)
5776 {
5777         return _alloc_mgtxmitframe(pxmitpriv, _TRUE);
5778 }
5779
5780 /****************************************************************************
5781
5782 Following are some TX fuctions for WiFi MLME
5783
5784 *****************************************************************************/
5785
5786 void update_mgnt_tx_rate(_adapter *padapter, u8 rate)
5787 {
5788         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5789
5790         pmlmeext->tx_rate = rate;
5791         DBG_8723A("%s(): rate = %x\n",__FUNCTION__, rate);
5792 }
5793
5794 void update_mgntframe_attrib(_adapter *padapter, struct pkt_attrib *pattrib)
5795 {
5796         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5797
5798         memset((u8 *)(pattrib), 0, sizeof(struct pkt_attrib));
5799
5800         pattrib->hdrlen = 24;
5801         pattrib->nr_frags = 1;
5802         pattrib->priority = 7;
5803         pattrib->mac_id = 0;
5804         pattrib->qsel = 0x12;
5805
5806         pattrib->pktlen = 0;
5807
5808         if(pmlmeext->cur_wireless_mode & WIRELESS_11B)
5809                 pattrib->raid = 6;/* b mode */
5810         else
5811                 pattrib->raid = 5;/* a/g mode */
5812
5813         pattrib->encrypt = _NO_PRIVACY_;
5814         pattrib->bswenc = _FALSE;
5815
5816         pattrib->qos_en = _FALSE;
5817         pattrib->ht_en = _FALSE;
5818         pattrib->bwmode = HT_CHANNEL_WIDTH_20;
5819         pattrib->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
5820         pattrib->sgi = _FALSE;
5821
5822         pattrib->seqnum = pmlmeext->mgnt_seq;
5823
5824         pattrib->retry_ctrl = _TRUE;
5825 }
5826
5827 void dump_mgntframe(_adapter *padapter, struct xmit_frame *pmgntframe)
5828 {
5829         if(padapter->bSurpriseRemoved == _TRUE ||
5830                 padapter->bDriverStopped == _TRUE)
5831                 return;
5832
5833         rtw_hal_mgnt_xmit(padapter, pmgntframe);
5834 }
5835
5836 s32 dump_mgntframe_and_wait(_adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
5837 {
5838         s32 ret = _FAIL;
5839         _irqL irqL;
5840         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
5841         struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
5842         struct submit_ctx sctx;
5843
5844         if(padapter->bSurpriseRemoved == _TRUE ||
5845                 padapter->bDriverStopped == _TRUE)
5846                 return ret;
5847
5848         rtw_sctx_init(&sctx, timeout_ms);
5849         pxmitbuf->sctx = &sctx;
5850
5851         ret = rtw_hal_mgnt_xmit(padapter, pmgntframe);
5852
5853         if (ret == _SUCCESS)
5854                 ret = rtw_sctx_wait(&sctx);
5855
5856         spin_lock_irqsave(&pxmitpriv->lock_sctx, irqL);
5857         pxmitbuf->sctx = NULL;
5858         spin_unlock_irqrestore(&pxmitpriv->lock_sctx, irqL);
5859
5860          return ret;
5861 }
5862
5863 s32 dump_mgntframe_and_wait_ack(_adapter *padapter, struct xmit_frame *pmgntframe)
5864 {
5865 #ifdef CONFIG_XMIT_ACK
5866         s32 ret = _FAIL;
5867         u32 timeout_ms = 500;/*   500ms */
5868         struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
5869         #ifdef CONFIG_CONCURRENT_MODE
5870         if (padapter->pbuddy_adapter && !padapter->isprimary)
5871                 pxmitpriv = &(padapter->pbuddy_adapter->xmitpriv);
5872         #endif
5873
5874         if(padapter->bSurpriseRemoved == _TRUE ||
5875                 padapter->bDriverStopped == _TRUE)
5876                 return -1;
5877
5878         _enter_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
5879         pxmitpriv->ack_tx = _TRUE;
5880
5881         pmgntframe->ack_report = 1;
5882         if (rtw_hal_mgnt_xmit(padapter, pmgntframe) == _SUCCESS) {
5883                 ret = rtw_ack_tx_wait(pxmitpriv, timeout_ms);
5884         }
5885
5886         pxmitpriv->ack_tx = _FALSE;
5887         _exit_critical_mutex(&pxmitpriv->ack_tx_mutex, NULL);
5888
5889          return ret;
5890 #else /* CONFIG_XMIT_ACK */
5891         dump_mgntframe(padapter, pmgntframe);
5892         rtw_msleep_os(50);
5893         return _SUCCESS;
5894 #endif /* CONFIG_XMIT_ACK */
5895 }
5896
5897 int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
5898 {
5899         u8 *ssid_ie;
5900         int ssid_len_ori;
5901         int len_diff = 0;
5902
5903         ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
5904
5905         /* DBG_8723A("%s hidden_ssid_mode:%u, ssid_ie:%p, ssid_len_ori:%d\n", __FUNCTION__, hidden_ssid_mode, ssid_ie, ssid_len_ori); */
5906
5907         if(ssid_ie && ssid_len_ori>0)
5908         {
5909                 switch(hidden_ssid_mode)
5910                 {
5911                         case 1:
5912                         {
5913                                 u8 *next_ie = ssid_ie + 2 + ssid_len_ori;
5914                                 u32 remain_len = 0;
5915
5916                                 remain_len = ies_len -(next_ie-ies);
5917
5918                                 ssid_ie[1] = 0;
5919                                 memcpy(ssid_ie+2, next_ie, remain_len);
5920                                 len_diff -= ssid_len_ori;
5921
5922                                 break;
5923                         }
5924                         case 2:
5925                                 memset(&ssid_ie[2], 0, ssid_len_ori);
5926                                 break;
5927                         default:
5928                                 break;
5929                 }
5930         }
5931
5932         return len_diff;
5933 }
5934
5935 void issue_beacon(_adapter *padapter, int timeout_ms)
5936 {
5937         struct xmit_frame       *pmgntframe;
5938         struct pkt_attrib       *pattrib;
5939         unsigned char   *pframe;
5940         struct rtw_ieee80211_hdr *pwlanhdr;
5941         unsigned short *fctrl;
5942         unsigned int    rate_len;
5943         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
5944 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
5945         _irqL irqL;
5946         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
5947 #endif /* if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */
5948         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
5949         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
5950         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
5951         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
5952 #ifdef CONFIG_P2P
5953         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
5954 #endif /* CONFIG_P2P */
5955
5956         /* DBG_8723A("%s\n", __FUNCTION__); */
5957
5958         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
5959         {
5960                 DBG_8723A("%s, alloc mgnt frame fail\n", __FUNCTION__);
5961                 return;
5962         }
5963 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
5964         spin_lock_bh(&pmlmepriv->bcn_update_lock);
5965 #endif /* if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */
5966
5967         /* update attribute */
5968         pattrib = &pmgntframe->attrib;
5969         update_mgntframe_attrib(padapter, pattrib);
5970         pattrib->qsel = 0x10;
5971
5972         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
5973
5974         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
5975         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
5976
5977         fctrl = &(pwlanhdr->frame_ctl);
5978         *(fctrl) = 0;
5979
5980         memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
5981         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
5982         memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
5983
5984         SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
5985         /* pmlmeext->mgnt_seq++; */
5986         SetFrameSubType(pframe, WIFI_BEACON);
5987
5988         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
5989         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
5990
5991         if( (pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
5992         {
5993                 /* DBG_8723A("ie len=%d\n", cur_network->IELength); */
5994 #ifdef CONFIG_P2P
5995                 /*  for P2P : Primary Device Type & Device Name */
5996                 u32 wpsielen=0, insert_len=0;
5997                 u8 *wpsie=NULL;
5998                 wpsie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wpsielen);
5999
6000                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wpsie && wpsielen>0)
6001                 {
6002                         uint wps_offset, remainder_ielen;
6003                         u8 *premainder_ie, *pframe_wscie;
6004
6005                         wps_offset = (uint)(wpsie - cur_network->IEs);
6006
6007                         premainder_ie = wpsie + wpsielen;
6008
6009                         remainder_ielen = cur_network->IELength - wps_offset - wpsielen;
6010
6011 #ifdef CONFIG_IOCTL_CFG80211
6012                         if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
6013                         {
6014                                 if(pmlmepriv->wps_beacon_ie && pmlmepriv->wps_beacon_ie_len>0)
6015                                 {
6016                                         memcpy(pframe, cur_network->IEs, wps_offset);
6017                                         pframe += wps_offset;
6018                                         pattrib->pktlen += wps_offset;
6019
6020                                         memcpy(pframe, pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len);
6021                                         pframe += pmlmepriv->wps_beacon_ie_len;
6022                                         pattrib->pktlen += pmlmepriv->wps_beacon_ie_len;
6023
6024                                         /* copy remainder_ie to pframe */
6025                                         memcpy(pframe, premainder_ie, remainder_ielen);
6026                                         pframe += remainder_ielen;
6027                                         pattrib->pktlen += remainder_ielen;
6028                                 }
6029                                 else
6030                                 {
6031                                         memcpy(pframe, cur_network->IEs, cur_network->IELength);
6032                                         pframe += cur_network->IELength;
6033                                         pattrib->pktlen += cur_network->IELength;
6034                                 }
6035                         }
6036                         else
6037 #endif /* CONFIG_IOCTL_CFG80211 */
6038                         {
6039                                 pframe_wscie = pframe + wps_offset;
6040                                 memcpy(pframe, cur_network->IEs, wps_offset+wpsielen);
6041                                 pframe += (wps_offset + wpsielen);
6042                                 pattrib->pktlen += (wps_offset + wpsielen);
6043
6044                                 /* now pframe is end of wsc ie, insert Primary Device Type & Device Name */
6045                                 /*      Primary Device Type */
6046                                 /*      Type: */
6047                                 *(u16*) ( pframe + insert_len) = cpu_to_be16( WPS_ATTR_PRIMARY_DEV_TYPE );
6048                                 insert_len += 2;
6049
6050                                 /*      Length: */
6051                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( 0x0008 );
6052                                 insert_len += 2;
6053
6054                                 /*      Value: */
6055                                 /*      Category ID */
6056                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
6057                                 insert_len += 2;
6058
6059                                 /*      OUI */
6060                                 *(u32*) ( pframe + insert_len ) = cpu_to_be32( WPSOUI );
6061                                 insert_len += 4;
6062
6063                                 /*      Sub Category ID */
6064                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
6065                                 insert_len += 2;
6066
6067                                 /*      Device Name */
6068                                 /*      Type: */
6069                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
6070                                 insert_len += 2;
6071
6072                                 /*      Length: */
6073                                 *(u16*) ( pframe + insert_len ) = cpu_to_be16( pwdinfo->device_name_len );
6074                                 insert_len += 2;
6075
6076                                 /*      Value: */
6077                                 memcpy(pframe + insert_len, pwdinfo->device_name, pwdinfo->device_name_len );
6078                                 insert_len += pwdinfo->device_name_len;
6079
6080                                 /* update wsc ie length */
6081                                 *(pframe_wscie+1) = (wpsielen -2) + insert_len;
6082
6083                                 /* pframe move to end */
6084                                 pframe+=insert_len;
6085                                 pattrib->pktlen += insert_len;
6086
6087                                 /* copy remainder_ie to pframe */
6088                                 memcpy(pframe, premainder_ie, remainder_ielen);
6089                                 pframe += remainder_ielen;
6090                                 pattrib->pktlen += remainder_ielen;
6091                         }
6092                 }
6093                 else
6094 #endif /* CONFIG_P2P */
6095                 {
6096                         int len_diff;
6097                         memcpy(pframe, cur_network->IEs, cur_network->IELength);
6098                         len_diff = update_hidden_ssid(
6099                                 pframe+_BEACON_IE_OFFSET_
6100                                 , cur_network->IELength-_BEACON_IE_OFFSET_
6101                                 , pmlmeinfo->hidden_ssid_mode
6102                         );
6103                         pframe += (cur_network->IELength+len_diff);
6104                         pattrib->pktlen += (cur_network->IELength+len_diff);
6105                 }
6106
6107                 {
6108                         u8 *wps_ie;
6109                         uint wps_ielen;
6110                         u8 sr = 0;
6111                         wps_ie = rtw_get_wps_ie(pmgntframe->buf_addr+TXDESC_OFFSET+sizeof (struct rtw_ieee80211_hdr_3addr)+_BEACON_IE_OFFSET_,
6112                                 pattrib->pktlen-sizeof (struct rtw_ieee80211_hdr_3addr)-_BEACON_IE_OFFSET_, NULL, &wps_ielen);
6113                         if (wps_ie && wps_ielen>0) {
6114                                 rtw_get_wps_attr_content(wps_ie,  wps_ielen, WPS_ATTR_SELECTED_REGISTRAR, (u8*)(&sr), NULL);
6115                         }
6116                         if (sr != 0)
6117                                 set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
6118                         else
6119                                 _clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
6120                 }
6121
6122 #ifdef CONFIG_P2P
6123                 if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
6124                 {
6125                         u32 len;
6126 #ifdef CONFIG_IOCTL_CFG80211
6127                         if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
6128                         {
6129                                 len = pmlmepriv->p2p_beacon_ie_len;
6130                                 if(pmlmepriv->p2p_beacon_ie && len>0)
6131                                         memcpy(pframe, pmlmepriv->p2p_beacon_ie, len);
6132                         }
6133                         else
6134 #endif /* CONFIG_IOCTL_CFG80211 */
6135                         {
6136                                 len = build_beacon_p2p_ie(pwdinfo, pframe);
6137                         }
6138
6139                         pframe += len;
6140                         pattrib->pktlen += len;
6141 #ifdef CONFIG_WFD
6142 #ifdef CONFIG_IOCTL_CFG80211
6143                         if(_TRUE == pwdinfo->wfd_info->wfd_enable)
6144 #endif /* CONFIG_IOCTL_CFG80211 */
6145                         {
6146                                 len = build_beacon_wfd_ie( pwdinfo, pframe );
6147                         }
6148 #ifdef CONFIG_IOCTL_CFG80211
6149                         else
6150                         {
6151                                 len = 0;
6152                                 if(pmlmepriv->wfd_beacon_ie && pmlmepriv->wfd_beacon_ie_len>0)
6153                                 {
6154                                         len = pmlmepriv->wfd_beacon_ie_len;
6155                                         memcpy(pframe, pmlmepriv->wfd_beacon_ie, len);
6156                         }
6157                         }
6158 #endif /* CONFIG_IOCTL_CFG80211 */
6159                         pframe += len;
6160                         pattrib->pktlen += len;
6161 #endif /* CONFIG_WFD */
6162                 }
6163 #endif /* CONFIG_P2P */
6164
6165                 goto _issue_bcn;
6166
6167         }
6168
6169         /* below for ad-hoc mode */
6170
6171         /* timestamp will be inserted by hardware */
6172         pframe += 8;
6173         pattrib->pktlen += 8;
6174
6175         /*  beacon interval: 2 bytes */
6176
6177         memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
6178
6179         pframe += 2;
6180         pattrib->pktlen += 2;
6181
6182         /*  capability info: 2 bytes */
6183
6184         memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
6185
6186         pframe += 2;
6187         pattrib->pktlen += 2;
6188
6189         /*  SSID */
6190         pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
6191
6192         /*  supported rates... */
6193         rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
6194         pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8)? 8: rate_len), cur_network->SupportedRates, &pattrib->pktlen);
6195
6196         /*  DS parameter set */
6197         pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
6198
6199         /* if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) */
6200         {
6201                 u8 erpinfo=0;
6202                 u32 ATIMWindow;
6203                 /*  IBSS Parameter Set... */
6204                 /* ATIMWindow = cur->Configuration.ATIMWindow; */
6205                 ATIMWindow = 0;
6206                 pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
6207
6208                 /* ERP IE */
6209                 pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
6210         }
6211
6212         /*  EXTERNDED SUPPORTED RATE */
6213         if (rate_len > 8)
6214         {
6215                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
6216         }
6217
6218         /* todo:HT for adhoc */
6219
6220 _issue_bcn:
6221
6222 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6223         pmlmepriv->update_bcn = _FALSE;
6224
6225         spin_unlock_bh(&pmlmepriv->bcn_update_lock);
6226 #endif /* if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */
6227
6228         if ((pattrib->pktlen + TXDESC_SIZE) > 512)
6229         {
6230                 DBG_8723A("beacon frame too large\n");
6231                 return;
6232         }
6233
6234         pattrib->last_txcmdsz = pattrib->pktlen;
6235
6236         /* DBG_8723A("issue bcn_sz=%d\n", pattrib->last_txcmdsz); */
6237         if(timeout_ms > 0)
6238                 dump_mgntframe_and_wait(padapter, pmgntframe, timeout_ms);
6239         else
6240                 dump_mgntframe(padapter, pmgntframe);
6241 }
6242
6243 void issue_probersp(_adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
6244 {
6245         struct xmit_frame                       *pmgntframe;
6246         struct pkt_attrib                       *pattrib;
6247         unsigned char                                   *pframe;
6248         struct rtw_ieee80211_hdr        *pwlanhdr;
6249         unsigned short                          *fctrl;
6250         unsigned char                                   *mac, *bssid;
6251         struct xmit_priv        *pxmitpriv = &(padapter->xmitpriv);
6252 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6253         u8 *pwps_ie;
6254         uint wps_ielen;
6255         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
6256 #endif /* if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME) */
6257         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6258         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6259         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
6260         unsigned int    rate_len;
6261 #ifdef CONFIG_P2P
6262         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
6263 #ifdef CONFIG_WFD
6264         u32                                     wfdielen = 0;
6265 #endif /* CONFIG_WFD */
6266 #endif /* CONFIG_P2P */
6267
6268         /* DBG_8723A("%s\n", __FUNCTION__); */
6269
6270         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
6271         {
6272                 DBG_8723A("%s, alloc mgnt frame fail\n", __FUNCTION__);
6273                 return;
6274         }
6275
6276         /* update attribute */
6277         pattrib = &pmgntframe->attrib;
6278         update_mgntframe_attrib(padapter, pattrib);
6279
6280         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
6281
6282         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6283         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
6284
6285         mac = myid(&(padapter->eeprompriv));
6286         bssid = cur_network->MacAddress;
6287
6288         fctrl = &(pwlanhdr->frame_ctl);
6289         *(fctrl) = 0;
6290         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
6291         memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
6292         memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
6293
6294         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
6295         pmlmeext->mgnt_seq++;
6296         SetFrameSubType(fctrl, WIFI_PROBERSP);
6297
6298         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
6299         pattrib->pktlen = pattrib->hdrlen;
6300         pframe += pattrib->hdrlen;
6301
6302         if(cur_network->IELength>MAX_IE_SZ)
6303                 return;
6304
6305 #if defined (CONFIG_AP_MODE) && defined (CONFIG_NATIVEAP_MLME)
6306         if( (pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
6307         {
6308                 pwps_ie = rtw_get_wps_ie(cur_network->IEs+_FIXED_IE_LENGTH_, cur_network->IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
6309
6310                 /* inerset & update wps_probe_resp_ie */
6311                 if((pmlmepriv->wps_probe_resp_ie!=NULL) && pwps_ie && (wps_ielen>0))
6312                 {
6313                         uint wps_offset, remainder_ielen;
6314                         u8 *premainder_ie;
6315
6316                         wps_offset = (uint)(pwps_ie - cur_network->IEs);
6317
6318                         premainder_ie = pwps_ie + wps_ielen;
6319
6320                         remainder_ielen = cur_network->IELength - wps_offset - wps_ielen;
6321
6322                         memcpy(pframe, cur_network->IEs, wps_offset);
6323                         pframe += wps_offset;
6324                         pattrib->pktlen += wps_offset;
6325
6326                         wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];/* to get ie data len */
6327                         if((wps_offset+wps_ielen+2)<=MAX_IE_SZ)
6328                         {
6329                                 memcpy(pframe, pmlmepriv->wps_probe_resp_ie, wps_ielen+2);
6330                                 pframe += wps_ielen+2;
6331                                 pattrib->pktlen += wps_ielen+2;
6332                         }
6333
6334                         if((wps_offset+wps_ielen+2+remainder_ielen)<=MAX_IE_SZ)
6335                         {
6336                                 memcpy(pframe, premainder_ie, remainder_ielen);
6337                                 pframe += remainder_ielen;
6338                                 pattrib->pktlen += remainder_ielen;
6339                         }
6340                 }
6341                 else
6342                 {
6343                         memcpy(pframe, cur_network->IEs, cur_network->IELength);
6344                         pframe += cur_network->IELength;
6345                         pattrib->pktlen += cur_network->IELength;
6346                 }
6347
6348                 /* retrieve SSID IE from cur_network->Ssid */
6349                 {
6350                         u8 *ssid_ie;
6351                         int ssid_ielen;
6352                         int ssid_ielen_diff;
6353                         u8 buf[MAX_IE_SZ];
6354                         u8 *ies = pmgntframe->buf_addr+TXDESC_OFFSET+sizeof(struct rtw_ieee80211_hdr_3addr);
6355
6356                         ssid_ie = rtw_get_ie(ies+_FIXED_IE_LENGTH_, _SSID_IE_, &ssid_ielen,
6357                                 (pframe-ies)-_FIXED_IE_LENGTH_);
6358
6359                         ssid_ielen_diff = cur_network->Ssid.SsidLength - ssid_ielen;
6360
6361                         if (ssid_ie &&  cur_network->Ssid.SsidLength) {
6362                                 uint remainder_ielen;
6363                                 u8 *remainder_ie;
6364                                 remainder_ie = ssid_ie+2;
6365                                 remainder_ielen = (pframe-remainder_ie);
6366
6367                                 DBG_8723A_LEVEL(_drv_warning_, FUNC_ADPT_FMT" remainder_ielen > MAX_IE_SZ\n", FUNC_ADPT_ARG(padapter));
6368                                 if (remainder_ielen > MAX_IE_SZ) {
6369                                         remainder_ielen = MAX_IE_SZ;
6370                                 }
6371
6372                                 memcpy(buf, remainder_ie, remainder_ielen);
6373                                 memcpy(remainder_ie+ssid_ielen_diff, buf, remainder_ielen);
6374                                 *(ssid_ie+1) = cur_network->Ssid.SsidLength;
6375                                 memcpy(ssid_ie+2, cur_network->Ssid.Ssid, cur_network->Ssid.SsidLength);
6376
6377                                 pframe += ssid_ielen_diff;
6378                                 pattrib->pktlen += ssid_ielen_diff;
6379                         }
6380                 }
6381         }
6382         else
6383 #endif
6384         {
6385
6386                 /* timestamp will be inserted by hardware */
6387                 pframe += 8;
6388                 pattrib->pktlen += 8;
6389
6390                 /*  beacon interval: 2 bytes */
6391
6392                 memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
6393
6394                 pframe += 2;
6395                 pattrib->pktlen += 2;
6396
6397                 /*  capability info: 2 bytes */
6398
6399                 memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
6400
6401                 pframe += 2;
6402                 pattrib->pktlen += 2;
6403
6404                 /* below for ad-hoc mode */
6405
6406                 /*  SSID */
6407                 pframe = rtw_set_ie(pframe, _SSID_IE_, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pattrib->pktlen);
6408
6409                 /*  supported rates... */
6410                 rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
6411                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8)? 8: rate_len), cur_network->SupportedRates, &pattrib->pktlen);
6412
6413                 /*  DS parameter set */
6414                 pframe =rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pattrib->pktlen);
6415
6416                 if( (pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
6417                 {
6418                         u8 erpinfo=0;
6419                         u32 ATIMWindow;
6420                         /*  IBSS Parameter Set... */
6421                         /* ATIMWindow = cur->Configuration.ATIMWindow; */
6422                         ATIMWindow = 0;
6423                         pframe = rtw_set_ie(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pattrib->pktlen);
6424
6425                         /* ERP IE */
6426                         pframe = rtw_set_ie(pframe, _ERPINFO_IE_, 1, &erpinfo, &pattrib->pktlen);
6427                 }
6428
6429                 /*  EXTERNDED SUPPORTED RATE */
6430                 if (rate_len > 8)
6431                 {
6432                         pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pattrib->pktlen);
6433                 }
6434
6435                 /* todo:HT for adhoc */
6436
6437         }
6438
6439 #ifdef CONFIG_P2P
6440         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && is_valid_p2p_probereq)
6441         {
6442                 u32 len;
6443 #ifdef CONFIG_IOCTL_CFG80211
6444                 if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
6445                 {
6446                         /* if pwdinfo->role == P2P_ROLE_DEVICE will call issue_probersp_p2p() */
6447                         len = pmlmepriv->p2p_go_probe_resp_ie_len;
6448                         if(pmlmepriv->p2p_go_probe_resp_ie && len>0)
6449                                 memcpy(pframe, pmlmepriv->p2p_go_probe_resp_ie, len);
6450                 }
6451                 else
6452 #endif /* CONFIG_IOCTL_CFG80211 */
6453                 {
6454                         len = build_probe_resp_p2p_ie(pwdinfo, pframe);
6455                 }
6456
6457                 pframe += len;
6458                 pattrib->pktlen += len;
6459
6460 #ifdef CONFIG_WFD
6461 #ifdef CONFIG_IOCTL_CFG80211
6462                 if(_TRUE == pwdinfo->wfd_info->wfd_enable)
6463 #endif /* CONFIG_IOCTL_CFG80211 */
6464                 {
6465                         len = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
6466                 }
6467 #ifdef CONFIG_IOCTL_CFG80211
6468                 else
6469                 {
6470                         len = 0;
6471                         if(pmlmepriv->wfd_probe_resp_ie && pmlmepriv->wfd_probe_resp_ie_len>0)
6472                         {
6473                                 len = pmlmepriv->wfd_probe_resp_ie_len;
6474                                 memcpy(pframe, pmlmepriv->wfd_probe_resp_ie, len);
6475                         }
6476                 }
6477 #endif /* CONFIG_IOCTL_CFG80211 */
6478                 pframe += len;
6479                 pattrib->pktlen += len;
6480 #endif /* CONFIG_WFD */
6481
6482         }
6483 #endif /* CONFIG_P2P */
6484
6485         pattrib->last_txcmdsz = pattrib->pktlen;
6486
6487         dump_mgntframe(padapter, pmgntframe);
6488
6489         return;
6490 }
6491
6492 int _issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da, int wait_ack)
6493 {
6494         int ret = _FAIL;
6495         struct xmit_frame               *pmgntframe;
6496         struct pkt_attrib               *pattrib;
6497         unsigned char                   *pframe;
6498         struct rtw_ieee80211_hdr        *pwlanhdr;
6499         unsigned short          *fctrl;
6500         unsigned char                   *mac;
6501         unsigned char                   bssrate[NumRates];
6502         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
6503         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
6504         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6505         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6506         int     bssrate_len = 0;
6507         u8      bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
6508
6509         RT_TRACE(_module_rtl871x_mlme_c_,_drv_notice_,("+issue_probereq\n"));
6510
6511         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
6512         {
6513                 goto exit;
6514         }
6515
6516         /* update attribute */
6517         pattrib = &pmgntframe->attrib;
6518         update_mgntframe_attrib(padapter, pattrib);
6519
6520         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
6521
6522         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6523         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
6524
6525         mac = myid(&(padapter->eeprompriv));
6526
6527         fctrl = &(pwlanhdr->frame_ctl);
6528         *(fctrl) = 0;
6529
6530         if (da)
6531         {
6532                 /*      unicast probe request frame */
6533                 memcpy(pwlanhdr->addr1, da, ETH_ALEN);
6534                 memcpy(pwlanhdr->addr3, da, ETH_ALEN);
6535         }
6536         else
6537         {
6538                 /*      broadcast probe request frame */
6539                 memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
6540                 memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
6541         }
6542
6543         memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
6544
6545         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
6546         pmlmeext->mgnt_seq++;
6547         SetFrameSubType(pframe, WIFI_PROBEREQ);
6548
6549         pframe += sizeof (struct rtw_ieee80211_hdr_3addr);
6550         pattrib->pktlen = sizeof (struct rtw_ieee80211_hdr_3addr);
6551
6552         if(pssid)
6553                 pframe = rtw_set_ie(pframe, _SSID_IE_, pssid->SsidLength, pssid->Ssid, &(pattrib->pktlen));
6554         else
6555                 pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &(pattrib->pktlen));
6556
6557         get_rate_set(padapter, bssrate, &bssrate_len);
6558
6559         if (bssrate_len > 8)
6560         {
6561                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
6562                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
6563         }
6564         else
6565         {
6566                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
6567         }
6568
6569         /* add wps_ie for wps2.0 */
6570         if(pmlmepriv->wps_probe_req_ie_len>0 && pmlmepriv->wps_probe_req_ie)
6571         {
6572                 memcpy(pframe, pmlmepriv->wps_probe_req_ie, pmlmepriv->wps_probe_req_ie_len);
6573                 pframe += pmlmepriv->wps_probe_req_ie_len;
6574                 pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
6575         }
6576
6577         pattrib->last_txcmdsz = pattrib->pktlen;
6578
6579         RT_TRACE(_module_rtl871x_mlme_c_,_drv_notice_,("issuing probe_req, tx_len=%d\n", pattrib->last_txcmdsz));
6580
6581         if (wait_ack) {
6582                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
6583         } else {
6584                 dump_mgntframe(padapter, pmgntframe);
6585                 ret = _SUCCESS;
6586         }
6587
6588 exit:
6589         return ret;
6590 }
6591
6592 inline void issue_probereq(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da)
6593 {
6594         _issue_probereq(padapter, pssid, da, _FALSE);
6595 }
6596
6597 int issue_probereq_ex(_adapter *padapter, NDIS_802_11_SSID *pssid, u8 *da,
6598         int try_cnt, int wait_ms)
6599 {
6600         int ret;
6601         int i = 0;
6602         u32 start = rtw_get_current_time();
6603
6604         do
6605         {
6606                 ret = _issue_probereq(padapter, pssid, da, wait_ms>0?_TRUE:_FALSE);
6607
6608                 i++;
6609
6610                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
6611                         break;
6612
6613                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
6614                         rtw_msleep_os(wait_ms);
6615
6616         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
6617
6618         if (ret != _FAIL) {
6619                 ret = _SUCCESS;
6620                 #ifndef DBG_XMIT_ACK
6621                 goto exit;
6622                 #endif
6623         }
6624
6625         if (try_cnt && wait_ms) {
6626                 if (da)
6627                         DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
6628                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
6629                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
6630                 else
6631                         DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
6632                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
6633                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
6634         }
6635 exit:
6636         return ret;
6637 }
6638
6639 /*  if psta == NULL, indiate we are station(client) now... */
6640 void issue_auth(_adapter *padapter, struct sta_info *psta, unsigned short status)
6641 {
6642         struct xmit_frame                       *pmgntframe;
6643         struct pkt_attrib                       *pattrib;
6644         unsigned char                                   *pframe;
6645         struct rtw_ieee80211_hdr        *pwlanhdr;
6646         unsigned short                          *fctrl;
6647         unsigned int                                    val32;
6648         unsigned short                          val16;
6649         int use_shared_key = 0;
6650         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
6651         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6652         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6653
6654         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
6655         {
6656                 return;
6657         }
6658
6659         /* update attribute */
6660         pattrib = &pmgntframe->attrib;
6661         update_mgntframe_attrib(padapter, pattrib);
6662
6663         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
6664
6665         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6666         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
6667
6668         fctrl = &(pwlanhdr->frame_ctl);
6669         *(fctrl) = 0;
6670
6671         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
6672         pmlmeext->mgnt_seq++;
6673         SetFrameSubType(pframe, WIFI_AUTH);
6674
6675         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
6676         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
6677
6678         if(psta)/*  for AP mode */
6679         {
6680 #ifdef CONFIG_NATIVEAP_MLME
6681
6682                 memcpy(pwlanhdr->addr1, psta->hwaddr, ETH_ALEN);
6683                 memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
6684                 memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
6685
6686                 /*  setting auth algo number */
6687                 val16 = (u16)psta->authalg;
6688
6689                 if(status != _STATS_SUCCESSFUL_)
6690                         val16 = 0;
6691
6692                 if (val16)      {
6693                         val16 = cpu_to_le16(val16);
6694                         use_shared_key = 1;
6695                 }
6696
6697                 pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
6698
6699                 /*  setting auth seq number */
6700                 val16 =(u16)psta->auth_seq;
6701                 val16 = cpu_to_le16(val16);
6702                 pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
6703
6704                 /*  setting status code... */
6705                 val16 = status;
6706                 val16 = cpu_to_le16(val16);
6707                 pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen));
6708
6709                 /*  added challenging text... */
6710                 if ((psta->auth_seq == 2) && (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
6711                 {
6712                         pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, psta->chg_txt, &(pattrib->pktlen));
6713                 }
6714 #endif
6715         }
6716         else
6717         {
6718                 memcpy(pwlanhdr->addr1, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
6719                 memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
6720                 memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
6721
6722                 /*  setting auth algo number */
6723                 val16 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)? 1: 0;/*  0:OPEN System, 1:Shared key */
6724                 if (val16)      {
6725                         val16 = cpu_to_le16(val16);
6726                         use_shared_key = 1;
6727                 }
6728                 /* DBG_8723A("%s auth_algo= %s auth_seq=%d\n",__FUNCTION__,(pmlmeinfo->auth_algo==0)?"OPEN":"SHARED",pmlmeinfo->auth_seq); */
6729
6730                 /* setting IV for auth seq #3 */
6731                 if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
6732                 {
6733                         /* DBG_8723A("==> iv(%d),key_index(%d)\n",pmlmeinfo->iv,pmlmeinfo->key_index); */
6734                         val32 = ((pmlmeinfo->iv++) | (pmlmeinfo->key_index << 30));
6735                         val32 = cpu_to_le32(val32);
6736                         pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *)&val32, &(pattrib->pktlen));
6737
6738                         pattrib->iv_len = 4;
6739                 }
6740
6741                 pframe = rtw_set_fixed_ie(pframe, _AUTH_ALGM_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
6742
6743                 /*  setting auth seq number */
6744                 val16 = pmlmeinfo->auth_seq;
6745                 val16 = cpu_to_le16(val16);
6746                 pframe = rtw_set_fixed_ie(pframe, _AUTH_SEQ_NUM_, (unsigned char *)&val16, &(pattrib->pktlen));
6747
6748                 /*  setting status code... */
6749                 val16 = status;
6750                 val16 = cpu_to_le16(val16);
6751                 pframe = rtw_set_fixed_ie(pframe, _STATUS_CODE_, (unsigned char *)&val16, &(pattrib->pktlen));
6752
6753                 /*  then checking to see if sending challenging text... */
6754                 if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key==1))
6755                 {
6756                         pframe = rtw_set_ie(pframe, _CHLGETXT_IE_, 128, pmlmeinfo->chg_txt, &(pattrib->pktlen));
6757
6758                         SetPrivacy(fctrl);
6759
6760                         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
6761
6762                         pattrib->encrypt = _WEP40_;
6763
6764                         pattrib->icv_len = 4;
6765
6766                         pattrib->pktlen += pattrib->icv_len;
6767
6768                 }
6769
6770         }
6771
6772         pattrib->last_txcmdsz = pattrib->pktlen;
6773
6774         rtw_wep_encrypt(padapter, (u8 *)pmgntframe);
6775         DBG_8723A("%s\n", __FUNCTION__);
6776         dump_mgntframe(padapter, pmgntframe);
6777
6778         return;
6779 }
6780
6781 void issue_asocrsp(_adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
6782 {
6783 #ifdef CONFIG_AP_MODE
6784         struct xmit_frame       *pmgntframe;
6785         struct rtw_ieee80211_hdr        *pwlanhdr;
6786         struct pkt_attrib *pattrib;
6787         unsigned char   *pbuf, *pframe;
6788         unsigned short val;
6789         unsigned short *fctrl;
6790         struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
6791         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
6792         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
6793         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6794         WLAN_BSSID_EX *pnetwork = &(pmlmeinfo->network);
6795         u8 *ie = pnetwork->IEs;
6796 #ifdef CONFIG_P2P
6797         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
6798 #ifdef CONFIG_WFD
6799         u32                                     wfdielen = 0;
6800 #endif /* CONFIG_WFD */
6801
6802 #endif /* CONFIG_P2P */
6803
6804         DBG_8723A("%s\n", __FUNCTION__);
6805
6806         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
6807         {
6808                 return;
6809         }
6810
6811         /* update attribute */
6812         pattrib = &pmgntframe->attrib;
6813         update_mgntframe_attrib(padapter, pattrib);
6814
6815         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
6816
6817         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
6818         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
6819
6820         fctrl = &(pwlanhdr->frame_ctl);
6821         *(fctrl) = 0;
6822
6823         memcpy((void *)GetAddr1Ptr(pwlanhdr), pstat->hwaddr, ETH_ALEN);
6824         memcpy((void *)GetAddr2Ptr(pwlanhdr), myid(&(padapter->eeprompriv)), ETH_ALEN);
6825         memcpy((void *)GetAddr3Ptr(pwlanhdr), get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
6826
6827         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
6828         pmlmeext->mgnt_seq++;
6829         if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
6830                 SetFrameSubType(pwlanhdr, pkt_type);
6831         else
6832                 return;
6833
6834         pattrib->hdrlen = sizeof(struct rtw_ieee80211_hdr_3addr);
6835         pattrib->pktlen += pattrib->hdrlen;
6836         pframe += pattrib->hdrlen;
6837
6838         /* capability */
6839         val = *(unsigned short *)rtw_get_capability_from_ie(ie);
6840
6841         pframe = rtw_set_fixed_ie(pframe, _CAPABILITY_ , (unsigned char *)&val, &(pattrib->pktlen));
6842
6843         status = cpu_to_le16(status);
6844         pframe = rtw_set_fixed_ie(pframe , _STATUS_CODE_ , (unsigned char *)&status, &(pattrib->pktlen));
6845
6846         val = cpu_to_le16(pstat->aid | BIT(14) | BIT(15));
6847         pframe = rtw_set_fixed_ie(pframe, _ASOC_ID_ , (unsigned char *)&val, &(pattrib->pktlen));
6848
6849         if (pstat->bssratelen <= 8)
6850         {
6851                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, pstat->bssratelen, pstat->bssrateset, &(pattrib->pktlen));
6852         }
6853         else
6854         {
6855                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, pstat->bssrateset, &(pattrib->pktlen));
6856                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (pstat->bssratelen-8), pstat->bssrateset+8, &(pattrib->pktlen));
6857         }
6858
6859 #ifdef CONFIG_80211N_HT
6860         if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option))
6861         {
6862                 uint ie_len=0;
6863
6864                 /* FILL HT CAP INFO IE */
6865                 /* p = hostapd_eid_ht_capabilities_info(hapd, p); */
6866                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
6867                 if(pbuf && ie_len>0)
6868                 {
6869                         memcpy(pframe, pbuf, ie_len+2);
6870                         pframe += (ie_len+2);
6871                         pattrib->pktlen +=(ie_len+2);
6872                 }
6873
6874                 /* FILL HT ADD INFO IE */
6875                 /* p = hostapd_eid_ht_operation(hapd, p); */
6876                 pbuf = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
6877                 if(pbuf && ie_len>0)
6878                 {
6879                         memcpy(pframe, pbuf, ie_len+2);
6880                         pframe += (ie_len+2);
6881                         pattrib->pktlen +=(ie_len+2);
6882                 }
6883
6884         }
6885 #endif
6886
6887         /* FILL WMM IE */
6888         if ((pstat->flags & WLAN_STA_WME) && (pmlmepriv->qospriv.qos_option))
6889         {
6890                 uint ie_len=0;
6891                 unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
6892
6893                 for (pbuf = ie + _BEACON_IE_OFFSET_; ;pbuf+= (ie_len + 2))
6894                 {
6895                         pbuf = rtw_get_ie(pbuf, _VENDOR_SPECIFIC_IE_, &ie_len, (pnetwork->IELength - _BEACON_IE_OFFSET_ - (ie_len + 2)));
6896                         if (pbuf && !memcmp(pbuf+2, WMM_PARA_IE, 6))
6897                         {
6898                                 memcpy(pframe, pbuf, ie_len+2);
6899                                 pframe += (ie_len+2);
6900                                 pattrib->pktlen +=(ie_len+2);
6901
6902                                 break;
6903                         }
6904
6905                         if ((pbuf == NULL) || (ie_len == 0))
6906                         {
6907                                 break;
6908                         }
6909                 }
6910
6911         }
6912
6913         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
6914         {
6915                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
6916         }
6917
6918         /* add WPS IE ie for wps 2.0 */
6919         if(pmlmepriv->wps_assoc_resp_ie && pmlmepriv->wps_assoc_resp_ie_len>0)
6920         {
6921                 memcpy(pframe, pmlmepriv->wps_assoc_resp_ie, pmlmepriv->wps_assoc_resp_ie_len);
6922
6923                 pframe += pmlmepriv->wps_assoc_resp_ie_len;
6924                 pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
6925         }
6926
6927 #ifdef CONFIG_P2P
6928 #ifndef CONFIG_IOCTL_CFG80211
6929         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && (pstat->is_p2p_device == _TRUE))
6930         {
6931                 u32 len;
6932
6933                 len = build_assoc_resp_p2p_ie(pwdinfo, pframe, pstat->p2p_status_code);
6934
6935                 pframe += len;
6936                 pattrib->pktlen += len;
6937         }
6938 #endif /* CONFIG_IOCTL_CFG80211 */
6939 #ifdef CONFIG_WFD
6940         if(rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)
6941 #ifdef CONFIG_IOCTL_CFG80211
6942                 && (_TRUE == pwdinfo->wfd_info->wfd_enable)
6943 #endif /* CONFIG_IOCTL_CFG80211 */
6944         )
6945         {
6946                 wfdielen = build_assoc_resp_wfd_ie(pwdinfo, pframe);
6947                 pframe += wfdielen;
6948                 pattrib->pktlen += wfdielen;
6949         }
6950 #endif /* CONFIG_WFD */
6951 #endif /* CONFIG_P2P */
6952
6953         pattrib->last_txcmdsz = pattrib->pktlen;
6954
6955         dump_mgntframe(padapter, pmgntframe);
6956
6957 #endif
6958 }
6959
6960 void issue_assocreq(_adapter *padapter)
6961 {
6962         int ret = _FAIL;
6963         struct xmit_frame                               *pmgntframe;
6964         struct pkt_attrib                               *pattrib;
6965         unsigned char                                   *pframe, *p;
6966         struct rtw_ieee80211_hdr                        *pwlanhdr;
6967         unsigned short                          *fctrl;
6968         unsigned short                          val16;
6969         unsigned int                                    i, j, ie_len, index=0;
6970         unsigned char                                   rf_type, bssrate[NumRates], sta_bssrate[NumRates];
6971         PNDIS_802_11_VARIABLE_IEs       pIE;
6972         struct registry_priv    *pregpriv = &padapter->registrypriv;
6973         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
6974         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
6975         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
6976         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
6977         int     bssrate_len = 0, sta_bssrate_len = 0;
6978 #ifdef CONFIG_P2P
6979         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
6980         u8                                      p2pie[ 255 ] = { 0x00 };
6981         u16                                     p2pielen = 0;
6982 #ifdef CONFIG_WFD
6983         u32                                     wfdielen = 0;
6984 #endif /* CONFIG_WFD */
6985 #endif /* CONFIG_P2P */
6986
6987 #ifdef CONFIG_DFS
6988         u16     cap;
6989 #endif /* CONFIG_DFS */
6990
6991         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
6992                 goto exit;
6993
6994         /* update attribute */
6995         pattrib = &pmgntframe->attrib;
6996         update_mgntframe_attrib(padapter, pattrib);
6997
6998         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
6999
7000         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7001         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7002
7003         fctrl = &(pwlanhdr->frame_ctl);
7004         *(fctrl) = 0;
7005         memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7006         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
7007         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7008
7009         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7010         pmlmeext->mgnt_seq++;
7011         SetFrameSubType(pframe, WIFI_ASSOCREQ);
7012
7013         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
7014         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7015
7016         /* caps */
7017
7018 #ifdef CONFIG_DFS
7019         memcpy(&cap, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
7020         cap |= BIT(8);
7021         memcpy(pframe, &cap, 2);
7022 #else
7023         memcpy(pframe, rtw_get_capability_from_ie(pmlmeinfo->network.IEs), 2);
7024 #endif /* CONFIG_DFS */
7025
7026         pframe += 2;
7027         pattrib->pktlen += 2;
7028
7029         /* listen interval */
7030         /* todo: listen interval for power saving */
7031         val16 = cpu_to_le16(3);
7032         memcpy(pframe ,(unsigned char *)&val16, 2);
7033         pframe += 2;
7034         pattrib->pktlen += 2;
7035
7036         /* SSID */
7037         pframe = rtw_set_ie(pframe, _SSID_IE_,  pmlmeinfo->network.Ssid.SsidLength, pmlmeinfo->network.Ssid.Ssid, &(pattrib->pktlen));
7038
7039         /* supported rate & extended supported rate */
7040
7041 #if 1   /*  Check if the AP's supported rates are also supported by STA. */
7042         get_rate_set(padapter, sta_bssrate, &sta_bssrate_len);
7043         /* DBG_8723A("sta_bssrate_len=%d\n", sta_bssrate_len); */
7044
7045         if(pmlmeext->cur_channel == 14)/*  for JAPAN, channel 14 can only uses B Mode(CCK) */
7046         {
7047                 sta_bssrate_len = 4;
7048         }
7049
7050         /* for (i = 0; i < sta_bssrate_len; i++) { */
7051         /*      DBG_8723A("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]); */
7052         /*  */
7053
7054         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
7055                 if (pmlmeinfo->network.SupportedRates[i] == 0) break;
7056                 DBG_8723A("network.SupportedRates[%d]=%02X\n", i, pmlmeinfo->network.SupportedRates[i]);
7057         }
7058
7059         for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
7060                 if (pmlmeinfo->network.SupportedRates[i] == 0) break;
7061
7062                 /*  Check if the AP's supported rates are also supported by STA. */
7063                 for (j=0; j < sta_bssrate_len; j++) {
7064                          /*  Avoid the proprietary data rate (22Mbps) of Handlink WSG-4000 AP */
7065                         if ( (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK)
7066                                         == (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)) {
7067                                 /* DBG_8723A("match i = %d, j=%d\n", i, j); */
7068                                 break;
7069                         } else {
7070                                 /* DBG_8723A("not match: %02X != %02X\n", (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK), (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)); */
7071                         }
7072                 }
7073
7074                 if (j == sta_bssrate_len) {
7075                         /*  the rate is not supported by STA */
7076                         DBG_8723A("%s(): the rate[%d]=%02X is not supported by STA!\n",__FUNCTION__, i, pmlmeinfo->network.SupportedRates[i]);
7077                 } else {
7078                         /*  the rate is supported by STA */
7079                         bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
7080                 }
7081         }
7082
7083         bssrate_len = index;
7084         DBG_8723A("bssrate_len = %d\n", bssrate_len);
7085
7086 #endif  /*  Check if the AP's supported rates are also supported by STA. */
7087
7088         if (bssrate_len == 0) {
7089                 rtw_free_xmitbuf(pxmitpriv, pmgntframe->pxmitbuf);
7090                 rtw_free_xmitframe(pxmitpriv, pmgntframe);
7091                 goto exit; /* don't connect to AP if no joint supported rate */
7092         }
7093
7094         if (bssrate_len > 8)
7095         {
7096                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , 8, bssrate, &(pattrib->pktlen));
7097                 pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_ , (bssrate_len - 8), (bssrate + 8), &(pattrib->pktlen));
7098         }
7099         else
7100         {
7101                 pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_ , bssrate_len , bssrate, &(pattrib->pktlen));
7102         }
7103
7104         /* RSN */
7105         p = rtw_get_ie((pmlmeinfo->network.IEs + sizeof(NDIS_802_11_FIXED_IEs)), _RSN_IE_2_, &ie_len, (pmlmeinfo->network.IELength - sizeof(NDIS_802_11_FIXED_IEs)));
7106         if (p != NULL)
7107         {
7108                 pframe = rtw_set_ie(pframe, _RSN_IE_2_, ie_len, (p + 2), &(pattrib->pktlen));
7109         }
7110
7111 #ifdef CONFIG_80211N_HT
7112         /* HT caps */
7113         if(padapter->mlmepriv.htpriv.ht_option==_TRUE)
7114         {
7115                 p = rtw_get_ie((pmlmeinfo->network.IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_CAPABILITY_IE_, &ie_len, (pmlmeinfo->network.IELength - sizeof(NDIS_802_11_FIXED_IEs)));
7116                 if ((p != NULL) && (!(is_ap_in_tkip(padapter))))
7117                 {
7118                         memcpy(&(pmlmeinfo->HT_caps), (p + 2), sizeof(struct HT_caps_element));
7119
7120                         /* to disable 40M Hz support while gd_bw_40MHz_en = 0 */
7121                         if (pregpriv->cbw40_enable == 0)
7122                         {
7123                                 pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info &= (~(BIT(6) | BIT(1)));
7124                         }
7125                         else
7126                         {
7127                                 pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= BIT(1);
7128                         }
7129
7130                         /* todo: disable SM power save mode */
7131                         pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= 0x000c;
7132
7133                         rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
7134                         /* switch (pregpriv->rf_config) */
7135                         switch(rf_type)
7136                         {
7137                                 case RF_1T1R:
7138
7139                                         if(pregpriv->rx_stbc)
7140                                                 pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0100);/* RX STBC One spatial stream */
7141
7142                                         memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_1R, 16);
7143                                         break;
7144
7145                                 case RF_2T2R:
7146                                 case RF_1T2R:
7147                                 default:
7148
7149                                         if((pregpriv->rx_stbc == 0x3) ||/* enable for 2.4/5 GHz */
7150                                                 ((pmlmeext->cur_wireless_mode & WIRELESS_11_24N) && (pregpriv->rx_stbc == 0x1)) || /* enable for 2.4GHz */
7151                                                 ((pmlmeext->cur_wireless_mode & WIRELESS_11_5N) && (pregpriv->rx_stbc == 0x2)) || /* enable for 5GHz */
7152                                                 (pregpriv->wifi_spec==1))
7153                                         {
7154                                                 DBG_8723A("declare supporting RX STBC\n");
7155                                                 pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0200);/* RX STBC two spatial stream */
7156                                         }
7157                                         #ifdef CONFIG_DISABLE_MCS13TO15
7158                                         if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40 && (pregpriv->wifi_spec!=1))
7159                                                 memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_2R_MCS13TO15_OFF, 16);
7160                                         else
7161                                         memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_2R, 16);
7162                                         #else /* CONFIG_DISABLE_MCS13TO15 */
7163                                         memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_2R, 16);
7164                                         #endif /* CONFIG_DISABLE_MCS13TO15 */
7165                                         break;
7166                         }
7167 #ifdef RTL8192C_RECONFIG_TO_1T1R
7168                         {
7169                                 if(pregpriv->rx_stbc)
7170                                         pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0100);/* RX STBC One spatial stream */
7171
7172                                 memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_1R, 16);
7173                         }
7174 #endif
7175                         pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = cpu_to_le16(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info);
7176
7177 #ifdef CONFIG_BT_COEXIST
7178                         if (BT_1Ant(padapter) == _TRUE)
7179                         {
7180                                 /*  set to 8K */
7181                                 pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para &= (u8)~IEEE80211_HT_AMPDU_PARM_FACTOR;
7182 /*                              pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para |= MAX_AMPDU_FACTOR_8K */
7183                         }
7184 #endif
7185
7186                         pframe = rtw_set_ie(pframe, _HT_CAPABILITY_IE_, ie_len , (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
7187
7188                 }
7189         }
7190 #endif
7191
7192         /* vendor specific IE, such as WPA, WMM, WPS */
7193         for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pmlmeinfo->network.IELength;)
7194         {
7195                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pmlmeinfo->network.IEs + i);
7196
7197                 switch (pIE->ElementID)
7198                 {
7199                         case _VENDOR_SPECIFIC_IE_:
7200                                 if (!memcmp(pIE->data, RTW_WPA_OUI, 4) ||
7201                                     !memcmp(pIE->data, WMM_OUI, 4) ||
7202                                     !memcmp(pIE->data, WPS_OUI, 4)) {
7203                                         if (!padapter->registrypriv.wifi_spec)
7204                                         {
7205                                                 /* Commented by Kurt 20110629 */
7206                                                 /* In some older APs, WPS handshake */
7207                                                 /* would be fail if we append vender extensions informations to AP */
7208                                                 if (!memcmp(pIE->data,
7209                                                             WPS_OUI, 4)){
7210                                                         pIE->Length=14;
7211                                                 }
7212                                         }
7213                                         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, pIE->Length, pIE->data, &(pattrib->pktlen));
7214                                 }
7215                                 break;
7216
7217                         default:
7218                                 break;
7219                 }
7220
7221                 i += (pIE->Length + 2);
7222         }
7223
7224         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
7225         {
7226                 pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, 6 , REALTEK_96B_IE, &(pattrib->pktlen));
7227         }
7228
7229 #ifdef CONFIG_WAPI_SUPPORT
7230         rtw_build_assoc_req_wapi_ie(padapter, pframe, pattrib);
7231 #endif
7232
7233 #ifdef CONFIG_P2P
7234
7235 #ifdef CONFIG_IOCTL_CFG80211
7236         if(wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
7237         {
7238                 if(pmlmepriv->p2p_assoc_req_ie && pmlmepriv->p2p_assoc_req_ie_len>0)
7239                 {
7240                         memcpy(pframe, pmlmepriv->p2p_assoc_req_ie, pmlmepriv->p2p_assoc_req_ie_len);
7241                         pframe += pmlmepriv->p2p_assoc_req_ie_len;
7242                         pattrib->pktlen += pmlmepriv->p2p_assoc_req_ie_len;
7243                 }
7244         }
7245         else
7246 #endif /* CONFIG_IOCTL_CFG80211 */
7247         {
7248                 if(!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
7249                 {
7250                         /*      Should add the P2P IE in the association request frame. */
7251                         /*      P2P OUI */
7252
7253                         p2pielen = 0;
7254                         p2pie[ p2pielen++ ] = 0x50;
7255                         p2pie[ p2pielen++ ] = 0x6F;
7256                         p2pie[ p2pielen++ ] = 0x9A;
7257                         p2pie[ p2pielen++ ] = 0x09;     /*      WFA P2P v1.0 */
7258
7259                         /*      Commented by Albert 20101109 */
7260                         /*      According to the P2P Specification, the association request frame should contain 3 P2P attributes */
7261                         /*      1. P2P Capability */
7262                         /*      2. Extended Listen Timing */
7263                         /*      3. Device Info */
7264                         /*      Commented by Albert 20110516 */
7265                         /*      4. P2P Interface */
7266
7267                         /*      P2P Capability */
7268                         /*      Type: */
7269                         p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
7270
7271                         /*      Length: */
7272                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
7273                         p2pielen += 2;
7274
7275                         /*      Value: */
7276                         /*      Device Capability Bitmap, 1 byte */
7277                         p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
7278
7279                         /*      Group Capability Bitmap, 1 byte */
7280                         if ( pwdinfo->persistent_supported )
7281                                 p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
7282                         else
7283                                 p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
7284
7285                         /*      Extended Listen Timing */
7286                         /*      Type: */
7287                         p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
7288
7289                         /*      Length: */
7290                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
7291                         p2pielen += 2;
7292
7293                         /*      Value: */
7294                         /*      Availability Period */
7295                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
7296                         p2pielen += 2;
7297
7298                         /*      Availability Interval */
7299                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
7300                         p2pielen += 2;
7301
7302                         /*      Device Info */
7303                         /*      Type: */
7304                         p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
7305
7306                         /*      Length: */
7307                         /*      21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
7308                         /*      + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
7309                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
7310                         p2pielen += 2;
7311
7312                         /*      Value: */
7313                         /*      P2P Device Address */
7314                         memcpy(p2pie + p2pielen, myid( &padapter->eeprompriv ), ETH_ALEN );
7315                         p2pielen += ETH_ALEN;
7316
7317                         /*      Config Method */
7318                         /*      This field should be big endian. Noted by P2P specification. */
7319                         if ( ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN ) ||
7320                                 ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN ) )
7321                         {
7322                                 *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
7323                         }
7324                         else
7325                         {
7326                                 *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_PBC );
7327                         }
7328
7329                         p2pielen += 2;
7330
7331                         /*      Primary Device Type */
7332                         /*      Category ID */
7333                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_MULIT_MEDIA );
7334                         p2pielen += 2;
7335
7336                         /*      OUI */
7337                         *(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
7338                         p2pielen += 4;
7339
7340                         /*      Sub Category ID */
7341                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_MEDIA_SERVER );
7342                         p2pielen += 2;
7343
7344                         /*      Number of Secondary Device Types */
7345                         p2pie[ p2pielen++ ] = 0x00;     /*      No Secondary Device Type List */
7346
7347                         /*      Device Name */
7348                         /*      Type: */
7349                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
7350                         p2pielen += 2;
7351
7352                         /*      Length: */
7353                         *(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
7354                         p2pielen += 2;
7355
7356                         /*      Value: */
7357                         memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
7358                         p2pielen += pwdinfo->device_name_len;
7359
7360                         /*      P2P Interface */
7361                         /*      Type: */
7362                         p2pie[ p2pielen++ ] = P2P_ATTR_INTERFACE;
7363
7364                         /*      Length: */
7365                         *(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x000D );
7366                         p2pielen += 2;
7367
7368                         /*      Value: */
7369                         memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );      /*      P2P Device Address */
7370                         p2pielen += ETH_ALEN;
7371
7372                         p2pie[ p2pielen++ ] = 1;        /*      P2P Interface Address Count */
7373
7374                         memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );      /*      P2P Interface Address List */
7375                         p2pielen += ETH_ALEN;
7376
7377                         pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen );
7378
7379 #ifdef CONFIG_WFD
7380                         /* wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe); */
7381                         /* pframe += wfdielen; */
7382                         /* pattrib->pktlen += wfdielen; */
7383 #endif /* CONFIG_WFD */
7384                 }
7385         }
7386
7387 #endif /* CONFIG_P2P */
7388
7389 #ifdef CONFIG_WFD
7390 #ifdef CONFIG_IOCTL_CFG80211
7391         if ( _TRUE == pwdinfo->wfd_info->wfd_enable )
7392 #endif /* CONFIG_IOCTL_CFG80211 */
7393         {
7394                 wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);
7395                 pframe += wfdielen;
7396                 pattrib->pktlen += wfdielen;
7397         }
7398 #ifdef CONFIG_IOCTL_CFG80211
7399         else if (pmlmepriv->wfd_assoc_req_ie != NULL && pmlmepriv->wfd_assoc_req_ie_len>0)
7400         {
7401                 /* WFD IE */
7402                 memcpy(pframe, pmlmepriv->wfd_assoc_req_ie, pmlmepriv->wfd_assoc_req_ie_len);
7403                 pattrib->pktlen += pmlmepriv->wfd_assoc_req_ie_len;
7404                 pframe += pmlmepriv->wfd_assoc_req_ie_len;
7405         }
7406 #endif /* CONFIG_IOCTL_CFG80211 */
7407 #endif /* CONFIG_WFD */
7408
7409         pattrib->last_txcmdsz = pattrib->pktlen;
7410         dump_mgntframe(padapter, pmgntframe);
7411
7412         ret = _SUCCESS;
7413
7414 exit:
7415         if (ret == _SUCCESS)
7416                 rtw_buf_update(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len, (u8 *)pwlanhdr, pattrib->pktlen);
7417         else
7418                 rtw_buf_free(&pmlmepriv->assoc_req, &pmlmepriv->assoc_req_len);
7419
7420         return;
7421 }
7422
7423 /* when wait_ack is ture, this function shoule be called at process context */
7424 static int _issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int wait_ack)
7425 {
7426         int ret = _FAIL;
7427         struct xmit_frame                       *pmgntframe;
7428         struct pkt_attrib                       *pattrib;
7429         unsigned char                                   *pframe;
7430         struct rtw_ieee80211_hdr        *pwlanhdr;
7431         unsigned short                          *fctrl;
7432         struct xmit_priv        *pxmitpriv;
7433         struct mlme_ext_priv    *pmlmeext;
7434         struct mlme_ext_info    *pmlmeinfo;
7435
7436         /* DBG_8723A("%s:%d\n", __FUNCTION__, power_mode); */
7437
7438         if(!padapter)
7439                 goto exit;
7440
7441         pxmitpriv = &(padapter->xmitpriv);
7442         pmlmeext = &(padapter->mlmeextpriv);
7443         pmlmeinfo = &(pmlmeext->mlmext_info);
7444
7445         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7446         {
7447                 goto exit;
7448         }
7449
7450         /* update attribute */
7451         pattrib = &pmgntframe->attrib;
7452         update_mgntframe_attrib(padapter, pattrib);
7453         pattrib->retry_ctrl = _FALSE;
7454
7455         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7456
7457         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7458         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7459
7460         fctrl = &(pwlanhdr->frame_ctl);
7461         *(fctrl) = 0;
7462
7463         if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
7464         {
7465                 SetFrDs(fctrl);
7466         }
7467         else if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
7468         {
7469                 SetToDs(fctrl);
7470         }
7471
7472         if (power_mode)
7473         {
7474                 SetPwrMgt(fctrl);
7475         }
7476
7477         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
7478         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
7479         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7480
7481         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7482         pmlmeext->mgnt_seq++;
7483         SetFrameSubType(pframe, WIFI_DATA_NULL);
7484
7485         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
7486         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7487
7488         pattrib->last_txcmdsz = pattrib->pktlen;
7489
7490         if(wait_ack)
7491         {
7492                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
7493         }
7494         else
7495         {
7496                 dump_mgntframe(padapter, pmgntframe);
7497                 ret = _SUCCESS;
7498         }
7499
7500 exit:
7501         return ret;
7502 }
7503
7504 /* when wait_ms >0 , this function shoule be called at process context */
7505 /* da == NULL for station mode */
7506 int issue_nulldata(_adapter *padapter, unsigned char *da, unsigned int power_mode, int try_cnt, int wait_ms)
7507 {
7508         int ret;
7509         int i = 0;
7510         u32 start = rtw_get_current_time();
7511         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7512         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7513
7514         /* da == NULL, assum it's null data for sta to ap*/
7515         if (da == NULL)
7516                 da = get_my_bssid(&(pmlmeinfo->network));
7517
7518         do
7519         {
7520                 ret = _issue_nulldata(padapter, da, power_mode, wait_ms>0?_TRUE:_FALSE);
7521
7522                 i++;
7523
7524                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
7525                         break;
7526
7527                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
7528                         rtw_msleep_os(wait_ms);
7529
7530         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
7531
7532         if (ret != _FAIL) {
7533                 ret = _SUCCESS;
7534                 #ifndef DBG_XMIT_ACK
7535                 goto exit;
7536                 #endif
7537         }
7538
7539         if (try_cnt && wait_ms) {
7540                 if (da)
7541                         DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
7542                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
7543                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7544                 else
7545                         DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
7546                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
7547                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7548         }
7549 exit:
7550         return ret;
7551 }
7552
7553 /* when wait_ack is ture, this function shoule be called at process context */
7554 static int _issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int wait_ack)
7555 {
7556         int ret = _FAIL;
7557         struct xmit_frame                       *pmgntframe;
7558         struct pkt_attrib                       *pattrib;
7559         unsigned char                                   *pframe;
7560         struct rtw_ieee80211_hdr        *pwlanhdr;
7561         unsigned short                          *fctrl, *qc;
7562         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
7563         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7564         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7565
7566         DBG_8723A("%s\n", __FUNCTION__);
7567
7568         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7569         {
7570                 goto exit;
7571         }
7572
7573         /* update attribute */
7574         pattrib = &pmgntframe->attrib;
7575         update_mgntframe_attrib(padapter, pattrib);
7576
7577         pattrib->hdrlen +=2;
7578         pattrib->qos_en = _TRUE;
7579         pattrib->eosp = 1;
7580         pattrib->ack_policy = 0;
7581         pattrib->mdata = 0;
7582
7583         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7584
7585         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7586         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7587
7588         fctrl = &(pwlanhdr->frame_ctl);
7589         *(fctrl) = 0;
7590
7591         if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
7592         {
7593                 SetFrDs(fctrl);
7594         }
7595         else if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
7596         {
7597                 SetToDs(fctrl);
7598         }
7599
7600         if(pattrib->mdata)
7601                 SetMData(fctrl);
7602
7603         qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
7604
7605         SetPriority(qc, tid);
7606
7607         SetEOSP(qc, pattrib->eosp);
7608
7609         SetAckpolicy(qc, pattrib->ack_policy);
7610
7611         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
7612         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
7613         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7614
7615         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7616         pmlmeext->mgnt_seq++;
7617         SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
7618
7619         pframe += sizeof(struct rtw_ieee80211_hdr_3addr_qos);
7620         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr_qos);
7621
7622         pattrib->last_txcmdsz = pattrib->pktlen;
7623
7624         if(wait_ack)
7625         {
7626                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
7627         }
7628         else
7629         {
7630                 dump_mgntframe(padapter, pmgntframe);
7631                 ret = _SUCCESS;
7632         }
7633
7634 exit:
7635         return ret;
7636 }
7637
7638 /* when wait_ms >0 , this function shoule be called at process context */
7639 /* da == NULL for station mode */
7640 int issue_qos_nulldata(_adapter *padapter, unsigned char *da, u16 tid, int try_cnt, int wait_ms)
7641 {
7642         int ret;
7643         int i = 0;
7644         u32 start = rtw_get_current_time();
7645         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7646         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7647
7648         /* da == NULL, assum it's null data for sta to ap*/
7649         if (da == NULL)
7650                 da = get_my_bssid(&(pmlmeinfo->network));
7651
7652         do
7653         {
7654                 ret = _issue_qos_nulldata(padapter, da, tid, wait_ms>0?_TRUE:_FALSE);
7655
7656                 i++;
7657
7658                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
7659                         break;
7660
7661                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
7662                         rtw_msleep_os(wait_ms);
7663
7664         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
7665
7666         if (ret != _FAIL) {
7667                 ret = _SUCCESS;
7668                 #ifndef DBG_XMIT_ACK
7669                 goto exit;
7670                 #endif
7671         }
7672
7673         if (try_cnt && wait_ms) {
7674                 if (da)
7675                         DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
7676                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
7677                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7678                 else
7679                         DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
7680                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
7681                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7682         }
7683 exit:
7684         return ret;
7685 }
7686
7687 static int _issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason, u8 wait_ack)
7688 {
7689         struct xmit_frame                       *pmgntframe;
7690         struct pkt_attrib                       *pattrib;
7691         unsigned char                                   *pframe;
7692         struct rtw_ieee80211_hdr        *pwlanhdr;
7693         unsigned short                          *fctrl;
7694         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
7695         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7696         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7697         int ret = _FAIL;
7698 #ifdef CONFIG_P2P
7699         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
7700 #endif /* CONFIG_P2P */
7701
7702         /* DBG_8723A("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); */
7703
7704 #ifdef CONFIG_P2P
7705         if ( !( rtw_p2p_chk_state( pwdinfo, P2P_STATE_NONE ) ) && ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) )
7706         {
7707                 _cancel_timer_ex( &pwdinfo->reset_ch_sitesurvey );
7708                 _set_timer( &pwdinfo->reset_ch_sitesurvey, 10 );
7709         }
7710 #endif /* CONFIG_P2P */
7711
7712         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7713         {
7714                 goto exit;
7715         }
7716
7717         /* update attribute */
7718         pattrib = &pmgntframe->attrib;
7719         update_mgntframe_attrib(padapter, pattrib);
7720         pattrib->retry_ctrl = _FALSE;
7721
7722         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7723
7724         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7725         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7726
7727         fctrl = &(pwlanhdr->frame_ctl);
7728         *(fctrl) = 0;
7729
7730         memcpy(pwlanhdr->addr1, da, ETH_ALEN);
7731         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
7732         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7733
7734         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7735         pmlmeext->mgnt_seq++;
7736         SetFrameSubType(pframe, WIFI_DEAUTH);
7737
7738         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
7739         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7740
7741         reason = cpu_to_le16(reason);
7742         pframe = rtw_set_fixed_ie(pframe, _RSON_CODE_ , (unsigned char *)&reason, &(pattrib->pktlen));
7743
7744         pattrib->last_txcmdsz = pattrib->pktlen;
7745
7746         if(wait_ack)
7747         {
7748                 ret = dump_mgntframe_and_wait_ack(padapter, pmgntframe);
7749         }
7750         else
7751         {
7752                 dump_mgntframe(padapter, pmgntframe);
7753                 ret = _SUCCESS;
7754         }
7755
7756 exit:
7757         return ret;
7758 }
7759
7760 int issue_deauth(_adapter *padapter, unsigned char *da, unsigned short reason)
7761 {
7762         DBG_8723A("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
7763         return _issue_deauth(padapter, da, reason, _FALSE);
7764 }
7765
7766 int issue_deauth_ex(_adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
7767         int wait_ms)
7768 {
7769         int ret;
7770         int i = 0;
7771         u32 start = rtw_get_current_time();
7772
7773         do
7774         {
7775                 ret = _issue_deauth(padapter, da, reason, wait_ms>0?_TRUE:_FALSE);
7776
7777                 i++;
7778
7779                 if (padapter->bDriverStopped || padapter->bSurpriseRemoved)
7780                         break;
7781
7782                 if(i < try_cnt && wait_ms > 0 && ret==_FAIL)
7783                         rtw_msleep_os(wait_ms);
7784
7785         }while((i<try_cnt) && ((ret==_FAIL)||(wait_ms==0)));
7786
7787         if (ret != _FAIL) {
7788                 ret = _SUCCESS;
7789                 #ifndef DBG_XMIT_ACK
7790                 goto exit;
7791                 #endif
7792         }
7793
7794         if (try_cnt && wait_ms) {
7795                 if (da)
7796                         DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
7797                                 FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
7798                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7799                 else
7800                         DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
7801                                 FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
7802                                 ret==_SUCCESS?", acked":"", i, try_cnt, rtw_get_passing_time_ms(start));
7803         }
7804 exit:
7805         return ret;
7806 }
7807
7808 void issue_action_spct_ch_switch(_adapter *padapter, u8 *ra, u8 new_ch, u8 ch_offset)
7809 {
7810         _irqL   irqL;
7811         _list           *plist, *phead;
7812         struct xmit_frame                       *pmgntframe;
7813         struct pkt_attrib                       *pattrib;
7814         unsigned char                           *pframe;
7815         struct rtw_ieee80211_hdr        *pwlanhdr;
7816         unsigned short                  *fctrl;
7817         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
7818         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
7819         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7820         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7821
7822         DBG_8723A(FUNC_NDEV_FMT" ra="MAC_FMT", ch:%u, offset:%u\n",
7823                 FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(ra), new_ch, ch_offset);
7824
7825         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7826                 return;
7827
7828         /* update attribute */
7829         pattrib = &pmgntframe->attrib;
7830         update_mgntframe_attrib(padapter, pattrib);
7831
7832         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7833
7834         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7835         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7836
7837         fctrl = &(pwlanhdr->frame_ctl);
7838         *(fctrl) = 0;
7839
7840         memcpy(pwlanhdr->addr1, ra, ETH_ALEN); /* RA */
7841         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN); /* TA */
7842         memcpy(pwlanhdr->addr3, ra, ETH_ALEN); /* DA = RA */
7843
7844         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7845         pmlmeext->mgnt_seq++;
7846         SetFrameSubType(pframe, WIFI_ACTION);
7847
7848         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
7849         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7850
7851         /* category, action */
7852         {
7853                 u8 category, action;
7854                 category = RTW_WLAN_CATEGORY_SPECTRUM_MGMT;
7855                 action = RTW_WLAN_ACTION_SPCT_CHL_SWITCH;
7856
7857                 pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
7858                 pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
7859         }
7860
7861         pframe = rtw_set_ie_ch_switch(pframe, &(pattrib->pktlen), 0, new_ch, 0);
7862         pframe = rtw_set_ie_secondary_ch_offset(pframe, &(pattrib->pktlen),
7863                 hal_ch_offset_to_secondary_ch_offset(ch_offset));
7864
7865         pattrib->last_txcmdsz = pattrib->pktlen;
7866
7867         dump_mgntframe(padapter, pmgntframe);
7868 }
7869
7870 void issue_action_BA(_adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short status)
7871 {
7872         u8      category = RTW_WLAN_CATEGORY_BACK;
7873         u16     start_seq;
7874         u16     BA_para_set;
7875         u16     reason_code;
7876         u16     BA_timeout_value;
7877         u16     BA_starting_seqctrl;
7878         int     max_rx_ampdu_factor;
7879         struct xmit_frame               *pmgntframe;
7880         struct pkt_attrib               *pattrib;
7881         u8                                      *pframe;
7882         struct rtw_ieee80211_hdr        *pwlanhdr;
7883         u16                                     *fctrl;
7884         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
7885         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
7886         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
7887         struct sta_info         *psta;
7888         struct sta_priv         *pstapriv = &padapter->stapriv;
7889         struct registry_priv            *pregpriv = &padapter->registrypriv;
7890 #ifdef CONFIG_BT_COEXIST
7891         u8 tendaAPMac[] = {0xC8, 0x3A, 0x35};
7892 #endif
7893
7894 #ifdef CONFIG_80211N_HT
7895         DBG_8723A("%s, category=%d, action=%d, status=%d\n", __FUNCTION__, category, action, status);
7896
7897         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
7898         {
7899                 return;
7900         }
7901
7902         /* update attribute */
7903         pattrib = &pmgntframe->attrib;
7904         update_mgntframe_attrib(padapter, pattrib);
7905
7906         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
7907
7908         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
7909         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
7910
7911         fctrl = &(pwlanhdr->frame_ctl);
7912         *(fctrl) = 0;
7913
7914         /* memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN); */
7915         memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
7916         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
7917         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
7918
7919         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
7920         pmlmeext->mgnt_seq++;
7921         SetFrameSubType(pframe, WIFI_ACTION);
7922
7923         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
7924         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
7925
7926         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
7927         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
7928
7929       status = cpu_to_le16(status);
7930
7931         if (category == 3)
7932         {
7933                 switch (action)
7934                 {
7935                         case 0: /* ADDBA req */
7936                                 do {
7937                                         pmlmeinfo->dialogToken++;
7938                                 } while (pmlmeinfo->dialogToken == 0);
7939                                 pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->dialogToken), &(pattrib->pktlen));
7940
7941 #ifdef CONFIG_BT_COEXIST
7942                                 if ((BT_1Ant(padapter) == _TRUE) &&
7943                                     ((pmlmeinfo->assoc_AP_vendor != broadcomAP) ||
7944                                      memcmp(raddr, tendaAPMac, 3)))
7945                                 {
7946                                         /*  A-MSDU NOT Supported */
7947                                         BA_para_set = 0;
7948                                         /*  immediate Block Ack */
7949                                         BA_para_set |= (1 << 1) & IEEE80211_ADDBA_PARAM_POLICY_MASK;
7950                                         /*  TID */
7951                                         BA_para_set |= (status << 2) & IEEE80211_ADDBA_PARAM_TID_MASK;
7952                                         /*  max buffer size is 8 MSDU */
7953                                         BA_para_set |= (8 << 6) & IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
7954                                 }
7955                                 else
7956 #endif
7957                                 {
7958                                         BA_para_set = (0x1002 | ((status & 0xf) << 2)); /* immediate ack & 64 buffer size */
7959                                 }
7960                                 BA_para_set = cpu_to_le16(BA_para_set);
7961                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
7962
7963                                 BA_timeout_value = 5000;/*  5ms */
7964                                 BA_timeout_value = cpu_to_le16(BA_timeout_value);
7965                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_timeout_value)), &(pattrib->pktlen));
7966
7967                                 /* if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL) */
7968                                 if ((psta = rtw_get_stainfo(pstapriv, raddr)) != NULL)
7969                                 {
7970                                         start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07]&0xfff) + 1;
7971
7972                                         DBG_8723A("BA_starting_seqctrl = %d for TID=%d\n", start_seq, status & 0x07);
7973
7974                                         psta->BA_starting_seqctrl[status & 0x07] = start_seq;
7975
7976                                         BA_starting_seqctrl = start_seq << 4;
7977                                 }
7978
7979                                 BA_starting_seqctrl = cpu_to_le16(BA_starting_seqctrl);
7980                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_starting_seqctrl)), &(pattrib->pktlen));
7981                                 break;
7982
7983                         case 1: /* ADDBA rsp */
7984                                 pframe = rtw_set_fixed_ie(pframe, 1, &(pmlmeinfo->ADDBA_req.dialog_token), &(pattrib->pktlen));
7985                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&status), &(pattrib->pktlen));
7986                                 rtw_hal_get_def_var(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
7987                                 if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_64K)
7988                                         BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
7989                                 else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_32K)
7990                                         BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0800); /* 32 buffer size */
7991                                 else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_16K)
7992                                         BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0400); /* 16 buffer size */
7993                                 else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_8K)
7994                                         BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0200); /* 8 buffer size */
7995                                 else
7996                                         BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
7997
7998 #ifdef CONFIG_BT_COEXIST
7999                                 if ((BT_1Ant(padapter) == _TRUE) &&
8000                                                 ((pmlmeinfo->assoc_AP_vendor != broadcomAP) ||
8001                                                 memcmp(raddr, tendaAPMac, 3)))
8002                                 {
8003                                         /*  max buffer size is 8 MSDU */
8004                                         BA_para_set &= ~IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
8005                                         BA_para_set |= (8 << 6) & IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
8006                                 }
8007 #endif
8008
8009                                 if(pregpriv->ampdu_amsdu==0)/* disabled */
8010                                         BA_para_set = cpu_to_le16(BA_para_set & ~BIT(0));
8011                                 else if(pregpriv->ampdu_amsdu==1)/* enabled */
8012                                         BA_para_set = cpu_to_le16(BA_para_set | BIT(0));
8013                                 else /* auto */
8014                                         BA_para_set = cpu_to_le16(BA_para_set);
8015
8016                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
8017                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(pmlmeinfo->ADDBA_req.BA_timeout_value)), &(pattrib->pktlen));
8018                                 break;
8019                         case 2:/* DELBA */
8020                                 BA_para_set = (status & 0x1F) << 3;
8021                                 BA_para_set = cpu_to_le16(BA_para_set);
8022                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(BA_para_set)), &(pattrib->pktlen));
8023
8024                                 reason_code = 37;/* Requested from peer STA as it does not want to use the mechanism */
8025                                 reason_code = cpu_to_le16(reason_code);
8026                                 pframe = rtw_set_fixed_ie(pframe, 2, (unsigned char *)(&(reason_code)), &(pattrib->pktlen));
8027                                 break;
8028                         default:
8029                                 break;
8030                 }
8031         }
8032
8033         pattrib->last_txcmdsz = pattrib->pktlen;
8034
8035         dump_mgntframe(padapter, pmgntframe);
8036 #endif /* CONFIG_80211N_HT */
8037 }
8038
8039 static void issue_action_BSSCoexistPacket(_adapter *padapter)
8040 {
8041         _irqL   irqL;
8042         _list           *plist, *phead;
8043         unsigned char category, action;
8044         struct xmit_frame                       *pmgntframe;
8045         struct pkt_attrib                       *pattrib;
8046         unsigned char                           *pframe;
8047         struct rtw_ieee80211_hdr        *pwlanhdr;
8048         unsigned short                  *fctrl;
8049         struct  wlan_network    *pnetwork = NULL;
8050         struct xmit_priv                        *pxmitpriv = &(padapter->xmitpriv);
8051         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
8052         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
8053         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8054         _queue          *queue  = &(pmlmepriv->scanned_queue);
8055         u8 InfoContent[16] = {0};
8056         u8 ICS[8][15];
8057 #ifdef CONFIG_80211N_HT
8058         if((pmlmepriv->num_FortyMHzIntolerant==0) || (pmlmepriv->num_sta_no_ht==0))
8059                 return;
8060
8061         if(_TRUE == pmlmeinfo->bwmode_updated)
8062                 return;
8063
8064         DBG_8723A("%s\n", __FUNCTION__);
8065
8066         category = RTW_WLAN_CATEGORY_PUBLIC;
8067         action = ACT_PUBLIC_BSSCOEXIST;
8068
8069         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
8070         {
8071                 return;
8072         }
8073
8074         /* update attribute */
8075         pattrib = &pmgntframe->attrib;
8076         update_mgntframe_attrib(padapter, pattrib);
8077
8078         memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
8079
8080         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
8081         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
8082
8083         fctrl = &(pwlanhdr->frame_ctl);
8084         *(fctrl) = 0;
8085
8086         memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8087         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
8088         memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
8089
8090         SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
8091         pmlmeext->mgnt_seq++;
8092         SetFrameSubType(pframe, WIFI_ACTION);
8093
8094         pframe += sizeof(struct rtw_ieee80211_hdr_3addr);
8095         pattrib->pktlen = sizeof(struct rtw_ieee80211_hdr_3addr);
8096
8097         pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
8098         pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
8099
8100         /*  */
8101         if(pmlmepriv->num_FortyMHzIntolerant>0)
8102         {
8103                 u8 iedata=0;
8104
8105                 iedata |= BIT(2);/* 20 MHz BSS Width Request */
8106
8107                 pframe = rtw_set_ie(pframe, EID_BSSCoexistence,  1, &iedata, &(pattrib->pktlen));
8108
8109         }
8110
8111         /*  */
8112         memset(ICS, 0, sizeof(ICS));
8113         if(pmlmepriv->num_sta_no_ht>0)
8114         {
8115                 int i;
8116
8117                 spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
8118
8119                 phead = get_list_head(queue);
8120                 plist = get_next(phead);
8121
8122                 while(1)
8123                 {
8124                         int len;
8125                         u8 *p;
8126                         WLAN_BSSID_EX *pbss_network;
8127
8128                         if (rtw_end_of_queue_search(phead,plist)== _TRUE)
8129                                 break;
8130
8131                         pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
8132
8133                         plist = get_next(plist);
8134
8135                         pbss_network = (WLAN_BSSID_EX *)&pnetwork->network;
8136
8137                         p = rtw_get_ie(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
8138                         if((p==NULL) || (len==0))/* non-HT */
8139                         {
8140                                 if((pbss_network->Configuration.DSConfig<=0) || (pbss_network->Configuration.DSConfig>14))
8141                                         continue;
8142
8143                                 ICS[0][pbss_network->Configuration.DSConfig]=1;
8144
8145                                 if(ICS[0][0] == 0)
8146                                         ICS[0][0] = 1;
8147                         }
8148
8149                 }
8150
8151                 spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
8152
8153                 for(i= 0;i<8;i++)
8154                 {
8155                         if(ICS[i][0] == 1)
8156                         {
8157                                 int j, k = 0;
8158
8159                                 InfoContent[k] = i;
8160                                 /* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent,i); */
8161                                 k++;
8162
8163                                 for(j=1;j<=14;j++)
8164                                 {
8165                                         if(ICS[i][j]==1)
8166                                         {
8167                                                 if(k<16)
8168                                                 {
8169                                                         InfoContent[k] = j; /* channel number */
8170                                                         /* SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j); */
8171                                                         k++;
8172                                                 }
8173                                         }
8174                                 }
8175
8176                                 pframe = rtw_set_ie(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &(pattrib->pktlen));
8177
8178                         }
8179
8180                 }
8181
8182         }
8183
8184         pattrib->last_txcmdsz = pattrib->pktlen;
8185
8186         dump_mgntframe(padapter, pmgntframe);
8187 #endif /* CONFIG_80211N_HT */
8188 }
8189
8190 unsigned int send_delba(_adapter *padapter, u8 initiator, u8 *addr)
8191 {
8192         struct sta_priv *pstapriv = &padapter->stapriv;
8193         struct sta_info *psta = NULL;
8194         /* struct recv_reorder_ctrl *preorder_ctrl; */
8195         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8196         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8197         u16 tid;
8198
8199         if((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
8200                 if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
8201                         return _SUCCESS;
8202
8203         psta = rtw_get_stainfo(pstapriv, addr);
8204         if(psta==NULL)
8205                 return _SUCCESS;
8206
8207         /* DBG_8723A("%s:%s\n", __FUNCTION__, (initiator==0)?"RX_DIR":"TX_DIR"); */
8208
8209         if(initiator==0) /*  recipient */
8210         {
8211                 for(tid = 0;tid<MAXTID;tid++)
8212                 {
8213                         if(psta->recvreorder_ctrl[tid].enable == _TRUE)
8214                         {
8215                                 DBG_8723A("rx agg disable tid(%d)\n",tid);
8216                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F));
8217                                 psta->recvreorder_ctrl[tid].enable = _FALSE;
8218                                 psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
8219                                 #ifdef DBG_RX_SEQ
8220                                 DBG_8723A("DBG_RX_SEQ %s:%d indicate_seq:%u \n", __FUNCTION__, __LINE__,
8221                                         psta->recvreorder_ctrl[tid].indicate_seq);
8222                                 #endif
8223                         }
8224                 }
8225         }
8226         else if(initiator == 1)/*  originator */
8227         {
8228 #ifdef CONFIG_80211N_HT
8229                 /* DBG_8723A("tx agg_enable_bitmap(0x%08x)\n", psta->htpriv.agg_enable_bitmap); */
8230                 for(tid = 0;tid<MAXTID;tid++)
8231                 {
8232                         if(psta->htpriv.agg_enable_bitmap & BIT(tid))
8233                         {
8234                                 DBG_8723A("tx agg disable tid(%d)\n",tid);
8235                                 issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid <<1) |initiator)&0x1F) );
8236                                 psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
8237                                 psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
8238
8239                         }
8240                 }
8241 #endif /* CONFIG_80211N_HT */
8242         }
8243
8244         return _SUCCESS;
8245 }
8246
8247 unsigned int send_beacon(_adapter *padapter)
8248 {
8249         u8      bxmitok = _FALSE;
8250         int     issue=0;
8251         int poll = 0;
8252 /* ifdef CONFIG_CONCURRENT_MODE */
8253         /* struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv); */
8254         /* struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info); */
8255         /* _adapter *pbuddy_adapter = padapter->pbuddy_adapter; */
8256         /* struct mlme_priv *pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv); */
8257 /* endif */
8258
8259         u32 start = rtw_get_current_time();
8260
8261         rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
8262         do{
8263                 issue_beacon(padapter, 100);
8264                 issue++;
8265                 do {
8266                         rtw_yield_os();
8267                         rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
8268                         poll++;
8269                 }while((poll%10)!=0 && _FALSE == bxmitok && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
8270
8271         }while(_FALSE == bxmitok && issue<100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
8272
8273         if(padapter->bSurpriseRemoved || padapter->bDriverStopped)
8274         {
8275                 return _FAIL;
8276         }
8277
8278         if(_FALSE == bxmitok)
8279         {
8280                 DBG_8723A("%s fail! %u ms\n", __FUNCTION__, rtw_get_passing_time_ms(start));
8281                 return _FAIL;
8282         }
8283         else
8284         {
8285                 u32 passing_time = rtw_get_passing_time_ms(start);
8286
8287                 if(passing_time > 100 || issue > 3)
8288                         DBG_8723A("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start));
8289                 /* else */
8290                 /*      DBG_8723A("%s success, issue:%d, poll:%d, %u ms\n", __FUNCTION__, issue, poll, rtw_get_passing_time_ms(start)); */
8291
8292                 return _SUCCESS;
8293         }
8294 }
8295
8296 /****************************************************************************
8297
8298 Following are some utitity fuctions for WiFi MLME
8299
8300 *****************************************************************************/
8301
8302 bool IsLegal5GChannel(
8303         IN PADAPTER                     Adapter,
8304         IN u8                   channel)
8305 {
8306
8307         int i=0;
8308         u8 Channel_5G[45] = {36,38,40,42,44,46,48,50,52,54,56,58,
8309                 60,62,64,100,102,104,106,108,110,112,114,116,118,120,122,
8310                 124,126,128,130,132,134,136,138,140,149,151,153,155,157,159,
8311                 161,163,165};
8312         for(i=0;i<sizeof(Channel_5G);i++)
8313                 if(channel == Channel_5G[i])
8314                         return _TRUE;
8315         return _FALSE;
8316 }
8317
8318 void site_survey(_adapter *padapter)
8319 {
8320         unsigned char           survey_channel = 0, val8;
8321         RT_SCAN_TYPE    ScanType = SCAN_PASSIVE;
8322         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8323         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8324         u32 initialgain = 0;
8325
8326 #ifdef CONFIG_P2P
8327
8328 #ifdef CONFIG_CONCURRENT_MODE
8329
8330 #ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
8331         u8 stay_buddy_ch = 0;
8332 #endif /* CONFIG_STA_MODE_SCAN_UNDER_AP_MODE */
8333
8334         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
8335         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
8336         struct mlme_ext_priv *pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
8337
8338 #endif /* CONFIG_CONCURRENT_MODE */
8339         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
8340         static unsigned char  prev_survey_channel = 0;
8341         static unsigned int p2p_scan_count = 0;
8342
8343         if ( ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) || ( pwdinfo->p2p_info.scan_op_ch_only ) )
8344         {
8345                 if ( pwdinfo->rx_invitereq_info.scan_op_ch_only )
8346                 {
8347                         survey_channel = pwdinfo->rx_invitereq_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
8348                 }
8349                 else
8350                 {
8351                         survey_channel = pwdinfo->p2p_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
8352                 }
8353                 ScanType = SCAN_ACTIVE;
8354         }
8355         else if(rtw_p2p_findphase_ex_is_social(pwdinfo))
8356         {
8357                 /*      Commented by Albert 2011/06/03 */
8358                 /*      The driver is in the find phase, it should go through the social channel. */
8359                 int ch_set_idx;
8360                 survey_channel = pwdinfo->social_chan[pmlmeext->sitesurvey_res.channel_idx];
8361                 ch_set_idx = rtw_ch_set_search_ch(pmlmeext->channel_set, survey_channel);
8362                 if (ch_set_idx >= 0)
8363                         ScanType = pmlmeext->channel_set[ch_set_idx].ScanType;
8364                 else
8365                         ScanType = SCAN_ACTIVE;
8366         }
8367         else
8368 #endif /* CONFIG_P2P */
8369         {
8370                 struct rtw_ieee80211_channel *ch;
8371                 if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
8372                         ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
8373                         survey_channel = ch->hw_value;
8374                         ScanType = (ch->flags & RTW_IEEE80211_CHAN_PASSIVE_SCAN) ? SCAN_PASSIVE : SCAN_ACTIVE;
8375                 }
8376         }
8377
8378         if (0){
8379                 DBG_8723A(FUNC_ADPT_FMT" ch:%u (cnt:%u,idx:%d) at %dms, %c%c%c\n"
8380                 , FUNC_ADPT_ARG(padapter)
8381                 , survey_channel
8382                 , pwdinfo->find_phase_state_exchange_cnt, pmlmeext->sitesurvey_res.channel_idx
8383                 , rtw_get_passing_time_ms(padapter->mlmepriv.scan_start_time)
8384                 , ScanType?'A':'P', pmlmeext->sitesurvey_res.scan_mode?'A':'P'
8385                 , pmlmeext->sitesurvey_res.ssid[0].SsidLength?'S':' '
8386                 );
8387                 #ifdef DBG_FIXED_CHAN
8388                 DBG_8723A(FUNC_ADPT_FMT" fixed_chan:%u\n", pmlmeext->fixed_chan);
8389                 #endif
8390         }
8391
8392         if(survey_channel != 0)
8393         {
8394                 /* PAUSE 4-AC Queue when site_survey */
8395                 /* rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
8396                 /* val8 |= 0x0f; */
8397                 /* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
8398 #ifdef CONFIG_CONCURRENT_MODE
8399 #ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
8400                 if((padapter->pbuddy_adapter->mlmeextpriv.mlmext_info.state&0x03) == WIFI_FW_AP_STATE)
8401                 {
8402                         if( pmlmeinfo->scan_cnt == RTW_SCAN_NUM_OF_CH )
8403                         {
8404                                 pmlmeinfo->scan_cnt = 0;
8405                                 survey_channel = pbuddy_mlmeext->cur_channel;
8406                                 ScanType = SCAN_ACTIVE;
8407                                 stay_buddy_ch = 1;
8408                         }
8409                         else
8410                         {
8411                                 if( pmlmeinfo->scan_cnt == 0 )
8412                                         stay_buddy_ch = 2;
8413                                 pmlmeinfo->scan_cnt++;
8414                         }
8415                 }
8416 #endif /* CONFIG_STA_MODE_SCAN_UNDER_AP_MODE */
8417 #endif /* CONFIG_CONCURRENT_MODE */
8418                 if(pmlmeext->sitesurvey_res.channel_idx == 0)
8419                 {
8420 #ifdef DBG_FIXED_CHAN
8421                         if(pmlmeext->fixed_chan !=0xff)
8422                                 set_channel_bwmode(padapter, pmlmeext->fixed_chan, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
8423                         else
8424 #endif
8425                                 set_channel_bwmode(padapter, survey_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
8426                 }
8427                 else
8428                 {
8429 #ifdef DBG_FIXED_CHAN
8430                         if(pmlmeext->fixed_chan!=0xff)
8431                                 SelectChannel(padapter, pmlmeext->fixed_chan);
8432                         else
8433 #endif
8434                                 SelectChannel(padapter, survey_channel);
8435                 }
8436
8437 #ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
8438                 if( stay_buddy_ch == 1 )
8439                 {
8440                         val8 = 0; /* survey done */
8441                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
8442
8443                         if(check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) &&
8444                                 check_buddy_fwstate(padapter, _FW_LINKED))
8445                         {
8446                                 update_beacon(padapter->pbuddy_adapter, 0, NULL, _TRUE);
8447                         }
8448                 }
8449                 else if( stay_buddy_ch == 2 )
8450                 {
8451                         val8 = 1; /* under site survey */
8452                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
8453                 }
8454 #endif /* CONFIG_STA_MODE_SCAN_UNDER_AP_MODE */
8455
8456                 if(ScanType == SCAN_ACTIVE) /* obey the channel plan setting... */
8457                 {
8458                         #ifdef CONFIG_P2P
8459                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) ||
8460                                 rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)
8461                         )
8462                         {
8463                                 issue_probereq_p2p(padapter, NULL);
8464                                 issue_probereq_p2p(padapter, NULL);
8465                                 issue_probereq_p2p(padapter, NULL);
8466                         }
8467                         else
8468                         #endif /* CONFIG_P2P */
8469                         {
8470                                 int i;
8471                                 for(i=0;i<RTW_SSID_SCAN_AMOUNT;i++){
8472                                         if(pmlmeext->sitesurvey_res.ssid[i].SsidLength) {
8473                                                 /* todo: to issue two probe req??? */
8474                                                 issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
8475                                                 /* rtw_msleep_os(SURVEY_TO>>1); */
8476                                                 issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
8477                                         }
8478                                 }
8479
8480                                 if(pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
8481                                         /* todo: to issue two probe req??? */
8482                                         issue_probereq(padapter, NULL, NULL);
8483                                         /* rtw_msleep_os(SURVEY_TO>>1); */
8484                                         issue_probereq(padapter, NULL, NULL);
8485                                 }
8486                         }
8487                 }
8488
8489 #ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
8490                 if( stay_buddy_ch == 1 )
8491                         set_survey_timer(pmlmeext, pmlmeext->chan_scan_time * RTW_STAY_AP_CH_MILLISECOND );
8492                 else
8493 #endif /* CONFIG_STA_MODE_SCAN_UNDER_AP_MODE */
8494                         set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
8495
8496         }
8497         else
8498         {
8499
8500                 /*      channel number is 0 or this channel is not valid. */
8501
8502 #ifdef CONFIG_CONCURRENT_MODE
8503                 u8 cur_channel;
8504                 u8 cur_bwmode;
8505                 u8 cur_ch_offset;
8506
8507                 if(check_fwstate(pmlmepriv, _FW_LINKED))
8508                 {
8509                         cur_channel = pmlmeext->cur_channel;
8510                         cur_bwmode = pmlmeext->cur_bwmode;
8511                         cur_ch_offset = pmlmeext->cur_ch_offset;
8512                 }
8513                 /* else if((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) */
8514                 else if(check_buddy_fwstate(padapter, _FW_LINKED)) /*  for AP or STA */
8515                 {
8516                         cur_channel = pbuddy_mlmeext->cur_channel;
8517                         cur_bwmode = pbuddy_mlmeext->cur_bwmode;
8518                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
8519                 }
8520                 else
8521                 {
8522                         cur_channel = pmlmeext->cur_channel;
8523                         cur_bwmode = pmlmeext->cur_bwmode;
8524                         cur_ch_offset = pmlmeext->cur_ch_offset;
8525                 }
8526 #endif
8527
8528 #ifdef CONFIG_P2P
8529                 if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
8530                 {
8531                         if( ( pwdinfo->rx_invitereq_info.scan_op_ch_only ) || ( pwdinfo->p2p_info.scan_op_ch_only ) )
8532                         {
8533                                 /*      Set the find_phase_state_exchange_cnt to P2P_FINDPHASE_EX_CNT. */
8534                                 /*      This will let the following flow to run the scanning end. */
8535                                 rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
8536                         }
8537                         #ifdef CONFIG_DBG_P2P
8538                         DBG_8723A( "[%s] find phase exchange cnt = %d\n", __FUNCTION__, pwdinfo->find_phase_state_exchange_cnt );
8539                         #endif
8540                 }
8541
8542                 if(rtw_p2p_findphase_ex_is_needed(pwdinfo))
8543                 {
8544                         /*      Set the P2P State to the listen state of find phase and set the current channel to the listen channel */
8545                         set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
8546                         rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN);
8547                         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
8548
8549                         initialgain = 0xff; /* restore RX GAIN */
8550                         rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
8551                         /* turn on dynamic functions */
8552                         Restore_DM_Func_Flag(padapter);
8553                         /* Switch_DM_Func(padapter, DYNAMIC_FUNC_DIG|DYNAMIC_FUNC_HP|DYNAMIC_FUNC_SS, _TRUE); */
8554
8555                         _set_timer( &pwdinfo->find_phase_timer, ( u32 ) ( ( u32 ) ( pwdinfo->listen_dwell ) * 100 ) );
8556                 }
8557                 else
8558 #endif /* CONFIG_P2P */
8559                 {
8560
8561 #ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
8562                         pmlmeinfo->scan_cnt = 0;
8563 #endif /* CONFIG_DMP_STA_NODE_SCAN_UNDER_AP_MODE */
8564
8565 #ifdef CONFIG_ANTENNA_DIVERSITY
8566                         /*  20100721:Interrupt scan operation here. */
8567                         /*  For SW antenna diversity before link, it needs to switch to another antenna and scan again. */
8568                         /*  It compares the scan result and select beter one to do connection. */
8569                         if(rtw_hal_antdiv_before_linked(padapter))
8570                         {
8571                                 pmlmeext->sitesurvey_res.bss_cnt = 0;
8572                                 pmlmeext->sitesurvey_res.channel_idx = -1;
8573                                 pmlmeext->chan_scan_time = SURVEY_TO /2;
8574                                 set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
8575                                 return;
8576                         }
8577 #endif
8578
8579 #ifdef CONFIG_P2P
8580                         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
8581                         {
8582                         #ifdef CONFIG_CONCURRENT_MODE
8583                                 #ifndef CONFIG_IOCTL_CFG80211
8584                                 if ( check_buddy_fwstate(padapter, _FW_LINKED ) )
8585                                 {
8586                                         _set_timer( &pwdinfo->ap_p2p_switch_timer, 500 );
8587                                 }
8588                                 #endif /* CONFIG_IOCTL_CFG80211 */
8589                                 rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
8590                         #else
8591                                 rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
8592                         #endif
8593                         }
8594                         rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
8595 #endif /* CONFIG_P2P */
8596
8597                         pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
8598
8599                         /* switch back to the original channel */
8600                         /* SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset); */
8601
8602                         {
8603 #ifdef CONFIG_DUALMAC_CONCURRENT
8604                                 dc_set_channel_bwmode_survey_done(padapter);
8605 #else
8606 #ifndef CONFIG_IOCTL_CFG80211
8607                                 if( rtw_p2p_chk_state(pwdinfo, P2P_STATE_LISTEN) )
8608                                 {
8609                                         set_channel_bwmode(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
8610                                 }
8611                                 else
8612 #endif /* CONFIG_IOCTL_CFG80211 */
8613 #ifdef CONFIG_CONCURRENT_MODE
8614                                         set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
8615 #else
8616                                         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
8617 #endif /* CONFIG_DUALMAC_CONCURRENT */
8618 #endif /* CONFIG_CONCURRENT_MODE */
8619                         }
8620
8621                         /* flush 4-AC Queue after site_survey */
8622                         /* val8 = 0; */
8623                         /* rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
8624
8625                         /* config MSR */
8626                         Set_MSR(padapter, (pmlmeinfo->state & 0x3));
8627
8628                         initialgain = 0xff; /* restore RX GAIN */
8629                         rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
8630                         /* turn on dynamic functions */
8631                         Restore_DM_Func_Flag(padapter);
8632                         /* Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE); */
8633
8634                         if (is_client_associated_to_ap(padapter) == _TRUE)
8635                         {
8636                                 issue_nulldata(padapter, NULL, 0, 3, 500);
8637
8638 #ifdef CONFIG_CONCURRENT_MODE
8639                                 if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
8640                                 {
8641                                         DBG_8723A("adapter is surveydone(buddy_adapter is linked), issue nulldata(pwrbit=0)\n");
8642
8643                                         issue_nulldata(padapter->pbuddy_adapter, NULL, 0, 3, 500);
8644                                 }
8645 #endif
8646                         }
8647 #ifdef CONFIG_CONCURRENT_MODE
8648                         else if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
8649                         {
8650                                 issue_nulldata(padapter->pbuddy_adapter, NULL, 0, 3, 500);
8651                         }
8652 #endif
8653
8654                         val8 = 0; /* survey done */
8655                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
8656
8657                         report_surveydone_event(padapter);
8658
8659                         pmlmeext->chan_scan_time = SURVEY_TO;
8660                         pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
8661
8662                         issue_action_BSSCoexistPacket(padapter);
8663                         issue_action_BSSCoexistPacket(padapter);
8664                         issue_action_BSSCoexistPacket(padapter);
8665
8666                 }
8667
8668 #ifdef CONFIG_CONCURRENT_MODE
8669                 if(check_buddy_mlmeinfo_state(padapter, WIFI_FW_AP_STATE) &&
8670                         check_buddy_fwstate(padapter, _FW_LINKED))
8671                 {
8672
8673                         DBG_8723A("survey done, current CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
8674
8675                         DBG_8723A("restart pbuddy_adapter's beacon\n");
8676
8677                         update_beacon(padapter->pbuddy_adapter, 0, NULL, _TRUE);
8678                 }
8679 #endif
8680
8681         }
8682
8683         return;
8684 }
8685
8686 /* collect bss info from Beacon and Probe request/response frames. */
8687 u8 collect_bss_info(_adapter *padapter, union recv_frame *precv_frame, WLAN_BSSID_EX *bssid)
8688 {
8689         int     i;
8690         u32     len;
8691         u8      *p;
8692         u16     val16, subtype;
8693         u8      *pframe = precv_frame->u.hdr.rx_data;
8694         u32     packet_len = precv_frame->u.hdr.len;
8695         u8 ie_offset;
8696         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
8697         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8698         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8699
8700         len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
8701
8702         if (len > MAX_IE_SZ)
8703         {
8704                 /* DBG_8723A("IE too long for survey event\n"); */
8705                 return _FAIL;
8706         }
8707
8708         memset(bssid, 0, sizeof(WLAN_BSSID_EX));
8709
8710         subtype = GetFrameSubType(pframe);
8711
8712         if(subtype==WIFI_BEACON) {
8713                 bssid->Reserved[0] = 1;
8714                 ie_offset = _BEACON_IE_OFFSET_;
8715         } else {
8716                 /*  FIXME : more type */
8717                 if (subtype == WIFI_PROBEREQ) {
8718                         ie_offset = _PROBEREQ_IE_OFFSET_;
8719                         bssid->Reserved[0] = 2;
8720                 } else if (subtype == WIFI_PROBERSP) {
8721                         ie_offset = _PROBERSP_IE_OFFSET_;
8722                         bssid->Reserved[0] = 3;
8723                 } else {
8724                         bssid->Reserved[0] = 0;
8725                         ie_offset = _FIXED_IE_LENGTH_;
8726                 }
8727         }
8728
8729         bssid->Length = sizeof(WLAN_BSSID_EX) - MAX_IE_SZ + len;
8730
8731         /* below is to copy the information element */
8732         bssid->IELength = len;
8733         memcpy(bssid->IEs, (pframe + sizeof(struct rtw_ieee80211_hdr_3addr)), bssid->IELength);
8734
8735         /* get the signal strength */
8736         bssid->Rssi = precv_frame->u.hdr.attrib.phy_info.RecvSignalPower; /*  in dBM.raw data */
8737         bssid->PhyInfo.SignalQuality = precv_frame->u.hdr.attrib.phy_info.SignalQuality;/* in percentage */
8738         bssid->PhyInfo.SignalStrength = precv_frame->u.hdr.attrib.phy_info.SignalStrength;/* in percentage */
8739 #ifdef CONFIG_ANTENNA_DIVERSITY
8740         /* rtw_hal_get_hwreg(padapter, HW_VAR_CURRENT_ANTENNA, (u8 *)(&bssid->PhyInfo.Optimum_antenna)); */
8741         rtw_hal_get_def_var(padapter, HAL_DEF_CURRENT_ANTENNA,  &bssid->PhyInfo.Optimum_antenna);
8742 #endif
8743
8744         /*  checking SSID */
8745         if ((p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset)) == NULL)
8746         {
8747                 DBG_8723A("marc: cannot find SSID for survey event\n");
8748                 return _FAIL;
8749         }
8750
8751         if (*(p + 1))
8752         {
8753                 if (len > NDIS_802_11_LENGTH_SSID)
8754                 {
8755                         DBG_8723A("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
8756                         return _FAIL;
8757                 }
8758                 memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
8759                 bssid->Ssid.SsidLength = *(p + 1);
8760         }
8761         else
8762         {
8763                 bssid->Ssid.SsidLength = 0;
8764         }
8765
8766         memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
8767
8768         /* checking rate info... */
8769         i = 0;
8770         p = rtw_get_ie(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
8771         if (p != NULL)
8772         {
8773                 if (len > NDIS_802_11_LENGTH_RATES_EX)
8774                 {
8775                         DBG_8723A("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
8776                         return _FAIL;
8777                 }
8778                 memcpy(bssid->SupportedRates, (p + 2), len);
8779                 i = len;
8780         }
8781
8782         p = rtw_get_ie(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
8783         if (p != NULL)
8784         {
8785                 if (len > (NDIS_802_11_LENGTH_RATES_EX-i))
8786                 {
8787                         DBG_8723A("%s()-%d: IE too long (%d) for survey event\n", __FUNCTION__, __LINE__, len);
8788                         return _FAIL;
8789                 }
8790                 memcpy(bssid->SupportedRates + i, (p + 2), len);
8791         }
8792
8793         /* todo: */
8794         {
8795                 bssid->NetworkTypeInUse = Ndis802_11OFDM24;
8796         }
8797
8798         if (bssid->IELength < 12)
8799                 return _FAIL;
8800
8801         /*  Checking for DSConfig */
8802         p = rtw_get_ie(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
8803
8804         bssid->Configuration.DSConfig = 0;
8805         bssid->Configuration.Length = 0;
8806
8807         if (p)
8808         {
8809                 bssid->Configuration.DSConfig = *(p + 2);
8810         }
8811         else
8812         {/*  In 5G, some ap do not have DSSET IE */
8813                 /*  checking HT info for channel */
8814                 p = rtw_get_ie(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
8815                 if(p)
8816                 {
8817                         struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
8818                         bssid->Configuration.DSConfig = HT_info->primary_channel;
8819                 }
8820                 else
8821                 { /*  use current channel */
8822                         bssid->Configuration.DSConfig = rtw_get_oper_ch(padapter);
8823                 }
8824         }
8825
8826         if (subtype==WIFI_PROBEREQ)
8827         {
8828                 /*  FIXME */
8829                 bssid->InfrastructureMode = Ndis802_11Infrastructure;
8830                 memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
8831                 bssid->Privacy = 1;
8832                 return _SUCCESS;
8833         }
8834
8835         memcpy(&bssid->Configuration.BeaconPeriod, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
8836         bssid->Configuration.BeaconPeriod = le32_to_cpu(bssid->Configuration.BeaconPeriod);
8837
8838         val16 = rtw_get_capability((WLAN_BSSID_EX *)bssid);
8839
8840         if (val16 & BIT(0))
8841         {
8842                 bssid->InfrastructureMode = Ndis802_11Infrastructure;
8843                 memcpy(bssid->MacAddress, GetAddr2Ptr(pframe), ETH_ALEN);
8844         }
8845         else
8846         {
8847                 bssid->InfrastructureMode = Ndis802_11IBSS;
8848                 memcpy(bssid->MacAddress, GetAddr3Ptr(pframe), ETH_ALEN);
8849         }
8850
8851         if (val16 & BIT(4))
8852                 bssid->Privacy = 1;
8853         else
8854                 bssid->Privacy = 0;
8855
8856         bssid->Configuration.ATIMWindow = 0;
8857
8858         /* 20/40 BSS Coexistence check */
8859         if((pregistrypriv->wifi_spec==1) && (_FALSE == pmlmeinfo->bwmode_updated))
8860         {
8861                 struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
8862 #ifdef CONFIG_80211N_HT
8863                 p = rtw_get_ie(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset);
8864                 if(p && len>0)
8865                 {
8866                         struct HT_caps_element  *pHT_caps;
8867                         pHT_caps = (struct HT_caps_element      *)(p + 2);
8868
8869                         if(pHT_caps->u.HT_cap_element.HT_caps_info&BIT(14))
8870                         {
8871                                 pmlmepriv->num_FortyMHzIntolerant++;
8872                         }
8873                 }
8874                 else
8875                 {
8876                         pmlmepriv->num_sta_no_ht++;
8877                 }
8878 #endif /* CONFIG_80211N_HT */
8879
8880         }
8881
8882 #ifdef CONFIG_INTEL_WIDI
8883         /* process_intel_widi_query_or_tigger(padapter, bssid); */
8884         if(process_intel_widi_query_or_tigger(padapter, bssid))
8885         {
8886                 return _FAIL;
8887         }
8888 #endif /*  CONFIG_INTEL_WIDI */
8889
8890         #if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) & 1
8891         if(strcmp(bssid->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
8892                 DBG_8723A("Receiving %s("MAC_FMT", DSConfig:%u) from ch%u with ss:%3u, sq:%3u, RawRSSI:%3ld\n"
8893                         , bssid->Ssid.Ssid, MAC_ARG(bssid->MacAddress), bssid->Configuration.DSConfig
8894                         , rtw_get_oper_ch(padapter)
8895                         , bssid->PhyInfo.SignalStrength, bssid->PhyInfo.SignalQuality, bssid->Rssi
8896                 );
8897         }
8898         #endif
8899
8900         /*  mark bss info receving from nearby channel as SignalQuality 101 */
8901         if(bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
8902         {
8903                 bssid->PhyInfo.SignalQuality= 101;
8904         }
8905
8906         return _SUCCESS;
8907 }
8908
8909 void start_create_ibss(_adapter* padapter)
8910 {
8911         unsigned short  caps;
8912         u8      val8;
8913         u8      join_type;
8914         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8915         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8916         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
8917         pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
8918         pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
8919
8920         /* update wireless mode */
8921         update_wireless_mode(padapter);
8922
8923         /* udpate capability */
8924         caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
8925         update_capinfo(padapter, caps);
8926         if(caps&cap_IBSS)/* adhoc master */
8927         {
8928                 val8 = 0xcf;
8929                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
8930
8931                 /* switch channel */
8932                 /* SelectChannel(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */
8933                 set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
8934
8935                 beacon_timing_control(padapter);
8936
8937                 /* set msr to WIFI_FW_ADHOC_STATE */
8938                 pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
8939                 Set_MSR(padapter, (pmlmeinfo->state & 0x3));
8940
8941                 /* issue beacon */
8942                 if(send_beacon(padapter)==_FAIL)
8943                 {
8944                         RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("issuing beacon frame fail....\n"));
8945
8946                         report_join_res(padapter, -1);
8947                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
8948                 }
8949                 else
8950                 {
8951                         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
8952                         join_type = 0;
8953                         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
8954
8955                         report_join_res(padapter, 1);
8956                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
8957                 }
8958         }
8959         else
8960         {
8961                 DBG_8723A("start_create_ibss, invalid cap:%x\n", caps);
8962                 return;
8963         }
8964 }
8965
8966 void start_clnt_join(_adapter* padapter)
8967 {
8968         unsigned short  caps;
8969         u8      val8;
8970         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
8971         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
8972         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
8973         int beacon_timeout;
8974
8975         pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
8976         pmlmeinfo->bcn_interval = get_beacon_interval(pnetwork);
8977
8978         /* update wireless mode */
8979         update_wireless_mode(padapter);
8980
8981         /* udpate capability */
8982         caps = rtw_get_capability((WLAN_BSSID_EX *)pnetwork);
8983         update_capinfo(padapter, caps);
8984         if (caps&cap_ESS)
8985         {
8986
8987 #ifdef CONFIG_DUALMAC_CONCURRENT
8988                 if(dc_handle_join_request(padapter) == _FAIL)
8989                {
8990                    DBG_8723A("dc_handle_join_request fail !!!\n");
8991                    return;
8992                 }
8993                 /* switch channel */
8994                set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
8995 #elif defined (CONFIG_CONCURRENT_MODE)
8996               if(concurrent_chk_start_clnt_join(padapter) == _FAIL)
8997                    return;
8998 #else /* NON CONCURRENT_MODE */
8999               /* switch channel */
9000                set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
9001 #endif
9002
9003                 Set_MSR(padapter, WIFI_FW_STATION_STATE);
9004
9005                 val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf;
9006
9007 #ifdef CONFIG_WAPI_SUPPORT
9008                 if (padapter->wapiInfo.bWapiEnable && pmlmeinfo->auth_algo == dot11AuthAlgrthm_WAPI)
9009                 {
9010                         /* Disable TxUseDefaultKey, RxUseDefaultKey, RxBroadcastUseDefaultKey. */
9011                         val8 = 0x4c;
9012                 }
9013 #endif
9014                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
9015
9016                 /* switch channel */
9017                 /* set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
9018
9019                 /* here wait for receiving the beacon to start auth */
9020                 /* and enable a timer */
9021                 beacon_timeout = decide_wait_for_beacon_timeout(pmlmeinfo->bcn_interval);
9022                 set_link_timer(pmlmeext, beacon_timeout);
9023                 _set_timer( &padapter->mlmepriv.assoc_timer,
9024                         (REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) +beacon_timeout);
9025
9026                 pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
9027         }
9028         else if (caps&cap_IBSS) /* adhoc client */
9029         {
9030 #ifdef CONFIG_DUALMAC_CONCURRENT
9031                 if(dc_handle_join_request(padapter) == _FAIL)
9032                 {
9033                         DBG_8723A("dc_handle_join_request for Ad-hoc fail !!!\n");
9034                         return;
9035                 }
9036 #endif
9037
9038                 Set_MSR(padapter, WIFI_FW_ADHOC_STATE);
9039
9040                 val8 = 0xcf;
9041                 rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
9042
9043                 /* switch channel */
9044                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
9045
9046                 beacon_timing_control(padapter);
9047
9048                 pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
9049
9050                 report_join_res(padapter, 1);
9051         }
9052         else
9053         {
9054                 /* DBG_8723A("marc: invalid cap:%x\n", caps); */
9055                 return;
9056         }
9057 }
9058
9059 void start_clnt_auth(_adapter* padapter)
9060 {
9061         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9062         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9063
9064         _cancel_timer_ex(&pmlmeext->link_timer);
9065
9066         pmlmeinfo->state &= (~WIFI_FW_AUTH_NULL);
9067         pmlmeinfo->state |= WIFI_FW_AUTH_STATE;
9068
9069         pmlmeinfo->auth_seq = 1;
9070         pmlmeinfo->reauth_count = 0;
9071         pmlmeinfo->reassoc_count = 0;
9072         pmlmeinfo->link_count = 0;
9073         pmlmeext->retry = 0;
9074
9075         /*  Because of AP's not receiving deauth before */
9076         /*  AP may: 1)not response auth or 2)deauth us after link is complete */
9077         /*  issue deauth before issuing auth to deal with the situation */
9078         /*      Commented by Albert 2012/07/21 */
9079         /*      For the Win8 P2P connection, it will be hard to have a successful connection if this Wi-Fi doesn't connect to it. */
9080         issue_deauth(padapter, (&(pmlmeinfo->network))->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
9081
9082         DBG_8723A_LEVEL(_drv_always_, "start auth\n");
9083         issue_auth(padapter, NULL, 0);
9084
9085         set_link_timer(pmlmeext, REAUTH_TO);
9086 }
9087
9088 void start_clnt_assoc(_adapter* padapter)
9089 {
9090         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9091         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9092
9093         _cancel_timer_ex(&pmlmeext->link_timer);
9094
9095         pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
9096         pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
9097
9098         issue_assocreq(padapter);
9099
9100         set_link_timer(pmlmeext, REASSOC_TO);
9101 }
9102
9103 unsigned int receive_disconnect(_adapter *padapter, unsigned char *MacAddr, unsigned short reason)
9104 {
9105         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9106         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9107
9108         /* check A3 */
9109         if (memcmp(MacAddr, get_my_bssid(&pmlmeinfo->network), ETH_ALEN))
9110                 return _SUCCESS;
9111
9112         DBG_8723A("%s\n", __FUNCTION__);
9113
9114         if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
9115         {
9116                 if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
9117                 {
9118                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
9119                         report_del_sta_event(padapter, MacAddr, reason);
9120
9121                 }
9122                 else if (pmlmeinfo->state & WIFI_FW_LINKING_STATE)
9123                 {
9124                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
9125                         report_join_res(padapter, -2);
9126                 }
9127         }
9128
9129         return _SUCCESS;
9130 }
9131
9132 #ifdef CONFIG_80211D
9133 static void process_80211d(PADAPTER padapter, WLAN_BSSID_EX *bssid)
9134 {
9135         struct registry_priv *pregistrypriv;
9136         struct mlme_ext_priv *pmlmeext;
9137         RT_CHANNEL_INFO *chplan_new;
9138         u8 channel;
9139         u8 i;
9140
9141         pregistrypriv = &padapter->registrypriv;
9142         pmlmeext = &padapter->mlmeextpriv;
9143
9144         /*  Adjust channel plan by AP Country IE */
9145         if (pregistrypriv->enable80211d &&
9146                 (!pmlmeext->update_channel_plan_by_ap_done))
9147         {
9148                 u8 *ie, *p;
9149                 u32 len;
9150                 RT_CHANNEL_PLAN chplan_ap;
9151                 RT_CHANNEL_INFO chplan_sta[MAX_CHANNEL_NUM];
9152                 u8 country[4];
9153                 u8 fcn; /*  first channel number */
9154                 u8 noc; /*  number of channel */
9155                 u8 j, k;
9156
9157                 ie = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
9158                 if (!ie) return;
9159                 if (len < 6) return;
9160
9161                 ie += 2;
9162                 p = ie;
9163                 ie += len;
9164
9165                 memset(country, 0, 4);
9166                 memcpy(country, p, 3);
9167                 p += 3;
9168                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
9169                                 ("%s: 802.11d country=%s\n", __FUNCTION__, country));
9170
9171                 i = 0;
9172                 while ((ie - p) >= 3)
9173                 {
9174                         fcn = *(p++);
9175                         noc = *(p++);
9176                         p++;
9177
9178                         for (j = 0; j < noc; j++)
9179                         {
9180                                 if (fcn <= 14) channel = fcn + j; /*  2.4 GHz */
9181                                 else channel = fcn + j*4; /*  5 GHz */
9182
9183                                 chplan_ap.Channel[i++] = channel;
9184                         }
9185                 }
9186                 chplan_ap.Len = i;
9187
9188 #ifdef CONFIG_DEBUG_RTL871X
9189                 i = 0;
9190                 DBG_8723A("%s: AP[%s] channel plan {", __func__, bssid->Ssid.Ssid);
9191                 while ((i < chplan_ap.Len) && (chplan_ap.Channel[i] != 0))
9192                 {
9193                         DBG_8723A("%02d,", chplan_ap.Channel[i]);
9194                         i++;
9195                 }
9196                 DBG_8723A("}\n");
9197 #endif
9198
9199                 memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
9200 #ifdef CONFIG_DEBUG_RTL871X
9201                 i = 0;
9202                 DBG_8723A("%s: STA channel plan {", __func__);
9203                 while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0))
9204                 {
9205                         DBG_8723A("%02d(%c),", chplan_sta[i].ChannelNum, chplan_sta[i].ScanType==SCAN_PASSIVE?'p':'a');
9206                         i++;
9207                 }
9208                 DBG_8723A("}\n");
9209 #endif
9210
9211                 memset(pmlmeext->channel_set, 0, sizeof(pmlmeext->channel_set));
9212                 chplan_new = pmlmeext->channel_set;
9213
9214                 i = j = k = 0;
9215                 if (pregistrypriv->wireless_mode & WIRELESS_11G) {
9216                         do {
9217                                 if ((i == MAX_CHANNEL_NUM) ||
9218                                         (chplan_sta[i].ChannelNum == 0) ||
9219                                         (chplan_sta[i].ChannelNum > 14))
9220                                         break;
9221
9222                                 if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
9223                                         break;
9224
9225                                 if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
9226                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
9227                                         chplan_new[k].ScanType = SCAN_ACTIVE;
9228                                         i++;
9229                                         j++;
9230                                         k++;
9231                                 } else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
9232                                         chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
9233                                         chplan_new[k].ScanType = SCAN_PASSIVE;
9234                                         i++;
9235                                         k++;
9236                                 } else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
9237                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
9238                                         chplan_new[k].ScanType = SCAN_ACTIVE;
9239                                         j++;
9240                                         k++;
9241                                 }
9242                         } while (1);
9243
9244                         /*  change AP not support channel to Passive scan */
9245                         while ((i < MAX_CHANNEL_NUM) &&
9246                                 (chplan_sta[i].ChannelNum != 0) &&
9247                                 (chplan_sta[i].ChannelNum <= 14)) {
9248                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
9249                                 chplan_new[k].ScanType = SCAN_PASSIVE;
9250                                 i++;
9251                                 k++;
9252                         }
9253
9254                         /*  add channel AP supported */
9255                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
9256                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
9257                                 chplan_new[k].ScanType = SCAN_ACTIVE;
9258                                 j++;
9259                                 k++;
9260                         }
9261                 } else {
9262                         /*  keep original STA 2.4G channel plan */
9263                         while ((i < MAX_CHANNEL_NUM) &&
9264                                 (chplan_sta[i].ChannelNum != 0) &&
9265                                 (chplan_sta[i].ChannelNum <= 14)) {
9266                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
9267                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
9268                                 i++;
9269                                 k++;
9270                         }
9271
9272                         /*  skip AP 2.4G channel plan */
9273                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
9274                                 j++;
9275                         }
9276                 }
9277
9278                 if (pregistrypriv->wireless_mode & WIRELESS_11A) {
9279                         do {
9280                                 if ((i == MAX_CHANNEL_NUM) ||
9281                                     (chplan_sta[i].ChannelNum == 0))
9282                                         break;
9283
9284                                 if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0))
9285                                         break;
9286
9287                                 if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j])
9288                                 {
9289                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
9290                                         chplan_new[k].ScanType = SCAN_ACTIVE;
9291                                         i++;
9292                                         j++;
9293                                         k++;
9294                                 }
9295                                 else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j])
9296                                 {
9297                                         chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
9298 /*                                      chplan_new[k].ScanType = chplan_sta[i].ScanType; */
9299                                         chplan_new[k].ScanType = SCAN_PASSIVE;
9300                                         i++;
9301                                         k++;
9302                                 }
9303                                 else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j])
9304                                 {
9305                                         chplan_new[k].ChannelNum = chplan_ap.Channel[j];
9306                                         chplan_new[k].ScanType = SCAN_ACTIVE;
9307                                         j++;
9308                                         k++;
9309                                 }
9310                         } while (1);
9311
9312                         /*  change AP not support channel to Passive scan */
9313                         while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
9314                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
9315                                 chplan_new[k].ScanType = SCAN_PASSIVE;
9316                                 i++;
9317                                 k++;
9318                         }
9319
9320                         /*  add channel AP supported */
9321                         while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] != 0)) {
9322                                 chplan_new[k].ChannelNum = chplan_ap.Channel[j];
9323                                 chplan_new[k].ScanType = SCAN_ACTIVE;
9324                                 j++;
9325                                 k++;
9326                         }
9327                 } else {
9328                         /*  keep original STA 5G channel plan */
9329                         while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
9330                                 chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
9331                                 chplan_new[k].ScanType = chplan_sta[i].ScanType;
9332                                 i++;
9333                                 k++;
9334                         }
9335                 }
9336
9337                 pmlmeext->update_channel_plan_by_ap_done = 1;
9338
9339 #ifdef CONFIG_DEBUG_RTL871X
9340                 k = 0;
9341                 DBG_8723A("%s: new STA channel plan {", __func__);
9342                 while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0)) {
9343                         DBG_8723A("%02d(%c),", chplan_new[k].ChannelNum, chplan_new[k].ScanType==SCAN_PASSIVE?'p':'c');
9344                         k++;
9345                 }
9346                 DBG_8723A("}\n");
9347 #endif
9348         }
9349
9350         /*  If channel is used by AP, set channel scan type to active */
9351         channel = bssid->Configuration.DSConfig;
9352         chplan_new = pmlmeext->channel_set;
9353         i = 0;
9354         while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0)) {
9355                 if (chplan_new[i].ChannelNum == channel)
9356                 {
9357                         if (chplan_new[i].ScanType == SCAN_PASSIVE) {
9358                                 /* 5G Bnad 2, 3 (DFS) doesn't change to active scan */
9359                                 if(channel >= 52 && channel <= 144)
9360                                         break;
9361
9362                                 chplan_new[i].ScanType = SCAN_ACTIVE;
9363                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
9364                                                  ("%s: change channel %d scan type from passive to active\n",
9365                                                   __FUNCTION__, channel));
9366                         }
9367                         break;
9368                 }
9369                 i++;
9370         }
9371 }
9372 #endif
9373
9374 /****************************************************************************
9375
9376 Following are the functions to report events
9377
9378 *****************************************************************************/
9379
9380 void report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
9381 {
9382         struct cmd_obj *pcmd_obj;
9383         u8      *pevtcmd;
9384         u32 cmdsz;
9385         struct survey_event     *psurvey_evt;
9386         struct C2HEvent_Header *pc2h_evt_hdr;
9387         struct mlme_ext_priv *pmlmeext;
9388         struct cmd_priv *pcmdpriv;
9389
9390         if(!padapter)
9391                 return;
9392
9393         pmlmeext = &padapter->mlmeextpriv;
9394         pcmdpriv = &padapter->cmdpriv;
9395
9396         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
9397                 return;
9398
9399         cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
9400         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL) {
9401                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
9402                 return;
9403         }
9404
9405         _rtw_init_listhead(&pcmd_obj->list);
9406
9407         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
9408         pcmd_obj->cmdsz = cmdsz;
9409         pcmd_obj->parmbuf = pevtcmd;
9410
9411         pcmd_obj->rsp = NULL;
9412         pcmd_obj->rspsz  = 0;
9413
9414         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
9415         pc2h_evt_hdr->len = sizeof(struct survey_event);
9416         pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
9417         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
9418
9419         psurvey_evt = (struct survey_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
9420
9421         if (collect_bss_info(padapter, precv_frame, (WLAN_BSSID_EX *)&psurvey_evt->bss) == _FAIL) {
9422                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
9423                 rtw_mfree((u8 *)pevtcmd, cmdsz);
9424                 return;
9425         }
9426
9427 #ifdef CONFIG_80211D
9428         process_80211d(padapter, &psurvey_evt->bss);
9429 #endif
9430
9431         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
9432
9433         pmlmeext->sitesurvey_res.bss_cnt++;
9434
9435         return;
9436 }
9437
9438 void report_surveydone_event(_adapter *padapter)
9439 {
9440         struct cmd_obj *pcmd_obj;
9441         u8      *pevtcmd;
9442         u32 cmdsz;
9443         struct surveydone_event *psurveydone_evt;
9444         struct C2HEvent_Header  *pc2h_evt_hdr;
9445         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
9446         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
9447
9448         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
9449         {
9450                 return;
9451         }
9452
9453         cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
9454         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
9455         {
9456                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
9457                 return;
9458         }
9459
9460         _rtw_init_listhead(&pcmd_obj->list);
9461
9462         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
9463         pcmd_obj->cmdsz = cmdsz;
9464         pcmd_obj->parmbuf = pevtcmd;
9465
9466         pcmd_obj->rsp = NULL;
9467         pcmd_obj->rspsz  = 0;
9468
9469         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
9470         pc2h_evt_hdr->len = sizeof(struct surveydone_event);
9471         pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
9472         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
9473
9474         psurveydone_evt = (struct surveydone_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
9475         psurveydone_evt->bss_cnt = pmlmeext->sitesurvey_res.bss_cnt;
9476
9477         DBG_8723A("survey done event(%x)\n", psurveydone_evt->bss_cnt);
9478
9479         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
9480
9481         return;
9482 }
9483
9484 void report_join_res(_adapter *padapter, int res)
9485 {
9486         struct cmd_obj *pcmd_obj;
9487         u8      *pevtcmd;
9488         u32 cmdsz;
9489         struct joinbss_event            *pjoinbss_evt;
9490         struct C2HEvent_Header  *pc2h_evt_hdr;
9491         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
9492         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9493         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
9494
9495         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
9496         {
9497                 return;
9498         }
9499
9500         cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
9501         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
9502         {
9503                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
9504                 return;
9505         }
9506
9507         _rtw_init_listhead(&pcmd_obj->list);
9508
9509         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
9510         pcmd_obj->cmdsz = cmdsz;
9511         pcmd_obj->parmbuf = pevtcmd;
9512
9513         pcmd_obj->rsp = NULL;
9514         pcmd_obj->rspsz  = 0;
9515
9516         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
9517         pc2h_evt_hdr->len = sizeof(struct joinbss_event);
9518         pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
9519         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
9520
9521         pjoinbss_evt = (struct joinbss_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
9522         memcpy((unsigned char *)(&(pjoinbss_evt->network.network)), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
9523         pjoinbss_evt->network.join_res  = pjoinbss_evt->network.aid = res;
9524
9525         DBG_8723A("report_join_res(%d)\n", res);
9526
9527         rtw_joinbss_event_prehandle(padapter, (u8 *)&pjoinbss_evt->network);
9528
9529         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
9530
9531         return;
9532 }
9533
9534 void report_del_sta_event(_adapter *padapter, unsigned char* MacAddr, unsigned short reason)
9535 {
9536         struct cmd_obj *pcmd_obj;
9537         u8      *pevtcmd;
9538         u32 cmdsz;
9539         struct sta_info *psta;
9540         int     mac_id;
9541         struct stadel_event                     *pdel_sta_evt;
9542         struct C2HEvent_Header  *pc2h_evt_hdr;
9543         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
9544         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
9545
9546         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
9547         {
9548                 return;
9549         }
9550
9551         cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
9552         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
9553         {
9554                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
9555                 return;
9556         }
9557
9558         _rtw_init_listhead(&pcmd_obj->list);
9559
9560         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
9561         pcmd_obj->cmdsz = cmdsz;
9562         pcmd_obj->parmbuf = pevtcmd;
9563
9564         pcmd_obj->rsp = NULL;
9565         pcmd_obj->rspsz  = 0;
9566
9567         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
9568         pc2h_evt_hdr->len = sizeof(struct stadel_event);
9569         pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
9570         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
9571
9572         pdel_sta_evt = (struct stadel_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
9573         memcpy((unsigned char *)(&(pdel_sta_evt->macaddr)), MacAddr, ETH_ALEN);
9574         memcpy((unsigned char *)(pdel_sta_evt->rsvd),(unsigned char *)(&reason),2);
9575
9576         psta = rtw_get_stainfo(&padapter->stapriv, MacAddr);
9577         if(psta)
9578                 mac_id = (int)psta->mac_id;
9579         else
9580                 mac_id = (-1);
9581
9582         pdel_sta_evt->mac_id = mac_id;
9583
9584         DBG_8723A("report_del_sta_event: delete STA, mac_id=%d\n", mac_id);
9585
9586         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
9587
9588         return;
9589 }
9590
9591 void report_add_sta_event(_adapter *padapter, unsigned char* MacAddr, int cam_idx)
9592 {
9593         struct cmd_obj *pcmd_obj;
9594         u8      *pevtcmd;
9595         u32 cmdsz;
9596         struct stassoc_event            *padd_sta_evt;
9597         struct C2HEvent_Header  *pc2h_evt_hdr;
9598         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
9599         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
9600
9601         if ((pcmd_obj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
9602         {
9603                 return;
9604         }
9605
9606         cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
9607         if ((pevtcmd = (u8*)rtw_zmalloc(cmdsz)) == NULL)
9608         {
9609                 rtw_mfree((u8 *)pcmd_obj, sizeof(struct cmd_obj));
9610                 return;
9611         }
9612
9613         _rtw_init_listhead(&pcmd_obj->list);
9614
9615         pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
9616         pcmd_obj->cmdsz = cmdsz;
9617         pcmd_obj->parmbuf = pevtcmd;
9618
9619         pcmd_obj->rsp = NULL;
9620         pcmd_obj->rspsz  = 0;
9621
9622         pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
9623         pc2h_evt_hdr->len = sizeof(struct stassoc_event);
9624         pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
9625         pc2h_evt_hdr->seq = ATOMIC_INC_RETURN(&pmlmeext->event_seq);
9626
9627         padd_sta_evt = (struct stassoc_event*)(pevtcmd + sizeof(struct C2HEvent_Header));
9628         memcpy((unsigned char *)(&(padd_sta_evt->macaddr)), MacAddr, ETH_ALEN);
9629         padd_sta_evt->cam_id = cam_idx;
9630
9631         DBG_8723A("report_add_sta_event: add STA\n");
9632
9633         rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
9634
9635         return;
9636 }
9637
9638 /****************************************************************************
9639
9640 Following are the event callback functions
9641
9642 *****************************************************************************/
9643
9644 /* for sta/adhoc mode */
9645 void update_sta_info(_adapter *padapter, struct sta_info *psta)
9646 {
9647         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
9648         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9649         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9650
9651         /* ERP */
9652         VCS_update(padapter, psta);
9653
9654 #ifdef CONFIG_80211N_HT
9655         /* HT */
9656         if(pmlmepriv->htpriv.ht_option)
9657         {
9658                 psta->htpriv.ht_option = _TRUE;
9659
9660                 psta->htpriv.ampdu_enable = pmlmepriv->htpriv.ampdu_enable;
9661
9662                 if (support_short_GI(padapter, &(pmlmeinfo->HT_caps)))
9663                         psta->htpriv.sgi = _TRUE;
9664
9665                 psta->qos_option = _TRUE;
9666
9667         }
9668         else
9669 #endif /* CONFIG_80211N_HT */
9670         {
9671 #ifdef CONFIG_80211N_HT
9672                 psta->htpriv.ht_option = _FALSE;
9673
9674                 psta->htpriv.ampdu_enable = _FALSE;
9675
9676                 psta->htpriv.sgi = _FALSE;
9677 #endif /* CONFIG_80211N_HT */
9678                 psta->qos_option = _FALSE;
9679
9680         }
9681 #ifdef CONFIG_80211N_HT
9682         psta->htpriv.bwmode = pmlmeext->cur_bwmode;
9683         psta->htpriv.ch_offset = pmlmeext->cur_ch_offset;
9684
9685         psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
9686         psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
9687 #endif /* CONFIG_80211N_HT */
9688
9689         /* QoS */
9690         if(pmlmepriv->qospriv.qos_option)
9691                 psta->qos_option = _TRUE;
9692
9693         psta->state = _FW_LINKED;
9694 }
9695
9696 void mlmeext_joinbss_event_callback(_adapter *padapter, int join_res)
9697 {
9698         struct sta_info         *psta, *psta_bmc;
9699         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9700         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9701         WLAN_BSSID_EX           *cur_network = &(pmlmeinfo->network);
9702         struct sta_priv         *pstapriv = &padapter->stapriv;
9703         u8      join_type;
9704         u16 media_status;
9705
9706         if(join_res < 0)
9707         {
9708                 join_type = 1;
9709                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
9710                 rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
9711
9712                 /* restore to initial setting. */
9713                 update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
9714
9715                 goto exit_mlmeext_joinbss_event_callback;
9716         }
9717
9718         if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
9719         {
9720                 /* for bc/mc */
9721                 psta_bmc = rtw_get_bcmc_stainfo(padapter);
9722                 if(psta_bmc)
9723                 {
9724                         pmlmeinfo->FW_sta_info[psta_bmc->mac_id].psta = psta_bmc;
9725                         update_bmc_sta_support_rate(padapter, psta_bmc->mac_id);
9726                         Update_RA_Entry(padapter, psta_bmc);
9727                 }
9728         }
9729
9730         /* turn on dynamic functions */
9731         Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, _TRUE);
9732
9733         /*  update IOT-releated issue */
9734         update_IOT_info(padapter);
9735
9736         rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates);
9737
9738         /* BCN interval */
9739         rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
9740
9741         /* udpate capability */
9742         update_capinfo(padapter, pmlmeinfo->capability);
9743
9744         /* WMM, Update EDCA param */
9745         WMMOnAssocRsp(padapter);
9746
9747         /* HT */
9748         HTOnAssocRsp(padapter);
9749
9750 #ifndef CONFIG_CONCURRENT_MODE
9751         /*      Call set_channel_bwmode when the CONFIG_CONCURRENT_MODE doesn't be defined. */
9752         /* Set cur_channel&cur_bwmode&cur_ch_offset */
9753         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
9754 #endif
9755
9756         psta = rtw_get_stainfo(pstapriv, cur_network->MacAddress);
9757         if (psta) /* only for infra. mode */
9758         {
9759                 pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
9760
9761                 /* DBG_8723A("set_sta_rate\n"); */
9762
9763                 psta->wireless_mode = pmlmeext->cur_wireless_mode;
9764
9765                 /* set per sta rate after updating HT cap. */
9766                 set_sta_rate(padapter, psta);
9767
9768                 #if (RATE_ADAPTIVE_SUPPORT==1)  /* for 88E RA */
9769                 rtw_hal_set_hwreg(padapter,HW_VAR_TX_RPT_MAX_MACID, (u8*)&psta->mac_id);
9770                 #endif
9771                 media_status = (psta->mac_id<<8)|1; /*   MACID|OPMODE: 1 means connect */
9772                 rtw_hal_set_hwreg(padapter,HW_VAR_H2C_MEDIA_STATUS_RPT,(u8 *)&media_status);
9773         }
9774
9775         join_type = 2;
9776         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
9777
9778         if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
9779         {
9780                 /*  correcting TSF */
9781                 correct_TSF(padapter, pmlmeext);
9782
9783                 /* set_link_timer(pmlmeext, DISCONNECT_TO); */
9784         }
9785
9786 #ifdef CONFIG_LPS
9787         rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_CONNECT, 0);
9788 #endif
9789
9790 exit_mlmeext_joinbss_event_callback:
9791
9792 #ifdef CONFIG_DUALMAC_CONCURRENT
9793         dc_handle_join_done(padapter, join_res);
9794 #endif
9795 #ifdef CONFIG_CONCURRENT_MODE
9796         concurrent_chk_joinbss_done(padapter, join_res);
9797 #endif
9798
9799         DBG_8723A("=>%s\n", __FUNCTION__);
9800 }
9801
9802 void mlmeext_sta_add_event_callback(_adapter *padapter, struct sta_info *psta)
9803 {
9804         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
9805         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9806         u8      join_type;
9807
9808         DBG_8723A("%s\n", __FUNCTION__);
9809
9810         if((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
9811         {
9812                 if(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)/* adhoc master or sta_count>1 */
9813                 {
9814                         /* nothing to do */
9815                 }
9816                 else/* adhoc client */
9817                 {
9818                         /* update TSF Value */
9819                         /* update_TSF(pmlmeext, pframe, len); */
9820
9821                         /*  correcting TSF */
9822                         correct_TSF(padapter, pmlmeext);
9823
9824                         /* start beacon */
9825                         if(send_beacon(padapter)==_FAIL)
9826                         {
9827                                 pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
9828
9829                                 pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
9830
9831                                 return;
9832                         }
9833
9834                         pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
9835
9836                 }
9837
9838                 join_type = 2;
9839                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
9840         }
9841
9842         pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
9843
9844         /* rate radaptive */
9845         Update_RA_Entry(padapter, psta);
9846
9847         /* update adhoc sta_info */
9848         update_sta_info(padapter, psta);
9849 }
9850
9851 void mlmeext_sta_del_event_callback(_adapter *padapter)
9852 {
9853         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9854         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9855
9856         if (is_client_associated_to_ap(padapter) || is_IBSS_empty(padapter))
9857         {
9858                 /* set_opmode_cmd(padapter, infra_client_with_mlme); */
9859
9860                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
9861                 rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
9862
9863                 /* restore to initial setting. */
9864                 update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
9865
9866 #ifdef CONFIG_DUALMAC_CONCURRENT
9867                 dc_set_channel_bwmode_disconnect(padapter);
9868 #else
9869 #ifdef CONFIG_CONCURRENT_MODE
9870                 if((check_buddy_fwstate(padapter, _FW_LINKED)) != _TRUE)
9871                 {
9872 #endif /* CONFIG_CONCURRENT_MODE */
9873
9874                 /* switch to the 20M Hz mode after disconnect */
9875                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
9876                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
9877
9878                 /* SelectChannel(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset); */
9879                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
9880
9881 #ifdef CONFIG_CONCURRENT_MODE
9882                 }
9883 #endif /* CONFIG_CONCURRENT_MODE */
9884 #endif /* CONFIG_DUALMAC_CONCURRENT */
9885
9886                 flush_all_cam_entry(padapter);
9887
9888                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
9889
9890                 /* set MSR to no link state -> infra. mode */
9891                 Set_MSR(padapter, _HW_STATE_STATION_);
9892
9893                 _cancel_timer_ex(&pmlmeext->link_timer);
9894
9895         }
9896 }
9897
9898 /****************************************************************************
9899
9900 Following are the functions for the timer handlers
9901
9902 *****************************************************************************/
9903 void _linked_rx_signal_strehgth_display(_adapter *padapter);
9904 void _linked_rx_signal_strehgth_display(_adapter *padapter)
9905 {
9906         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9907       struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9908         u8 mac_id;
9909         int UndecoratedSmoothedPWDB;
9910         if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
9911         {
9912                 mac_id=0;
9913         }
9914         else if((pmlmeinfo->state&0x03) == _HW_STATE_AP_)
9915         {
9916                 mac_id=2;
9917         }
9918
9919         rtw_hal_get_def_var(padapter, HW_DEF_RA_INFO_DUMP,&mac_id);
9920
9921         rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
9922         DBG_8723A("UndecoratedSmoothedPWDB:%d\n",UndecoratedSmoothedPWDB);
9923 }
9924
9925 u8 chk_ap_is_alive(_adapter *padapter, struct sta_info *psta)
9926 {
9927         u8 ret = _FALSE;
9928         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9929         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9930
9931         #ifdef DBG_EXPIRATION_CHK
9932         DBG_8723A(FUNC_ADPT_FMT" rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu"
9933                                 /*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/
9934                                 ", retry:%u\n"
9935                 , FUNC_ADPT_ARG(padapter)
9936                 , STA_RX_PKTS_DIFF_ARG(psta)
9937                 , psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts
9938                 , psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts
9939                 /*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts
9940                 , psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts
9941                 , psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts
9942                 , pmlmeinfo->bcn_interval*/
9943                 , pmlmeext->retry
9944         );
9945
9946         DBG_8723A(FUNC_ADPT_FMT" tx_pkts:%llu, link_count:%u\n", FUNC_ADPT_ARG(padapter)
9947                 , padapter->xmitpriv.tx_pkts
9948                 , pmlmeinfo->link_count
9949         );
9950         #endif
9951
9952         if((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta))
9953                 && sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta)
9954                 && sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta)
9955         )
9956         {
9957                 ret = _FALSE;
9958         }
9959         else
9960         {
9961                 ret = _TRUE;
9962         }
9963
9964         sta_update_last_rx_pkts(psta);
9965
9966         return ret;
9967 }
9968
9969 void linked_status_chk(_adapter *padapter)
9970 {
9971         u32     i;
9972         struct sta_info         *psta;
9973         struct xmit_priv                *pxmitpriv = &(padapter->xmitpriv);
9974         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
9975         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
9976         struct sta_priv         *pstapriv = &padapter->stapriv;
9977
9978         if(padapter->bRxRSSIDisplay)
9979                  _linked_rx_signal_strehgth_display(padapter);
9980
9981         #ifdef DBG_CONFIG_ERROR_DETECT
9982         rtw_hal_sreset_linked_status_check(padapter);
9983         #endif
9984
9985         if (is_client_associated_to_ap(padapter))
9986         {
9987                 /* linked infrastructure client mode */
9988
9989                 int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
9990                 int rx_chk_limit;
9991
9992                 #if defined(DBG_ROAMING_TEST)
9993                 rx_chk_limit = 1;
9994                 #elif defined(CONFIG_ACTIVE_KEEP_ALIVE_CHECK)
9995                 rx_chk_limit = 4;
9996                 #else
9997                 rx_chk_limit = 8;
9998                 #endif
9999
10000                 #ifdef CONFIG_INTEL_WIDI
10001                 if (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_NONE)
10002                         rx_chk_limit = 1;
10003                 #endif
10004
10005                 if ((psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
10006                 {
10007                         bool is_p2p_enable = _FALSE;
10008                         #ifdef CONFIG_P2P
10009                         is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
10010                         #endif
10011
10012                         if (chk_ap_is_alive(padapter, psta) == _FALSE)
10013                                 rx_chk = _FAIL;
10014
10015                         if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
10016                                 tx_chk = _FAIL;
10017
10018                         #ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
10019                         if (pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL)) {
10020                                 u8 backup_oper_channel=0;
10021
10022                                 /* switch to correct channel of current network  before issue keep-alive frames */
10023                                 if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
10024                                         backup_oper_channel = rtw_get_oper_ch(padapter);
10025                                         SelectChannel(padapter, pmlmeext->cur_channel);
10026                                 }
10027
10028                                 if (rx_chk != _SUCCESS)
10029                                         issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
10030
10031                                 if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) || rx_chk != _SUCCESS) {
10032                                         tx_chk = issue_nulldata(padapter, psta->hwaddr, 0, 3, 1);
10033                                         /* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */
10034                                         if (tx_chk == _SUCCESS && !is_p2p_enable)
10035                                                 rx_chk = _SUCCESS;
10036                                 }
10037
10038                                 /* back to the original operation channel */
10039                                 if(backup_oper_channel>0)
10040                                         SelectChannel(padapter, backup_oper_channel);
10041
10042                         }
10043                         else
10044                         #endif /* CONFIG_ACTIVE_KEEP_ALIVE_CHECK */
10045                         {
10046                                 if (rx_chk != _SUCCESS) {
10047                                         if (pmlmeext->retry == 0) {
10048                                                 #ifdef DBG_EXPIRATION_CHK
10049                                                 DBG_8723A("issue_probereq to trigger probersp, retry=%d\n", pmlmeext->retry);
10050                                                 #endif
10051                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
10052                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
10053                                                 issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
10054                                         }
10055                                 }
10056
10057                                 if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) {
10058                                         #ifdef DBG_EXPIRATION_CHK
10059                                         DBG_8723A("%s issue_nulldata 0\n", __FUNCTION__);
10060                                         #endif
10061                                         tx_chk = issue_nulldata(padapter, NULL, 0, 1, 0);
10062                                 }
10063                         }
10064
10065                         if (rx_chk == _FAIL) {
10066                                 pmlmeext->retry++;
10067                                 if (pmlmeext->retry > rx_chk_limit) {
10068                                         DBG_8723A_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n",
10069                                                 FUNC_ADPT_ARG(padapter));
10070                                         receive_disconnect(padapter, pmlmeinfo->network.MacAddress
10071                                                 , WLAN_REASON_EXPIRATION_CHK);
10072                                         return;
10073                                 }
10074                         } else {
10075                                 pmlmeext->retry = 0;
10076                         }
10077
10078                         if (tx_chk == _FAIL) {
10079                                 pmlmeinfo->link_count &= 0xf;
10080                         } else {
10081                                 pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
10082                                 pmlmeinfo->link_count = 0;
10083                         }
10084
10085                 } /* end of if ((psta = rtw_get_stainfo(pstapriv, passoc_res->network.MacAddress)) != NULL) */
10086         }
10087         else if (is_client_associated_to_ibss(padapter))
10088         {
10089                 /* linked IBSS mode */
10090                 /* for each assoc list entry to check the rx pkt counter */
10091                 for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
10092                 {
10093                         if (pmlmeinfo->FW_sta_info[i].status == 1)
10094                         {
10095                                 psta = pmlmeinfo->FW_sta_info[i].psta;
10096
10097                                 if(NULL==psta) continue;
10098
10099                                 if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta))
10100                                 {
10101
10102                                         if(pmlmeinfo->FW_sta_info[i].retry<3)
10103                                         {
10104                                                 pmlmeinfo->FW_sta_info[i].retry++;
10105                                         }
10106                                         else
10107                                         {
10108                                                 pmlmeinfo->FW_sta_info[i].retry = 0;
10109                                                 pmlmeinfo->FW_sta_info[i].status = 0;
10110                                                 report_del_sta_event(padapter, psta->hwaddr
10111                                                         , 65535/*  indicate disconnect caused by no rx */
10112                                                 );
10113                                         }
10114                                 }
10115                                 else
10116                                 {
10117                                         pmlmeinfo->FW_sta_info[i].retry = 0;
10118                                         pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
10119                                 }
10120                         }
10121                 }
10122
10123                 /* set_link_timer(pmlmeext, DISCONNECT_TO); */
10124
10125         }
10126 }
10127
10128 void survey_timer_hdl(_adapter *padapter)
10129 {
10130         struct cmd_obj  *ph2c;
10131         struct sitesurvey_parm  *psurveyPara;
10132         struct cmd_priv                                 *pcmdpriv=&padapter->cmdpriv;
10133         struct mlme_ext_priv            *pmlmeext = &padapter->mlmeextpriv;
10134 #ifdef CONFIG_P2P
10135         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
10136 #endif
10137
10138         /* DBG_8723A("marc: survey timer\n"); */
10139         /* issue rtw_sitesurvey_cmd */
10140         if (pmlmeext->sitesurvey_res.state > SCAN_START)
10141         {
10142                 if(pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
10143                 {
10144 #ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
10145                         if( padapter->mlmeextpriv.mlmext_info.scan_cnt != RTW_SCAN_NUM_OF_CH )
10146 #endif /* CONFIG_STA_MODE_SCAN_UNDER_AP_MODE */
10147                                 pmlmeext->sitesurvey_res.channel_idx++;
10148                 }
10149
10150                 if(pmlmeext->scan_abort == _TRUE)
10151                 {
10152                         #ifdef CONFIG_P2P
10153                         if(!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE))
10154                         {
10155                                 rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
10156                                 pmlmeext->sitesurvey_res.channel_idx = 3;
10157                                 DBG_8723A("%s idx:%d, cnt:%u\n", __FUNCTION__
10158                                         , pmlmeext->sitesurvey_res.channel_idx
10159                                         , pwdinfo->find_phase_state_exchange_cnt
10160                                 );
10161                         }
10162                         else
10163                         #endif
10164                         {
10165                                 pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
10166                                 DBG_8723A("%s idx:%d\n", __FUNCTION__
10167                                         , pmlmeext->sitesurvey_res.channel_idx
10168                                 );
10169                         }
10170
10171                         pmlmeext->scan_abort = _FALSE;/* reset */
10172                 }
10173
10174                 if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
10175                 {
10176                         goto exit_survey_timer_hdl;
10177                 }
10178
10179                 if ((psurveyPara = (struct sitesurvey_parm*)rtw_zmalloc(sizeof(struct sitesurvey_parm))) == NULL)
10180                 {
10181                         rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
10182                         goto exit_survey_timer_hdl;
10183                 }
10184
10185                 init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
10186                 rtw_enqueue_cmd(pcmdpriv, ph2c);
10187         }
10188
10189 exit_survey_timer_hdl:
10190         return;
10191 }
10192
10193 void link_timer_hdl(_adapter *padapter)
10194 {
10195         /* static unsigned int          rx_pkt = 0; */
10196         /* static u64                           tx_cnt = 0; */
10197         /* struct xmit_priv             *pxmitpriv = &(padapter->xmitpriv); */
10198         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10199         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10200         /* struct sta_priv              *pstapriv = &padapter->stapriv; */
10201
10202         if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
10203         {
10204                 DBG_8723A("link_timer_hdl:no beacon while connecting\n");
10205                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
10206                 report_join_res(padapter, -3);
10207         }
10208         else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE)
10209         {
10210                 /* re-auth timer */
10211                 if (++pmlmeinfo->reauth_count > REAUTH_LIMIT)
10212                 {
10213                         /* if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto) */
10214                         /*  */
10215                                 pmlmeinfo->state = 0;
10216                                 report_join_res(padapter, -1);
10217                                 return;
10218                         /*  */
10219                         /* else */
10220                         /*  */
10221                         /*      pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */
10222                         /*      pmlmeinfo->reauth_count = 0; */
10223                         /*  */
10224                 }
10225
10226                 DBG_8723A("link_timer_hdl: auth timeout and try again\n");
10227                 pmlmeinfo->auth_seq = 1;
10228                 issue_auth(padapter, NULL, 0);
10229                 set_link_timer(pmlmeext, REAUTH_TO);
10230         }
10231         else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)
10232         {
10233                 /* re-assoc timer */
10234                 if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT)
10235                 {
10236                         pmlmeinfo->state = WIFI_FW_NULL_STATE;
10237                         report_join_res(padapter, -2);
10238                         return;
10239                 }
10240
10241                 DBG_8723A("link_timer_hdl: assoc timeout and try again\n");
10242                 issue_assocreq(padapter);
10243                 set_link_timer(pmlmeext, REASSOC_TO);
10244         }
10245
10246         return;
10247 }
10248
10249 void addba_timer_hdl(struct sta_info *psta)
10250 {
10251 #ifdef CONFIG_80211N_HT
10252         struct ht_priv  *phtpriv;
10253
10254         if(!psta)
10255                 return;
10256
10257         phtpriv = &psta->htpriv;
10258
10259         if((phtpriv->ht_option==_TRUE) && (phtpriv->ampdu_enable==_TRUE))
10260         {
10261                 if(phtpriv->candidate_tid_bitmap)
10262                         phtpriv->candidate_tid_bitmap=0x0;
10263
10264         }
10265 #endif /* CONFIG_80211N_HT */
10266 }
10267
10268 u8 NULL_hdl(_adapter *padapter, u8 *pbuf)
10269 {
10270         return H2C_SUCCESS;
10271 }
10272
10273 u8 setopmode_hdl(_adapter *padapter, u8 *pbuf)
10274 {
10275         u8      type;
10276         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10277         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10278         struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
10279
10280         if(psetop->mode == Ndis802_11APMode)
10281         {
10282                 pmlmeinfo->state = WIFI_FW_AP_STATE;
10283                 type = _HW_STATE_AP_;
10284 #ifdef CONFIG_NATIVEAP_MLME
10285                 /* start_ap_mode(padapter); */
10286 #endif
10287         }
10288         else if(psetop->mode == Ndis802_11Infrastructure)
10289         {
10290                 pmlmeinfo->state &= ~(BIT(0)|BIT(1));/*  clear state */
10291                 pmlmeinfo->state |= WIFI_FW_STATION_STATE;/* set to     STATION_STATE */
10292                 type = _HW_STATE_STATION_;
10293         }
10294         else if(psetop->mode == Ndis802_11IBSS)
10295         {
10296                 type = _HW_STATE_ADHOC_;
10297         }
10298         else
10299         {
10300                 type = _HW_STATE_NOLINK_;
10301         }
10302
10303         rtw_hal_set_hwreg(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
10304         /* Set_NETYPE0_MSR(padapter, type); */
10305
10306         return H2C_SUCCESS;
10307 }
10308
10309 u8 createbss_hdl(_adapter *padapter, u8 *pbuf)
10310 {
10311         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10312         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10313         WLAN_BSSID_EX   *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
10314         struct joinbss_parm *pparm = (struct joinbss_parm *)pbuf;
10315         /* u32  initialgain; */
10316
10317         if(pparm->network.InfrastructureMode == Ndis802_11APMode)
10318         {
10319 #ifdef CONFIG_AP_MODE
10320
10321                 if(pmlmeinfo->state == WIFI_FW_AP_STATE)
10322                 {
10323                         /* todo: */
10324                         return H2C_SUCCESS;
10325                 }
10326 #endif
10327         }
10328
10329         /* below is for ad-hoc master */
10330         if(pparm->network.InfrastructureMode == Ndis802_11IBSS)
10331         {
10332                 rtw_joinbss_reset(padapter);
10333
10334                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
10335                 pmlmeext->cur_ch_offset= HAL_PRIME_CHNL_OFFSET_DONT_CARE;
10336                 pmlmeinfo->ERP_enable = 0;
10337                 pmlmeinfo->WMM_enable = 0;
10338                 pmlmeinfo->HT_enable = 0;
10339                 pmlmeinfo->HT_caps_enable = 0;
10340                 pmlmeinfo->HT_info_enable = 0;
10341                 pmlmeinfo->agg_enable_bitmap = 0;
10342                 pmlmeinfo->candidate_tid_bitmap = 0;
10343
10344                 /* disable dynamic functions, such as high power, DIG */
10345                 Save_DM_Func_Flag(padapter);
10346                 Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
10347
10348                 /* config the initial gain under linking, need to write the BB registers */
10349                 /* initialgain = 0x1E; */
10350                 /* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
10351
10352                 /* cancel link timer */
10353                 _cancel_timer_ex(&pmlmeext->link_timer);
10354
10355                 /* clear CAM */
10356                 flush_all_cam_entry(padapter);
10357
10358                 memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength));
10359                 pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength;
10360
10361                 if(pnetwork->IELength>MAX_IE_SZ)/* Check pbuf->IELength */
10362                         return H2C_PARAMETERS_ERROR;
10363
10364                 memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength);
10365
10366                 start_create_ibss(padapter);
10367
10368         }
10369
10370         return H2C_SUCCESS;
10371 }
10372
10373 u8 join_cmd_hdl(_adapter *padapter, u8 *pbuf)
10374 {
10375         u8      join_type;
10376         PNDIS_802_11_VARIABLE_IEs       pIE;
10377         struct registry_priv    *pregpriv = &padapter->registrypriv;
10378         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10379         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10380         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
10381 #ifdef CONFIG_ANTENNA_DIVERSITY
10382         struct joinbss_parm     *pparm = (struct joinbss_parm *)pbuf;
10383 #endif /* CONFIG_ANTENNA_DIVERSITY */
10384         u32 i;
10385         /* u32  initialgain; */
10386         /* u32  acparm; */
10387
10388         /* check already connecting to AP or not */
10389         if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
10390         {
10391                 if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
10392                 {
10393                         issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, 5, 100);
10394                 }
10395
10396                 pmlmeinfo->state = WIFI_FW_NULL_STATE;
10397
10398                 /* clear CAM */
10399                 flush_all_cam_entry(padapter);
10400
10401                 _cancel_timer_ex(&pmlmeext->link_timer);
10402
10403                 /* set MSR to nolink -> infra. mode */
10404                 /* Set_MSR(padapter, _HW_STATE_NOLINK_); */
10405                 Set_MSR(padapter, _HW_STATE_STATION_);
10406
10407                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
10408         }
10409
10410 #ifdef CONFIG_ANTENNA_DIVERSITY
10411         rtw_antenna_select_cmd(padapter, pparm->network.PhyInfo.Optimum_antenna, _FALSE);
10412 #endif
10413
10414 #ifdef CONFIG_WAPI_SUPPORT
10415         rtw_wapi_clear_all_cam_entry(padapter);
10416 #endif
10417
10418         rtw_joinbss_reset(padapter);
10419
10420         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
10421         pmlmeext->cur_ch_offset= HAL_PRIME_CHNL_OFFSET_DONT_CARE;
10422         pmlmeinfo->ERP_enable = 0;
10423         pmlmeinfo->WMM_enable = 0;
10424         pmlmeinfo->HT_enable = 0;
10425         pmlmeinfo->HT_caps_enable = 0;
10426         pmlmeinfo->HT_info_enable = 0;
10427         pmlmeinfo->agg_enable_bitmap = 0;
10428         pmlmeinfo->candidate_tid_bitmap = 0;
10429         pmlmeinfo->bwmode_updated = _FALSE;
10430         /* pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX; */
10431
10432         memcpy(pnetwork, pbuf, FIELD_OFFSET(WLAN_BSSID_EX, IELength));
10433         pnetwork->IELength = ((WLAN_BSSID_EX *)pbuf)->IELength;
10434
10435         if(pnetwork->IELength>MAX_IE_SZ)/* Check pbuf->IELength */
10436                 return H2C_PARAMETERS_ERROR;
10437
10438         memcpy(pnetwork->IEs, ((WLAN_BSSID_EX *)pbuf)->IEs, pnetwork->IELength);
10439
10440         /* Check AP vendor to move rtw_joinbss_cmd() */
10441         /* pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->IEs, pnetwork->IELength); */
10442
10443         for (i = sizeof(NDIS_802_11_FIXED_IEs); i < pnetwork->IELength;)
10444         {
10445                 pIE = (PNDIS_802_11_VARIABLE_IEs)(pnetwork->IEs + i);
10446
10447                 switch (pIE->ElementID)
10448                 {
10449                         case _VENDOR_SPECIFIC_IE_:/* Get WMM IE. */
10450                                 if (!memcmp(pIE->data, WMM_OUI, 4))
10451                                 {
10452                                         pmlmeinfo->WMM_enable = 1;
10453                                 }
10454                                 break;
10455
10456                         case _HT_CAPABILITY_IE_:        /* Get HT Cap IE. */
10457                                 pmlmeinfo->HT_caps_enable = 1;
10458                                 break;
10459
10460                         case _HT_EXTRA_INFO_IE_:        /* Get HT Info IE. */
10461 #ifdef CONFIG_80211N_HT
10462                                 pmlmeinfo->HT_info_enable = 1;
10463
10464                                 /* spec case only for cisco's ap because cisco's ap issue assoc rsp using mcs rate @40MHz or @20MHz */
10465 /* if !defined(CONFIG_CONCURRENT_MODE) && !defined(CONFIG_DUALMAC_CONCURRENT) */
10466 /*                              if(pmlmeinfo->assoc_AP_vendor == ciscoAP) */
10467 /* endif */
10468                                 {
10469                                         struct HT_info_element *pht_info = (struct HT_info_element *)(pIE->data);
10470
10471                                         if ((pregpriv->cbw40_enable) &&  (pht_info->infos[0] & BIT(2)))
10472                                         {
10473                                                 /* switch to the 40M Hz mode according to the AP */
10474                                                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
10475                                                 switch (pht_info->infos[0] & 0x3)
10476                                                 {
10477                                                         case 1:
10478                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
10479                                                                 break;
10480
10481                                                         case 3:
10482                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
10483                                                                 break;
10484
10485                                                         default:
10486                                                                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
10487                                                                 break;
10488                                                 }
10489
10490                                                 DBG_8723A("set ch/bw before connected\n");
10491                                         }
10492                                 }
10493 #endif /* CONFIG_80211N_HT */
10494                                 break;
10495
10496                         default:
10497                                 break;
10498                 }
10499
10500                 i += (pIE->Length + 2);
10501         }
10502         /* disable dynamic functions, such as high power, DIG */
10503         /* Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE); */
10504
10505         /* config the initial gain under linking, need to write the BB registers */
10506         /* initialgain = 0x1E; */
10507         /* rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
10508
10509         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
10510         join_type = 0;
10511         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
10512
10513         /* cancel link timer */
10514         _cancel_timer_ex(&pmlmeext->link_timer);
10515
10516         start_clnt_join(padapter);
10517
10518         return H2C_SUCCESS;
10519 }
10520
10521 u8 disconnect_hdl(_adapter *padapter, unsigned char *pbuf)
10522 {
10523         struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
10524         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10525         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10526         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
10527         u8      val8;
10528
10529         if (is_client_associated_to_ap(padapter))
10530         {
10531                 issue_deauth_ex(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100);
10532         }
10533
10534         /* set_opmode_cmd(padapter, infra_client_with_mlme); */
10535
10536         /* pmlmeinfo->state = WIFI_FW_NULL_STATE; */
10537
10538         rtw_hal_set_hwreg(padapter, HW_VAR_MLME_DISCONNECT, 0);
10539         rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, null_addr);
10540
10541         /* restore to initial setting. */
10542         update_tx_basic_rate(padapter, padapter->registrypriv.wireless_mode);
10543
10544         if(((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
10545         {
10546                 /* Stop BCN */
10547                 val8 = 0;
10548                 rtw_hal_set_hwreg(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8));
10549         }
10550
10551         /* set MSR to no link state -> infra. mode */
10552         Set_MSR(padapter, _HW_STATE_STATION_);
10553
10554         pmlmeinfo->state = WIFI_FW_NULL_STATE;
10555
10556 #ifdef CONFIG_DUALMAC_CONCURRENT
10557         dc_set_channel_bwmode_disconnect(padapter);
10558 #else
10559 #ifdef CONFIG_CONCURRENT_MODE
10560         if((check_buddy_fwstate(padapter, _FW_LINKED)) != _TRUE)
10561         {
10562 #endif /* CONFIG_CONCURRENT_MODE */
10563                 /* switch to the 20M Hz mode after disconnect */
10564                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
10565                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
10566
10567                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
10568 #ifdef CONFIG_CONCURRENT_MODE
10569         }
10570 #endif /* CONFIG_CONCURRENT_MODE */
10571 #endif /* CONFIG_DUALMAC_CONCURRENT */
10572
10573         flush_all_cam_entry(padapter);
10574
10575         _cancel_timer_ex(&pmlmeext->link_timer);
10576
10577         rtw_free_uc_swdec_pending_queue(padapter);
10578
10579         return  H2C_SUCCESS;
10580 }
10581
10582 int rtw_scan_ch_decision(_adapter *padapter, struct rtw_ieee80211_channel *out,
10583         u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
10584 {
10585         int i, j;
10586         int scan_ch_num = 0;
10587         int set_idx;
10588         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10589
10590         /* clear out first */
10591         memset(out, 0, sizeof(struct rtw_ieee80211_channel)*out_num);
10592
10593         /* acquire channels from in */
10594         j = 0;
10595         for (i=0;i<in_num;i++) {
10596                 if (0)
10597                 DBG_8723A(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[i]));
10598                 if(in[i].hw_value && !(in[i].flags & RTW_IEEE80211_CHAN_DISABLED)
10599                         && (set_idx=rtw_ch_set_search_ch(pmlmeext->channel_set, in[i].hw_value)) >=0
10600                 )
10601                 {
10602                         memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel));
10603
10604                         if(pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
10605                                 out[j].flags &= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
10606
10607                         j++;
10608                 }
10609                 if(j>=out_num)
10610                         break;
10611         }
10612
10613         /* if out is empty, use channel_set as default */
10614         if(j == 0) {
10615                 for (i=0;i<pmlmeext->max_chan_nums;i++) {
10616                         out[i].hw_value = pmlmeext->channel_set[i].ChannelNum;
10617
10618                         if(pmlmeext->channel_set[i].ScanType == SCAN_PASSIVE)
10619                                 out[i].flags &= RTW_IEEE80211_CHAN_PASSIVE_SCAN;
10620
10621                         j++;
10622                 }
10623         }
10624
10625         if (padapter->setband == GHZ_24) {                              /*  2.4G */
10626                 for (i=0; i < j ; i++) {
10627                         if (out[i].hw_value > 35)
10628                                 memset(&out[i], 0 , sizeof(struct rtw_ieee80211_channel));
10629                         else
10630                                 scan_ch_num++;
10631                 }
10632                 j = scan_ch_num;
10633         } else if  (padapter->setband == GHZ_50) {                      /*  5G */
10634                 for (i=0; i < j ; i++) {
10635                         if (out[i].hw_value > 35) {
10636                                 memcpy(&out[scan_ch_num++], &out[i], sizeof(struct rtw_ieee80211_channel));
10637                         }
10638                 }
10639                 j = scan_ch_num;
10640         } else
10641                 {}
10642
10643         return j;
10644 }
10645
10646 u8 sitesurvey_cmd_hdl(_adapter *padapter, u8 *pbuf)
10647 {
10648         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10649         struct sitesurvey_parm  *pparm = (struct sitesurvey_parm *)pbuf;
10650         u8      bdelayscan = _FALSE;
10651         u8      val8;
10652         u32     initialgain;
10653         u32     i;
10654
10655 #ifdef CONFIG_P2P
10656         struct wifidirect_info* pwdinfo = &padapter->wdinfo;
10657 #endif
10658
10659         if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE)
10660         {
10661                 /* for first time sitesurvey_cmd */
10662                 rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
10663
10664                 pmlmeext->sitesurvey_res.state = SCAN_START;
10665                 pmlmeext->sitesurvey_res.bss_cnt = 0;
10666                 pmlmeext->sitesurvey_res.channel_idx = 0;
10667
10668                 for(i=0;i<RTW_SSID_SCAN_AMOUNT;i++){
10669                         if(pparm->ssid[i].SsidLength) {
10670                                 memcpy(pmlmeext->sitesurvey_res.ssid[i].Ssid, pparm->ssid[i].Ssid, IW_ESSID_MAX_SIZE);
10671                                 pmlmeext->sitesurvey_res.ssid[i].SsidLength= pparm->ssid[i].SsidLength;
10672                         } else {
10673                                 pmlmeext->sitesurvey_res.ssid[i].SsidLength= 0;
10674                         }
10675                 }
10676
10677                 pmlmeext->sitesurvey_res.ch_num = rtw_scan_ch_decision(padapter
10678                         , pmlmeext->sitesurvey_res.ch, RTW_CHANNEL_SCAN_AMOUNT
10679                         , pparm->ch, pparm->ch_num
10680                 );
10681
10682                 pmlmeext->sitesurvey_res.scan_mode = pparm->scan_mode;
10683
10684 #ifdef CONFIG_DUALMAC_CONCURRENT
10685                 bdelayscan = dc_handle_site_survey(padapter);
10686 #endif
10687
10688                 /* issue null data if associating to the AP */
10689                 if (is_client_associated_to_ap(padapter) == _TRUE)
10690                 {
10691                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
10692
10693                         /* switch to correct channel of current network  before issue keep-alive frames */
10694                         if (rtw_get_oper_ch(padapter) != pmlmeext->cur_channel) {
10695                                 SelectChannel(padapter, pmlmeext->cur_channel);
10696                         }
10697
10698                         issue_nulldata(padapter, NULL, 1, 3, 500);
10699
10700 #ifdef CONFIG_CONCURRENT_MODE
10701                         if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
10702                         {
10703                                 DBG_8723A("adapter is scanning(buddy_adapter is linked), issue nulldata(pwrbit=1)\n");
10704
10705                                 issue_nulldata(padapter->pbuddy_adapter, NULL, 1, 3, 500);
10706                         }
10707 #endif
10708                         bdelayscan = _TRUE;
10709                 }
10710 #ifdef CONFIG_CONCURRENT_MODE
10711                 else if(is_client_associated_to_ap(padapter->pbuddy_adapter) == _TRUE)
10712                 {
10713                         #ifdef CONFIG_TDLS
10714                         if(padapter->pbuddy_adapter->wdinfo.wfd_tdls_enable == 1)
10715                         {
10716                                 issue_tunneled_probe_req(padapter->pbuddy_adapter);
10717                         }
10718                         #endif /* CONFIG_TDLS */
10719
10720                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
10721
10722                         issue_nulldata(padapter->pbuddy_adapter, NULL, 1, 3, 500);
10723
10724                         bdelayscan = _TRUE;
10725                 }
10726 #endif
10727                 if(bdelayscan)
10728                 {
10729                         /* delay 50ms to protect nulldata(1). */
10730                         set_survey_timer(pmlmeext, 50);
10731                         return H2C_SUCCESS;
10732                 }
10733         }
10734
10735         if ((pmlmeext->sitesurvey_res.state == SCAN_START) || (pmlmeext->sitesurvey_res.state == SCAN_TXNULL))
10736         {
10737                 /* disable dynamic functions, such as high power, DIG */
10738                 Save_DM_Func_Flag(padapter);
10739                 Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
10740
10741                 /* config the initial gain under scaning, need to write the BB registers */
10742 #ifdef CONFIG_IOCTL_CFG80211
10743                 if((wdev_to_priv(padapter->rtw_wdev))->p2p_enabled == _TRUE)
10744                 {
10745                         initialgain = 0x30;
10746                 }
10747                 else
10748                         initialgain = 0x1E;
10749 #else   /*      go through the WEXT interface CONFIG_IOCTL_CFG80211 */
10750 #ifdef CONFIG_P2P
10751                 if ( rtw_p2p_chk_state( pwdinfo, P2P_STATE_NONE ) )
10752                         initialgain = 0x1E;
10753                 else
10754                         initialgain = 0x28;
10755 #else   /*  CONFIG_P2P */
10756                 initialgain = 0x1E;
10757 #endif /*  CONFIG_P2P */
10758 #endif /*  CONFIG_IOCTL_CFG80211 */
10759
10760                 rtw_hal_set_hwreg(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
10761
10762                 /* set MSR to no link state */
10763                 Set_MSR(padapter, _HW_STATE_NOLINK_);
10764
10765                 val8 = 1; /* under site survey */
10766                 rtw_hal_set_hwreg(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
10767
10768                 pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
10769         }
10770
10771         site_survey(padapter);
10772
10773         return H2C_SUCCESS;
10774 }
10775
10776 u8 setauth_hdl(_adapter *padapter, unsigned char *pbuf)
10777 {
10778         struct setauth_parm             *pparm = (struct setauth_parm *)pbuf;
10779         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10780         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10781
10782         if (pparm->mode < 4)
10783         {
10784                 pmlmeinfo->auth_algo = pparm->mode;
10785         }
10786
10787         return  H2C_SUCCESS;
10788 }
10789
10790 u8 setkey_hdl(_adapter *padapter, u8 *pbuf)
10791 {
10792         unsigned short                          ctrl;
10793         struct setkey_parm              *pparm = (struct setkey_parm *)pbuf;
10794         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10795         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10796         unsigned char                                   null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
10797
10798         /* main tx key for wep. */
10799         if(pparm->set_tx)
10800                 pmlmeinfo->key_index = pparm->keyid;
10801
10802         /* write cam */
10803         ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;
10804
10805         DBG_8723A_LEVEL(_drv_always_, "set group key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) "
10806                         "keyid:%d\n", pparm->algorithm, pparm->keyid);
10807         write_cam(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
10808
10809         /* allow multicast packets to driver */
10810         padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_ON_RCR_AM, null_addr);
10811
10812         return H2C_SUCCESS;
10813 }
10814
10815 u8 set_stakey_hdl(_adapter *padapter, u8 *pbuf)
10816 {
10817         u16 ctrl=0;
10818         u8 cam_id;/* cam_entry */
10819         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10820         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10821         struct set_stakey_parm  *pparm = (struct set_stakey_parm *)pbuf;
10822 #ifdef CONFIG_TDLS
10823         struct tdls_info        *ptdlsinfo = &padapter->tdlsinfo;
10824         struct sta_priv *pstapriv = &padapter->stapriv;
10825         struct sta_info *psta;
10826 #endif /* CONFIG_TDLS */
10827
10828         /* cam_entry: */
10829         /* 0~3 for default key */
10830
10831         /* for concurrent mode (ap+sta): */
10832         /* default key is disable, using sw encrypt/decrypt */
10833         /* cam_entry = 4  for sta mode (macid=0) */
10834         /* cam_entry(macid+3) = 5 ~ N for ap mode (aid=1~N, macid=2 ~N) */
10835
10836         /* for concurrent mode (sta+sta): */
10837         /* default key is disable, using sw encrypt/decrypt */
10838         /* cam_entry = 4 mapping to macid=0 */
10839         /* cam_entry = 5 mapping to macid=2 */
10840
10841 #ifdef CONFIG_CONCURRENT_MODE
10842         if((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
10843         {
10844                 struct sta_priv *pstapriv = &padapter->stapriv;
10845                 struct sta_info *psta;
10846
10847                 psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
10848
10849                 if(psta && psta->mac_id==2)
10850                 {
10851                         cam_id = 5;
10852                 }
10853                 else
10854                 {
10855                         cam_id = 4;
10856                 }
10857 /*
10858                 if(padapter->iface_type > PRIMARY_IFACE)
10859                 {
10860                         cam_id = 5;
10861                 }
10862                 else
10863                 {
10864                         cam_id = 4;
10865                 }
10866 */
10867         }
10868 #else
10869         cam_id = 4;
10870 #endif
10871
10872         DBG_8723A_LEVEL(_drv_always_, "set pairwise key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) camid:%d\n",
10873                         pparm->algorithm, cam_id);
10874         if((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
10875         {
10876
10877                 struct sta_info *psta;
10878                 struct sta_priv *pstapriv = &padapter->stapriv;
10879
10880                 if(pparm->algorithm == _NO_PRIVACY_)    /*  clear cam entry */
10881                 {
10882                         clear_cam_entry(padapter, pparm->id);
10883                         return H2C_SUCCESS_RSP;
10884                 }
10885
10886                 psta = rtw_get_stainfo(pstapriv, pparm->addr);
10887                 if(psta)
10888                 {
10889                         ctrl = (BIT(15) | ((pparm->algorithm) << 2));
10890
10891                         DBG_8723A("r871x_set_stakey_hdl(): enc_algorithm=%d\n", pparm->algorithm);
10892
10893                         if((psta->mac_id<1) || (psta->mac_id>(NUM_STA-4)))
10894                         {
10895                                 DBG_8723A("r871x_set_stakey_hdl():set_stakey failed, mac_id(aid)=%d\n", psta->mac_id);
10896                                 return H2C_REJECTED;
10897                         }
10898
10899                         cam_id = (psta->mac_id + 3);/* 0~3 for default key, cmd_id=macid + 3, macid=aid+1; */
10900
10901                         DBG_8723A("Write CAM, mac_addr=%x:%x:%x:%x:%x:%x, cam_entry=%d\n", pparm->addr[0],
10902                                                 pparm->addr[1], pparm->addr[2], pparm->addr[3], pparm->addr[4],
10903                                                 pparm->addr[5], cam_id);
10904
10905                         write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
10906
10907                         return H2C_SUCCESS_RSP;
10908
10909                 }
10910                 else
10911                 {
10912                         DBG_8723A("r871x_set_stakey_hdl(): sta has been free\n");
10913                         return H2C_REJECTED;
10914                 }
10915
10916         }
10917
10918         /* below for sta mode */
10919
10920         if(pparm->algorithm == _NO_PRIVACY_)    /*  clear cam entry */
10921         {
10922                 clear_cam_entry(padapter, pparm->id);
10923                 return H2C_SUCCESS;
10924         }
10925
10926         ctrl = BIT(15) | ((pparm->algorithm) << 2);
10927
10928 #ifdef CONFIG_TDLS
10929         if(ptdlsinfo->clear_cam!=0){
10930                 clear_cam_entry(padapter, ptdlsinfo->clear_cam);
10931                 ptdlsinfo->clear_cam=0;
10932
10933                 return H2C_SUCCESS;
10934         }
10935
10936         psta = rtw_get_stainfo(pstapriv, pparm->addr);/* Get TDLS Peer STA */
10937         if( psta->tdls_sta_state&TDLS_LINKED_STATE ){
10938                 write_cam(padapter, psta->mac_id, ctrl, pparm->addr, pparm->key);
10939         }
10940         else
10941 #endif /* CONFIG_TDLS */
10942         write_cam(padapter, cam_id, ctrl, pparm->addr, pparm->key);
10943
10944         pmlmeinfo->enc_algo = pparm->algorithm;
10945
10946         return H2C_SUCCESS;
10947 }
10948
10949 u8 add_ba_hdl(_adapter *padapter, unsigned char *pbuf)
10950 {
10951         struct addBaReq_parm    *pparm = (struct addBaReq_parm *)pbuf;
10952         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10953         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10954
10955         struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, pparm->addr);
10956
10957         if(!psta)
10958                 return  H2C_SUCCESS;
10959
10960 #ifdef CONFIG_80211N_HT
10961         if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && (pmlmeinfo->HT_enable)) ||
10962                 ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
10963         {
10964                 /* pmlmeinfo->ADDBA_retry_count = 0; */
10965                 /* pmlmeinfo->candidate_tid_bitmap |= (0x1 << pparm->tid); */
10966                 /* psta->htpriv.candidate_tid_bitmap |= BIT(pparm->tid); */
10967                 issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
10968                 /* _set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO); */
10969                 _set_timer(&psta->addba_retry_timer, ADDBA_TO);
10970         }
10971 #ifdef CONFIG_TDLS
10972         else if((psta->tdls_sta_state & TDLS_LINKED_STATE)&&
10973                 (psta->htpriv.ht_option==_TRUE) &&
10974                 (psta->htpriv.ampdu_enable==_TRUE) )
10975         {
10976                 issue_action_BA(padapter, pparm->addr, RTW_WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
10977                 /* _set_timer(&pmlmeext->ADDBA_timer, ADDBA_TO); */
10978                 _set_timer(&psta->addba_retry_timer, ADDBA_TO);
10979         }
10980 #endif /* CONFIG */
10981         else
10982         {
10983                 psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
10984         }
10985 #endif /* CONFIG_80211N_HT */
10986         return  H2C_SUCCESS;
10987 }
10988
10989 u8 set_tx_beacon_cmd(_adapter* padapter)
10990 {
10991         struct cmd_obj  *ph2c;
10992         struct Tx_Beacon_param  *ptxBeacon_parm;
10993         struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
10994         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
10995         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
10996         u8      res = _SUCCESS;
10997         int len_diff = 0;
10998
10999 _func_enter_;
11000
11001         if ((ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj))) == NULL)
11002         {
11003                 res= _FAIL;
11004                 goto exit;
11005         }
11006
11007         if ((ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param))) == NULL)
11008         {
11009                 rtw_mfree((unsigned char *)ph2c, sizeof(struct  cmd_obj));
11010                 res= _FAIL;
11011                 goto exit;
11012         }
11013
11014         memcpy(&(ptxBeacon_parm->network), &(pmlmeinfo->network), sizeof(WLAN_BSSID_EX));
11015
11016         len_diff = update_hidden_ssid(
11017                 ptxBeacon_parm->network.IEs+_BEACON_IE_OFFSET_
11018                 , ptxBeacon_parm->network.IELength-_BEACON_IE_OFFSET_
11019                 , pmlmeinfo->hidden_ssid_mode
11020         );
11021         ptxBeacon_parm->network.IELength += len_diff;
11022
11023         init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
11024
11025         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
11026
11027 exit:
11028
11029 _func_exit_;
11030
11031         return res;
11032 }
11033
11034 u8 mlme_evt_hdl(_adapter *padapter, unsigned char *pbuf)
11035 {
11036         u8 evt_code, evt_seq;
11037         u16 evt_sz;
11038         uint    *peventbuf;
11039         void (*event_callback)(_adapter *dev, u8 *pbuf);
11040         struct evt_priv *pevt_priv = &(padapter->evtpriv);
11041
11042         peventbuf = (uint*)pbuf;
11043         evt_sz = (u16)(*peventbuf&0xffff);
11044         evt_seq = (u8)((*peventbuf>>24)&0x7f);
11045         evt_code = (u8)((*peventbuf>>16)&0xff);
11046
11047         #ifdef CHECK_EVENT_SEQ
11048         /*  checking event sequence... */
11049         if (evt_seq != (ATOMIC_READ(&pevt_priv->event_seq) & 0x7f) )
11050         {
11051                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("Evetn Seq Error! %d vs %d\n", (evt_seq & 0x7f), (ATOMIC_READ(&pevt_priv->event_seq) & 0x7f)));
11052
11053                 pevt_priv->event_seq = (evt_seq+1)&0x7f;
11054
11055                 goto _abort_event_;
11056         }
11057         #endif
11058
11059         /*  checking if event code is valid */
11060         if (evt_code >= MAX_C2HEVT) {
11061                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nEvent Code(%d) mismatch!\n", evt_code));
11062                 goto _abort_event_;
11063         }
11064
11065         /*  checking if event size match the event parm size */
11066         if ((wlanevents[evt_code].parmsize != 0) &&
11067             (wlanevents[evt_code].parmsize != evt_sz)) {
11068                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
11069                         evt_code, wlanevents[evt_code].parmsize, evt_sz));
11070                 goto _abort_event_;
11071         }
11072
11073         ATOMIC_INC(&pevt_priv->event_seq);
11074
11075         peventbuf += 2;
11076
11077         if (peventbuf) {
11078                 event_callback = wlanevents[evt_code].event_callback;
11079                 event_callback(padapter, (u8*)peventbuf);
11080
11081                 pevt_priv->evt_done_cnt++;
11082         }
11083
11084 _abort_event_:
11085
11086         return H2C_SUCCESS;
11087 }
11088
11089 u8 h2c_msg_hdl(_adapter *padapter, unsigned char *pbuf)
11090 {
11091         if(!pbuf)
11092                 return H2C_PARAMETERS_ERROR;
11093
11094         return H2C_SUCCESS;
11095 }
11096
11097 u8 tx_beacon_hdl(_adapter *padapter, unsigned char *pbuf)
11098 {
11099         if(send_beacon(padapter)==_FAIL)
11100         {
11101                 DBG_8723A("issue_beacon, fail!\n");
11102                 return H2C_PARAMETERS_ERROR;
11103         }
11104 #ifdef CONFIG_AP_MODE
11105         else /* tx bc/mc frames after update TIM */
11106         {
11107                 _irqL irqL;
11108                 struct sta_info *psta_bmc;
11109                 _list   *xmitframe_plist, *xmitframe_phead;
11110                 struct xmit_frame *pxmitframe=NULL;
11111                 struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
11112                 struct sta_priv  *pstapriv = &padapter->stapriv;
11113
11114                 /* for BC/MC Frames */
11115                 psta_bmc = rtw_get_bcmc_stainfo(padapter);
11116                 if(!psta_bmc)
11117                         return H2C_SUCCESS;
11118
11119                 if((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len>0))
11120                 {
11121                         rtw_msleep_os(10);/*  10ms, ATIM(HIQ) Windows */
11122                         /* spin_lock_bh(&psta_bmc->sleep_q.lock); */
11123                         spin_lock_bh(&pxmitpriv->lock);
11124
11125                         xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
11126                         xmitframe_plist = get_next(xmitframe_phead);
11127
11128                         while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
11129                         {
11130                                 pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
11131
11132                                 xmitframe_plist = get_next(xmitframe_plist);
11133
11134                                 rtw_list_delete(&pxmitframe->list);
11135
11136                                 psta_bmc->sleepq_len--;
11137                                 if(psta_bmc->sleepq_len>0)
11138                                         pxmitframe->attrib.mdata = 1;
11139                                 else
11140                                         pxmitframe->attrib.mdata = 0;
11141
11142                                 pxmitframe->attrib.triggered=1;
11143
11144                                 pxmitframe->attrib.qsel = 0x11;/* HIQ */
11145
11146                                 rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
11147
11148                                 /* pstapriv->tim_bitmap &= ~BIT(0); */
11149
11150                         }
11151
11152                         /* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
11153                         spin_unlock_bh(&pxmitpriv->lock);
11154                 }
11155
11156         }
11157 #endif
11158
11159         return H2C_SUCCESS;
11160 }
11161
11162 #ifdef CONFIG_DUALMAC_CONCURRENT
11163 void dc_SelectChannel(_adapter *padapter, unsigned char channel)
11164 {
11165         PADAPTER ptarget_adapter;
11166
11167         if( (padapter->pbuddy_adapter != NULL) &&
11168                 (padapter->DualMacConcurrent == _TRUE) &&
11169                 (padapter->adapter_type == SECONDARY_ADAPTER))
11170         {
11171                 /*  only mac0 could control BB&RF */
11172                 ptarget_adapter = padapter->pbuddy_adapter;
11173         }
11174         else
11175         {
11176                 ptarget_adapter = padapter;
11177         }
11178
11179         _enter_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setch_mutex), NULL);
11180
11181         rtw_hal_set_chan(ptarget_adapter, channel);
11182
11183         _exit_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setch_mutex), NULL);
11184 }
11185
11186 void dc_SetBWMode(_adapter *padapter, unsigned short bwmode, unsigned char channel_offset)
11187 {
11188         PADAPTER ptarget_adapter;
11189
11190         if( (padapter->pbuddy_adapter != NULL) &&
11191                 (padapter->DualMacConcurrent == _TRUE) &&
11192                 (padapter->adapter_type == SECONDARY_ADAPTER))
11193         {
11194                 /*  only mac0 could control BB&RF */
11195                 ptarget_adapter = padapter->pbuddy_adapter;
11196         }
11197         else
11198         {
11199                 ptarget_adapter = padapter;
11200         }
11201
11202         _enter_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setbw_mutex), NULL);
11203
11204         rtw_hal_set_bwmode(ptarget_adapter, (HT_CHANNEL_WIDTH)bwmode, channel_offset);
11205
11206         _exit_critical_mutex(&(adapter_to_dvobj(ptarget_adapter)->setbw_mutex), NULL);
11207 }
11208
11209 static void dc_change_band(_adapter *padapter, WLAN_BSSID_EX *pnetwork)
11210 {
11211         u8      network_type,rate_len, total_rate_len,remainder_rate_len;
11212         struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
11213         struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
11214         u8      erpinfo=0x4;
11215
11216         /* DBG_8723A("%s\n", __FUNCTION__); */
11217
11218         if(pmlmeext->cur_channel >= 36)
11219         {
11220                 network_type = WIRELESS_11A;
11221                 total_rate_len = IEEE80211_NUM_OFDM_RATESLEN;
11222                 DBG_8723A("%s(): change to 5G Band\n",__FUNCTION__);
11223                 rtw_remove_bcn_ie(padapter, pnetwork, _ERPINFO_IE_);
11224         }
11225         else
11226         {
11227                 network_type = WIRELESS_11BG;
11228                 total_rate_len = IEEE80211_CCK_RATE_LEN+IEEE80211_NUM_OFDM_RATESLEN;
11229                 DBG_8723A("%s(): change to 2.4G Band\n",__FUNCTION__);
11230                 rtw_add_bcn_ie(padapter, pnetwork, _ERPINFO_IE_, &erpinfo, 1);
11231         }
11232
11233         rtw_set_supported_rate(pnetwork->SupportedRates, network_type);
11234
11235         UpdateBrateTbl(padapter, pnetwork->SupportedRates);
11236         rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
11237
11238         if(total_rate_len > 8)
11239         {
11240                 rate_len = 8;
11241                 remainder_rate_len = total_rate_len - 8;
11242         }
11243         else
11244         {
11245                 rate_len = total_rate_len;
11246                 remainder_rate_len = 0;
11247         }
11248
11249         rtw_add_bcn_ie(padapter, pnetwork, _SUPPORTEDRATES_IE_, pnetwork->SupportedRates, rate_len);
11250
11251         if(remainder_rate_len)
11252         {
11253                 rtw_add_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_, (pnetwork->SupportedRates+8), remainder_rate_len);
11254         }
11255         else
11256         {
11257                 rtw_remove_bcn_ie(padapter, pnetwork, _EXT_SUPPORTEDRATES_IE_);
11258         }
11259 }
11260
11261 void dc_set_channel_bwmode_disconnect(_adapter *padapter)
11262 {
11263         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11264         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
11265         struct mlme_priv *pbuddy_mlmepriv = NULL;
11266
11267         if(pbuddy_adapter != NULL &&
11268                 padapter->DualMacConcurrent == _TRUE)
11269         {
11270                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
11271                 if((check_fwstate(pbuddy_mlmepriv, _FW_LINKED)) != _TRUE)
11272                 {
11273                         /* switch to the 20M Hz mode after disconnect */
11274                         pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
11275                         pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
11276
11277                         set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
11278                 }
11279         }
11280         else
11281         {
11282                 /* switch to the 20M Hz mode after disconnect */
11283                 pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
11284                 pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
11285
11286                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
11287         }
11288 }
11289
11290 u8 dc_handle_join_request(_adapter *padapter)
11291 {
11292         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11293         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11294         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX*)(&(pmlmeinfo->network));
11295         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
11296         struct mlme_ext_priv *pbuddy_mlmeext = NULL;
11297         struct mlme_priv        *pbuddy_mlmepriv = NULL;
11298         u8      ret = _SUCCESS;
11299
11300         if(pbuddy_adapter != NULL &&
11301                 padapter->DualMacConcurrent == _TRUE)
11302         {
11303                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
11304                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
11305
11306                 if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel ||
11307                         pmlmeext->cur_bwmode != pbuddy_mlmeext->cur_bwmode ||
11308                         pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset)
11309                 {
11310                         if((check_fwstate(pbuddy_mlmepriv, WIFI_AP_STATE)) == _TRUE)
11311                         {
11312                                 /* issue deauth to all stas if if2 is at ap mode */
11313                                 rtw_sta_flush(pbuddy_adapter);
11314
11315                                 /* rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0); */
11316                                 rtw_hal_set_hwreg(pbuddy_adapter, HW_VAR_CHECK_TXBUF, 0);
11317                         }
11318                         else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED) == _TRUE)
11319                         {
11320                                 if(pmlmeext->cur_channel == pbuddy_mlmeext->cur_channel)
11321                                 {
11322                                         /*  HT_CHANNEL_WIDTH_40 or HT_CHANNEL_WIDTH_20 but channel offset is different */
11323                                         if((pmlmeext->cur_bwmode == pbuddy_mlmeext->cur_bwmode) &&
11324                                                 (pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset) )
11325                                         {
11326                                                 report_join_res(padapter, -4);
11327                                                 ret = _FAIL;
11328                                         }
11329                                 }
11330                                 else
11331                                 {
11332                                         report_join_res(padapter, -4);
11333                                         ret = _FAIL;
11334                                 }
11335                         }
11336                 }
11337                 else     if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
11338                 {
11339                         issue_nulldata(pbuddy_adapter, NULL, 1, 0, 0);
11340                 }
11341         }
11342
11343         return ret;
11344 }
11345
11346 void dc_handle_join_done(_adapter *padapter, u8 join_res)
11347 {
11348         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11349         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11350         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
11351         struct mlme_priv *pbuddy_mlmepriv = NULL;
11352         struct mlme_ext_priv *pbuddy_mlmeext = NULL;
11353         struct mlme_ext_info *pbuddy_mlmeinfo = NULL;
11354         WLAN_BSSID_EX *pbuddy_network_mlmeext = NULL;
11355         u8      change_band = _FALSE;
11356
11357         if(pbuddy_adapter != NULL &&
11358                 padapter->DualMacConcurrent == _TRUE)
11359         {
11360                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
11361                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
11362                 pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
11363                 pbuddy_network_mlmeext = &(pbuddy_mlmeinfo->network);
11364
11365                 if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
11366                                 check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
11367                 {
11368                         /* restart and update beacon */
11369                         DBG_8723A("after join, current adapter, CH=%d, BW=%d, offset=%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
11370
11371                         if(join_res >= 0)
11372                         {
11373                                 u8 *p;
11374                                 int     ie_len;
11375                                 struct HT_info_element *pht_info=NULL;
11376
11377                                 if((pbuddy_mlmeext->cur_channel <= 14 && pmlmeext->cur_channel >= 36) ||
11378                                         (pbuddy_mlmeext->cur_channel >= 36 && pmlmeext->cur_channel <= 14))
11379                                 {
11380                                         change_band = _TRUE;
11381                                 }
11382
11383                                 /* sync channel/bwmode/ch_offset with another adapter */
11384                                 pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
11385
11386                                 if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
11387                                 {
11388                                         p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
11389                                         if( p && ie_len)
11390                                         {
11391                                                 pht_info = (struct HT_info_element *)(p+2);
11392                                                 pht_info->infos[0] &= ~(BIT(0)|BIT(1)); /* no secondary channel is present */
11393                                         }
11394
11395                                         if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
11396                                         {
11397                                                 pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
11398
11399                                                 /* to update cur_ch_offset value in beacon */
11400                                                 if( pht_info )
11401                                                 {
11402                                                         switch(pmlmeext->cur_ch_offset)
11403                                                         {
11404                                                                 case HAL_PRIME_CHNL_OFFSET_LOWER:
11405                                                                         pht_info->infos[0] |= 0x1;
11406                                                                         break;
11407                                                                 case HAL_PRIME_CHNL_OFFSET_UPPER:
11408                                                                         pht_info->infos[0] |= 0x3;
11409                                                                         break;
11410                                                                 case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
11411                                                                 default:
11412                                                                         break;
11413                                                         }
11414                                                 }
11415                                         }
11416                                         else if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_20)
11417                                         {
11418                                                 pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
11419                                                 pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
11420
11421                                                 if(pmlmeext->cur_channel>0 && pmlmeext->cur_channel<5)
11422                                                 {
11423                                                         if(pht_info)
11424                                                                 pht_info->infos[0] |= 0x1;
11425
11426                                                         pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
11427                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
11428                                                 }
11429
11430                                                 if(pmlmeext->cur_channel>7 && pmlmeext->cur_channel<(14+1))
11431                                                 {
11432                                                         if(pht_info)
11433                                                                 pht_info->infos[0] |= 0x3;
11434
11435                                                         pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
11436                                                         pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
11437                                                 }
11438
11439                                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
11440                                         }
11441                                 }
11442
11443                                 /*  to update channel value in beacon */
11444                                 pbuddy_network_mlmeext->Configuration.DSConfig = pmlmeext->cur_channel;
11445                                 p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
11446                                 if(p && ie_len>0)
11447                                         *(p + 2) = pmlmeext->cur_channel;
11448
11449                                 p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
11450                                 if( p && ie_len)
11451                                 {
11452                                         pht_info = (struct HT_info_element *)(p+2);
11453                                         pht_info->primary_channel = pmlmeext->cur_channel;
11454                                 }
11455
11456                                 /*  update mlmepriv's cur_network */
11457                                 memcpy(&pbuddy_mlmepriv->cur_network.network, pbuddy_network_mlmeext, pbuddy_network_mlmeext->Length);
11458                         }
11459                         else
11460                         {
11461                                 /*  switch back to original channel/bwmode/ch_offset; */
11462                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
11463                         }
11464
11465                         DBG_8723A("after join, another adapter, CH=%d, BW=%d, offset=%d\n", pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
11466
11467                         if(change_band == _TRUE)
11468                                 dc_change_band(pbuddy_adapter, pbuddy_network_mlmeext);
11469
11470                         DBG_8723A("update pbuddy_adapter's beacon\n");
11471
11472                         update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
11473                 }
11474                 else     if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
11475                 {
11476                         if((pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40) &&
11477                                 (pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_20))
11478                         {
11479                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
11480                         }
11481
11482                         issue_nulldata(pbuddy_adapter, NULL, 0, 0, 0);
11483                 }
11484         }
11485 }
11486
11487 int     dc_check_fwstate(_adapter *padapter, int fw_state)
11488 {
11489         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
11490         struct mlme_priv *pbuddy_mlmepriv = NULL;
11491
11492         if(padapter->pbuddy_adapter != NULL &&
11493                 padapter->DualMacConcurrent == _TRUE)
11494
11495         {
11496                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
11497
11498                 return check_fwstate(pbuddy_mlmepriv, fw_state);
11499         }
11500
11501         return _FALSE;
11502 }
11503
11504 u8 dc_handle_site_survey(_adapter *padapter)
11505 {
11506         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11507         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11508         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
11509
11510         /*  only mac0 can do scan request, help issue nulldata(1) for mac1 */
11511         if(pbuddy_adapter != NULL &&
11512                 padapter->DualMacConcurrent == _TRUE)
11513         {
11514                 if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
11515                 {
11516                         pmlmeext->sitesurvey_res.state = SCAN_TXNULL;
11517
11518                         issue_nulldata(pbuddy_adapter, NULL, 1, 2, 0);
11519
11520                         return _TRUE;
11521                 }
11522         }
11523
11524         return _FALSE;
11525 }
11526
11527 void    dc_report_survey_event(_adapter *padapter, union recv_frame *precv_frame)
11528 {
11529         if(padapter->pbuddy_adapter != NULL &&
11530                 padapter->DualMacConcurrent == _TRUE)
11531         {
11532                 report_survey_event(padapter->pbuddy_adapter, precv_frame);
11533         }
11534 }
11535
11536 void dc_set_channel_bwmode_survey_done(_adapter *padapter)
11537 {
11538         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
11539         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11540         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
11541         struct mlme_priv *pbuddy_mlmepriv = NULL;
11542         struct mlme_ext_priv *pbuddy_mlmeext = NULL;
11543         struct mlme_ext_info *pbuddy_mlmeinfo = NULL;
11544         u8 cur_channel;
11545         u8 cur_bwmode;
11546         u8 cur_ch_offset;
11547
11548         if(pbuddy_adapter != NULL &&
11549                 padapter->DualMacConcurrent == _TRUE)
11550         {
11551                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
11552                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
11553                 pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
11554
11555                 if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
11556                 {
11557                         if(check_fwstate(pmlmepriv, _FW_LINKED) &&
11558                                 (pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40))
11559                         {
11560                                 cur_channel = pmlmeext->cur_channel;
11561                                 cur_bwmode = pmlmeext->cur_bwmode;
11562                                 cur_ch_offset = pmlmeext->cur_ch_offset;
11563                         }
11564                         else
11565                         {
11566                                 cur_channel = pbuddy_mlmeext->cur_channel;
11567                                 cur_bwmode = pbuddy_mlmeext->cur_bwmode;
11568                                 cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
11569                         }
11570                 }
11571                 else
11572                 {
11573                         cur_channel = pmlmeext->cur_channel;
11574                         cur_bwmode = pmlmeext->cur_bwmode;
11575                         cur_ch_offset = pmlmeext->cur_ch_offset;
11576                 }
11577
11578                 set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
11579
11580                 if (is_client_associated_to_ap(pbuddy_adapter) == _TRUE)
11581                 {
11582                         /* issue null data */
11583                         issue_nulldata(pbuddy_adapter, NULL, 0, 0, 0);
11584                 }
11585
11586                 if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
11587                         check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
11588                 {
11589
11590                         DBG_8723A("survey done, current CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
11591
11592                         DBG_8723A("restart pbuddy_adapter's beacon\n");
11593
11594                         update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
11595                 }
11596         }
11597         else
11598         {
11599                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
11600         }
11601 }
11602
11603 void dc_set_ap_channel_bandwidth(_adapter *padapter, u8 channel, u8 channel_offset, u8 bwmode)
11604 {
11605         u8      *p;
11606         u8      val8, cur_channel, cur_bwmode, cur_ch_offset, change_band;
11607         int     ie_len;
11608         struct registry_priv    *pregpriv = &padapter->registrypriv;
11609         struct mlme_priv                *pmlmepriv = &(padapter->mlmepriv);
11610         WLAN_BSSID_EX           *pnetwork = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;
11611         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
11612         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
11613         struct HT_info_element  *pht_info=NULL;
11614         _adapter        *pbuddy_adapter = padapter->pbuddy_adapter;
11615         struct mlme_priv        *pbuddy_mlmepriv = NULL;
11616         struct mlme_ext_priv    *pbuddy_mlmeext = NULL;
11617
11618         DBG_8723A("dualmac_concurrent_ap_set_channel_bwmode ==>\n");
11619
11620         cur_channel = channel;
11621         cur_bwmode = bwmode;
11622         cur_ch_offset = channel_offset;
11623         change_band = _FALSE;
11624
11625         p = rtw_get_ie((pnetwork->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
11626         if( p && ie_len)
11627         {
11628                 pht_info = (struct HT_info_element *)(p+2);
11629         }
11630
11631         if(pbuddy_adapter != NULL &&
11632                 padapter->DualMacConcurrent == _TRUE)
11633         {
11634                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
11635                 pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
11636
11637                 if(!check_fwstate(pbuddy_mlmepriv, _FW_LINKED|_FW_UNDER_LINKING|_FW_UNDER_SURVEY))
11638                 {
11639                         set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
11640                 }
11641                 else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED)==_TRUE)
11642                 {
11643                         /* To sync cur_channel/cur_bwmode/cur_ch_offset with another adapter */
11644                         DBG_8723A("Another iface is at linked state, sync cur_channel/cur_bwmode/cur_ch_offset\n");
11645                         DBG_8723A("Another adapter, CH=%d, BW=%d, offset=%d\n", pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
11646                         DBG_8723A("Current adapter, CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
11647
11648                         cur_channel = pbuddy_mlmeext->cur_channel;
11649                         if(cur_bwmode == HT_CHANNEL_WIDTH_40)
11650                         {
11651                                 if(pht_info)
11652                                         pht_info->infos[0] &= ~(BIT(0)|BIT(1));
11653
11654                                 if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
11655                                 {
11656                                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
11657
11658                                         /* to update cur_ch_offset value in beacon */
11659                                         if(pht_info)
11660                                         {
11661                                                 switch(cur_ch_offset)
11662                                                 {
11663                                                         case HAL_PRIME_CHNL_OFFSET_LOWER:
11664                                                                 pht_info->infos[0] |= 0x1;
11665                                                                 break;
11666                                                         case HAL_PRIME_CHNL_OFFSET_UPPER:
11667                                                                 pht_info->infos[0] |= 0x3;
11668                                                                 break;
11669                                                         case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
11670                                                         default:
11671                                                                 break;
11672                                                 }
11673                                         }
11674                                 }
11675                                 else if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_20)
11676                                 {
11677                                         cur_bwmode = HT_CHANNEL_WIDTH_20;
11678                                         cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
11679
11680                                         if(cur_channel>0 && cur_channel<5)
11681                                         {
11682                                                 if(pht_info)
11683                                                         pht_info->infos[0] |= 0x1;
11684
11685                                                 cur_bwmode = HT_CHANNEL_WIDTH_40;
11686                                                 cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
11687                                         }
11688
11689                                         if(cur_channel>7 && cur_channel<(14+1))
11690                                         {
11691                                                 if(pht_info)
11692                                                         pht_info->infos[0] |= 0x3;
11693
11694                                                 cur_bwmode = HT_CHANNEL_WIDTH_40;
11695                                                 cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
11696                                         }
11697
11698                                         set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
11699                                 }
11700                         }
11701
11702                         /*  to update channel value in beacon */
11703                         pnetwork->Configuration.DSConfig = cur_channel;
11704                         p = rtw_get_ie((pnetwork->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (pnetwork->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
11705                         if(p && ie_len>0)
11706                                 *(p + 2) = cur_channel;
11707
11708                         if(pht_info)
11709                                 pht_info->primary_channel = cur_channel;
11710                 }
11711         }
11712         else
11713         {
11714                 set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
11715         }
11716
11717         DBG_8723A("CH=%d, BW=%d, offset=%d\n", cur_channel, cur_bwmode, cur_ch_offset);
11718
11719         if((channel <= 14 && cur_channel >= 36) ||
11720                 (channel >= 36 && cur_channel <= 14))
11721         {
11722                 change_band = _TRUE;
11723         }
11724
11725         pmlmeext->cur_channel = cur_channel;
11726         pmlmeext->cur_bwmode = cur_bwmode;
11727         pmlmeext->cur_ch_offset = cur_ch_offset;
11728
11729         if(change_band == _TRUE)
11730                 dc_change_band(padapter, pnetwork);
11731
11732         DBG_8723A("dualmac_concurrent_ap_set_channel_bwmode <==\n");
11733 }
11734
11735 void dc_resume_xmit(_adapter *padapter)
11736 {
11737         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
11738
11739         if(pbuddy_adapter != NULL &&
11740                 padapter->DualMacConcurrent == _TRUE)
11741         {
11742                 DBG_8723A("dc_resume_xmit,  resume pbuddy_adapter Tx\n");
11743                 rtw_os_xmit_schedule(pbuddy_adapter);
11744         }
11745 }
11746
11747 u8      dc_check_xmit(_adapter *padapter)
11748 {
11749         PADAPTER pbuddy_adapter = padapter->pbuddy_adapter;
11750         struct mlme_priv *pbuddy_mlmepriv = NULL;
11751
11752         if(pbuddy_adapter != NULL &&
11753                 padapter->DualMacConcurrent == _TRUE)
11754         {
11755                 pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
11756                 if (check_fwstate(pbuddy_mlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == _TRUE)
11757                 {
11758                         DBG_8723A("dc_check_xmit  pbuddy_adapter is under survey or under linking\n");
11759                         return _FALSE;
11760                 }
11761         }
11762
11763         return _TRUE;
11764 }
11765 #endif
11766
11767 #ifdef CONFIG_CONCURRENT_MODE
11768 int check_buddy_mlmeinfo_state(_adapter *padapter, u32 state)
11769 {
11770         PADAPTER pbuddy_adapter;
11771         struct mlme_ext_priv *pbuddy_mlmeext;
11772         struct mlme_ext_info *pbuddy_mlmeinfo;
11773
11774         if(padapter == NULL)
11775                 return _FALSE;
11776
11777         pbuddy_adapter = padapter->pbuddy_adapter;
11778
11779         if(pbuddy_adapter == NULL)
11780                 return _FALSE;
11781
11782         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
11783         pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
11784
11785         if((pbuddy_mlmeinfo->state&0x03) == state)
11786                 return _TRUE;
11787
11788         return _FALSE;
11789 }
11790
11791 int concurrent_chk_start_clnt_join(_adapter *padapter)
11792 {
11793         int ret = _FAIL;
11794         PADAPTER pbuddy_adapter;
11795         struct mlme_ext_priv *pbuddy_mlmeext;
11796         struct mlme_ext_info    *pbuddy_pmlmeinfo;
11797         struct mlme_priv *pbuddy_mlmepriv;
11798         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
11799         unsigned char   cur_ch = pmlmeext->cur_channel;
11800         unsigned char   cur_bw = pmlmeext->cur_bwmode;
11801         unsigned char   cur_ch_offset = pmlmeext->cur_ch_offset;
11802
11803         if(!rtw_buddy_adapter_up(padapter))
11804         {
11805                 goto start_join_set_ch_bw;
11806         }
11807
11808         pbuddy_adapter = padapter->pbuddy_adapter;
11809         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
11810         pbuddy_pmlmeinfo = &(pbuddy_mlmeext->mlmext_info);
11811         pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
11812
11813         if((pbuddy_pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)/* for AP MODE */
11814         {
11815                 bool inform_ch_switch = _FALSE;
11816                 if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel)
11817                 {
11818                         inform_ch_switch = _TRUE;
11819                 }
11820                 else if((pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40) &&
11821                         (pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40) &&
11822                         (pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset))
11823                 {
11824                         inform_ch_switch = _TRUE;
11825                 }
11826                 else if((pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_20) &&
11827                         (pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40))
11828                 {
11829                         inform_ch_switch = _FALSE;
11830                         cur_ch = pmlmeext->cur_channel;
11831                         cur_bw = pbuddy_mlmeext->cur_bwmode;
11832                         cur_ch_offset = pbuddy_mlmeext->cur_ch_offset;
11833                 }
11834
11835                 if (inform_ch_switch) {
11836                         #ifdef CONFIG_SPCT_CH_SWITCH
11837                         if (1) {
11838                                 rtw_ap_inform_ch_switch(pbuddy_adapter, pmlmeext->cur_channel , pmlmeext->cur_ch_offset);
11839                         } else
11840                         #endif
11841                         {
11842                                 /* issue deauth to all stas if if2 is at ap mode */
11843                                 rtw_sta_flush(pbuddy_adapter);
11844                         }
11845                         rtw_hal_set_hwreg(padapter, HW_VAR_CHECK_TXBUF, 0);
11846                 }
11847         }
11848         else if(check_fwstate(pbuddy_mlmepriv, _FW_LINKED) == _TRUE &&
11849                 check_fwstate(pbuddy_mlmepriv, WIFI_STATION_STATE) == _TRUE) /* for Client Mode/p2p client */
11850         {
11851 #if defined(CONFIG_P2P) && defined(CONFIG_IOCTL_CFG80211)
11852                 struct wifidirect_info *pbuddy_wdinfo = &(pbuddy_adapter->wdinfo);
11853                 if(!rtw_p2p_chk_state(pbuddy_wdinfo, P2P_STATE_NONE))
11854                 {
11855                         goto start_join_set_ch_bw;/* wlan0-sta mode has higher priority than p2p0-p2p client */
11856                 }
11857 #endif /* CONFIG_P2P && CONFIG_IOCTL_CFG80211 */
11858
11859                 if(pmlmeext->cur_channel != pbuddy_mlmeext->cur_channel)
11860                 {
11861                         DBG_8723A("start_clnt_join(ch=%d), but channel mismatch with buddy(ch=%d) interface\n",
11862                                 pmlmeext->cur_channel, pbuddy_mlmeext->cur_channel);
11863
11864                         report_join_res(padapter, (-4));
11865
11866                         return ret;
11867                 }
11868
11869                 if((pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40) &&
11870                         (pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40) &&
11871                         (pmlmeext->cur_ch_offset != pbuddy_mlmeext->cur_ch_offset))
11872                 {
11873                         DBG_8723A("start_clnt_join(bwmode=%d, ch_offset=%d), but bwmode & ch_offset mismatch with buddy(bwmode=%d, ch_offset=%d) interface\n",
11874                                 pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
11875
11876                         report_join_res(padapter, (-4));
11877
11878                         return ret;
11879                 }
11880
11881         }
11882
11883 start_join_set_ch_bw:
11884
11885         set_channel_bwmode(padapter, cur_ch, cur_ch_offset, cur_bw);
11886
11887         return _SUCCESS;
11888 }
11889
11890 void concurrent_chk_joinbss_done(_adapter *padapter, int join_res)
11891 {
11892         struct mlme_ext_priv    *pmlmeext;
11893         struct mlme_ext_info    *pmlmeinfo;
11894         PADAPTER pbuddy_adapter;
11895         struct mlme_priv *pbuddy_mlmepriv;
11896         struct mlme_ext_priv *pbuddy_mlmeext;
11897         struct mlme_ext_info *pbuddy_mlmeinfo;
11898         WLAN_BSSID_EX *pbuddy_network_mlmeext;
11899
11900         if(!rtw_buddy_adapter_up(padapter))
11901         {
11902                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
11903                 return;
11904         }
11905
11906         pmlmeext = &padapter->mlmeextpriv;
11907         pmlmeinfo = &(pmlmeext->mlmext_info);
11908
11909         pbuddy_adapter = padapter->pbuddy_adapter;
11910         pbuddy_mlmepriv = &(pbuddy_adapter->mlmepriv);
11911         pbuddy_mlmeext = &pbuddy_adapter->mlmeextpriv;
11912         pbuddy_mlmeinfo = &(pbuddy_mlmeext->mlmext_info);
11913         pbuddy_network_mlmeext = &(pbuddy_mlmeinfo->network);
11914
11915         if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_AP_STATE) &&
11916                         check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
11917         {
11918                 /* restart and update beacon */
11919
11920                 DBG_8723A("after join,primary adapter, CH=%d, BW=%d, offset=%d\n"
11921                         , pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
11922
11923                 if(join_res >= 0)
11924                 {
11925                         u8 *p;
11926                         int     ie_len;
11927                         struct HT_info_element *pht_info=NULL;
11928
11929                         /* sync channel/bwmode/ch_offset with primary adapter */
11930                         pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
11931                         if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
11932                         {
11933                                 p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
11934                                 if( p && ie_len)
11935                                 {
11936                                         pht_info = (struct HT_info_element *)(p+2);
11937                                         pht_info->infos[0] &= ~(BIT(0)|BIT(1)); /* no secondary channel is present */
11938                                 }
11939
11940                                 if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
11941                                 {
11942                                         pbuddy_mlmeext->cur_ch_offset = pmlmeext->cur_ch_offset;
11943
11944                                         /* to update cur_ch_offset value in beacon */
11945                                         if( pht_info )
11946                                         {
11947                                                 switch(pmlmeext->cur_ch_offset)
11948                                                 {
11949                                                         case HAL_PRIME_CHNL_OFFSET_LOWER:
11950                                                                 pht_info->infos[0] |= 0x1;
11951                                                                 break;
11952                                                         case HAL_PRIME_CHNL_OFFSET_UPPER:
11953                                                                 pht_info->infos[0] |= 0x3;
11954                                                                 break;
11955                                                         case HAL_PRIME_CHNL_OFFSET_DONT_CARE:
11956                                                         default:
11957                                                                 break;
11958                                                 }
11959
11960                                         }
11961
11962                                 }
11963                                 else if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_20)
11964                                 {
11965                                         if(pmlmeext->cur_channel>=1 && pmlmeext->cur_channel<=4)
11966                                         {
11967                                                 if(pht_info)
11968                                                         pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;
11969
11970                                                 pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
11971                                                 pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
11972                                         }
11973                                         else if(pmlmeext->cur_channel>=5 && pmlmeext->cur_channel<=14)
11974                                         {
11975                                                 if(pht_info)
11976                                                         pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
11977
11978                                                 pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
11979                                                 pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
11980                                         }
11981                                         else
11982                                         {
11983                                                 switch(pmlmeext->cur_channel)
11984                                                 {
11985                                                         case 36:
11986                                                         case 44:
11987                                                         case 52:
11988                                                         case 60:
11989                                                         case 100:
11990                                                         case 108:
11991                                                         case 116:
11992                                                         case 124:
11993                                                         case 132:
11994                                                         case 149:
11995                                                         case 157:
11996                                                         {
11997                                                                 if(pht_info)
11998                                                                         pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE;
11999                                                                 pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
12000                                                                 pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
12001                                                                 break;
12002                                                         }
12003                                                         case 40:
12004                                                         case 48:
12005                                                         case 56:
12006                                                         case 64:
12007                                                         case 104:
12008                                                         case 112:
12009                                                         case 120:
12010                                                         case 128:
12011                                                         case 136:
12012                                                         case 153:
12013                                                         case 161:
12014                                                         {
12015                                                                 if(pht_info)
12016                                                                         pht_info->infos[0] |= HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
12017
12018                                                                 pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
12019                                                                 pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
12020                                                                 break;
12021                                                         }
12022                                                         default:
12023                                                                 if(pht_info)
12024                                                                         pht_info->infos[0] &= ~HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW;
12025                                                                 pbuddy_mlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
12026                                                                 pbuddy_mlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
12027                                                                 break;
12028
12029                                                 }
12030
12031                                         }
12032
12033                                 }
12034
12035                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
12036
12037                         }
12038                         else
12039                         {
12040                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
12041                         }
12042
12043                         /*  to update channel value in beacon */
12044                         pbuddy_network_mlmeext->Configuration.DSConfig = pmlmeext->cur_channel;
12045                         p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _DSSET_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
12046                         if(p && ie_len>0)
12047                                 *(p + 2) = pmlmeext->cur_channel;
12048
12049                         p = rtw_get_ie((pbuddy_network_mlmeext->IEs + sizeof(NDIS_802_11_FIXED_IEs)), _HT_ADD_INFO_IE_, &ie_len, (pbuddy_network_mlmeext->IELength - sizeof(NDIS_802_11_FIXED_IEs)));
12050                         if( p && ie_len)
12051                         {
12052                                 pht_info = (struct HT_info_element *)(p+2);
12053                                 pht_info->primary_channel = pmlmeext->cur_channel;
12054                         }
12055
12056                 }
12057                 else
12058                 {
12059                         /*  switch back to original channel/bwmode/ch_offset; */
12060                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
12061                 }
12062
12063                 DBG_8723A("after join, second adapter, CH=%d, BW=%d, offset=%d\n", pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_bwmode, pbuddy_mlmeext->cur_ch_offset);
12064
12065                 DBG_8723A("update pbuddy_adapter's beacon\n");
12066
12067                 update_beacon(pbuddy_adapter, 0, NULL, _TRUE);
12068
12069         }
12070         else if(((pbuddy_mlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) &&
12071                         check_fwstate(pbuddy_mlmepriv, _FW_LINKED))
12072         {
12073                 if(join_res >= 0)
12074                 {
12075                         pbuddy_mlmeext->cur_channel = pmlmeext->cur_channel;
12076                         if(pbuddy_mlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
12077                                 set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
12078                         else if(pmlmeext->cur_bwmode == HT_CHANNEL_WIDTH_40)
12079                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
12080                         else
12081                                 set_channel_bwmode(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
12082                 }
12083                 else
12084                 {
12085                         /*  switch back to original channel/bwmode/ch_offset; */
12086                         set_channel_bwmode(padapter, pbuddy_mlmeext->cur_channel, pbuddy_mlmeext->cur_ch_offset, pbuddy_mlmeext->cur_bwmode);
12087                 }
12088         }
12089         else
12090         {
12091                 set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
12092         }
12093 }
12094 #endif /* CONFIG_CONCURRENT_MODE */
12095
12096 u8 set_ch_hdl(_adapter *padapter, u8 *pbuf)
12097 {
12098         struct set_ch_parm *set_ch_parm;
12099         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
12100         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12101
12102         if(!pbuf)
12103                 return H2C_PARAMETERS_ERROR;
12104
12105         set_ch_parm = (struct set_ch_parm *)pbuf;
12106
12107         DBG_8723A(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
12108                 FUNC_NDEV_ARG(padapter->pnetdev),
12109                 set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
12110
12111         pmlmeext->cur_channel = set_ch_parm->ch;
12112         pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
12113         pmlmeext->cur_bwmode = set_ch_parm->bw;
12114
12115         set_channel_bwmode(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
12116
12117         return  H2C_SUCCESS;
12118 }
12119
12120 u8 set_chplan_hdl(_adapter *padapter, unsigned char *pbuf)
12121 {
12122         struct SetChannelPlan_param *setChannelPlan_param;
12123         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
12124         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
12125
12126         if(!pbuf)
12127                 return H2C_PARAMETERS_ERROR;
12128
12129         setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
12130
12131         pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
12132         init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
12133
12134         return  H2C_SUCCESS;
12135 }
12136
12137 u8 led_blink_hdl(_adapter *padapter, unsigned char *pbuf)
12138 {
12139         struct LedBlink_param *ledBlink_param;
12140
12141         if(!pbuf)
12142                 return H2C_PARAMETERS_ERROR;
12143
12144         ledBlink_param = (struct LedBlink_param *)pbuf;
12145
12146         #ifdef CONFIG_LED_HANDLED_BY_CMD_THREAD
12147         BlinkHandler(ledBlink_param->pLed);
12148         #endif
12149
12150         return  H2C_SUCCESS;
12151 }
12152
12153 u8 set_csa_hdl(_adapter *padapter, unsigned char *pbuf)
12154 {
12155 #ifdef CONFIG_DFS
12156         struct SetChannelSwitch_param *setChannelSwitch_param;
12157         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
12158         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
12159         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
12160         u8 new_ch_no;
12161         u8 gval8 = 0x00, sval8 = 0xff;
12162
12163         if(!pbuf)
12164                 return H2C_PARAMETERS_ERROR;
12165
12166         setChannelSwitch_param = (struct SetChannelSwitch_param *)pbuf;
12167         new_ch_no = setChannelSwitch_param->new_ch_no;
12168
12169         rtw_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
12170
12171         rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &sval8);
12172
12173         DBG_8723A("DFS detected! Swiching channel to %d!\n", new_ch_no);
12174         SelectChannel(padapter, new_ch_no);
12175
12176         rtw_hal_set_hwreg(padapter, HW_VAR_TXPAUSE, &gval8);
12177
12178         rtw_free_network_queue(padapter, _TRUE);
12179         rtw_indicate_disconnect(padapter);
12180
12181         if ( ((new_ch_no >= 52) && (new_ch_no <= 64)) ||((new_ch_no >= 100) && (new_ch_no <= 140)) ) {
12182                 DBG_8723A("Switched to DFS band (ch %02x) again!!\n", new_ch_no);
12183         }
12184
12185         return  H2C_SUCCESS;
12186 #else
12187         return  H2C_REJECTED;
12188 #endif /* CONFIG_DFS */
12189 }
12190
12191 /*  TDLS_WRCR           : write RCR DATA BIT */
12192 /*  TDLS_SD_PTI         : issue peer traffic indication */
12193 /*  TDLS_CS_OFF         : go back to the channel linked with AP, terminating channel switch procedure */
12194 /*  TDLS_INIT_CH_SEN    : init channel sensing, receive all data and mgnt frame */
12195 /*  TDLS_DONE_CH_SEN: channel sensing and report candidate channel */
12196 /*  TDLS_OFF_CH         : first time set channel to off channel */
12197 /*  TDLS_BASE_CH                : go back tp the channel linked with AP when set base channel as target channel */
12198 /*  TDLS_P_OFF_CH       : periodically go to off channel */
12199 /*  TDLS_P_BASE_CH      : periodically go back to base channel */
12200 /*  TDLS_RS_RCR         : restore RCR */
12201 /*  TDLS_CKALV_PH1      : check alive timer phase1 */
12202 /*  TDLS_CKALV_PH2      : check alive timer phase2 */
12203 /*  TDLS_FREE_STA       : free tdls sta */
12204 u8 tdls_hdl(_adapter *padapter, unsigned char *pbuf)
12205 {
12206 #ifdef CONFIG_TDLS
12207         _irqL irqL;
12208         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
12209         struct TDLSoption_param *TDLSoption;
12210         struct sta_info *ptdls_sta;
12211         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
12212         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
12213         u8 survey_channel, i, min, option;
12214
12215         if(!pbuf)
12216                 return H2C_PARAMETERS_ERROR;
12217
12218         TDLSoption = (struct TDLSoption_param *)pbuf;
12219
12220         ptdls_sta = rtw_get_stainfo( &(padapter->stapriv), TDLSoption->addr );
12221         option = TDLSoption->option;
12222
12223         if( ptdls_sta == NULL )
12224         {
12225                 if( option != TDLS_RS_RCR )
12226                         return H2C_REJECTED;
12227         }
12228
12229         /* spin_lock_bh(&(ptdlsinfo->hdl_lock)); */
12230         DBG_8723A("[%s] option:%d\n", __FUNCTION__, option);
12231
12232         switch(option){
12233                 case TDLS_WRCR:
12234                         /* As long as TDLS handshake success, we should set RCR_CBSSID_DATA bit to 0 */
12235                         /* such we can receive all kinds of data frames. */
12236                         rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_WRCR, 0);
12237                         DBG_8723A("TDLS with "MAC_FMT"\n", MAC_ARG(ptdls_sta->hwaddr));
12238
12239                         pmlmeinfo->FW_sta_info[ptdls_sta->mac_id].psta = ptdls_sta;
12240                         /* set TDLS sta rate. */
12241                         set_sta_rate(padapter, ptdls_sta);
12242                         break;
12243                 case TDLS_SD_PTI:
12244                         issue_tdls_peer_traffic_indication(padapter, ptdls_sta);
12245                         break;
12246                 case TDLS_CS_OFF:
12247                         _cancel_timer_ex(&ptdls_sta->base_ch_timer);
12248                         _cancel_timer_ex(&ptdls_sta->off_ch_timer);
12249                         SelectChannel(padapter, pmlmeext->cur_channel);
12250                         ptdls_sta->tdls_sta_state &= ~(TDLS_CH_SWITCH_ON_STATE |
12251                                                                 TDLS_PEER_AT_OFF_STATE |
12252                                                                 TDLS_AT_OFF_CH_STATE);
12253                         DBG_8723A("go back to base channel\n ");
12254                         issue_nulldata(padapter, NULL, 0, 0, 0);
12255                         break;
12256                 case TDLS_INIT_CH_SEN:
12257                         rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_INIT_CH_SEN, 0);
12258                         pmlmeext->sitesurvey_res.channel_idx = 0;
12259                         ptdls_sta->option = TDLS_DONE_CH_SEN;
12260                         rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_DONE_CH_SEN);
12261                         break;
12262                 case TDLS_DONE_CH_SEN:
12263                         survey_channel = pmlmeext->channel_set[pmlmeext->sitesurvey_res.channel_idx].ChannelNum;
12264                         if(survey_channel){
12265                                 SelectChannel(padapter, survey_channel);
12266                                 ptdlsinfo->cur_channel = survey_channel;
12267                                 pmlmeext->sitesurvey_res.channel_idx++;
12268                                 _set_timer(&ptdls_sta->option_timer, SURVEY_TO);
12269                         }else{
12270                                 SelectChannel(padapter, pmlmeext->cur_channel);
12271
12272                                 rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_DONE_CH_SEN, 0);
12273
12274                                 if(ptdlsinfo->ch_sensing==1){
12275                                         ptdlsinfo->ch_sensing=0;
12276                                         ptdlsinfo->cur_channel=1;
12277                                         min=ptdlsinfo->collect_pkt_num[0];
12278                                         for(i=1; i<MAX_CHANNEL_NUM-1; i++){
12279                                                 if(min > ptdlsinfo->collect_pkt_num[i]){
12280                                                         ptdlsinfo->cur_channel=i+1;
12281                                                         min=ptdlsinfo->collect_pkt_num[i];
12282                                                 }
12283                                                 ptdlsinfo->collect_pkt_num[i]=0;
12284                                         }
12285                                         ptdlsinfo->collect_pkt_num[0]=0;
12286                                         ptdlsinfo->candidate_ch=ptdlsinfo->cur_channel;
12287                                         DBG_8723A("TDLS channel sensing done, candidate channel: %02x\n", ptdlsinfo->candidate_ch);
12288                                         ptdlsinfo->cur_channel=0;
12289
12290                                 }
12291
12292                                 if(ptdls_sta->tdls_sta_state & TDLS_PEER_SLEEP_STATE){
12293                                         ptdls_sta->tdls_sta_state |= TDLS_APSD_CHSW_STATE;
12294                                 }else{
12295                                         /* send null data with pwrbit==1 before send ch_switching_req to peer STA. */
12296                                         issue_nulldata(padapter, NULL, 1, 0, 0);
12297
12298                                         ptdls_sta->tdls_sta_state |= TDLS_CH_SW_INITIATOR_STATE;
12299
12300                                         issue_tdls_ch_switch_req(padapter, ptdls_sta->hwaddr);
12301                                         DBG_8723A("issue tdls ch switch req\n");
12302                                 }
12303                         }
12304                         break;
12305                 case TDLS_OFF_CH:
12306                         issue_nulldata(padapter, NULL, 1, 0, 0);
12307                         SelectChannel(padapter, ptdls_sta->off_ch);
12308
12309                         DBG_8723A("change channel to tar ch:%02x\n", ptdls_sta->off_ch);
12310                         ptdls_sta->tdls_sta_state |= TDLS_AT_OFF_CH_STATE;
12311                         ptdls_sta->tdls_sta_state &= ~(TDLS_PEER_AT_OFF_STATE);
12312                         _set_timer(&ptdls_sta->option_timer, (u32)ptdls_sta->ch_switch_time);
12313                         break;
12314                 case TDLS_BASE_CH:
12315                         _cancel_timer_ex(&ptdls_sta->base_ch_timer);
12316                         _cancel_timer_ex(&ptdls_sta->off_ch_timer);
12317                         SelectChannel(padapter, pmlmeext->cur_channel);
12318                         ptdls_sta->tdls_sta_state &= ~(TDLS_CH_SWITCH_ON_STATE |
12319                                                                 TDLS_PEER_AT_OFF_STATE |
12320                                                                 TDLS_AT_OFF_CH_STATE);
12321                         DBG_8723A("go back to base channel\n ");
12322                         issue_nulldata(padapter, NULL, 0, 0, 0);
12323                         _set_timer(&ptdls_sta->option_timer, (u32)ptdls_sta->ch_switch_time);
12324                         break;
12325                 case TDLS_P_OFF_CH:
12326                         SelectChannel(padapter, pmlmeext->cur_channel);
12327                         issue_nulldata(padapter, NULL, 0, 0, 0);
12328                         DBG_8723A("change channel to base ch:%02x\n", pmlmeext->cur_channel);
12329                         ptdls_sta->tdls_sta_state &= ~(TDLS_PEER_AT_OFF_STATE| TDLS_AT_OFF_CH_STATE);
12330                         _set_timer(&ptdls_sta->off_ch_timer, TDLS_STAY_TIME);
12331                         break;
12332                 case TDLS_P_BASE_CH:
12333                         issue_nulldata(ptdls_sta->padapter, NULL, 1, 0, 0);
12334                         SelectChannel(padapter, ptdls_sta->off_ch);
12335                         DBG_8723A("change channel to off ch:%02x\n", ptdls_sta->off_ch);
12336                         ptdls_sta->tdls_sta_state |= TDLS_AT_OFF_CH_STATE;
12337                         if((ptdls_sta->tdls_sta_state & TDLS_PEER_AT_OFF_STATE) != TDLS_PEER_AT_OFF_STATE){
12338                                 issue_nulldata_to_TDLS_peer_STA(padapter, ptdls_sta, 0);
12339                         }
12340                         _set_timer(&ptdls_sta->base_ch_timer, TDLS_STAY_TIME);
12341                         break;
12342                 case TDLS_RS_RCR:
12343                         rtw_hal_set_hwreg(padapter, HW_VAR_TDLS_RS_RCR, 0);
12344                         DBG_8723A("wirte REG_RCR, set bit6 on\n");
12345                         break;
12346                 case TDLS_CKALV_PH1:
12347                         _set_timer(&ptdls_sta->alive_timer2, TDLS_ALIVE_TIMER_PH2);
12348                         break;
12349                 case TDLS_CKALV_PH2:
12350                         _set_timer(&ptdls_sta->alive_timer1, TDLS_ALIVE_TIMER_PH1);
12351                         break;
12352                 case TDLS_FREE_STA:
12353                         free_tdls_sta(padapter, ptdls_sta);
12354                         break;
12355
12356         }
12357
12358         /* spin_unlock_bh(&(ptdlsinfo->hdl_lock)); */
12359
12360         return H2C_SUCCESS;
12361 #else
12362         return H2C_REJECTED;
12363 #endif /* CONFIG_TDLS */
12364 }